/*
 * utils.c
 *
 *  Created on: Jun 7, 2022
 *      Author: Keith.Lloyd
 */

#include	"arm_math.h"
#include	"fsl_gpio.h"
#include 	"fsl_spi.h"
#include	"spi.h"
#include	"lcd.h"
#include	"eeprom.h"
#include	"display.h"
#include	"utils.h"
#include	"psu_ctrl.h"
#include	"amps.h"
#include 	"main.h"
#include	"ports.h"
#include	"adc.h"
#include	"timer.h"
#include	"mode.h"
#include	"stdbool.h"
#include <pwr_level.h>
#include	"sys_chk.h"
#include	"pro_key.h"
#include <stdio.h>
#include "hwFixes.h"
#include "System\system.h"
#include "io.h"
#include "soft_timer.h"

//#define	ON	1

extern uint8_t Port_State[];
extern uint16_t Estop_timer;
extern uint8_t Over_Voltage_Flag, LD_Flag;
extern uint8_t Over_Current_Flag;
extern uint8_t Hardware_Error_Flag;
extern uint8_t frequency, Task, Cur_Mode;
extern spi_master_config_t SPI0_config;
extern AMP_XFRMR_SLCT_t Amp2xfrmr;
extern uint8_t frequency,old_freq,frq_chg_tmr;

extern float32_t Watts, Milli_amps,test_val2;
extern uint8_t Dds_Pot_Val[], Power_Level;
extern uint16_t Pot_Value_CLAMP1[];
extern uint16_t Pot_Value_CLAMP_AB[]; // DDS Output amplitude. (AB amp)
extern uint16_t Pot_Value_AB[];
extern uint16_t Pot_Value[];
extern uint32_t new_freq,test_val1;
extern uint16_t Vchktmr, Over_Voltage_tmr;
extern float32_t Watts_Filt;
extern uint8_t Psu_Pot_Val[]; // 2 byte Data for SPI
extern uint16_t PSU_Check_tmr;
extern uint8_t tempString[40]; //
extern HARDWARE_FIX_t hwf;
extern SYSTEM_DATA_t sys;

extern ClampData_t clampData;

uint8_t Error, Suspend_Step_Chk;


uint8_t Check_Output_Status(char Connector)
{
	uint32_t x = 1;

	if (Connector == 1)
		x = sys.adc.V_ID1; // Todo Don't forget limits.
	else
		x = sys.adc.V_ID2;

	if (x > TGT_NOT_USED)
		return (EMPTY);
	if (x > CLAMP_MIN && x < CLAMP_MAX)
		return (CLAMP);
	if (x > DC_MIN && x < DC_MAX)
		return (CONNECT_LEAD);
	if (x > DDC_MIN && x < DDC_MAX)
		return (DUAL_DC);

}

void Clear_Relays()
{
	uint8_t sendData;			//ToDo SSELx will be wrong double check and fix

	Port_State[0] &= ALL_RELAYS_OFF_UPPER;// clears all relays and disconnects from outputs

	Port_State[1] &= ALL_RELAYS_OFF_LOWER;

	Send_Update_Port();

}

void Send_Update_Port(void)
{
#if 1
	SPI0_SendBytes(&Port_State[0], 3, EXPANDER);	//2 bytes

#else
	GPIO_PinWrite(GPIO, 0, PORT_LE, 0);			// Activate CS

											// First byte
SPI3_SendBytes(&Port_State[0], 1);		//send first byte via SPI

											// Reset all relays on second port.
SPI3_SendBytes(&Port_State[1], 1);				//send second byte via SPI

GPIO_PinWrite(GPIO, 0, PORT_LE, 1);			// De-select CS
#endif
}

void Select_Bypass(RELAY_SELECT_t bypass) // Bypass allows transmitting w/o full protection
{
#if 0
	sys.status[BYPASS] = bypass;

	if (bypass)
	{
		if ((freqArray[frequency].frequency1 < MIN_BLOCK_FREQ) && (!sys.status[OVERVOLTAGE]))
		{
			EXPANDER_SET(BYPASS_ON, true);
		}
	}
	else
	{
		EXPANDER_CLEAR(BYPASS_ON, true);
	}
#endif

}

