// Connect to terminal program via this uart at 115200 baud
#define UART1_RX_BUFFER_SIZE 40

#include "sys/Axon.h"
#include "Gait/GaitDesigner.h"
#include "servos.h"
#include "rprintf.h"
#include "uart.h"

// Define servos for my robot
#include "../../AAA_WebbotLib/hardware.c"

// Create the list - remember the place an & at the start of each servo name
SERVO_LIST right_arm[] 	= {&right_collar, &right_shoulder, &right_elbow};
SERVO_LIST left_arm[] 	= {&left_collar, &left_shoulder, &left_elbow};
SERVO_LIST right_leg[] 	= {&right_foot, &right_ankle, &right_knee, &right_thigh, &right_hip};
SERVO_LIST left_leg[] 	= {&left_foot, &left_ankle, &left_knee, &left_thigh, &left_hip};

// Create a driver for the list of servos
SERVO_DRIVER bank1 = MAKE_SERVO_DRIVER(right_arm);
SERVO_DRIVER bank2 = MAKE_SERVO_DRIVER(left_arm);
SERVO_DRIVER bank3 = MAKE_SERVO_DRIVER(right_leg);
SERVO_DRIVER bank4 = MAKE_SERVO_DRIVER(left_leg);

// Create a list of ALL servos across ALL banks
// The ordering in this list is the ordeing used by hte teminal program
ACTUATOR_LIST body[] = 
	{&right_foot.actuator, &right_ankle.actuator, &right_knee.actuator, &right_thigh.actuator, 
	&right_hip.actuator, &right_collar.actuator, &right_shoulder.actuator, &right_elbow.actuator, 
	&left_foot.actuator, &left_ankle.actuator, &left_knee.actuator, &left_thigh.actuator, 
	&left_hip.actuator,	&left_collar.actuator, &left_shoulder.actuator, &left_elbow.actuator};
			
GAIT_DESIGNER gait = MAKE_GAIT_DESIGNER(body, UART1, (BAUD_RATE)115200);

void appInitHardware(void)
{
	// Initialise the servo controller
	servosInit(&bank1, TIMER1);
	servosInit(&bank2, TIMER3);
	servosInit(&bank3, TIMER4);
	servosInit(&bank4, TIMER5);
	
	rprintfInit(&uart1SendByte);
}

TICK_COUNT appInitSoftware(TICK_COUNT loopStart)
{
	return 0;
}

// This is hte main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart)
{
	gaitDesignerProcess(&gait);
	return 0;
}