/*
 * AMS_HighLevel.c
 *
 *  Created on: 20.07.2022
 *      Author: max
 */


#include "AMS_HighLevel.h"

Cell_Module module;
uint32_t balancedCells = 0;
uint8_t BalancingActive = 0;
uint8_t stateofcharge = 100;
int64_t currentintegrator = 0;
uint32_t lastticks = 0;
uint32_t currenttick = 0;
uint8_t eepromconfigured = 0;

uint8_t internalbalancingalgo = 1;
uint16_t startbalancingthreshold = 41000;
uint16_t stopbalancingthreshold = 30000;
uint16_t balancingvoltagedelta = 10;


uint16_t amsuv = 0;
uint16_t amsov = 0;

uint8_t amserrorcode = 0;
uint8_t amswarningcode = 0;

uint8_t numberofCells = 17;
uint8_t numberofAux = 0;


amsState currentAMSState = AMSDEACTIVE;
amsState lastAMSState = AMSDEACTIVE;

void AMS_Init(SPI_HandleTypeDef *hspi)
{
	if(eepromconfigured == 1)
	{
		/*amsov = eepromcellovervoltage>>4;
		amsuv = (eepromcellundervoltage-1)>>4;
		numberofCells = eepromnumofcells;
		numberofAux = eepromnumofaux;
		initAMS(hspi, eepromnumofcells, eepromnumofaux);*/
		amsConfigOverVoltage(amsov);
		amsConfigUnderVoltage(amsuv);
	}
	else
	{
		initAMS(hspi, numberofCells, numberofAux);
		amsov = DEFAULT_OV;
		amsuv = DEFAULT_UV;
	}


	currentAMSState = AMSIDLE;


}

void AMS_Loop()
{

	//On Transition Functions called ones if the State Changed

	if(currentAMSState != lastAMSState)
	{
		switch(currentAMSState)
		{
		case AMSIDLE:
		break;
		case AMSDEACTIVE:
		break;
		case AMSCHARGING:
		break;
		case AMSIDLEBALANCING:
		break;
		case AMSDISCHARGING:
		break;
		case AMSWARNING:
			writeWarningLog(0x01);
		break;
		case AMSERROR:
			writeErrorLog(amserrorcode);
		break;
		}
		lastAMSState = currentAMSState;
	}

	//Main Loops for different AMS States

	switch(currentAMSState)
	{
	case AMSIDLE:
		AMS_Idle_Loop();
	break;
	case AMSDEACTIVE:
	break;
	case AMSCHARGING:
	break;
	case AMSIDLEBALANCING:
		AMS_Idle_Loop();
	break;
	case AMSDISCHARGING:
	break;
	case AMSWARNING:
		AMS_Warning_Loop();
	break;
	case AMSERROR:
	break;
	}
}

uint8_t AMS_Idle_Loop()
{
	  amsWakeUp();
	  amsConfigOverVoltage(amsov);
	  amsConfigUnderVoltage(amsuv);
	  amsConfigAuxMeasurement(0xFFFF);
	  amsClearAux();
	  amsCellMeasurement(&module);
	  amsInternalStatusMeasurement(&module);
	  amsAuxMeasurement(&module);
	  amsCheckUnderOverVoltage(&module);
	  integrateCurrent();

	  static uint32_t channelstobalance = 1;

	  channelstobalance = 0x1FFFF;
	/*  if(channelstobalance & 0x20000){
		 channelstobalance = 1;
	  }*/

	  amsConfigBalancing(channelstobalance);
	  amsStartBalancing(100);

	  if((module.overVoltage | module.underVoltage))
	  {
		  //amsSendWarning();
		//  currentAMSState = AMSWARNING;
	  }

	 // AMS_Balancing_Loop();

	 /* if(BalancingActive)
	  {
		  amsStartBalancing(100);
	  }
	  else
	  {
		  amsStopBalancing();
	  }*/
	  //amsConfigBalancing(balancedCells);
	  //volatile amscheck = amscheckOpenCellWire(&module);
	  return 0;
}