void Select_Estop(ESTOP_SELECT_t stop)
{

	sys.status[ESTOP] = (bool)stop;

	// HACK
	stop = ON;

	if (stop)
	{
		GPIO_WRITE(PIN_ESTOP, LOW);		// Isolated
	}
	else
	{
		GPIO_WRITE(PIN_ESTOP, HIGH);	// Not Isolated
	}
		

}
void Select_Transformer()
{
#if 0
	if (freqArray[frequency].frequency1 > MAX_LOW_FRQ)//ie. > 80K  Was MAX_DTYPE
	{
		Port_State[MID_SR] |= SLCT_XFMR; // Switch to High freq xfrmr Taps O/P
		Port_State[TOP_SR] &= ~MUX_AB_AMP; // 0 AB Amp to HF Xfrmr
	}

	if (freqArray[frequency].frequency1 <= MAX_DTYPE)
	{										// ie. < 44100
		Port_State[MID_SR] &= ~SLCT_XFMR;	// Switch to Low freq Xfmr Taps O/P
//		Port_State[TOP_SR] &= ~MUX_AB_AMP;	// Switch to Low freq Xfmr
		Port_State[BOTTOM_SR] &= ~SLCT_AMP; // 0 D AMP routed to LF Xfrmr

	}
//	if(((freqArray[frequency].frequency1 == 65536) || (freqArray[frequency].frequency1 == 200000) || (freqArray[frequency].frequency1 == MAX_LOW_FRQ)))
	if ((freqArray[frequency].frequency1 >= 60000)
			&& (freqArray[frequency].frequency1 <= 200000))
	{
		Port_State[TOP_SR] |= MUX_AB_AMP;	// 1 AB Routed Away from HF Xfrmr
		Port_State[BOTTOM_SR] |= SLCT_AMP; // 1 AB AMP routed to LF Xfrmr
		Port_State[MID_SR] &= ~SLCT_XFMR; //  0 LF Transformer taps output selected
	}

//	Port_State[BOTTOM_SR] &= ~SLCT_AMP;	// Switch to AMP D to Low freq Xfmr by default whether used
	// or not

	SPI0_SendBytes(Port_State, 3, EXPANDER);
#endif
}

void Select_Amp_Xfrmr_Rly(AMP_XFRMR_SLCT_t Amp2xfrmr) // Allows any combination of Amp and transformer
{ //  NOT CURRENTLY USED

	switch (Amp2xfrmr)

	{
	case LFA_LFX:
		Port_State[MID_SR] &= ~XFRMR_HF_ON;	// Switch path to Low freq Xfmr
		Port_State[BOTTOM_SR] &= ~HF_AMP_ON;// Switch path for Low freq Amplifier to LF Xfrmr

		break;

	case HFA_LFX:
		Port_State[MID_SR] &= ~XFRMR_HF_ON;	// Switch path to Low freq Xfmr
		Port_State[BOTTOM_SR] |= HF_AMP_ON;	// Switch path for High freq Amplifier to LF Xfrmr

		break;

	case HFA_HFX:
		Port_State[MID_SR] |= XFRMR_HF_ON;	// Switch path to High freq Xfmr
		Port_State[BOTTOM_SR] |= HF_AMP_ON;	// Switch path for High freq Amplifier to LF Xfrmr

		break;

	}

}

void Power_ON_OFF(RELAY_SELECT_t Power)
{
	if (Power)
		GPIO_PinWrite(GPIO, 1, PWR_CTL, PWR_ON);	// Switch or keep power on
	else
		GPIO_PinWrite(GPIO, 1, PWR_CTL, PWR_OFF);			// Switch power off
}

