/*
 * pwr_level.c
 *
 *  Created on: Jun 30, 2022
 *      Author: Keith.Lloyd
 */

#include <stdbool.h>
#include <stdint.h>
#include <arm_math.h>
#include <pwr_level.h>


#include "display.h"
#include "spi.h"
#include "mode.h"
#include "pwm.h"
#include "amps.h"
#include "frq.h"
#include "adc.h"
#include "init.h"
#include "utils.h"
#include "ports.h"
#include "timer.h"
#include "sys_chk.h"
#include "battery.h"
#include "System/system.h"

extern float32_t Watts;
extern	uint8_t frequency,Test_Mode,catch_up_flag;
extern 	float32_t Milli_amps;
extern SYSTEM_DATA_t sys;

uint16_t Old_Pot_Val;
uint8_t Power_Level,Bcast_Pwr_Level;
float Requested_Current_Lith[5] = {10.0/1000,50.0/1000.0,150.0/1000.0,400.0/1000.0,1000.0/1000.0}; // Requested Current Output Value

float Requested_Current_Alk[5] = {10.0/1000,50.0/1000.0,100.0/1000.0,150.0/1000.0,200.0/1000.0}; // Requested Current Output Value
float Requested_Current_HF[5] = {10.0/1000,50.0/1000.0,100.0/1000.0,150.0/1000.0,200.0/1000.0}; // Requested Current Output Value


uint16_t Pot_Value[5] = {23,30,45,93,180}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_New_CLAMP0[5] = {20,30,45,83,100}; // <512Hz DDS Output amplitude. (D amp)

uint16_t Pot_Value_New_CLAMP1[5] = {20,30,45,100,200}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_New_CLAMP2[5] = {20,30,45,93,250}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_New_CLAMP3[5] = {20,30,45,93,250}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_New_CLAMP4[5] = {20,30,45,63,65}; // <512Hz DDS Output amplitude. (D amp)

uint16_t Pot_Value_New_CLAMP_AB[5] = {5,10,45,93,150}; // DDS Output amplitude. (AB amp)
uint16_t Pot_Value_New_CLAMP_AB2[5] = {5,10,45,93,140}; // DDS Output amplitude. (AB amp)

uint16_t Pot_Value_New_CLAMP_PK[5] = {20,25,50,60,130}; // DDS Output amplitude. (D amp)


uint16_t Pot_Value_CLAMP0[5] = {20,30,45,83,103}; // <512Hz DDS Output amplitude. (D amp)

uint16_t Pot_Value_CLAMP1[5] = {20,30,45,93,120}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_CLAMP2[5] = {20,30,45,93,140}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_CLAMP3[5] = {20,30,45,65,73}; // DDS Output amplitude. (D amp)
uint16_t Pot_Value_CLAMP4[5] = {20,30,45,62,66}; // DDS Output amplitude. (D amp)

uint16_t Pot_Value_CLAMP_AB[5] = {5,10,45,93,150}; // DDS Output amplitude. (AB amp)
uint16_t Pot_Value_CLAMP_AB2[5] = {5,10,45,93,140}; // DDS Output amplitude. (AB amp)

uint16_t Pot_Value_CLAMP_PK[5] = {20,25,30,40,47}; // DDS Output amplitude. (D amp)


uint8_t Bcast_LF_Value[5] = {0,5,10,15,20}; // DDS Output amplitude. (D amp)
uint8_t Bcast_HF_Value[5] = {0,12,25,37,50}; // DDS Output amplitude. (D amp)

uint8_t Bcast_VLF_Value_Alk[5] = {0,2,4,6,8}; // DDS Output amplitude. (D amp)
uint8_t Bcast_VLF2_Value_Alk[5] = {0,5,7,10,12}; // DDS Output amplitude. (D amp)

//uint8_t Bcast_LF_Value_Alk[5] = {0,3,5,7,9}; // DDS Output amplitude. (D amp)
uint8_t Bcast_LF_Value_Alk[5] = {0,8,10,14,18}; // DDS Output amplitude. (D amp)
uint8_t Bcast_MLF_Value_Alk[5] = {0,12,14,18,26}; // DDS Output amplitude. (D amp)
uint8_t Bcast_MLF2_Value_Alk[5] = {0,16,24,30,36}; // DDS Output amplitude. (D amp)