uint8_t AMS_Warning_Loop()
{

	  amsWakeUp();
	  amsConfigOverVoltage(amsov);
	  amsConfigUnderVoltage(amsuv);
	  amsConfigAuxMeasurement(0xFFFF);
	  amsClearAux();
	  amsCellMeasurement(&module);
	  amsInternalStatusMeasurement(&module);
	  amsAuxMeasurement(&module);
	  amsCheckUnderOverVoltage(&module);

	  if(!(module.overVoltage | module.underVoltage))
	  {
		  currentAMSState = AMSIDLE;
		  amsClearWarning();
	  }
	  amsStopBalancing();

	return 0;
}

uint8_t AMS_Error_Loop()
{
	return 0;
}

uint8_t AMS_Charging_Loop()
{
	return 0;
}

uint8_t AMS_Discharging_Loop()
{
	return 0;
}

uint8_t AMS_Balancing_Loop()
{
	uint8_t balancingdone = 1;
	if((eepromconfigured == 1) && (internalbalancingalgo == 1) && (module.internalDieTemp<28000/*Thermal Protection 93°C*/))	//If the EEPROM is configured and the internal Balancing Algorithm should be used
	{
		uint16_t highestcellvoltage = module.cellVoltages[0];
		uint16_t lowestcellvoltage = module.cellVoltages[0];
		uint8_t highestcell = 0;
		uint8_t lowestcell = 0;

		for(uint8_t n = 0; n < numberofCells; n++)
		{
			if(module.cellVoltages[n] > highestcellvoltage)
			{
				highestcellvoltage = module.cellVoltages[n];
				highestcell = n;
			}
			if(module.cellVoltages[n] < lowestcellvoltage)
			{
				lowestcellvoltage = module.cellVoltages[n];
				lowestcell = n;
			}
		}

		if(currentAMSState == AMSCHARGING)	//Balancing is only Active if the BMS is in Charging Mode
		{

			uint32_t channelstobalance = 0;

			if(highestcellvoltage > startbalancingthreshold)
			{
				for(uint8_t n = 0; n < numberofCells; n++)
				{
					if(module.cellVoltages[n] > stopbalancingthreshold)
						{
						uint16_t dv = module.cellVoltages[n]-lowestcellvoltage;
						if(dv > (balancingvoltagedelta*1000))
						{
							balancingdone = 0;
							channelstobalance |= 1<<n;
						}
					}
				}
			}

			amsConfigBalancing(channelstobalance);
			amsStartBalancing(100);
		}

		else if(currentAMSState == AMSIDLEBALANCING)
		{

			uint32_t channelstobalance = 0;

			if(lowestcellvoltage < stopbalancingthreshold)	//If under Voltage of one Cell is reached
			{
				amsStopBalancing();
				balancingdone = 1;
			}
			else											// otherwise continue with regular Balancing Algorithm
			{
					for(uint8_t n = 0; n < numberofCells; n++)
					{
						uint16_t dv = module.cellVoltages[n] - lowestcellvoltage;
						if(dv > balancingvoltagedelta)
						{
							balancingdone = 0;
							channelstobalance |= 1<<n;
						}
					}

					amsConfigBalancing(channelstobalance);
					amsStartBalancing(100);
			}

		}
	}
	else
	{
		amsStopBalancing();
		balancingdone = 1;
	}
	return balancingdone;
}

uint8_t writeWarningLog(uint8_t warningCode)
{
	//eepromWriteWarningLog(warningCode);
	return 0;
}
uint8_t writeErrorLog(uint8_t errorCode)
{
	//eepromWriteErrorLog(errorCode);
	return 0;
}

uint8_t integrateCurrent()
{
	lastticks = currenttick;
	currenttick = HAL_GetTick();
	if(currenttick < lastticks)
	{
		currentintegrator += (module.auxVoltages[0] - module.auxVoltages[2])*(currenttick-lastticks);
	}
	return 0;
}
