//###########################################################################
//
// FILE:    states.c
//
// TITLE:
//
// NOTES:
//  1)
//###########################################################################
//
//  Ver  | dd mmm yyyy | Who  		| Description of changes
// ======|=============|============|========================================
//  00 	   05 04 2015 	 HPCS
//
//  Texas Instruments, Inc
//  Copyright Texas Instruments 2008. All rights reserved.
//###########################################################################


#include "system_defines.h"
#include "cyclone_device.h"
#include "variables.h"
#include "function_definitions.h"
#include "software_interrupts.h"
#include "cyclone_defines.h"
#include "pmbus_common.h"
#include "pmbus_topology.h"
#include "pmbus_constants.h"


volatile int div;


int32 local_state = 0;
Uint32 low_duty_for_buck;

int32 filter_duty_low,filter_duty_max;
int32 number_of_checks=0;
int32 Variable_threshold=0;
Uint32 time_in_buck_after_backup=0;


void clear_filter1(void) {
	Filter1Regs.FILTERCTRL.bit.FORCE_START =(Uint32) 0;
	Filter1Regs.FILTERCTRL.bit.FILTER_EN = 0U;
	Filter1Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter1Regs.FILTERPRESET.bit.PRESET_REG_SEL = 0U;
	Filter1Regs.FILTERPRESET.bit.PRESET_EN = 1U;

	while (Filter1Regs.FILTERPRESET.bit.PRESET_EN == 1U) {
		//wait for previous preset to take effect
	}
	Filter1Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter1Regs.FILTERPRESET.bit.PRESET_REG_SEL = 1U;
	Filter1Regs.FILTERPRESET.bit.PRESET_EN = 1U;
	while (Filter1Regs.FILTERPRESET.bit.PRESET_EN == 1U) {
		//wait for previous preset to take effect
	}
	Filter1Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter1Regs.FILTERPRESET.bit.PRESET_REG_SEL = 2U;
	Filter1Regs.FILTERPRESET.bit.PRESET_EN = 1U;

	while (Filter1Regs.FILTERPRESET.bit.PRESET_EN == 1U) {
		//wait for previous preset to take effect
	}
	Filter1Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter1Regs.FILTERPRESET.bit.PRESET_REG_SEL = 3U;
	Filter1Regs.FILTERPRESET.bit.PRESET_EN = 1U;
	Filter1Regs.CPUXN.bit.CPU_SAMPLE = 0;
	Filter1Regs.FILTERCTRL.bit.USE_CPU_SAMPLE = 1U;
	Filter1Regs.FILTERCTRL.bit.FILTER_EN = 1U;
	Filter1Regs.FILTERCTRL.bit.FORCE_START = 1U;
}

void clear_filter0(void) {
	Filter0Regs.FILTERCTRL.bit.FORCE_START = 0U;
	Filter0Regs.FILTERCTRL.bit.FILTER_EN = 0U;
	Filter0Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter0Regs.FILTERPRESET.bit.PRESET_REG_SEL = 0U;
	Filter0Regs.FILTERPRESET.bit.PRESET_EN = 1U;

	while (Filter0Regs.FILTERPRESET.bit.PRESET_EN == 1U) {
		//wait for previous preset to take effect
	}
	Filter0Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter0Regs.FILTERPRESET.bit.PRESET_REG_SEL = 1U;
	Filter0Regs.FILTERPRESET.bit.PRESET_EN = 1U;
	while (Filter0Regs.FILTERPRESET.bit.PRESET_EN == 1U) {
		//wait for previous preset to take effect
	}
	Filter0Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter0Regs.FILTERPRESET.bit.PRESET_REG_SEL = 2U;
	Filter0Regs.FILTERPRESET.bit.PRESET_EN = 1U;

	while (Filter0Regs.FILTERPRESET.bit.PRESET_EN == 1U) {
		//wait for previous preset to take effect
	}
	Filter0Regs.FILTERPRESET.bit.PRESET_VALUE = 0;
	Filter0Regs.FILTERPRESET.bit.PRESET_REG_SEL = 3U;
	Filter0Regs.FILTERPRESET.bit.PRESET_EN = 1U;
	Filter0Regs.CPUXN.bit.CPU_SAMPLE = 0;
	Filter0Regs.FILTERCTRL.bit.USE_CPU_SAMPLE = 1U;
	Filter0Regs.FILTERCTRL.bit.FILTER_EN = 1U;
	Filter0Regs.FILTERCTRL.bit.FORCE_START = 1U;
}