uint8_t Bcast_HF_Value_Alk[5] = {0,12,24,48,50}; // DDS Output amplitude. (D amp)


uint16_t Pot_Value_AB[5] = {4,10,45,93,112}; // DDS Output amplitude. (AB amp)

uint16_t new_pot_val;



//uint16_t Bcast_PwePot_Value[5] = {10,100,130,150,220}; // DDS Output amplitude.

uint8_t Dds_Pot_Val[2]; // 2 byte Data for SPI
extern uint8_t Cur_Mode,Taps_Flag, Bat_Type;
extern volatile uint8_t BC_Duty_Cycle;
extern volatile bool PWM_Update_Flag;
extern uint16_t Sys_Chk_tmr,Taps_adjust_timer;
extern uint8_t	Bat_Type;
REGULATE_t reg;

extern uint32_t systemTime;
extern ClampData_t clampData;


void setTestPot(void)
{
	clampData.pot = 40;
	Dds_Pot_Val[1] = clampData.pot;
	SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
}

void doClampPower(void)
{
	float tmpF;
	float power = 0.0f;
	bool update = true;

	clampData.voltage = sys.adc.V_OUT_FastFilt;
	clampData.current = sys.adc.I_OUT_FastFilt;
	power = clampData.voltage * clampData.current;

	if (power > clampData.maxPower)
	{
		clampData.regulate = true;
		clampData.timeout = systemTime;
	}

	// calculate clamp voltage setting based on test pot value and frequency
	if (clampData.regulate)
	{
		bool newLevel = (clampData.prevPowerLevel != Power_Level);
		
		clampData.impedance = clampData.voltage / clampData.current;

		if ((clampData.timeout > 0) && (systemTime >= clampData.timeout))
		{

			if (!newLevel && fabs(clampData.targetPower - power) < 0.01f)
			{
				// target power reached
				clampData.regulate = false;
				
			}
			else
			if (!newLevel && fabs(MAX_CLAMP_VOLTAGE - clampData.voltage) < 1.0f)
			{
				// max clamp voltage reached
				clampData.regulate = false;
			}
			else
			if (newLevel && (clampData.prevPowerLevel == 0))
			{
				setTestPot();
				clampData.timeout = systemTime + CLAMP_REGULATE_DELAY;
			}
			else
			{
			
				if (clampData.pot > 0)
				{
					clampData.slope = clampData.voltage / (float)clampData.pot;
				}

				tmpF = sqrtf(clampData.impedance * clampData.targetPower);

				if (tmpF > MAX_CLAMP_VOLTAGE)
				{
					tmpF = MAX_CLAMP_VOLTAGE;
				}

				float pot = tmpF / clampData.slope;

				if (pot > 255)
				{
					clampData.pot = 255;
				}
				else
				{
					clampData.pot = (uint8_t)pot;
				}
				Dds_Pot_Val[1] = clampData.pot;
				SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
				
				clampData.timeout = systemTime + CLAMP_REGULATE_DELAY;

				
			}

			clampData.prevPowerLevel = Power_Level;
		}
	}
}

void inc_pwr(void)
{
	if(Cur_Mode != BROADCAST)
	{
		if(Power_Level < LEVEL_MAX-1)
			Power_Level++;

//		if((!Test_Mode) && (ACCY_GetConnectedAccessory(1) != ID_CLAMP) && (ACCY_GetConnectedAccessory(2) != ID_CLAMP))
		if((!Check_For_Clamp()))
		{
			if((Power_Level == 1))// && !Block_Off)
			{
				//Block_Off = true;
				Check_For_Connected_Volts();
			}
			else
				Request_Current_Change(); // Normal running no clamp
			}
		else
		{
			Dds_Pot_Val[0] = 0; // address


			if(Power_Level ==1)
			{
				Check_For_Connected_Volts();
			}
			
			Get_Clamp_Value();

			if (ACCY_GetActive() == ID_CLAMP)
			{
				SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
			}
			else
			{
				clampData.regulate = true;
			}

		}
	}
	else
	{
		if(Bcast_Pwr_Level < MAX_BCAST_PWR)
			Increase_BC_PWR();
	}

}