void Check_Clamp_OverVoltage(void)
{
#if 0
	if (Cur_Mode != BROADCAST)
	{

		if((Check_For_Clamp()))
		{
			if (ACCY_GetActive() != ID_CLAMP2)
			{
				if (Compare_Voltage(sys.adc.V_OUT, MAX_CLAMP_VOLTAGE))
					Adjust_Clamp_Volts();
			}
		}
	}
#endif
}
void Check_Live_Voltage()
{

	bool dc1 = ((ACCY_GetConnectedAccessory(1) == ID_TX_SINGLE_DIRECT)
			|| (ACCY_GetConnectedAccessory(1) == ID_TX_DUAL_DIRECT));
	bool dc2 = ((ACCY_GetConnectedAccessory(2) == ID_TX_SINGLE_DIRECT)
			|| (ACCY_GetConnectedAccessory(2) == ID_TX_DUAL_DIRECT));


	if ((Power_Level == 0) && (dc1 || dc2))
	{
		if (Vchktmr == 0)
		{
			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.
				//		Over_Voltage_Flag = true;
				//		Estop_timer = DELAY_5S;
				Task = SAFETY_TASK;

				
			}

			if (Compare_Voltage(sys.adc.V_CHK, MAX_BYPASS_VOLTS)) // damaging voltage but allow operation
			{											// to continue.
				sys.status[OVERVOLTAGE] = true;
				Select_Bypass(OFF);					// Force into blocked state.
			}
		}
	}
}

bool Compare_Voltage(float32_t source, float32_t limit)
{

	if (source > limit)
		return (true);
	else
		return (false);
}

void Check_Over_Current()
{
#if 0
float x;

	x = Get_Max_Current();
	if (sys.adc.I_OUT_FastFilt * 1000 > x)
	{
		Power_Level = 0;
		Cut_Signal_To_Min();

		if (ACCY_GetActive() == ID_CLAMP2)
		{
			Get_Clamp_Value();
			clampData.pot = 1;
		}

	}
#endif
}

float Get_Max_Current(void)
{
#if 0
	if (freqArray[frequency].frequency1 <= MAX_DTYPE)
		return (MAX_CURRENT);
	else
		return (MAX_HF_CURRENT);
#endif
}

void Check_Over_Power(void)
{
#if 0
//uint8_t New_Pot_Val;
	if (Cur_Mode != BROADCAST)
	{
		if (ACCY_GetActive() != ID_CLAMP2)
		{
			if (freqArray[frequency].frequency1 < MAX_DTYPE)
				Adjust_Output_Power(sys.maxPowerLimit);
			else
				Adjust_Output_Power(1.0);
		}
	}
//	Delay_Ticks(100);
#endif
}

float32_t Get_Power_Limit(void)
{
#if 0
	if (freqArray[frequency].frequency1 < MAX_DTYPE)
		return(sys.maxPowerLimit);
	else
		return(1.0);
#endif
}

// if over power, scale amplitude
void Adjust_Output_Power(float32_t New_Power_Limit)
{
#if 0
	if (Watts_Filt > New_Power_Limit)
	{
		uint8_t New_Pot_Val;

		New_Pot_Val = New_Power_Limit / Watts_Filt * Dds_Pot_Val[1];
		if (New_Pot_Val > freqArray[frequency].max_pot)
			Dds_Pot_Val[1] = freqArray[frequency].max_pot;
		else
			Dds_Pot_Val[1] = New_Pot_Val;

		Dds_Pot_Val[0] = 0; // address
		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
		Suspend_Step_Chk = true;
	}
	else
		Suspend_Step_Chk = false;
#endif

}

void Check_PSU_Short(void) // if output less than 10%  of expected PSU voltage
{							// then a fault condition is present.
	if (PSU_Check_tmr == 0)
	{
		if ((sys.adc.V_PSU * 1.2) < Convert_Pot2_Volts(Psu_Pot_Val[1]))
		{
			Select_Estop(ISOLATED);	// Disconnect from external threats

			Power_Level = 0;		// set demand to minimum to prevent damage
			//		Update_Amp_Pot();
			Cut_Signal_To_Min();

			Disable_DC();			// power off all active systems

			Disable_BC();			//Disable BC

			Port_State[BOTTOM_SR] |= ~AMP_PSU_ON;	// switch AMP PSU OFF
			SPI0_SendBytes(Port_State, 3, EXPANDER); // Update shift register

			Task = FATAL_ERROR_TASK;
			Error = 1;
		}
	}
}

