#include "arm_math.h"
#include "fsl_gpio.h"
#include "spi.h"

#include "utils.h"
#include "battery.h"
#include "main.h"
#include "display.h"
#include "timer.h"
#include "lcd.h"
#include "mode.h"
#include "ports.h"
#include "utils.h"
#include "safety_key.h"
#include "sys_chk.h"
#include "pwr_level.h"
#include "init.h"
#include "frq.h"
#include "adc.h"
#include "taps.h"
#include "amps.h"
#include "stdbool.h"
#include "System/system.h"

extern	uint8_t Task,Cur_Mode,Power_Level,catch_up_flag;
extern	uint8_t	Over_Voltage_Flag,Bat_Type,Suspend_Step_Chk;
extern uint16_t Pot_Value_CLAMP_AB[];
extern uint16_t Pot_Value_AB[],Over_Voltage_tmr,ByPass_tmr;
extern uint8_t Dds_Pot_Val[]; // 2 byte Data for SPI
extern uint16_t Vchktmr, Pot_Value[];
//extern float Requested_Current[];
extern uint8_t Port_State[];
uint8_t Ports_Cleared_Flag,step_count;
extern uint8_t frequency,Over_Current_Flag, Taps_Flag;
extern FREQUENCY_t freqArray[FREQ_MAX_NUM];
extern float32_t Watts_Filt;
extern SYSTEM_DATA_t sys;

uint16_t Power_tmr, Display_warning_tmr;

void System_Check()
{

	if(freqArray[frequency].frequency1 <= MAX_DTYPE)
	{
		Check_Over_Power(); // check for excessive power setting
	}
	else
	{
		if(step_count >= 10)
			Check_Over_Power();
	}
//	Check_Power_Monitor(); // Check for persistent condition longer than 3 seconds

//		Vchktmr = 0;
	Check_Live_Voltage();		// Check Live voltage external source

	//	Safety_Check();				redundant code

	Check_Over_Voltage();		// Check Direct Connect Over voltage

	Check_Clamp_OverVoltage();

	Check_Over_Current();		// Check if current > 1 Amp 9/4/24 temp

//	Check_PSU_Short();			// Checks if PSU rail is short

//######## temporarily removed 9/3/24 for debugging
//	if(Watts_Filt > Get_Power_Limit())
//		Check_Over_Power(); // check for excessive power setting
//	else
//	{
	if(Cur_Mode != BROADCAST && step_count < 10)
//		Check_Selected_Current();	//Checks for current output > Selected Value
		Chk_Selected_Current2();
//	}
	//#########


	Check_Trickle_Current();

	Chk_Bat_Level();
#if 1	//## 9/24/24
	if(Short_Circuit_Chk()) 	// Check for short in High gain.
	{
		Over_Current_Flag = true;
		Power_Level = 0;
		Cut_Signal_To_Min();
//		Update_Amp_Pot();
	}
#endif
}

void Safety_Check(void)
{
	Check_Live_Voltage();
	if(Task == SAFETY_TASK)
		{
			Task = SAFETY_TASK;
			LCD_Clear();

			while(1)
			{
				safe_key();
				Display_Update();
				Delay_Ticks(10);
			}
		}
}

void Normal_Bypass_Chk(void)
{
	if(!sys.status[OVERVOLTAGE])
		Select_Bypass(ON);
}



void Check_For_Open_Circuit(void)
{
	if(Open_Circuit_Chk())		// if open Circuit
	{		// Switch blocking Cap ON
		Select_Bypass(OFF);
		ByPass_tmr = DELAY_5S;
	}
	else if (sys.adc.V_CHK < MAX_BYPASS_VOLTS2)	// else if NOT HVP
	{	//   Switch Blocking cap off
		Delay_Ticks(10);  // Check again after delay
		if (sys.adc.V_CHK < MAX_BYPASS_VOLTS2 && ByPass_tmr == 0)
			{
			Select_Bypass(ON);
			step_count = 0;
			Display_warning_tmr = DELAY_5S; // Allow voltage surge to drop before displaying warning
			}

	}

	if (sys.adc.V_CHK > MAX_BYPASS_VOLTS2)
		sys.status[OVERVOLTAGE] = true;
	else
		sys.status[OVERVOLTAGE] = false;	// added 1/6/25

	if (Compare_Voltage(sys.adc.V_CHK, MAX_OP_VOLTS)) //Potentially fatal voltage
	{
		Select_Estop(ISOLATED);			// Fault condition disable output
		Select_Bypass(OFF);					// Force into blocked state.
		Task = SAFETY_TASK;
	}

}

bool Open_Circuit_Chk(void)

{
	if(sys.adc.I_OUT_FastFilt <= OPEN_CIRCUIT_CURRENT)
		return(true);
	else
		return(false);


}



void Check_For_Connected_Volts(void)
{
	Vchktmr = 0;
//	Check_Live_Voltage();		// Check Live voltage external source
	Safety_Check();
	if(sys.status[OVERVOLTAGE])
		Select_Bypass(OFF);
	else
	{
		Select_Bypass(ON);

		//Scale down standby pot value for frequency: compensate for blocking cap
		float32_t scale = 0.6 * freqArray[frequency].frequency1 / 20000.0 + 0.4;
		if(scale > 1.0)
		{
			scale = 1.0;
		}
		Dds_Pot_Val[0] = 0; // address
		Dds_Pot_Val[1] = (uint8_t)(scale * Dds_Pot_Val[1]);
		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);

		Delay_Ticks(50);

		Taps_Flag = false;
		Check_Taps();		// Check for optimum Taps before making jump
		Delay_Ticks(30);

#if 0
		for(uint32_t i = 0; i < 10; i++)
		{
			Current1[i] = sys.adc.I_OUT_FastFilt;
			Delay_Ticks(10);	//100mS to stabilize measurement after cap bypassed
		}

		uint32_t moo = 0;
		moo++;		//place for breakpoint
#endif
	}

