#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)




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, 2000);
				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);
							BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
						//toc(DEBUG_4);
	 				case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */
						//tic(DEBUG_3);
						//tic(DEBUG_4);
							//tic(DEBUG_2);
								calculate_motor_speed();
							//toc(DEBUG_2);
							//tic(DEBUG_3);
								BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
							//toc(DEBUG_3);
						//toc(DEBUG_4);
						//toc(DEBUG_3);
					default: /* PWM FREQ - 25kHz */
						//tic(DEBUG_2);
							//tic(DEBUG_3);	
								select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern);
							////toc(DEBUG_2);
							////tic(DEBUG_3);
								BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
							//toc(DEBUG_3);
						//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))		
		default: break;
	} // End switch(applicationStatus.currentstate)
	//gpio_set_pin_level(DRV_RST, true);
	exec_commutation();
	
} // 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);
}



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();
	configure_tcc_pwm();
	config_pins();
	configure_adc();
	configure_ethercat_dma_descriptors();
	ethercat_update();
	custom_logic_enable();
	configure_TC_CCL_SPEED();
	
	ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback);
	enable_NVIC_IRQ();
	
	/* Replace with your application code */
	while (1) {
		//tic(DEBUG_1);
		//tic_port(DEBUG_3_PORT);
		//toc(DEBUG_1);
		//toc_port(DEBUG_3_PORT);
		
		
		//tic(DEBUG_1_PORT);
		//toc(DEBUG_1_PORT);
		
		if(Motor1.timerflags.motor_telemetry_flag)
		{
			Motor1.timerflags.motor_telemetry_flag = false;
			update_setpoints();
			update_telemetry();
		}
		//{	
			//gpio_set_pin_level(DRV_RST, true);
			//Motor1.timerflags.pwm_cycle_tic = false;
			//tic(DEBUG_1);
			//exec_commutation();	
			//toc(DEBUG_1);	
		//}
		if(Motor1.timerflags.current_loop_tic) {
			Motor1.timerflags.current_loop_tic = false;
			tic_port(DEBUG_1_PORT);
			CONTROLLER_StateMachine();
			toc_port(DEBUG_1_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));		
	}
}




