#include <stdio.h>

#define	   MAX_SEQUENCE_STEPS	16

typedef unsigned char u08 ;

enum servoChannels { HEAD, LEFT_LEGS, RIGHT_LEGS, CENTER_LEGS, ARM, GRIPPER, CHANNEL_MAX};
//typedef u08 pose_t[CHANNEL_MAX][2];

enum presetNames {
	HARD_LEFT=0, 	LEFT=1, 		CENTER=2, 	RIGHT=3, 		HARD_RIGHT=4, 
	FORWARD=0, 		SEMI_FORWARD=1, /*CENTER,*/ SEMI_BACKWARD=3,BACKWARD=4, 	
	FULL_CLOSE=0, 	CLOSE=1, 		/*CENTER,*/ OPEN=3, 		FULL_OPEN=4 	
};
//typedef struct pair_t { int a; int b;);

//a pair is a single servo and its position
typedef struct { enum servoChannels channel; enum presetNames position; } pair_t ;
//a pose is a set of pairs - size is so we know how many pairs are in each pose
//typedef struct  { pair_t set[CHANNEL_MAX]; } pose_t;
typedef struct  { size_t count; pair_t set[CHANNEL_MAX]; } pose_t ;
//typedef pair_t pose_t[CHANNEL_MAX];

typedef pair_t preset_t[CHANNEL_MAX];
//a sequence is a set of poses (null-terminated to save memory)
typedef pose_t* sequence_t[] ;



//provides a way to set one servo to a particular named position
const u08 presetData[5][CHANNEL_MAX]= {
/*head 	left	right	center	arm		gripper */
{50,	180,	80, 	50, 	50, 	50 },	//HARD_LEFT/FORWARD/FULL_OPEN
{80,	150,	110,	80, 	80, 	80 },	//LEFT/SEMI_FORWARD/OPEN
{128,	128,	128,	128,	128,	128 },  //CENTER/CENTER/CENTER
{170,	100,	160,	170,	145,	170 },	//RIGHT/SEMI_BACKWARD/CLOSE
{200,	80, 	180,	200,	160,	200 },	//HARD_RIGHT/BACKWARD/FULL_CLOSE
};

//preset_t preset1 = {{HEAD, CENTER}, {RIGHT_LEGS, CENTER}};
pose_t pose1 = { 2, {{HEAD, CENTER}, {RIGHT_LEGS, CENTER}} };
//preset_t preset2 = {{ARM, CLOSE}, {GRIPPER, FULL_OPEN}, {LEFT_LEGS, BACKWARD}};
pose_t pose2 = { 3, {{ARM, CLOSE}, {GRIPPER, FULL_OPEN}, {LEFT_LEGS, BACKWARD}} };

preset_t preset3 = {{1, 2}, {3,128}};
preset_t preset4 = {};

//usage: servoPreset(LEFT_LEGS, FORWARD);
void servoPreset(int channel, int position){
	printf("%d, %d\n", channel, presetData[position][channel]);
}

void doPresetPose(pose_t pose) {
	int i =0;
	printf("\npose: \n");
	while (i<pose.count){ 
	//	servoPreset(pose[i].channel, pose[i].position);
		servoPreset(pose.set[i].channel, pose.set[i].position);
		i++;
	}
}
//struct pose *ptr = myseq;  while ( ptr ) { // do stuff; ptr++; }


void doSeq(sequence_t mySequence) {
	//want to increment along by one pose_t width, starting at mySequence
	int i=0;
	//pose_t* ptr = (pose_t*) &mySequence;
	//while (ptr) { //sequence should end in a NULL
	while (( mySequence[i] != NULL ) && (i<MAX_SEQUENCE_STEPS)) {
		//doPresetPose((*sequence)[i]);
		doPresetPose(*mySequence[i]);
		//doPresetPose(*ptr);
		//timerPause(WALKPAUSE);
		i++;
		//ptr++;
		//ptr+=*ptr.size;
	}
}

int main(){
	//servoPreset(1,1);
	doPresetPose(pose2);
	//pair_t pair;
	//printf("%d\n", (sizeof(pose2)/sizeof(pair)) );
	//printf("%d\n", sizeof(preset1) );
	printf("meep\n");	
	sequence_t myseq = { &pose1, &pose2, &pose1, NULL };
	doSeq(myseq);
}