void Estop_Mode(void)
{
	Select_Estop(ISOLATED);
	Task = ESTOP_TASK;
}

void Power_Down()
{
	static bool flasher = false;

	LCD_Clear();				    // Clear the display.

	Display_Bye_Bye();				// Display Power down message to user

	EE_SaveData();			// Save necessary settings to E2PROM,timer freq' etc

	Controlled_Pwr_Dwn();
	Delay_Ticks(DELAY_200MS);
	uint8_t count_down;

	count_down = 0;

	while (1)
	{
		Power_ON_OFF(OFF);				// Power down system
		Delay_Ticks(DELAY_200MS);

		if (count_down == 4)
		{
			if (flasher)
			{
				LCD_Clear();
				Display_USB();
				count_down = 0;
				LCD_Update();
			}
			else
			{
				LCD_Clear();
				LCD_Update();

			}

			flasher ^= 1;		//toggle

		}
		else
			count_down++;

	}

}

void Chk_Gain(void)
{

	if (!Short_Circuit_Chk())		//Check for short condition first
	{
		uint32_t fre = 0;
		fre++;

		if ((Port_State[MID_SR] & I_GAIN) > 0) // High gain
		{
			if (sys.adc.I_OUT_SlowFilt > 0.035)
			{
				Port_State[MID_SR] &= ~I_GAIN; // U12 switch to low gain
				SPI0_SendBytes(Port_State, 3, EXPANDER);
			}
		}
		else
		{
			if (sys.adc.I_OUT_SlowFilt < 0.025)
				{
				Port_State[MID_SR] |= I_GAIN; // switch to High gain
				SPI0_SendBytes(Port_State, 3, EXPANDER);

				}

		}

//		Port_State[MID_SR] &= ~I_GAIN; // U12 always low gain for test
//		Port_State[MID_SR]|= I_GAIN; // switch to high gain
//		SPI0_SendBytes(Port_State, 3, EXPANDER);

	}

}

void Count_Down_PWR_OFF(void)	//Display_Bat_Error();
{

}

void Adjust_Clamp_Volts(void) // Set clamp output voltages to PL1
{
#if 0
	if (freqArray[frequency].frequency1 <= MAX_DTYPE)
	{
		Dds_Pot_Val[1] = Pot_Value_CLAMP1[1]; // data
		Power_Level = 1;
	}
	else
	{
		Dds_Pot_Val[1] = Pot_Value_CLAMP_AB[1]; // data
		Power_Level = 1;
	}

	Dds_Pot_Val[0] = 0; // address
	SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
#endif

}
void Check_Over_Voltage(void)	// Check Direct Connect Over voltage
{
#if 0
	uint8_t New_Pot_Val;
	uint8_t test1;
	if (Over_Voltage_tmr == 0)
	{
//		if (Cur_Mode != BROADCAST && (ACCY_GetConnectedAccessory(1) != ID_CLAMP)
//				&& (ACCY_GetConnectedAccessory(2) != ID_CLAMP))
//		test1 = Check_For_Clamp();
		if (Cur_Mode != BROADCAST && !Check_For_Clamp())
		{
			if (Compare_Voltage(sys.adc.V_OUT_FastFilt, MAX_DC_VOLTAGE))
			{
				Vchktmr = LOCK_OUT_DELAY;
				New_Pot_Val = MAX_DC_VOLTAGE / sys.adc.V_OUT_FastFilt
						* Dds_Pot_Val[1];
				if (New_Pot_Val > freqArray[frequency].max_pot)
					Dds_Pot_Val[1] = freqArray[frequency].max_pot;
				else
					Dds_Pot_Val[1] = New_Pot_Val;

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

				//		dec_pwr();
				//		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
				Over_Voltage_tmr == DELAY_5S;
			}
		}
	}
#endif
}

