#include <atmel_start.h>
#include <hal_gpio.h>
#include <hal_delay.h>
#include <arm_math.h>

// ----------------------------------------------------------------------
// Header Files
// ----------------------------------------------------------------------
#include "interrupt_handlers.h"
#include "ethercat/ethercat_e54.h"
#include "configuration.h"
#include "pins.h"
#include "statemachine.h"
#include "ethercat/ethercat_slave_def.h"
#include "utilities.h"

// ----------------------------------------------------------------------
// Defines
// ----------------------------------------------------------------------
//#define DBG_PIN1 GPIO(GPIO_PORTD, 0)

//#define DEBUG_1_PORT PORT_PC01
//#define DEBUG_2_PORT PORT_PC03
//#define DEBUG_3_PORT PORT_PC02
//#define DEBUG_4_PORT PORT_PC30
//
//inline void tic(const uint8_t pin)
//{
	//gpio_set_pin_level(pin, true);
//}
//
//inline void toc(const uint8_t pin)
//{
	//gpio_set_pin_level(pin, false);
//}
//
//inline void tic_port(const uint32_t port)
//{
	//REG_PORT_OUTSET2 = port;
//}
//
//inline void toc_port(const uint32_t port)
//{
	//REG_PORT_OUTCLR2 = port;
//}