//	if((ACCY_GetConnectedAccessory(1) != ID_CLAMP) && (ACCY_GetConnectedAccessory(2) != ID_CLAMP))
	if((!Check_For_Clamp()))

	{
		Request_Current_Change(); // Normal running no clamp
	}


}

void Check_Trickle_Current(void)
{
float32_t x;
uint8_t y,z;

	if(Power_Level == 0 && sys.adc.I_OUT_SlowFilt > 0.050)
	{
		z = Dds_Pot_Val[1];
		x = 0.050 /sys.adc.I_OUT_SlowFilt;
		y = (Dds_Pot_Val[1]*x);

		if (y < 3)
			y=3;

		Dds_Pot_Val[1] = y;
		Dds_Pot_Val[0] = 0; // address
//		Adjust_Pot_Val();
		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);


//		Pot_Value[0] = 3;
//		Init_Amplitude();

	}
}

void Check_Selected_Current(void)
{
	if(!Check_For_Clamp())
	{
		if((!Suspend_Step_Chk) && (!sys.status[OVERVOLTAGE]))
		{
			if(Demand_Check() && Power_Level > 0) //
				{
					if(!Check_Maxed_Out())
					{
						Request_Current_Change();
						catch_up_flag = false;
						step_count++;
					}
				}
		}

	}
}


bool Demand_Check(void)
{
float32_t current,num;
uint8_t xval;

	xval = Dds_Pot_Val[1];

//	num = Requested_Current[Power_Level] * 1.2; 3/8/24
	num = Requested_Current(Bat_Type);

	current = sys.adc.I_OUT_SlowFilt; //
//	if(current > (Requested_Current[Power_Level] * 1.2) && Power_Level > 0) 3/8/24
	if(current > (num * 1.1) && Power_Level > 0)
		return(true);
	else
	{//######
//		if(current < (Requested_Current[Power_Level] * 0.8))
		if(current < (num * 0.9) && Power_Level > 0)
			return(true);
		else
			return(false);
	}
}

void Chk_Selected_Current2(void)
{

	if(!Check_For_Clamp() && (!Suspend_Step_Chk) && (!sys.status[OVERVOLTAGE]))
	{
		if(sys.adc.I_OUT_SlowFilt > (Requested_Current(Bat_Type) * 1.05) || (Watts_Filt > Get_Power_Limit()))
		{
			Request_Current_Change(); // reduce current
			catch_up_flag = false;
			step_count++;

		}
		else
			{
			if (sys.adc.I_OUT_SlowFilt < (Requested_Current(Bat_Type) * 0.95))
				{
					if(!Check_Maxed_Out())
					{
						Request_Current_Change(); //increase current
						catch_up_flag = false;
						step_count++;
					}

				}
			}
	}

}



void Check_For_Usb(void)
{
	if(GPIO_PinRead(GPIO,1,6))
		Clear_All_Bits();


}

void Clear_All_Bits(void)
{
	//clear shift registers to known state
	Port_State[MID_SR] = USB_STATE_MID;			//
	Port_State[BOTTOM_SR] = USB_STATE_BOT;		//
	Port_State[TOP_SR] = USB_STATE_TOP;
	SPI0_SendBytes(Port_State, 3, EXPANDER); // Update shift register
	Delay_Ticks(1);
	Send_Ctrl_Word(SLP_CTRL_WRD2,SIGNAL); // Switch signal off

	Ports_Cleared_Flag = true;
}

bool Short_Circuit_Chk(void)
{
float32_t x;

	if((Port_State[MID_SR] & I_GAIN) > 0)
	{ // High Gain
		if(sys.adc.I_OUT_FastFilt > 0.05 && sys.adc.V_OUT_FastFilt < 1 && Power_Level > 1)
			return(true);
		else
			return(false);
	}
	else
	{ // Low GAIN
		x = Get_Max_Current();

		if(sys.adc.I_OUT_FastFilt * 1000 > x)
			return(true);
		else
			return(false);

	}
}


bool Check_Maxed_Out(void)
{
	float32_t x,Power_Limit,Current_Request;

	if (freqArray[frequency].frequency1 < MAX_DTYPE)
		Power_Limit = sys.maxPowerLimit;
	else
		Power_Limit = 1.0;

	x = Get_Max_Current(); // HF 250mA LF 950mA
	Current_Request = Requested_Current(Bat_Type);

	if(sys.adc.I_OUT_FastFilt * 1000 > x)
		return(true);
	else if (sys.adc.V_OUT > (MAX_DC_VOLTAGE * 0.95))
		return(true);
	else if (Watts_Filt > (Power_Limit * 0.95))
		return(true);
	else if ((Current_Request * sys.adc.V_OUT_SlowFilt) > Power_Limit)
		return (true);
	else
		return(false);


}

void Check_Power_Monitor(void)
{
#define MAX_PWR_TIME 300 // 3 Seconds

	if(Watts_Filt > Get_Power_Limit() * 1.2)
	{
		Power_tmr++;
		if(Power_tmr > MAX_PWR_TIME)
		{
			Power_Level = 0;
			Cut_Signal_To_Min();
		}
		else
			Power_tmr = 0;
	}



}