#include "h-bridge.h"
#include "pwm.h"
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
//#define H_BRIDGE_DEBUG

// state to function mapping, MUST BE same order as the state_t enumeration in h-bridge.h
static const state_fn_t state_function_map[NUM_STATES] =  {
	&is_halted,		// IS_HALTED
	&full_brake,	// FULL_BRAKE
	&going_fwd,		// GOING_FWD
	&going_rev,		// GOING_REV
	&braking_fwd1, 	// BRAKING_FWD1
	&braking_fwd2,	// BRAKING_FWD2
	&braking_rev1,	// BRAKING_REV1
	&braking_rev2,	// BRAKING_REV2
	&coasting_fwd,	// COASTING_FWD
	&coasting_rev,	// COASTING_REV
	&fail			// REV
};

// State transition table, maps (event x state) -> state
static const state_t state_tr_table[NUM_EVENTS][NUM_STATES]= {
/*						IS_HALTED		FULL_BRAKE,		GOING_FWD,		GOING_REV,	 	BRAKING_FWD1,	BRAKING_FWD2,	BRAKING_REV1,	BRAKING_REV2,	COASTING_FWD,	COASTING_REV	FAIL*/ 
/********************************************************************************************************************************************************************************************/
/* FWD 		*/			{GOING_FWD,		FULL_BRAKE,		GOING_FWD,		BRAKING_REV2,	GOING_FWD,		GOING_FWD,		BRAKING_REV2,	BRAKING_REV2,	GOING_FWD,		BRAKING_REV2,	FAIL},	
/* REV 		*/			{GOING_REV,		FULL_BRAKE,		BRAKING_FWD2,	GOING_REV,		BRAKING_FWD2,	BRAKING_FWD2,	GOING_REV,		GOING_REV,		BRAKING_FWD2,	GOING_REV,		FAIL},	
/* BRAKE	*/			{FULL_BRAKE,	FULL_BRAKE,		BRAKING_FWD1,	BRAKING_FWD1,	BRAKING_FWD1,	BRAKING_FWD1,	BRAKING_REV1,	BRAKING_REV1,	BRAKING_FWD1,	BRAKING_REV1,	FAIL},	
/* COAST	*/			{IS_HALTED,		IS_HALTED,		COASTING_FWD,	COASTING_REV,	COASTING_FWD,	COASTING_FWD,	COASTING_REV,	COASTING_REV,	COASTING_FWD,	COASTING_REV,	FAIL},	
/* HALTED	*/			{IS_HALTED,		FULL_BRAKE,		FAIL,			FAIL,			IS_HALTED,		GOING_REV,		IS_HALTED,		GOING_FWD,		IS_HALTED,		IS_HALTED,		FAIL}};

// Internal variables
state_t _curr_state;
unsigned char _run_duty;
unsigned char _brake_duty;
int _prev_event;

// Event input to the state machine
// Determines the next state and runs the 
// function associated with the current state
void h_bridge_main(int event) {
	state_t next_state;
	state_fn_t fn;

	// Event sanity check
	if (event < 0 || event > (NUM_EVENTS - 1)) {
		printf("Illegal input\r\n");
		return;
	}

	// Determine the next state
	if ((next_state = state_tr_table[event][_curr_state]) != UNDEF) {
		_curr_state = next_state;

		// Get the function associated to the state and run it
		if ((fn = state_function_map[_curr_state]) != NULL) 		
			fn();
		else
			printf("Function undefined for event %i\r\n",_curr_state);
	}
	else
		printf("Event %i undefined for state %i\r\n",event,_curr_state);
}

/**** START OF FUNCTION DEFINITIONS FOR STATES  ****/
/**** Define H_BRIDGE_DEBUG for debug 			****/

void is_halted(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	pwm_setduty(_run_duty);
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void full_brake(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	pwm_setduty(_brake_duty);
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void going_fwd(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_REV;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
}
void going_rev(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void braking_fwd1(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void braking_fwd2(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void braking_rev1(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void braking_rev2(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT |= H_BRIDGE_REV;
}
void coasting_fwd(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_REV;
}
void coasting_rev(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_REV;
}
void fail(void) {
#ifdef H_BRIDGE_DEBUG
	printf("%s\r\n",__PRETTY_FUNCTION__);
#endif
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_FWD;
	H_BRIDGE_CTRL_PORT &= ~H_BRIDGE_REV;
}
/**** END OF FUNCTION DEFINITIONS FOR STATES  ****/

// Initialization of hardware ports and state machine
void h_bridge_init(unsigned char run_duty, unsigned char brake_duty) {
#ifdef H_BRIDGE_DEBUG
	// Sanity checks of function map, array size must match number of states
	assert((sizeof(state_function_map) / sizeof(state_fn_t)) == NUM_STATES);
#endif

	// setup of h-bridge control ports
	H_BRIDGE_CTRL_DDR |= H_BRIDGE_FWD | H_BRIDGE_REV;
	H_BRIDGE_ENA_DDR |= H_BRIDGE_ENA;

	h_bridge_set_run_duty(run_duty);
	h_bridge_set_brake_duty(brake_duty);

	// reset edge detection
	_prev_event = 0xFF;

	// Starting state
	_curr_state = IS_HALTED;
}

// Sets the running duty for the state machine
void h_bridge_set_run_duty(unsigned char run_duty) {
	_run_duty = run_duty;

	if (_curr_state == GOING_FWD || _curr_state == GOING_REV) {
		// If the system is running, run duty should be effective
		pwm_setduty(_run_duty);
	}
	else {
		// Otherwise, braking duty should be effective
		pwm_setduty(_brake_duty);
	}
}

// Sets the braking duty for the state machine
void h_bridge_set_brake_duty(unsigned char brake_duty) {
	_brake_duty = brake_duty;

	if (_curr_state == GOING_FWD || _curr_state == GOING_REV) {
		// If the system is running, run duty should be effective
		pwm_setduty(_run_duty);
	}
	else {
		// Otherwise, braking duty should be effective
		pwm_setduty(_brake_duty);
	}
}

// Prints the current state 
void h_bridge_printstate(void) {
	printf("Current state %i\r\n",_curr_state);
}