void dec_pwr(void)
{
	if(Cur_Mode != BROADCAST)
	{

		if(Power_Level > 0)
			Power_Level--;

		if((!Check_For_Clamp()))
		{
			Request_Current_Change();	//		Request_Current_Change();
		}
		else
		{
			Dds_Pot_Val[0] = 0; // address

			Get_Clamp_Value();

			if (ACCY_GetActive() == ID_CLAMP)
			{
				SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
			}
			else
			{
				clampData.regulate = true;
			}
		}
	}
	else
	if(Bcast_Pwr_Level > MIN_BCAST_PWR)
	{
		Decrease_BC_PWR();
	}

}

void Increase_BC_PWR(void)
{

	Bcast_Pwr_Level++;
//uint8_t Old_Bat;
//	Old_Bat = Bat_Type;
//	Bat_Type = ALKALINE; // Testing Only

	if(Bat_Type == LITHIUM)
	{
		if(freqArray[frequency].frequency1 <= 8192) //8010
			BC_Duty_Cycle = Bcast_LF_Value[Bcast_Pwr_Level];
		else
			BC_Duty_Cycle = Bcast_HF_Value[Bcast_Pwr_Level];
	}
	else
		BC_Duty_Cycle = Get_BC_Alk_Value();

//	{
//		if(freqArray[frequency].frequency1 <= 14000)
//		{

//			if(freqArray[frequency].frequency1 <= 4000)
//				BC_Duty_Cycle = Bcast_VLF_Value_Alk[Bcast_Pwr_Level];
//			else
//				BC_Duty_Cycle = Bcast_LF_Value_Alk[Bcast_Pwr_Level];
//		}
//		else
//			BC_Duty_Cycle = Bcast_HF_Value_Alk[Bcast_Pwr_Level];
//	}

		PWM_Setup(freqArray[frequency].frequency1, BC_Duty_Cycle);//freqArray[frequency].frequency1

//		Bat_Type = Old_Bat; // Testing Only

}

void Decrease_BC_PWR(void)
{
		Bcast_Pwr_Level--;

//uint8_t Old_Bat;
//			Old_Bat = Bat_Type;
//			Bat_Type = ALKALINE; // Testing Only

		if(Bat_Type == LITHIUM)
		{
			if(freqArray[frequency].frequency1 > 8192) //8010
				BC_Duty_Cycle = Bcast_HF_Value[Bcast_Pwr_Level];
			else
				BC_Duty_Cycle = Bcast_LF_Value[Bcast_Pwr_Level];
		}
		else
//		{
			BC_Duty_Cycle = Get_BC_Alk_Value();
//			if(freqArray[frequency].frequency1 > 14000)
//			{
//				if(freqArray[frequency].frequency1 <= 4000)
//					BC_Duty_Cycle = Bcast_VLF_Value_Alk[Bcast_Pwr_Level];
//				else
//					BC_Duty_Cycle = Bcast_HF_Value_Alk[Bcast_Pwr_Level];
//			}
//			else
//				BC_Duty_Cycle = Bcast_LF_Value_Alk[Bcast_Pwr_Level];
//		}

		PWM_Setup(freqArray[frequency].frequency1, BC_Duty_Cycle);//freqArray[frequency].frequency1

//		Bat_Type = Old_Bat; // Testing Only

}