void buck_loop_updater(void) {

	switch (local_state)

	{
	case 0://  This state executes once only

		if (FeCtrl1Regs.EADCVALUE.bit.ERROR_VALUE > 2) {
			FeCtrl1Regs.EADCDAC.bit.DAC_VALUE--;
		}

		if (FeCtrl1Regs.EADCVALUE.bit.ERROR_VALUE < -2) {
			FeCtrl1Regs.EADCDAC.bit.DAC_VALUE++;
		}

		if (FeCtrl1Regs.EADCVALUE.bit.ERROR_VALUE == 0) {
			voltage_loop_DAC_value =FeCtrl1Regs.EADCDAC.bit.DAC_VALUE;

			LoopMuxRegs.SAMPTRIGCTRL.bit.FE1_TRIG_DPWM2_EN = 0U; // stop EADC1 triggering
			clear_filter1();
			Filter1Regs.FILTERCTRL.bit.USE_CPU_SAMPLE = 0U;

			// Set Digital comparator for system power loss detection
			FaultMuxRegs.DCOMPCTRL0.bit.FE_SEL = 4U;
			FaultMuxRegs.DCOMPCTRL0.bit.COMP_POL = 1U;
			FaultMuxRegs.DCOMPCTRL0.bit.CNT_THRESH = 3U;
			FaultMuxRegs.DCOMPCTRL0.bit.CNT_CLR = 0U;
			FaultMuxRegs.DCOMPCTRL0.bit.CNT_CONFIG = 1U;
			FaultMuxRegs.DCOMPCTRL0.bit.COMP_EN = 1U;
			FaultMuxRegs.DCOMPCTRL0.bit.THRESH = 10;
			FaultMuxRegs.DCOMPCTRL0.bit.COMP_EN = 0U;
			FaultMuxRegs.DCOMPCTRL0.bit.COMP_EN = 1U;

			if(dflash_variables.variables.integration_calibrated == 0xBBBB)//This means that the power supply voltage levels are available and saved in DFlash
			{
				reduced_voltage_loop_DAC_value =dflash_variables.variables.Min_Vbus_EADC_DAC- 50U ;
				if ( voltage_loop_DAC_value < reduced_voltage_loop_DAC_value)
				{
					reduced_voltage_loop_DAC_value =voltage_loop_DAC_value;
				}
				FaultMuxRegs.DCOMPCTRL0.bit.THRESH = dflash_variables.variables.Vbus_EADC_error_threshold;
				FaultMuxRegs.DCOMPCTRL0.bit.INT_EN = 1U;
			}
			else
			{
				system_voltage_treshold_finding_routine();
				Variable_threshold=0;
			}
			local_state = 1;
			count=0;
		}
		break;
	case 1:
		count ++;
/*
		if ( abs(FeCtrl1Regs.EADCVALUE.bit.ERROR_VALUE) > dflash_variables.variables.Vbus_error_diff_margin)
		{
			local_state=0;
		}

		if ( abs(FeCtrl1Regs.EADCVALUE.bit.ERROR_VALUE ) > Variable_threshold )
		{
			Variable_threshold = abs(FeCtrl1Regs.EADCVALUE.bit.ERROR_VALUE);
		}

		if (count == 1000)
		{
			count =0;
			if (Sensitive ==1)
			{
				FaultMuxRegs.DCOMPCTRL0.bit.THRESH = Variable_threshold+1;
			}
		}
*/
		FaultMuxRegs.DCOMPCTRL0.bit.THRESH =25;

		LoopMuxRegs.SAMPTRIGCTRL.bit.FE1_TRIG_DPWM2_EN = 0U; // Stop triggering EADC1/Filter1

		while (Filter1Regs.FILTERPRESET.bit.PRESET_EN == 1U) {

		}
		Filter1Regs.FILTERPRESET.bit.PRESET_VALUE = Discharge_Ratio;
		Filter1Regs.FILTERPRESET.bit.PRESET_REG_SEL = 1U; //into I holding register
		Filter1Regs.FILTERPRESET.bit.PRESET_EN = 1U; //set bit to make it happen
		while (Filter1Regs.FILTERPRESET.bit.PRESET_EN == 1U) {

		}
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE1_TRIG_DPWM2_EN = 1U; // resume triggering

// For debug only, checking prebias duty
        MiscAnalogRegs.GLBIOVAL.bit.DPWM0B_IO_VALUE=1;
        MiscAnalogRegs.GLBIOOE.bit.DPWM0B_IO_OE=1;
        MiscAnalogRegs.GLBIOEN.bit.DPWM0B_IO_EN=1;

		break;
	case 3:
		break;

	}

}



