/*
 * b_cccv_algo.c
 *
 *  Created on: 16.06.2023
 *      Author: max
 */

#include "b_cccv_algo.h"
#include "charger_control.h"
#include "main.h"

static uint32_t dt;
static uint32_t lasttick;
static float errorintegration = 0;
static float finalvoltage = 4.15;
static float chargevoltage = 430;
static float chargecurrentlimit = CHARGE_CURRENT_LIMIT;

static float resistancelut[1024];
static float voltageresistancelut[1024];

#define CURRENT_R 2.2
#define VOLTAGE_R 18

CCCV_CONTROL_STATE chargerstate = NO_CHARGING;

#ifdef SIMULINKTEST
CCCV_CONTROL_STATE cccvloop(float maxcellvoltage, float voltagesetpoint, float maxcurrent, float*ccurrent)
#else
CCCV_CONTROL_STATE cccvloop(float maxcellvoltage, float voltagesetpoint, float maxcurrent)
#endif

{
#ifdef SIMULINKTEST
	dt = 1000;
#else
	dt = HAL_GetTick() - lasttick;
	lasttick = HAL_GetTick();
#endif





	float chargecurrent = 0;

	//Calculate Voltage Error and I and P Factors
	float voltageerror = voltagesetpoint-maxcellvoltage;

	float errorproportional = P_GAIN * voltageerror;
	errorintegration += I_GAIN*voltageerror * ((float)dt)/1000;


	//Limit I and P Factors to maximum charge current

	if(errorintegration > maxcurrent)
		errorintegration = maxcurrent;

	if(errorproportional > maxcurrent)
		errorproportional = maxcurrent;

	if(errorproportional <= -maxcurrent)
		errorproportional = -maxcurrent;

	if(errorintegration <= -maxcurrent)
		errorintegration = -maxcurrent;

    float errorintegrationout = errorintegration;
	chargecurrent = (errorproportional + errorintegrationout);

	if(chargecurrent > maxcurrent)
		chargecurrent = maxcurrent;

	if(chargecurrent <= 0)
		chargecurrent = 0;

    if(chargecurrent < 0.01)	//If Charge Current ist below minimum threshold, stop charging
		return CHARGING_COMPLETED;
	#ifdef SIMULINKTEST
		*ccurrent = chargecurrent;
	#else
		setchargevoltage(103,finalvoltage+0.05);
		setchargecurrent(chargecurrent);
	#endif


	return CHARGING_IN_PROGRESS;
}

#ifndef SIMULINKTEST

void initChargerAlgo(uint8_t numberofcells, float maximumcellvoltage)
{
	chargerstate = NO_CHARGING;
	chargevoltage = numberofcells * maximumcellvoltage;

	for(uint32_t i = 0; i < 1024; i++)
	{
		resistancelut[i] = 10*CURRENT_R/(CURRENT_R+(20*((float)i)/1024));
	}

	for(uint32_t i = 0; i < 1024; i++)
	{
		voltageresistancelut[i] = (600*VOLTAGE_R)/(VOLTAGE_R+(20*((float)i)/1024));
	}
}

void setchargecurrent(float chargecurrent)
{
	float targetresistance;
	if(chargecurrent != 0)
	{
		for(uint32_t setpoint = 0; setpoint < 1024; setpoint++)
		{
			if(resistancelut[setpoint] <= chargecurrent)
			{
				targetresistance = (20000*((float)setpoint)/1024);
				break;
			}
		}
	}
	else
	{
		targetresistance = 20000;
	}

	float wiperpos = targetresistance/20000 * (1023);
	charger_control_set_current((uint32_t) wiperpos);
}

void setchargevoltage(uint8_t numberofcells,float maximumcellvoltage)
{

	float chargevoltage = numberofcells * maximumcellvoltage;
	float targetresistance;

	if(chargevoltage != 0)
	{
		for(uint32_t setpoint = 0; setpoint < 1024; setpoint++)
			{
				if(voltageresistancelut[setpoint] <= chargevoltage)
				{
					targetresistance = (20000*((float)setpoint)/1024);
					break;
				}
			}
	}
	else
	{
		targetresistance = 20000;
	}

	float wiperpos = targetresistance/20000 * (1023);
	charger_control_set_voltage((uint32_t) wiperpos);
	//@TODO Call Function to set Resistance for Voltage
}

void chargingloop(float maximumcellvoltage)
{
	switch(chargerstate)
	{
	case NO_CHARGING:	//Do Nothing IDLE Loop
		chargerstate = NO_CHARGING;
		charger_control_disable_remote();
		errorintegration = 0;
		break;
	case CHARGING_IN_PROGRESS:	//Run charging algo periodically
		charger_control_enable_remote();
		chargerstate = cccvloop(maximumcellvoltage, finalvoltage, chargecurrentlimit);
		break;
	case CHARGING_COMPLETED:	//Signal Completion of Charging here
		chargerstate = NO_CHARGING;
		charger_control_disable_remote();
		errorintegration = 0;
		break;
	}
}

void startcharging(float endvoltage)
{
	chargerstate = CHARGING_IN_PROGRESS;
	finalvoltage = endvoltage;
}

void stopcharging()
{
	chargerstate = NO_CHARGING;
	charger_control_disable_remote();
}


#endif

#ifdef SIMULINKTEST

float matlabvalidationwrapper(float maxcellvoltage, float voltagesetpoint, float maxcurrent)
{
	float chargecurrent = 0;
	(void*) cccvloop(maxcellvoltage,voltagesetpoint,maxcurrent, &chargecurrent);
	return chargecurrent;

}

#endif