void Controlled_Pwr_Dwn()
{
	#if 0
	uint8_t count;
	uint8_t avalue; // for debugging only

	if (Cur_Mode != BROADCAST)		// decrementally adjust amplitude to zero
	{
		count = 0;
		Dds_Pot_Val[0] = 0; // address

		while (Dds_Pot_Val[1] > MIN_POT && count < SENTINAL)
		{
			Dds_Pot_Val[1]--;
			SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
//				Update_Amp_Pot();

			count++;
			Delay_Ticks(1);
		}
		Delay_Ticks(2);
		All_Amps_Off();

		Set_PSU_Voltage(V_18V);
		Delay_Ticks(20);

	}

	All_Amps_Off(); //turn off amps
	Disable_BC(); // Set duty cycle to zero

	Delay_Ticks(2);

	Set_PSU_Voltage(V_18V); // switch off main psu down
	Delay_Ticks(DELAY_250MS);
	#endif
}

void Update_Amp_Pot(void)
{
#if 0
	uint8_t avalue; // for debugging only
	Dds_Pot_Val[0] = 0; // address

	if (freqArray[frequency].frequency1 <= MAX_DTYPE)
		Dds_Pot_Val[1] = Pot_Value[Power_Level]; // data
	else
		Dds_Pot_Val[1] = Pot_Value_AB[Power_Level]; // data

	avalue = Dds_Pot_Val[1];
	SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
#endif
}

void Check_For_Clamp_On_Pwr_Up(void)
{
#if 0
	uint32_t idNumber1 = (uint32_t) ((sys.adc.V_ID2 * 3.3333) + 0.5);//multiply by 3.3333 and round to nearest integer
	uint32_t idNumber2 = (uint32_t) ((sys.adc.V_ID1 * 3.3333) + 0.5);//multiply by 3.3333 and round to nearest integer

	uint8_t count;

	if (idNumber1 == 5 || idNumber2 == 5)
//		Change_to_next_dc_frq();
	{
		if (freqArray[frequency].frequency1 <= MIN_CTYPE)// is frequecny > min
		{
			count = 0;

			while (freqArray[frequency].frequency1 < MIN_CTYPE
					&& count < FREQ_MAX_NUM)
			{
				frequency = Next_Frequency(frequency);// increment the frequency
				new_freq = freqArray[frequency].frequency1;
				count++;

			}
		}
	}
#endif

}
bool Check_For_Clamp_New()
{
	uint32_t idNumber1 = (uint32_t) ((sys.adc.V_ID2 * 3.3333) + 0.25);//multiply by 3.3333 and round to nearest integer
	uint32_t idNumber2 = (uint32_t) ((sys.adc.V_ID1 * 3.3333) + 0.25);//multiply by 3.3333 and round to nearest integer

	uint8_t count;

	if (idNumber1 == ID_CLAMP || idNumber2 == ID_CLAMP || idNumber1 == ID_CLAMP2 || idNumber2 == ID_CLAMP2)
		return(true);
	 else
		 return(false);
}

void Reduce_Kick_Back(void)	// reduce amplitude

{
	uint8_t count;
	count = 0;
	Dds_Pot_Val[0] = 0; // address

	while (Dds_Pot_Val[1] > MIN_POT && count < SENTINAL)
	{
		Dds_Pot_Val[1] -= 2;
		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);	// reduce amplitude

		count++;
		Delay_Ticks(1);
	}

	Set_PSU_Voltage(V_18V);		// reduce power supply voltage
	Delay_Ticks(20);
	// delay for catch up

	//

}

float32_t Convert_Pot2_Volts(uint8_t value)
{
	switch (value)
	{

	case V_18V:
		return (18.0);
		break;

	case V_21V:
		return (21.0);
		break;

	case V_24V:
		return (24.0);
		break;

	case V_27V:
		return (27.0);
		break;

	case V_30V:
		return (30.0);
		break;

	case V_36V:
		return (36.0);
		break;

	case V_55V:
		return (55.0);
		break;

	default:
		return (36.0);

	} 
}
void Cut_Signal_To_Min(void)
{
	Dds_Pot_Val[0] = 0; // address
	Dds_Pot_Val[1] = 2; // data

	SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);

}