inline void CONTROLLER_StateMachine(void)
{
	if (applicationStatus.fault)
	{
		applicationStatus.previousstate = applicationStatus.currentstate;
		applicationStatus.currentstate = FAULT;
	}
	
	switch(applicationStatus.currentstate)                    							 	/* process current motor state*/
	{
		case SYSTEM_INIT:			
			/* Toggle driver reset Latch */
			gpio_set_pin_level(DRV_RESET, true);
			gpio_set_pin_level(DRV_RESET, false);
			/* Update State Variables */
			applicationStatus.previousstate = applicationStatus.currentstate;                                                                                 
			applicationStatus.currentstate = SYSTEM_IDLE;
			break;
		case SYSTEM_IDLE:
			/* If received MOTOR START command, move to MOTOR_INIT */
			//if(HostController.StartStopMotor == 0)
			//{
				applicationStatus.previousstate = applicationStatus.currentstate;
				applicationStatus.currentstate = MOTOR_IDLE;
			//}
			break;
		case MOTOR_IDLE:
			applicationStatus.previousstate = applicationStatus.currentstate;
			applicationStatus.currentstate = MOTOR_PVI_CTRL_STATE;
			//applicationStatus.currentstate = MOTOR_V_CTRL_STATE;
			break;
		
		case MOTOR_I_CTRL_STATE:
			break;
		
		case MOTOR_V_CTRL_STATE:
			switch (Motor1.regulation_loop_count) {
				case 0: /* PWM FREQ / 25 - 1kHz */
					Motor1.timerflags.motor_telemetry_flag = true;
					/* Blank */
	 		    case 6: /* PWM FREQ / 6.25 - 4kHz */
					
					calculate_motor_speed();
					
					BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed);
					//BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, 3000);
				default: /* PWM FREQ - 25kHz */
					select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); 	/* Still measure current */
					break;
			} // end switch(regulation_loop_count)
		
			if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
			else Motor1.regulation_loop_count++;
			break;
		
		case MOTOR_P_CTRL_STATE:
			break;
		
		case MOTOR_VI_CTRL_STATE:
			switch (Motor1.regulation_loop_count) {
					case 0: /* PWM FREQ / 25 - 1kHz */
						Motor1.timerflags.motor_telemetry_flag = true;
						/* Blank */
	 				case 6: /* PWM FREQ / 6.25 - 4kHz */
						calculate_motor_speed();
						BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed);
					default: /* PWM FREQ - 25kHz */
						select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern);
						BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
						break;
				} // end switch(regulation_loop_count)

			if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
			else Motor1.regulation_loop_count++;
			break;
		
		case MOTOR_PVI_CTRL_STATE:
			switch (Motor1.regulation_loop_count) {
					case 0: /* PWM FREQ / 25 - 1kHz */
						Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag
						//tic(DEBUG_4);
							//tic_port(DEBUG_2_PORT);
								BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
							//toc_port(DEBUG_2_PORT);
								BLDC_runPosCntl(&Motor2, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
							//BLDC_runPosCntl(&Motor3, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
							//toc_port(DEBUG_2_PORT);
						//toc(DEBUG_4);
						//tic_port(DEBUG_2_PORT);
						//toc_port(DEBUG_2_PORT);
						
	 				case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */
						//tic(DEBUG_3);
						//tic(DEBUG_4);
							//tic(DEBUG_2);
							//tic_port(DEBUG_2_PORT);
							//tic_port(DEBUG_2_PORT);
							//tic_port(DEBUG_2_PORT);
							calculate_motor_speed();
							calculate_motor_speed();
							//toc_port(DEBUG_2_PORT);	
								//calculate_motor_speed();
								//calculate_motor_speed();
								//tic_port(DEBUG_2_PORT);
							//toc_port(DEBUG_2_PORT);
							//tic(DEBUG_3);
							//tic_port(DEBUG_2_PORT);
							BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
							BLDC_runSpeedCntl(&Motor2, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
							//toc_port(DEBUG_2_PORT);
								//BLDC_runSpeedCntl(&Motor3, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
							//toc_port(DEBUG_2_PORT);
						//toc(DEBUG_4);
						//toc(DEBUG_3);
						//tic_port(DEBUG_3_PORT);
						//toc_port(DEBUG_3_PORT);
					default: /* PWM FREQ - 25kHz */
						//tic(DEBUG_2);
								//tic_port(DEBUG_2_PORT);
								select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern);
								select_active_phase(&Motor2, Motor2.motor_status.currentHallPattern);
								//select_active_phase(&Motor3, Motor3.motor_status.currentHallPattern);
								//toc_port(DEBUG_2_PORT);
							//tic_port(DEBUG_2_PORT);
								//tic_port(DEBUG_2_PORT);
								BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
								BLDC_runCurrentCntl(&Motor2, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
								//toc_port(DEBUG_2_PORT);
								//toc_port(DEBUG_3_PORT);
								//BLDC_runCurrentCntl(&Motor3, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
							//toc_port(DEBUG_2_PORT);
						//toc(DEBUG_2);
						break;
				} // end switch(regulation_loop_count)
				

			if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
			else Motor1.regulation_loop_count++;
			break;
						
		case MOTOR_STOP:
			applicationStatus.previousstate = applicationStatus.currentstate;
			applicationStatus.currentstate = SYSTEM_IDLE;
			break;
		case FAULT:
			//DisableGateDrivers(&Motor1);
			switch(applicationStatus.fault)
			{
				case NOFAULT:           
					break;
				case HALLSENSORINVALID:
					break;
				case MOTOR_STALL:
					break;
				case VOLTAGE:
					break;
				case OVERCURRENT:
					break;
				case GATE_DRIVER:
					break;
				case UNKNOWN:                          						
					break;
				default: break;
			}// End switch(switch(applicationStatus.fault))	
			
			case COMMSTEST:
		
		default: break;
	} // End switch(applicationStatus.currentstate)
	//gpio_set_pin_level(DRV_RST, true);
	//tic_port(DEBUG_3_PORT);
	//tic_port(DEBUG_2_PORT);
	//tic_port(DEBUG_3_PORT);
	//tic_port(DEBUG_3_PORT);
	exec_commutation();
	//toc_port(DEBUG_3_PORT);
	//toc_port(DEBUG_3_PORT);
	//toc_port(DEBUG_2_PORT);
	
} // inline void CONTROLLER_StateMachine(void)

void enable_NVIC_IRQ(void)
{
	NVIC_EnableIRQ(TC7_IRQn);	 // TC7: TC_ECAT
	NVIC_EnableIRQ(TC0_IRQn);	 // TC0: TC_SPEED
	NVIC_EnableIRQ(DMAC_0_IRQn);
	NVIC_EnableIRQ(TCC1_0_IRQn);
	NVIC_EnableIRQ(TCC1_0_IRQn);
	NVIC_EnableIRQ(EIC_5_IRQn);
}


#define NOF  64
volatile uint32_t samples[NOF];
volatile float32_t Fsamples[NOF];
float32_t fZeroCurrent = 8.0;

