#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, 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(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);
								calculate_motor_speed();
							//toc_port(DEBUG_2_PORT);	
								//calculate_motor_speed();
								//calculate_motor_speed();
								//tic_port(DEBUG_2_PORT);
							//toc_port(DEBUG_2_PORT);
							//toc(DEBUG_2);
							//tic(DEBUG_3);
								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);
								//BLDC_runSpeedCntl(&Motor3, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
							//toc(DEBUG_3);
						//toc(DEBUG_4);
						//toc(DEBUG_3);
						//tic_port(DEBUG_3_PORT);
						//toc_port(DEBUG_3_PORT);
					default: /* PWM FREQ - 25kHz */
						//tic(DEBUG_2);
								
								select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern);
								//select_active_phase(&Motor2, Motor2.motor_status.currentHallPattern);
								//select_active_phase(&Motor3, Motor3.motor_status.currentHallPattern);
							
							//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);
								//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);

	exec_commutation();
	
	//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 float Fsamples[NOF];
float fZeroCurrent = 8.0;

static void ProcessSamples(void) {
	volatile int i;
	tic_port(DEBUG_2_PORT);
	//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_2_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();
	
	/* Replace with your application code */
	while (1) {
		
		ProcessSamples();
		
		
		
		
		
		//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;
			//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));		
	}
}