/*
 **	Copyright 2021 Tjitte van der Ploeg	tjitte@tpee.nl
 **
 **	This file is part of the OpenBoost firmware.
 **	The OpenBoost firmware firmware is free software: you can redistribute
 **	it and/or modify it under the terms of the GNU General Public License
 **	as published by the Free Software Foundation, either version 3 of the
 **	License, or (at your option) any later version. The VESC firmware is
 **	distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
 **	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 **	PARTICULAR PURPOSE. See the GNU General Public License for more details.
 **
 **	You should have received a copy of the GNU General Public License along
 **	with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdlib.h>

#include "control.h"
#include "testing.h"
#include "crc.h"
#include "string.h"


#define METER_EMA 0.95f
#define EMA(_A_, _B_, _F_)	_A_ = (_F_ * _A_) + ((1 - _F_) * _B_)
#define pi 3.14159265359f

ConverterPhase_t phase;
ConverterMueasurements_t meter;
ConverterMueasurements_t meter_slow;
ConverterScope_t scope;

CalibrationData_t cal;
ConverterSettings_t settings;

float Ts; //Sample period, s
float ControllerR;
extern uint32_t adc5data[6];

modTestingSimState_t simstate_c;

void control_init(const ConverterSettings_t* s, const CalibrationData_t * c) {
	//Check if settings are valid.
	if(control_check_parameters(s,c)){
		phase.fault = Converter_ConfigError;
	}

	//Even though there is a config fault, push on anyway. Do not enable output.
	settings=*s;
	cal=*c;

	phase.HSEpin  = PHSEN_Pin;
	phase.HSEport = PHSEN_GPIO_Port;
	phase.PENpin  = PEN_Pin;
	phase.PENport = PEN_GPIO_Port;
	phase.timerindex = HRTIM_TIMERINDEX_TIMER_B;
	phase.LimLEDpin = LED1_Pin;
	phase.LimLEDport= LED1_GPIO_Port;

	phase.Vsp = 10000;
	phase.Vhighlim = settings.HighSideVoltageLimitSoft;
	phase.Iindlim = settings.LowSideCurrentMaxLimitSoft;
	phase.Ihighlim = settings.HighSideCurrentLimitSoft;

	modConverterPWMOutputDisable();

	pwm_init(HW_SWITCHINGFREQUENCY, HW_CONTROLLERFREQUENCY, HW_DEADTIMERISING, HW_DEADTIMEFALLING, HW_ADC_DELAY);

	analog_init();

	Ts = pwm_GetControllerPeriod();

#ifdef SIMULATION
	pwm_init(HW_SWITCHINGFREQUENCY*0.5, HW_CONTROLLERFREQUENCY*0.5, HW_DEADTIMERISING, HW_DEADTIMEFALLING, HW_ADC_DELAY);
#endif

	ControllerR = sqrtf(HW_L/HW_CLOW)/ HW_Q  - HW_RLINT;

	HAL_Delay(100);
}

float t;

void control_controlloop(ConverterPhase_t* p){

	//LED3_GPIO_Port->BSRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BSRR = LED3_Pin;

	bool TemperatureLimited = false;
	bool OutputCurrentLimited = false;
	bool OutputVoltageLimited = false;




	//Ipv observer
	float dvdt = (p->Vlow - p->Vlowm1) / (float)Ts;
	float Ilowest = dvdt*HW_CLOW  + (p->Iind + p->Iindm1)/2.0f;
	p->Vlowm1 = p->Vlow;
	p->Iindm1 = p->Iind;
	EMA(p->Ilow, Ilowest, CURRENT_PV_FORGETING_FACTOR);

	//Time delay compensation
	if (p->pwm_enabled){


		p->Iind_pred  = p->Iind // Zeroth order taylor term
				+ HW_delay/HW_L*(p->Vlow - (1.0f-p->dutycycle)*p->Vhigh - HW_RLINT*p->Iind) //1st order taylor term
				+ HW_delay*HW_delay/(HW_L*HW_CLOW)*(p->Ilow - p->Iind); //second order (reduced) taylor term.

		p->Vhigh_pred = p->Vhigh;// + HW_delay/HW_CHIGH*(p->dutycycle*p->Iind  -  p->Ihigh);
		p->Vlow_pred  = p->Vlow;//  + HW_delay/HW_CLOW *(p->Ilow - p->Iind);
	}else{
		p->Iind_pred  = p->Iind
				+ 0.5f*HW_delay/HW_L*(p->Vlow - (1.0f-p->dutycycle)*p->Vhigh - HW_RLINT*p->Iind); //1st order taylor term
		p->Vhigh_pred = p->Vhigh;// + HW_delay/HW_CHIGH*(- p->Ihigh);
		p->Vlow_pred  = p->Vlow; // + HW_delay/HW_CLOW *(p->Ilow);
	}

	//These variables need to be set by the controller.
	float Vn = 0.0f;
	float Ilim = 0.0f;

#ifdef HW_TOPOLOGY_BOOST

	float Vnn = p->Vsp - (p->Ilow *(ControllerR+HW_RLINT));

	Ilim = p->Iindlim;


	//Temperature de-rating
	if (p->TemperatureHeatsink > settings.TemperatureLimitStart){
		t = (p->TemperatureHeatsink - settings.TemperatureLimitStart)/(settings.TemperatureLimitEnd - settings.TemperatureLimitStart);
		Ilim = p->Iindlim * (1 - t);
		if (Ilim < 0){
			Ilim = 0;
		}
		TemperatureLimited = true;
	}

	//Output voltage limit
	float Ioutlim = HW_KLIM_VOUT*HW_CHIGH*(p->Vhighlim - p->Vhigh_pred)/(Ts);// + 0.5f*p->Ihigh;

	if(Ioutlim > p->Ihighlim){
		Ioutlim =  p->Ihighlim;
		OutputCurrentLimited = true;
	}

	float Ilimmaxvout = Ioutlim/(1.0f-p->dutycycle);

	//Limit the output current loop gain at high duty cycles (low input voltages).
	if(p->dutycycle > HW_IOUT_MAXDUTY){
		Ilimmaxvout = Ioutlim/(1.0f-HW_IOUT_MAXDUTY);
	}

	//Limit the input current to higher than 0, to prevent current under-shoot.
	//if(Ilimmaxvout < 0.0){
	//	Ilimmaxvout = 0.0f;
	//}

	if(Ilim > Ilimmaxvout){
		Ilim = Ilimmaxvout;
		OutputVoltageLimited = true;
	}

	p->Ilimvout = Ilim / 1000.0f;


	//Current limit
	//float Vnlimup = -p->Vlow - (settings.RLint * p->Iind) + ((settings.Klim * settings.L /Ts)*(settings.PhaseCurrentMin - p->Iind) );
	//float Vnlimlo = -p->Vlow - (settings.RLint * p->Iind) + ((settings.Klim * settings.L /Ts)*(Ilim - p->Iind) );
	float Vnlimup = +p->Vlow_pred - (HW_RLINT * p->Iind_pred) - (HW_KLIM*HW_L*(settings.LowSideCurrentMinLimitSoft - p->Iind_pred)/Ts)
		 + Ts/(2*HW_CLOW)*(p->Ilow - p->Iind_pred);
	float Vnlimlo = +p->Vlow_pred - (HW_RLINT * p->Iind_pred) - (HW_KLIM*HW_L*(Ilim - p->Iind_pred)/Ts )
		+ Ts/(2*HW_CLOW)*(p->Ilow - p->Iind_pred);

	//Reset the mode variable.

	PhaseMode_t newmode = PhaseMode_CIV;

	//Unlimited controller
	Vn = Vnn + (p->Iind_pred*ControllerR);

	//Limit Vnn to limit phase current
	if(Vn > Vnlimup){
		Vn = Vnlimup;
		newmode = PhaseMode_MinInputCurrent;
	}

	if(Vn < Vnlimlo) {
		Vn = Vnlimlo;
		newmode = PhaseMode_CIC;
	}

	//Select correct operating mode

	if(p->mode == PhaseMode_CIC){
		if(TemperatureLimited){
			newmode = PhaseMode_TD;
		}

		if(OutputVoltageLimited){
			if(OutputCurrentLimited)newmode = PhaseMode_COC;
			else newmode = PhaseMode_COV;
		}
	}
	p->mode = newmode;

#elif defined(HW_TOPOLOGY_BUCK)
	//p->Vsp = 50000;

	PhaseMode_t newmode = PhaseMode_CIV;

	//Limit low boost factor.
	volatile float vsp = p->Vsp;
	if (vsp < (p->Vlow*1.05)){
		vsp = (p->Vlow*1.05);
		p->mode = PhaseMode_MinInputVoltage;
	}

	volatile float Ioutsp = HW_KLIM_VOUT*HW_CHIGH*(vsp - p->Vhigh_pred)/(Ts);
	volatile float Isp = Ioutsp +p->Ihigh*(vsp/p->Vlow_pred);

    p->Ilimvout = (-0.5f*HW_KLIM*HW_CLOW*(settings.LowSideVoltageLimitSoft - p->Vlow)/Ts) +p->Ilow;


    Ilim = p->Iindlim;

	//Temperature de-rating
	if (p->TemperatureHeatsink > settings.TemperatureLimitStart){
		t = (p->TemperatureHeatsink - settings.TemperatureLimitStart)/(settings.TemperatureLimitEnd - settings.TemperatureLimitStart);
		Ilim = p->Iindlim * (1 - t);
		if (Ilim > 0){
			Ilim = 0;
		}
		TemperatureLimited = true;
	}


    if (Isp > -settings.LowSideCurrentMinLimitSoft){
    	Isp = -settings.LowSideCurrentMinLimitSoft;
    	newmode  = PhaseMode_MinInputCurrent;
    }
    if (Isp < -Ilim ){
		Isp = -Ilim;
		newmode  = PhaseMode_COC;

	}

    if (Isp < p->Ilimvout){
    	Isp = p->Ilimvout;
    	newmode  = PhaseMode_COV;
    }

    if(p->mode == PhaseMode_CIC){
    		if(TemperatureLimited){
    			newmode = PhaseMode_TD;
    		}
    }
    p->mode = newmode;

    //Vn = +p->Vlow_pred - (HW_RLINT * p->Iind_pred) - (HW_KLIM*HW_L*(Isp - p->Iind_pred)/Ts )
    //		+ Ts/(2*HW_CLOW)*(p->Ilow - p->Iind_pred);

    Vn = +p->Vlow - (HW_KLIM*HW_L/Ts*(Isp - p->Iind));

#endif

	float Dn = Vn / p->Vhigh_pred;

	if (Dn > 0.999f){
		Dn = 0.999f;
	}
	if (Dn < 0.001f){
		Dn = 0.001f;
	}

	p->dutycycle = 1.0f-Dn;



	//If the output is smaller then the input voltage, make sure the duty cycle does not drop to zero.
	if((p->Vlow*1.05f) > p->Vhigh){
		//if(p->dutycycle < 0.15f)p->dutycycle = 0.15f; TODO TODO
	}


	//Control PWM enable. Startup disabled to calculate initial pwm, then depending on output current.
	//If the upper current limit is lower then the lower current limit, set the duty cycle to 0
	//This ensure full shutdown when the power level gets towards zero.
	bool disable_voutlim = false;
	if(Ilim < 0){
		p->HSEport->BRR = p->HSEpin;
		p->PENport->BRR = p->PENpin;

		disable_voutlim = true;
		p->pwm_enabled =  false;
	}else{
		if(Ilim >  HW_IOUT_EN_HYST){
			if(p->enabled){

				p->pwm_enabled =  true;
			}
		}
	}

	if(p->fault != Converter_OK){
		modConverterPWMOutputDisable();
		scope_trigger_fault();
	}else{


		//Set PWM



		//Deadtime compensation
		float dI = (p->Vlow_pred*p->dutycycle)/ (HW_SWITCHINGFREQUENCY*1e3*HW_L);
		float dtcomp = 0;

		if (p->Iind_pred +  dI/2 > 0.0f){
			dtcomp += HW_DEADTIMERISING*1e-6*HW_SWITCHINGFREQUENCY;
		}
		else{
			dtcomp -= HW_DEADTIMERISING*1e-6*HW_SWITCHINGFREQUENCY;
		}

		if (p->Iind_pred -  dI/2 > 0.0f){
			dtcomp += HW_DEADTIMEFALLING*1e-6*HW_SWITCHINGFREQUENCY;
		}
		else{
			dtcomp -= HW_DEADTIMEFALLING*1e-6*HW_SWITCHINGFREQUENCY;
		}


		pwm_setDuty(p->dutycycle + dtcomp);

		if(p->pwm_enabled){

			#ifndef SIMULATION
			p->PENport->BSRR = p->PENpin;
			#ifdef HSEN

			if(p->dutycycle < 0.01f){
				//Do not, ever put on the high side FET continuesly.
				//e.g. when the duty cycle is zero.
				p->HSEport->BRR = p->HSEpin;
			}

			#ifdef HW_TOPOLOGY_BOOST
			else if(p->Iind > settings.PhaseHighSideEnableCurrent){
				if(disable_voutlim == false){
					p->HSEport->BSRR = p->HSEpin;
				}
			}
			#elif defined HW_TOPOLOGY_BUCK
			else if(disable_voutlim == false){
				p->HSEport->BSRR = p->HSEpin;
			}
			#endif
			else{
				p->HSEport->BRR = p->HSEpin;
			}

			#else
					p->HSEport->BRR = p->HSEpin;
			#endif
			#endif
		}else{
			p->HSEport->BRR = p->HSEpin;
			p->PENport->BRR = p->PENpin;
		}
	}

	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BSRR = LED3_Pin;


	//Do Lower priority duties now.

	//Calculate converter input and output currents
	//p->Power = (p->Ihigh*p->Vhigh)*1.0e-6f;


	p->PowerLow = (p->Ilow*p->Vlow)*1.0e-6f;
	p->PowerHigh = (p->Ihigh*p->Vhigh)*1.0e-6f;


	if(p->PowerHigh && p->PowerLow){
#ifdef HW_TOPOLOGY_BOOST
		p->eff = p->PowerHigh/p->PowerLow;
#elif defined HW_TOPOLOGY_BUCK
		p->eff = p->PowerLow/p->PowerHigh;
#endif
	}else{
		p->eff = 1.0f;
	}

	EMA(meter.Iind, p->Iind*0.001f,settings.meterfilterCoeficient);
	EMA(meter.Ihigh, p->Ihigh*0.001f,settings.meterfilterCoeficient);
	EMA(meter.Ilow, p->Ilow*0.001f,settings.meterfilterCoeficient);
	EMA(meter.Vlow, p->Vlow*0.001f,settings.meterfilterCoeficient);
	EMA(meter.Vhigh, p->Vhigh*0.001f,settings.meterfilterCoeficient);
	EMA(meter.Eff, p->eff,settings.meterfilterCoeficient);
	EMA(meter.TemperatureAmbient, p->TemperatureAmbient,settings.meterfilterCoeficient);
	EMA(meter.TemperatureHeatsink, p->TemperatureHeatsink,settings.meterfilterCoeficient);
	EMA(meter.PowerHigh, p->PowerHigh, settings.meterfilterCoeficient);
	EMA(meter.PowerLow, p->PowerLow, settings.meterfilterCoeficient);

	EMA(meter_slow.Iind, p->Iind*0.001f, SLOW_METER_COEF);
	EMA(meter_slow.Ihigh, p->Ihigh*0.001f,SLOW_METER_COEF);
	EMA(meter_slow.Ilow, p->Ilow*0.001f,SLOW_METER_COEF);
	EMA(meter_slow.Vlow, p->Vlow*0.001f,SLOW_METER_COEF);
	EMA(meter_slow.Vhigh, p->Vhigh*0.001f,SLOW_METER_COEF);
	EMA(meter_slow.Eff, p->eff,SLOW_METER_COEF);
	EMA(meter_slow.TemperatureAmbient, p->TemperatureAmbient,SLOW_METER_COEF);
	EMA(meter_slow.TemperatureHeatsink, p->TemperatureHeatsink,SLOW_METER_COEF);
	EMA(meter_slow.PowerHigh, p->PowerHigh, SLOW_METER_COEF);
	EMA(meter_slow.PowerLow, p->PowerLow, SLOW_METER_COEF);


	if(scope.running){
		if((scope.dividerindex++ >= scope.divider) || (scope.divider <= 1)){
			scope.dividerindex = 0;

			for(int ch = 0; ch < CONVERTER_SCOPE_CHANNELS; ch++){
				float tempval = 0.0f;

				switch (scope.channel[ch].source){
				case SourceIndex_Iind:
					tempval = p->Iind / 1.0e3f;
					break;
				case SourceIndex_Ihigh:
					tempval = p->Ihigh / 1.0e3f;
					break;
				case SourceIndex_Vlow:
					tempval = p->Vlow / 1.0e3f;
					break;
				case SourceIndex_Vhigh:
					tempval = p->Vhigh / 1.0e3f;
					break;
				case SourceIndex_Ilow:
					tempval = p->Ilow / 1.0e3f;
					break;
				case SourceIndex_PowerHigh:
					tempval = p->PowerHigh;
					break;
				case SourceIndex_PowerLow:
					tempval = p->PowerLow;
					break;

				case SourceIndex_Eff:
					tempval = p->eff;
					break;
				case SourceIndex_Iind_Filtered:
					tempval = meter.Iind;
					break;
				case SourceIndex_Ihigh_Filtered:
					tempval = meter.Ihigh;
					break;
				case SourceIndex_Vlow_Filtered:
					tempval = meter.Vlow;
					break;
				case SourceIndex_Vhigh_Filtered:
					tempval = meter.Vhigh;
					break;
				case SourceIndex_Ilow_Filtered:
					tempval = meter.Ilow;
					break;
				case SourceIndex_PowerHigh_Filtered:
					tempval = meter.PowerHigh;
					break;
				case SourceIndex_PowerLow_Filtered:
					tempval = meter.PowerLow;
					break;

				case SourceIndex_Eff_Filtered:
					tempval = meter.Eff;
					break;
				}

				scope.channel[ch].samples[scope.writeindex] = tempval;
			}
			scope.writeindex++;

			if (scope.writeindex >= scope.samples){
				scope.writeindex = 0;
			}

			if(scope.trigered){
				int stopindex = scope.triggerindex - scope.pretrigger;
				if (stopindex < 0){
					stopindex = scope.samples + stopindex;
				}

				if(scope.writeindex == stopindex){
					scope.running = false;
				}
			}
		}
	}

	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BSRR = LED3_Pin;

#ifdef SIMULATION
	simstate_c = modTestingSimstep(Ts, &phase);




#endif
	//LED3_GPIO_Port->BRR = LED3_Pin;
	//LED3_GPIO_Port->BSRR = LED3_Pin;

}

bool control_check_parameters(ConverterSettings_t* s, CalibrationData_t * c){
	bool error = false;

	//Check calibration CRC
	uint16_t crc = libCRCCalcCRC16((unsigned char *)c, sizeof(CalibrationData_t) - sizeof(uint16_t) - 2);
	if (crc != c->calcrc){
		error = true;
	}

	//Check Firmware hwname against stored HW name
	#ifndef SIMULATION // In simulation mode, this does not matter.
	if (strcmp(HW_NAME, c->HardwareName) != 0){
		error = true;
	}
	#endif

	//Check firmware verison with calibration compatibility


	//Check UUID with calibraiton id, t check if calibration is done for this hardware

	//float fres = sqrtf(1/(2*pi*pi*HW_L*c->Clow))*1.0e-3f;
	//if(c->ControllerFrequency > 45.0f)error = true;
	//if(c->ControllerFrequency < (8*fres))error = true;
	//if(c->SwitchingFrequency  < (5*fres))error = true;
	//if(c->calibrated == false) error = true;
	//if(c->Q > 2.0f)error = true;

	//Check deadtime values
	//if(c->DeadTimeRising  > 188.0)error = true;
	//if(c->DeadTimeRising  < 5)error = true;
	//if(c->DeadtimeFalling > 188.0)error = true;
	//if(c->DeadtimeFalling < 5)error = true;

	return error;
}

void control_disable(void){
	phase.HSEport->BRR = phase.HSEpin;
	phase.PENport->BRR = phase.PENpin;
	phase.pwm_enabled =  false;
	phase.dutycycle = 0;
	phase.enabled = false;
	pwm_disable();
}


inline void modConverterPWMOutputDisable(){
#ifndef SIMULATION
	phase.PENport->BRR = phase.PENpin;
	phase.HSEport->BRR = phase.HSEpin;
	DREN_GPIO_Port->BRR = DREN_Pin;
#endif
	phase.enabled = false;
	phase.pwm_enabled =  false;
}

inline void modConverterPWMOutputEnable() {
	//Make sure MPPT is not enabled when a fault is active.
	if(phase.fault == Converter_OK){
		if(settings.outputEnable){
	#ifndef SIMULATION
			//phase.PENport->BSRR = phase.PENpin;
			DREN_GPIO_Port->BSRR = DREN_Pin;
	#endif
			phase.enabled = true;
			//phase.pwm_enabled =  true;
		}else{
			modConverterPWMOutputDisable();
		}
	}
}

float convertTemperature(uint32_t data){
	float vt = ((float)data) * HW_ADCREF * 0.001f / ((float)(1<<16));
	float i = ((3.24f-vt) / cal.Temperature_R);
	float r = vt / i;


	float inverseKelvin = (1.0f / (cal.Temperature_Ref+273.16f)) + (logf(r / cal.Temperature_R))/cal.Temperature_B;

	return (1.0f / inverseKelvin) - 273.16f;
	return vt;
}


float noise(float amp){
	return (((float)rand()/(float)__RAND_MAX)-0.5f)*amp;
	//return 0.0f;
}


void control_convert_vls(uint32_t raw){
	float V = 0;

	#ifdef SIMULATION
	V = 1.0e3f*(simstate_c.Vlow + noise(0.05f));
	#else
	V = ((((float)raw) * cal.InputVoltageGain * HW_ADCREF) / (float)0x1000) + cal.InputVoltageOffset;
	#endif

	if(fabsf(V) > HW_LIMIT_LS_VOLTAGE_HARD){
		phase.fault = Converter_InputOverVolt;
	}
	phase.Vlowm1 = phase.Vlow;
	EMA(phase.Vlow, V , VOLTAGE_IN_FORGETING_FACTOR);

	//modConverterIpvObserver(phase);
}

void control_convert_vhs(uint32_t raw){
	float V = 0.0;
	#ifdef SIMULATION
	V = 1.0e3f*(simstate_c.Vhigh + noise(0.05f));
	#else
	V = ((((float)raw) * cal.OutputVoltageGain * HW_ADCREF) / (float)0x1000) + cal.OutputVoltageOffset;
	#endif

	if(fabsf(V) > HW_LIMIT_HS_VOLTAGE_HARD){
		phase.fault = Converter_OutputOverVolt;
	}
	EMA(phase.Vhigh, V , VOLTAGE_OUT_FORGETING_FACTOR);

}

void control_convert_iind(uint32_t raw){
	float I = 0.0;
	#ifdef SIMULATION
	I = 1.0e3f*(simstate_c.Iind + noise(0.05f));
	#else
	I = ((((float)raw-(float)0x800) * cal.InputCurrentGain *2.0f* HW_ADCREF) / (float)0x1000) + cal.InputCurrentOffset;
	#endif

	if(I > HW_LIMIT_LS_CURRENT_HARD){
		phase.fault = Converter_InputOverCurrent;
	}else if(I < -HW_LIMIT_LS_CURRENT_HARD){
		phase.fault = Converter_InputUnderCurrent;
	}

	phase.Iind = I;

	//EMA(phase.Iind,I, CURRENT_IN_FORGETING_FACTOR);
}

void control_convert_ihs(uint32_t raw){
	float I = 0.0f;
	#ifdef SIMULATION
	I = 1.0e3f*(simstate_c.Ihigh + noise(0.05f));
	#else
	I = ( ((float)raw-(float)0x800) * cal.OutputCurrentGain *2.0f* (HW_ADCREF / (float)0x1000)) + cal.OutputCurrentOffset;
	#endif

	if (settings.DisableHighSideCurrentFault == false){
		if(I > HW_LIMIT_HS_CURRENT_HARD){
			phase.fault = Converter_OutputOverCurrent;
		}
		if(I < -HW_LIMIT_HS_CURRENT_HARD){
			phase.fault = Converter_OutputOverCurrent;
		}
	}
	EMA(phase.Ihigh,I, CURRENT_IN_FORGETING_FACTOR);
}

void convertAdc5(uint32_t* data){
	EMA(phase.TemperatureMCU, __LL_ADC_CALC_TEMPERATURE((uint32_t)HW_ADCREF,data[0],LL_ADC_RESOLUTION_12B), TEMP_FORGETING_FACTOR);
	EMA(phase.TemperatureHeatsink, convertTemperature(data[2]), TEMP_FORGETING_FACTOR);
	EMA(phase.TemperatureAmbient, convertTemperature(data[1]), TEMP_FORGETING_FACTOR);
}

void HAL_ADC_ConvHalfCpltCallback(ADC_HandleTypeDef *hadc){
	if(hadc->Instance == ADC5){
		convertAdc5(&adc5data[3]);
	}
}
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc){
	if(hadc->Instance == ADC5){
		convertAdc5(&adc5data[0]);
	}
}


PhaseMode_t modConverterGetMode(void){
	return phase.mode;
}

PhaseFault_t modConverterGetFault(void){
	return phase.fault;
}

void control_set_setpoint(float v){
	if (!isnan(v)){
#if defined HW_TOPOLOGY_BOOST
		if(v > settings.LowSideVoltageLimitSoft)v = settings.LowSideVoltageLimitSoft;

#elif defined HW_TOPOLOGY_BUCK
		if(v > settings.HighSideVoltageLimitSoft)v = settings.HighSideVoltageLimitSoft;
#endif
		if(v < HW_LIMIT_LS_VOLTAGE_SOFT_MIN)v = HW_LIMIT_LS_VOLTAGE_SOFT_MIN;
		phase.Vsp = v;
	}
}

float control_get_setpoint(){
	return phase.Vsp;
}

float control_get_regulated_voltage(){
#if defined(HW_TOPOLOGY_BOOST)
	return meter.Vlow;
#elif defined(HW_TOPOLOGY_BUCK)
	return meter.Vhigh;
#endif
}

float control_get_regulated_current(){
#if defined(HW_TOPOLOGY_BOOST)
	return meter.Ilow;
#elif defined(HW_TOPOLOGY_BUCK)
	return -meter.Ihigh;
#endif
}

void control_set_output_current_limit(float c_mA){
	if(c_mA > settings.HighSideCurrentLimitSoft)
		c_mA = settings.HighSideCurrentLimitSoft;
	if(c_mA < HW_LIMIT_HS_CURRENT_SOFT_MIN)
		c_mA = HW_LIMIT_HS_CURRENT_SOFT_MIN;
	phase.Ihighlim = c_mA;
}

void control_set_output_voltage_limit(float v_mV){
	if(v_mV > settings.HighSideVoltageLimitSoft)
		v_mV = settings.HighSideCurrentLimitSoft;
	if(v_mV < HW_LIMIT_HS_VOLTAGE_SOFT_MIN)
		v_mV = HW_LIMIT_HS_VOLTAGE_SOFT_MIN;

	phase.Vhighlim = v_mV;
}

/*
void modConverterSetInputVoltage_dep(float v){

}

void modConverterSetOutputCurrent_dep(float c){
	if(c > settings.HighSideCurrentLimitSoft)c = settings.HighSideCurrentLimitSoft;
	if(c < 1000.0f)c = 1000.0f;
	phase.Ihighlim;
}

void modConverterSetInputCurrent_dep(float c){
	phase.Iindlim = c;
}
*/


void control_set_vhs_limit(float v){
	//TODO make this dependent work for a buck converter
	if(v > settings.HighSideVoltageLimitSoft)v = settings.HighSideVoltageLimitSoft;
	if(v < 5000.0f)v = 5000.0f;
	phase.Vhighlim = v;
}

void scope_trigger_fault(){
	if(scope.faulttrigger == true){
		if(scope.running && (scope.trigered == false)){
			if(scope.writeindex > scope.pretrigger){
				scope.trigered = true;
				scope.triggerindex = scope.writeindex;
			}
		}
	}
}

void scope_trigger(){
	if(scope.faulttrigger == false){
		if(scope.running && (scope.trigered == false)){
			if(scope.writeindex > scope.pretrigger){
				scope.trigered = true;
				scope.triggerindex = scope.writeindex;
			}
		}
	}
}

void scope_start(bool OnFault){
	scope.faulttrigger = OnFault;
	scope.running = false;
	int div = 1;
	if (scope.divider > 1){
		div = scope.divider;
	}
	scope.samplerate = 1/(Ts*div);
	scope.writeindex = 0;
	scope.trigered = false;
	scope.triggerindex = 0;

	scope.running = true;


}