/*
 * Battery.c
 *
 *  Created on: Jun 10, 2022
 *      Author: Keith.Lloyd
 */
#include "arm_math.h"
#include	"utils.h"
#include "battery.h"
#include "adc.h"
#include "main.h"
#include "timer.h"
#include "lcd.h"
#include "display.h"
#include "safety_key.h"
#include "amps.h"
#include "ports.h"
#include "hwFixes.h"
#include "System/system.h"

uint8_t	Bat_Type;

extern uint8_t Task, Error;
extern uint16_t Low_Bat_timer;
extern	uint16_t battery;
//uint16_t Bat_Level[] = {100,200,300,400,500,125,225,325,425,525}; // Battery life levels 5 Alkaline and 5 Lithium.
extern HARDWARE_FIX_t hwf;

extern SYSTEM_DATA_t sys;

bool Compare_With_Limit(float32_t V1 ,float32_t V2,float32_t limit)
{
float32_t tolerance;

bool result;

	result = false;

	tolerance = limit / 100.0;
	tolerance = tolerance + 1.0;
	tolerance = tolerance * V2;

	if(V1 < tolerance)
		result = true;

	tolerance = limit /100.0;
	tolerance = 1.0 - tolerance;
	tolerance = tolerance * V2;

	if(V1 > tolerance)
		result = true;

	return(result);
}



void battery_checkId(void)
{

}

void Check_Bat_Id(void)
{
float32_t Battery;
float32_t Bat_Mid_Point;


	if(sys.adc.V_BID > LITHIUM_MID_POINT)		// BAT_ID = 0V for lithium, mid point for Alkaline
		{
		 if(Compare_With_Limit(sys.adc.V_BID,sys.adc.V_BAT,5.0))
		  {
			 Bat_Type = EXT_DC;
			 sys.maxPowerLimit = POWER_LIMIT_EXT_DC;			// Limit max power to 5W.

		  }
		 else
		  {
			 Bat_Type = ALKALINE;
			 sys.maxPowerLimit = POWER_LIMIT_ALKALINE;			// Limit max power to 5W.

			 Battery = sys.adc.V_BAT;				// Calculate mid-point for alkaline
			 Bat_Mid_Point = Battery / 2;
			 if ((sys.adc.V_BID > (Bat_Mid_Point * 1.1)) || (sys.adc.V_BID < (Bat_Mid_Point * 0.9))  ) // Check if not at mid-point
				 Bat_Type = BAT_ERROR;	// indicate battery insertion error
		  }
		}
	else
	  {
		Bat_Type = LITHIUM;
		sys.maxPowerLimit = POWER_LIMIT_LITHIUM;			// Limit max power to 5W. 1.7 = 10.0 1.7X = 25.0
	  }

	if(Bat_Type == BAT_ERROR)
	{
		LCD_Clear();
		Task=BAT_INSERTION_ERROR;

		while(1)
		{
			//safe_key();
			Display_Update();
			Delay_Ticks(10);
		}

	}

}

void Chk_Bat_Level(void)
{
	if(Bat_Type == LITHIUM)
		{
		if(hwf.vBattCap_021)
			Test_Bat_Level(CF_LITHIUM_CUT_OFF);
		else
			Test_Bat_Level(LITHIUM_CUT_OFF);
		}
	else
		{
		if(hwf.vBattCap_021)
			Test_Bat_Level(CF_ALKALINE_CUT_OFF);
		else
			Test_Bat_Level(ALKALINE_CUT_OFF);
		}
}

void Test_Bat_Level(float32_t Bat_Cut_Off)
{
	if(sys.adc.V_BAT < Bat_Cut_Off && Task != LOW_BATTERY_TASK)
	{
		if(Task != PWR_OFF_TASK)
			{
				All_Amps_Off();
				Disable_BC();			//Disable BC
				Select_Estop(ISOLATED);

				Delay_Ticks(25); 	//wait for battery voltage to rise

//				if(sys.adc.V_BAT <= Bat_Cut_Off)
//				{
					Task = LOW_BATTERY_TASK;
					Low_Bat_timer = DELAY_2S;
//				}
//				else
//				{
	//				Task = FATAL_ERROR_TASK;
	//				Error = 2;
	//			}

			}
	}
}

float32_t Adjust_Battery_For_Load() // adjust to true battery voltage due to battery cable
									//drop and current demand
{									// assumed frequency and efficiency may need table for frequency adjust
	float32_t battery;				//assumes taps are correct
	battery = sys.adc.V_BAT;
	if(Bat_Type == LITHIUM)
	{
		if(sys.adc.I_OUT_SlowFilt < DEMAND1)
			battery = battery + 1.1;
		else if(sys.adc.I_OUT_SlowFilt < DEMAND2)
			battery = battery + 1.2;
		else if(sys.adc.I_OUT_SlowFilt < DEMAND3)
			battery = battery + 1.3;
		else if(sys.adc.I_OUT_SlowFilt < DEMAND4)
			battery = battery + 1.4;
		else if(sys.adc.I_OUT_SlowFilt < DEMAND5)
			battery = battery + 1.5;
		else if(sys.adc.I_OUT_SlowFilt < DEMAND6)
			battery = battery + 1.6;
		else
			battery = battery + 2;

		return(battery);
	}

	else
	{

	}

}