bool Check_For_Clamp(void) // Assumes BROADCAST already checked for
{
	if(Cur_Mode == PORT1_A)
	{
		if(ACCY_GetConnectedAccessory(1) == ID_CLAMP || ACCY_GetConnectedAccessory(1) == ID_CLAMP2)
			return(true);
		else
			return(false);
	}
	else
	{
		if(ACCY_GetConnectedAccessory(2) == ID_CLAMP || ACCY_GetConnectedAccessory(2) == ID_CLAMP2)
			return(true);
		else
			return(false);

	}

}




void freq_key_process(void)
{
#if 0
	uint32_t tmp_frqx;

	LD_Flag = false;

	frq_chg_tmr = KEY_DELAY;	// Set timer to delay h/w update
	old_freq = frequency;

	if(Cur_Mode != BROADCAST)
	{
		Change_to_next_dc_frq();
	}
	else
	{
		if (freqArray[frequency].frequency1 < BCAST_MIN)	// if (freq <  min freq) ToDo
		{
			tmp_frqx = Search_Frequency(3140);				//  select minimum frequency for BCAST
			if (tmp_frqx < FREQ_MAX_NUM)
			{
				frequency = tmp_frqx;
				new_freq = freqArray[frequency].frequency1;
			}
		}
		else
		{
			frequency = Next_Frequency(frequency);	// increment the frequency
			new_freq = freqArray[frequency].frequency1;
			if(new_freq == 263)
			{
				tmp_frqx = Search_Frequency(3140);				//  select minimum frequency for BCAST
				if (tmp_frqx < FREQ_MAX_NUM)
				{
				 frequency = tmp_frqx;
				 new_freq = freqArray[frequency].frequency1;
				}
			}
		}
	}
#endif
}

void LD_key_process(void)
{
#if 0
	if(freqArray[frequency].frequency1  <= MAX_LD_FREQ && Cur_Mode != BROADCAST)
	{
		LD_Flag  ^= 1;

		if(LD_Flag)
			Init_LD_Sync();		// Set up Second frequency
		else
			Init_LD_Sync();		// clear the second freq
	}
#endif

}

void Leica_Patch(void)
{

//#########################
   		if(Read_Model_type() == LEICA)	// Read Model Patches erroneous boards sent to Leica 8/2/24
   		{
   		    sprintf(tempString, "%d", hwf.mainPcbaPN);

   			if(strncmp(tempString,"208021",6) == 0)				// check for HW rev.
   			{
   	   		    hwf.mainPcbaPN = 208025;
   				EE_WriteUINT32(EE_HWFIX_MAIN_PCBA_PN, hwf.mainPcbaPN);// Change to 208025 Assembly

   			}
   		}
}
//########################

void Umag_Patch(void)
{
 char str1[4] = "10W";

//#########################
   		if(Read_Model_type() == UMAG)	// Read Model Patches erroneous boards sent to Leica 8/2/24
   			strncpy(sys.modelName,str1,4);

 //  			sys.modelName = "10W";
 //  		{
 //  		    sprintf(tempString, "%d", hwf.mainPcbaPN);

 //  			if(strncmp(tempString,"208021",6) == 0)				// check for HW rev.
 //  			{
 //  	   		    hwf.mainPcbaPN = 208025;
 //  				EE_WriteUINT32(EE_HWFIX_MAIN_PCBA_PN, hwf.mainPcbaPN);// Change to 208025 Assembly

 //  			}
 //  		}
}
//########################

//Power_Level = 0;
//if(freqArray[frequency].frequency1 < 44100)
//	Dds_Pot_Val[1] = Pot_Value[Power_Level]; // data
//else
//	Dds_Pot_Val[1] = Pot_Value_AB[Power_Level]; // data
//



void delayms(uint32_t msec)
{
	stimer_delay(msec);
}