uint8_t Get_BC_Alk_Value(void)
{
uint8_t BC_Duty_val;

	if(freqArray[frequency].frequency1 <= 4000)
		BC_Duty_val = Bcast_VLF_Value_Alk[Bcast_Pwr_Level];
	else if(freqArray[frequency].frequency1 <= 6000)
		BC_Duty_val = Bcast_VLF2_Value_Alk[Bcast_Pwr_Level];
	else if(freqArray[frequency].frequency1 <= 10000)
		BC_Duty_val = Bcast_LF_Value_Alk[Bcast_Pwr_Level];
	else if(freqArray[frequency].frequency1 <= 14000)
		BC_Duty_val = Bcast_MLF_Value_Alk[Bcast_Pwr_Level];
	else if(freqArray[frequency].frequency1 <= 20000)
		BC_Duty_val = Bcast_MLF2_Value_Alk[Bcast_Pwr_Level];
	else
		BC_Duty_val = Bcast_HF_Value_Alk[Bcast_Pwr_Level];

	return(BC_Duty_val);
}

float Calc_Max_current(void)
{
	float a;
	if(freqArray[frequency].frequency1 <= MAX_DTYPE)
		a = sqrtf(sys.maxPowerLimit / sys.adc.Ohms_slowfilt);
	else
		a = sqrtf(1/sys.adc.Ohms_slowfilt);

	return(a);

}

void Request_Current_Change(void)
{
	float Max_Current_Allowed,Current_Request;

	if (Power_Level == 0 || Milli_amps == 0.0) //Power_Level == 0 ||
		Init_Amplitude();
	else
	{
		Max_Current_Allowed = Calc_Max_current();
//		Current_Request = Requested_Current[Power_Level];
		Current_Request = Requested_Current(Bat_Type);

		if(Current_Request > Max_Current_Allowed)
			Current_Request = Max_Current_Allowed; //make it comply}

		Old_Pot_Val = Dds_Pot_Val[1];
		new_pot_val = (Current_Request /sys.adc.I_OUT_SlowFilt) * Dds_Pot_Val[1];  // Request divided by Pot current contents

		if(new_pot_val < 3)
			new_pot_val = 3;

		if(new_pot_val < freqArray[frequency].max_pot)
			Dds_Pot_Val[1] = new_pot_val;
		else
			Dds_Pot_Val[1] = freqArray[frequency].max_pot;

		Dds_Pot_Val[0] = 0; // address
//		Adjust_Pot_Val();

		Check_New_Pot_Volts();

		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);


		Sys_Chk_tmr= DELAY_500MS;  // Allow system settling before checking measurements
		Taps_Flag = false;
		Taps_adjust_timer = DELAY_2S;
		catch_up_flag = true;
	}
}

/*
 * 	Initialize current regulation
 */
void InitRegulate(void)
{
	reg.state = RS_STOP;

	//set up variables: maxCurrent, maxPower, etc?
}

/*
 * 	Regulate current to requested target - Direct Connect Only
 * 	Call this function @ 10Hz from the main loop
 * 	Do not exceed voltage limit
 * 	Do not exceed power limit
 */
void Regulate(void)
{
//	reg.requestedCurrent = Requested_Current[Power_Level];
	reg.maxCurrent = 1000;		//TODO: determine max current - does this ever change?
	reg.maxVoltage = 90.0;
	reg.maxPower = 10.0;		//TODO: determine max power by model and frequency


	switch(reg.state)
	{
	case RS_STOP:
		break;
	case RS_START:
		//initialize. Make initial guesses for pot value, taps, HV rail, etc.
		break;
	case RS_RUN:
		//Adjust DDS pot
			//look at current, voltage, power compared to target and make adjustment
		break;
	case RS_MAINTAIN:
		//Maintain settings unless something changes
			//Make sure we're not exceeding max values
			//Check for current > 10% from stopping point
		break;
	}

}

void Adjust_Pot_Val(void)
{
	if(new_pot_val < Old_Pot_Val)
	{
		Dds_Pot_Val[1] = new_pot_val; // just send it
		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);

	}

	Dds_Pot_Val[1] = Old_Pot_Val;
	while(Old_Pot_Val < new_pot_val)
	{
		Old_Pot_Val++;
		Dds_Pot_Val[1] = Old_Pot_Val;
		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
		Delay_Ticks(1);

	}

}