void preset_filter0(unsigned int preset_value)
{
     Filter0Regs.FILTERCTRL.bit.USE_CPU_SAMPLE = 1;
     Filter0Regs.CPUXN.bit.CPU_SAMPLE = 0;
     LoopMuxRegs.SAMPTRIGCTRL.bit.FE0_TRIG_DPWM2_EN = 0; // Stop triggering
     Filter0Regs.FILTERPRESET.all = (1 << 27) + (1 << 24) + preset_value;
     LoopMuxRegs.SAMPTRIGCTRL.bit.FE0_TRIG_DPWM2_EN = 1; // Resume triggering
     Filter0Regs.FILTERCTRL.bit.FORCE_START = 1;
     Filter0Regs.FILTERCTRL.bit.USE_CPU_SAMPLE = 0;
}


void boost_buck_handler(void) {

static Uint32 i;


	switch (mini_state) {
	case WAIT:
		count++;
		interrupt_clearer= FaultMuxRegs.FAULTMUXINTSTAT.all;
		if ( (count > 1000) && ((wait_done ==1))) {
			count = 0;
			mini_state = BOOST_INITIALIZE;
		}
		break;

	case BOOST_INITIALIZE:
		//clear_filter0();
		filter_duty_low=100000;
		filter_duty_max=0;

		observe_1=0;
		Dpwm2Regs.DPWMCTRL0.bit.PWM_EN = 0U;
		Dpwm3Regs.DPWMCTRL0.bit.PWM_EN = 0U;
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE0_TRIG_DPWM2_EN = 0U; // Stop triggering EADC0/Filter0
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE1_TRIG_DPWM2_EN = 0U; // Stop triggering EADC1/Filter1
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE2_TRIG_DPWM2_EN = 0U; // Stop triggering EADC2/Filter2

		Dpwm3Regs.DPWMCTRL0.bit.PWM_A_INTRA_MUX = 0U; //Default DPWMA and DPWMB out configuration
		Dpwm3Regs.DPWMCTRL0.bit.PWM_B_INTRA_MUX = 0U;
		LoopMuxRegs.DPWMMUX.bit.DPWM3_FILTER_SEL = 0U; //filter0 is providing duty input to this DPWM


		FaultMuxRegs.ACOMPCTRL0.bit.ACOMP_EN = 1U;
		FaultMuxRegs.ACOMPCTRL0.bit.ACOMP_A_INT_EN = 0U;


		FeCtrl0Regs.EADCCTRL.bit.EADC_INV = 0U;
		Dpwm2Regs.DPWMCTRL1.bit.EXT_SYNC_EN = 0U;


	    Filter0Regs.FILTERCTRL.bit.FILTER_EN = 1;
	    Dpwm3Regs.DPWMCTRL0.bit.CLA_EN = 1U;





		/*
		 Prebias begin
		 */

		Charge_Ratio = (Uint32)((0x7FFFFF ) * DPWM2_FIXED_DUTY);
		Charge_Ratio = (Uint32)((battery_voltage * Charge_Ratio )/system_voltage );


		preset_filter0(Charge_Ratio);



		Dpwm2Regs.DPWMCTRL0.bit.PWM_EN = 1U;
		Dpwm3Regs.DPWMCTRL0.bit.PWM_EN = 1U;
	    FeCtrl0Regs.EADCDAC.bit.DAC_VALUE =dflash_variables.variables.EADC_DAC_value_for_constant_current;//I_OUT_ZERO;





		LoopMuxRegs.SAMPTRIGCTRL.bit.FE0_TRIG_DPWM2_EN = 1U; // Start triggering EADC0/Filter0
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE2_TRIG_DPWM2_EN = 1U;// Start triggering EADC2/Filter2



    	mini_state = BOOST_STABILIZE;
//    	while(1);

		break;
	case BOOST_STABILIZE:
/*
	    if( FeCtrl0Regs.EADCDAC.bit.DAC_VALUE <dflash_variables.variables.EADC_DAC_value_for_constant_current)
	    {
	    FeCtrl0Regs.EADCDAC.bit.DAC_VALUE++;

	    }
	    else
	    {
*/
	         mini_state = BUCK_INITIALIZE;
//	    }

		break;
	case BUCK_INITIALIZE:
		FeCtrl1Regs.EADCDAC.bit.DAC_VALUE = 13860U;
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE1_TRIG_DPWM2_EN = 1U;
		mini_state = BOOST_CC;
		break;
	case BOOST_CC:
/*
		if (adc_struct.battery_voltage_avg_32 >
		(dflash_variables.variables.ADC_Value_for_CCtoCV_switch_ON_BATT_SIDE)  ) {

			mini_state =BOOST_CC_TO_CV;
			break;
		}
*/
	buck_loop_updater();

		break;
	case BOOST_CC_TO_CV:

		Filter0Regs.FILTEROCLPHI.bit.OUTPUT_CLAMP_HIGH =Dpwm3Regs.DPWMFILTERDUTYREAD.bit.FILTER_DUTY;
		LoopMuxRegs.SAMPTRIGCTRL.bit.FE0_TRIG_DPWM2_EN = 0U; // Stop triggering EADC0/Filter0
		LoopMuxRegs.FILTERMUX.bit.FILTER0_FE_SEL = 2U;
		mini_state = BOOST_CV;
		break;
	case BOOST_CV:
		buck_loop_updater();
		Vbus_probe_1= AdcRegs.ADCRESULT[0].all;
		break;
	case BUCK_CHECK:
	    count ++;

		if(count > 2000) // Have been checking long enough .  No battery spike observed. So this is a true power loss condition.
		{
			FeCtrl1Regs.EADCDAC.bit.DAC_VALUE =(Uint32)voltage_loop_DAC_value;
			battery_reading_sampled=AdcRegs.ADCAVGRESULT[1].all;
			mini_state=BUCK;
			power_back_possible_counter=0;
			local_state=0;
			count=0;
			i=0U;
			Filter1Regs.FILTERKPCOEF0.bit.KP_COEF_0=1000;
			Filter1Regs.FILTERCTRL.bit.NL_MODE =0U;
		}


		if (battery_reading_before_mode_switch < ( AdcRegs.ADCAVGRESULT[1].all - 5U ) ) //Keep checking if the battery voltage has spiked up.
		{                                                                               // If so , then the PSU is still on . Check for false trigger.
			battery_reading_sampled=AdcRegs.ADCAVGRESULT[1].all;
			mini_state = FALSE_TRIGGER_VALIDATE;
			count=0;
			i=0U;
		}
		break;
	case FALSE_TRIGGER_VALIDATE:
		count ++;

		if (count ==2)
		{
			Dpwm3Regs.DPWMCTRL0.bit.PWM_EN = 0U;
			Dpwm2Regs.DPWMCTRL0.bit.PWM_EN = 0U;
		}
		if (count == 10000)
		{
			count =0;
			converter_state = PS_OFF;
		}

		break;

	case BUCK_RECOVER:
		count++;
		if(count == 500)
		{
			 mini_state=BUCK;
			 count=0;
			 if (Dpwm3Regs.DPWMFILTERDUTYREAD.bit.FILTER_DUTY < low_duty_for_buck)
			 {
			 low_duty_for_buck=Dpwm3Regs.DPWMFILTERDUTYREAD.bit.FILTER_DUTY;
			 low_duty_for_buck -=10U;
			 }
		}
				break;
	case BUCK:
	 //   time_in_buck_after_backup++;
//		/*
//		 * Experiment to demonstrate 100 % duty cycle
//		 */
//		if (reduce == 8) {
//			MiscAnalogRegs.GLBIOVAL.all = 0x0A;
//			MiscAnalogRegs.GLBIOEN.all = 0x0400000F;
//			MiscAnalogRegs.GLBIOEN.all = 0x0400000F;
//			MiscAnalogRegs.GLBIOEN.all = 0x0400000F;
//			Dpwm0Regs.DPWMCTRL0.all = 0;
//			Dpwm1Regs.DPWMCTRL0.all = 0;
//			MiscAnalogRegs.GLBIOEN.all = 0x04000000;
//			reduce = 7;
//		}
//		/*
//		 * End of experiment
//		 */
		//current_balance();

/*
 * This below switch statement is used for determining if the power has come back up.
 * The idea behind this is that the battery voltage always goes lower or stays the same
 * if the buck operation is doing a successful back up. But if the PSU comes back up ,
 * then if the system voltageb regulating point by the BBU is lower that the PSU, current
 * starts to flow towards the battery and causes a spike.
 *
 * The spike in this case is nit really observed on the battery
 */
//
//		switch (local_state)
//			{
//				case 0: battery_reading_sampled_array[i++]=AdcRegs.ADCAVGRESULT[1].all;
//						local_state=1;
//						break;
//				case 1:
//					count++;
//					if(count ==500)
//					{
//						count=0;
//						local_state=0;
//						if (i==9U)
//						{
//							i=0U;
//							local_state=2;
//						}
//
//					}
//
//					break;
//				case 2 :
//					sum_of_samples_1=0;
//					sum_of_samples_2=0;
//					for (i=0U;i<4U;i++)
//					{
//						sum_of_samples_1+=battery_reading_sampled_array[i];
//						sum_of_samples_2+=battery_reading_sampled_array[i+5U];
//
//					}
//					i=0U;
//					if(sum_of_samples_2 > sum_of_samples_1) //Oscillations or change of battery voltage where battery voltage has increased .
//					{
//					    if ( (error_state == NO_ERROR )&&(time_in_buck_after_backup > ) )
//					    {
//					        mini_state=POWER_BACK_CHECK;
//					    }
//					    else
//					    {
//					        error_state = POOR_BATTERY_CONNECTION ;
//					    }
//
//
//					}
//						local_state=0;
//					break;
//			}
			break;
	case POWER_BACK_CHECK:// check for existance of PSU by forceably cahanging the DAC and observing the current change.
				switch (local_state)
				{
					case 0:
						FeCtrl1Regs.EADCDAC.bit.DAC_VALUE=dflash_variables.variables.Max_Vbus_EADC_DAC+50U;
						local_state=1;
						count=0;
						break;
					case 1:// Wait for a while before taking current reading
						count++;
						if (count == 2000)
						{
							count=0;
							local_state=2;
						}
						break;
					case 2:
						number_of_checks++;
						if( (int) (Iout_prev_1 - adc_struct.Current_sum ) > 20 )
						{
							power_back_possible_counter++;
						}

						FeCtrl1Regs.EADCDAC.bit.DAC_VALUE=dflash_variables.variables.Max_Vbus_EADC_DAC;
						local_state=3;
						if(number_of_checks > 5)
						{

							if (power_back_possible_counter > 3)
							{
								converter_state=PS_OFF;
							}
							else
							{
								 mini_state=BUCK;
							}
							local_state=0;
							number_of_checks=0;
							power_back_possible_counter=0;
						}
						break;
					case 3:// Wait for a while before taking current reading
						count++;
						if (count == 2000)
						{
							count=0;
							local_state=0;
							Iout_prev_1=adc_struct.Current_sum;
						}
						break;

					}

		break;


	}

}