static void ProcessSamples(void) {
	volatile int i;
	
	//Fsamples[i] = samples[i]*3.3/4096.0 - fZeroCurrent;
	for (i=0; i < NOF; i++) {
 		Fsamples[i] = samples[i]*3.3f/4096.0f - fZeroCurrent;
	}
	toc_port(DEBUG_3_PORT);
}


int main(void)
{
	/* Initializes MCU, drivers and middleware */
	//__disable_irq();
	atmel_start_init();
	BldcInitStruct(&Motor1);
	BldcInitStruct(&Motor2);
	BldcInitStruct(&Motor3);
	BldcInitFunctions();
		
	read_zero_current_offset_value(&Motor1);
	read_zero_current_offset_value(&Motor2);
	read_zero_current_offset_value(&Motor3);
	
	configure_tcc_pwm();
	config_pins();
	configure_adc();
	configure_ethercat_dma_descriptors();
	adc_init_dma();
		
	ethercat_update();
	custom_logic_enable();
	configure_TC_CCL_SPEED();
	
	//ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback);
	enable_NVIC_IRQ();
	
	//volatile float32_t a = 0.0;
	//volatile float32_t b = 123.456f;
	//volatile float32_t c = 654.321f;
	
	
	/* Replace with your application code */
	while (1) {
		//tic_port(DEBUG_3_PORT);
		//a = (b * c)/6.0f;
		//toc_port(DEBUG_3_PORT);
		
		//ProcessSamples();
		
		if(Motor1.timerflags.adc_readings_ready_tic) {
			tic_port(DEBUG_2_PORT);
			/* Clear Flag */
			Motor1.timerflags.adc_readings_ready_tic = false;
			
			//tic_port(DEBUG_2_PORT);			
			volatile int16_t phase_A_current_raw, phase_B_current_raw;
			
			/* Motor 1 */
			phase_A_current_raw = (adc_res[0] - Motor1.Voffset_lsb.A);
			phase_B_current_raw = (adc_res[1] - Motor1.Voffset_lsb.B)*-1;
			// Covert from LSB to PU (A) and filter out small readings
			Motor1.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU;
			Motor1.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU;
			// i_c = -i_a - i_b because i_a + i_b + i_c = 0
			Motor1.Iphase_pu.C = -Motor1.Iphase_pu.A - Motor1.Iphase_pu.B;
			
			/* Motor 2 */
			phase_A_current_raw = (adc_res[2] - Motor2.Voffset_lsb.A);
			phase_B_current_raw = (adc_res[3] - Motor2.Voffset_lsb.B)*-1;
			// Covert from LSB to PU (A) and filter out small readings
			Motor2.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU;
			Motor2.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU;
			// i_c = -i_a - i_b because i_a + i_b + i_c = 0
			Motor2.Iphase_pu.C = -Motor2.Iphase_pu.A - Motor2.Iphase_pu.B;
			
			// Set Current Loop Flag
			Motor1.timerflags.current_loop_tic = true;
			toc_port(DEBUG_2_PORT);
		}
		
		
		
		if(Motor1.timerflags.motor_telemetry_flag) {
			//tic_port(DEBUG_2_PORT);
			Motor1.timerflags.motor_telemetry_flag = false;
			update_setpoints();
			//toc_port(DEBUG_2_PORT);
			//tic_port(DEBUG_3_PORT);
			update_telemetry();
			//toc_port(DEBUG_3_PORT);
		}
		
		if(Motor1.timerflags.current_loop_tic) {
			Motor1.timerflags.current_loop_tic = false;
			//toc_port(DEBUG_2_PORT);
			tic_port(DEBUG_3_PORT);
			CONTROLLER_StateMachine();
			toc_port(DEBUG_3_PORT);
		}
		
		//toc_port(DEBUG_3_PORT);
				
		//do {
			//delay_ms(10);
		//} while (gpio_get_pin_level(SW0));
		//
		//if(Motor1.dir_hall_code < 14) {
			//Motor1.dir_hall_code++;
		//} else {
			//Motor1.dir_hall_code=1;
		//}
			//
		//do {
			//delay_ms(10);
		//} while (!gpio_get_pin_level(SW0));		
	}
}