void Get_Clamp_Value()
{
	if((ACCY_GetConnectedAccessory(1) == ID_CLAMP) || (ACCY_GetConnectedAccessory(2) == ID_CLAMP))
	{
		if (freqArray[frequency].frequency1  > MAX_DTYPE)
			Dds_Pot_Val[1] = Pot_Value_CLAMP_AB[Power_Level]; // data

		if (freqArray[frequency].frequency1 > 90000)
			Dds_Pot_Val[1] = Pot_Value_CLAMP_AB2[Power_Level]; // data

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

		if ((freqArray[frequency].frequency1 < 40000) && (freqArray[frequency].frequency1 > 34000))
			Dds_Pot_Val[1] = Pot_Value_CLAMP_PK[Power_Level]; // data

		if (freqArray[frequency].frequency1 <= 9820)
			Dds_Pot_Val[1] = Pot_Value_CLAMP2[Power_Level]; // data

		if (freqArray[frequency].frequency1 <= 3140)
			Dds_Pot_Val[1] = Pot_Value_CLAMP1[Power_Level]; // data

		if (freqArray[frequency].frequency1 <= 512)
			Dds_Pot_Val[1] = Pot_Value_CLAMP0[Power_Level]; // data
		if (freqArray[frequency].frequency1 < 512)
			Dds_Pot_Val[1] = Pot_Value_CLAMP4[Power_Level]; // data

	}
	else // Clamp 2
	{
		//if (freqArray[frequency].frequency1 <= MAX_DTYPE)
		{
			switch (Power_Level)
			{
				case LEVEL1: 
					clampData.targetPower = clampData.maxPower / 64.0f;
					break;

				case LEVEL2: 
					clampData.targetPower = clampData.maxPower / 16.0f;
					break;

				case LEVEL3: 
					clampData.targetPower = clampData.maxPower / 4.0f;
					break;

				case LEVEL4: 
					clampData.targetPower = clampData.maxPower / 1.0f;
					break;

				default:

					clampData.targetPower = -1.0f;

			}
			
		}
#if 0		
		if (freqArray[frequency].frequency1 > MAX_DTYPE)
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP_AB[Power_Level]; // data
		if (freqArray[frequency].frequency1 > 90000)
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP_AB2[Power_Level]; // data

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

		if ((freqArray[frequency].frequency1 < 40000) && (freqArray[frequency].frequency1 > 34000))
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP_PK[Power_Level]; // data

		if (freqArray[frequency].frequency1 <= 9820)
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP2[Power_Level]; // data

		if (freqArray[frequency].frequency1 <= 3140)
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP1[Power_Level]; // data

		if (freqArray[frequency].frequency1 <= 512)
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP0[Power_Level]; // data

		if (freqArray[frequency].frequency1 < 512)
			Dds_Pot_Val[1] = Pot_Value_New_CLAMP4[Power_Level]; // data
#endif

	}
}

void Check_New_Pot_Volts()
{
float32_t V_Calc;	// Currently set value is Old_Pot requested value is Pot_Val[]

	if(sys.adc.V_OUT_SlowFilt > 0.0)
	{
		V_Calc = (sys.adc.V_OUT_FastFilt / Old_Pot_Val);

		if(V_Calc * Dds_Pot_Val[1] > 90.0)
			Dds_Pot_Val[1] = MAX_DC_VOLTAGE / sys.adc.V_OUT_FastFilt * Old_Pot_Val;

		if(Dds_Pot_Val[1] > freqArray[frequency].max_pot)
			Dds_Pot_Val[1] = freqArray[frequency].max_pot;
	}

}

float Requested_Current(uint8_t Battery_Type)
{
	if(Battery_Type == LITHIUM || Battery_Type == EXT_DC)
		{
		if(freqArray[frequency].frequency1 <= MAX_DTYPE)
			return (Requested_Current_Lith[Power_Level]);
		else
			return(Requested_Current_HF[Power_Level]);
		}
	else
	{
		if(freqArray[frequency].frequency1 <= MAX_DTYPE)
			return(Requested_Current_Alk[Power_Level]);
		else
			return (Requested_Current_HF[Power_Level]);

	}
}
//Current_Request = Requested_Current[Power_Level];