//
//void current_balance()
//{
//	static Uint32 state=0U;
//	static Uint32 counter =0U;
//	static Uint32 previous_current_value=0U;
//	switch (state)
//	{
//	case 0U:
//		counter ++;
//	        if (counter == 1000U)
//	        {
//	        	counter=0U;
//	        	state =1U;
//	        }
//	        break;
//	case 1U:
//		   if(abs (previous_current_value - ( adc_struct.Iout_1 +adc_struct.Iout_2) )  < 10U )
//		   {
//			   state =2U;
//		   }
//		   else
//		   {
//			   previous_current_value = ( adc_struct.Iout_1 +adc_struct.Iout_2);
//			   state =0U;
//		   }
//		   break;
//	case 2U:
//			if( (int)(adc_struct.Iout_2 - adc_struct.Iout_1) > 4)
//			{
//			state =3;
//			}
//			else if ( (int)(adc_struct.Iout_1 - adc_struct.Iout_2) > 4)
//			{
//			state =4;
//			}
//			else
//			{
//			state =0;
//			}
//			break;
//	case 3U:
//			if (counter++ == 10000)
//			{
//				counter =0;
//				if (Dpwm0Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A > 0)
//				{
//					Dpwm0Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A--;
//				}
//				else if(Dpwm1Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A < 500)
//				{
//					Dpwm1Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A++;
//				}
//			}
//			if( (int)(adc_struct.Iout_2 - adc_struct.Iout_1) < 4)
//			{
//				state =0;
//				counter=0;
//			}
//			break;
//	case 4U:
//			if (counter++ == 10000)
//			{
//				counter =0;
//				if (Dpwm1Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A > 0)
//				{
//				Dpwm1Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A--;
//				}
//				else if(Dpwm0Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A < 500)
//				{
//				Dpwm0Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A++;
//				}
//			}
//			if ( (int)(adc_struct.Iout_1 - adc_struct.Iout_2) < 4)
//			{
//				state =0;
//				counter=0;
//			}
//			break;
//
//	}
//}
void ps_off_handler(void) {

	Dpwm3Regs.DPWMCTRL0.bit.PWM_EN = 0U;
	Dpwm2Regs.DPWMCTRL0.bit.PWM_EN = 0U;
	interrupt_clearer= FaultMuxRegs.FAULTMUXINTSTAT.all;


	FaultMuxRegs.DCOMPCTRL1.bit.COMP_EN = 0U;
	FaultMuxRegs.DCOMPCTRL1.bit.COMP_EN = 1U;
	init_filter();
	Dpwm3Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A =0;
	Dpwm2Regs.DPWMCYCADJA.bit.CYCLE_ADJUST_A =0;

	LoopMuxRegs.FILTERMUX.bit.FILTER0_FE_SEL = 0U;

	interrupt_clearer = FaultMuxRegs.FAULTMUXINTSTAT.all;
	init_fault();

	mini_state = WAIT;
	local_state=0;
	count=0;
	wait_done=0;
	low_duty_for_buck=3900U;
	power_back_possible_counter=0;
	if (error_state == NO_ERROR)
	{
		if (adc_struct.system_voltage > dflash_variables.variables.ADC_Value_for_trigger_point_ON_PS_SIDE) {

				converter_state = BOOST_BUCK;

		}
	}

}
