/*  Standard C Included Files */

/*
 *
 * Author: Priya
 */
#include <stdio.h>
#include <string.h>
#include "pin_mux.h"
#include "clock_config.h"
#include "board.h"
#include "fsl_debug_console.h"
#include "fsl_lpi2c.h"
#include "battery_monitor.h"
#include "stdbool.h"



uint8_t deviceAddress = 0x00U;
uint8_t rx_data;

uint16_t HibCFG;

lpi2c_master_config_t masterConfig;




void init_bat_mon_i2c()
{
	/*
	 * masterConfig.debugEnable = false;
	 * masterConfig.ignoreAck = false;
	 * masterConfig.pinConfig = kLPI2C_2PinOpenDrain;
	 * masterConfig.baudRate_Hz = 100000U;
	 * masterConfig.busIdleTimeout_ns = 0;
	 * masterConfig.pinLowTimeout_ns = 0;
	 * masterConfig.sdaGlitchFilterWidth_ns = 0;
	 * masterConfig.sclGlitchFilterWidth_ns = 0;
	 */
	LPI2C_MasterGetDefaultConfig(&masterConfig);

	/* Change the default baudrate configuration */
	masterConfig.baudRate_Hz = LPI2C_BAUDRATE;


	// Added by HKS
	//	masterConfig.sdaGlitchFilterWidth_ns = 10U;
	//	masterConfig.sclGlitchFilterWidth_ns = 10U;
	//	masterConfig.ignoreAck = true;

	/* Initialize the LPI2C master peripheral */
	LPI2C_MasterInit(EXAMPLE_I2C_MASTER, &masterConfig, LPI2C_MASTER_CLOCK_FREQUENCY);

}

void battery_initialize_config()
{

}


status_t write_reg_bat_mon(uint8_t reg_address, uint16_t reg_data)
{
	size_t txCount        = 0xFFU;
	status_t reVal        = kStatus_Fail;




	/* Send master blocking data to slave */
	if (kStatus_Success == LPI2C_MasterStart(EXAMPLE_I2C_MASTER, LPI2C_MASTER_SLAVE_ADDR_7BIT, kLPI2C_Write))
	{
		/* Check master tx FIFO empty or not */
		LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
		while (txCount)
		{
			LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
		}
		/* Check communicate with slave successful or not */
		if (LPI2C_MasterGetStatusFlags(EXAMPLE_I2C_MASTER) & kLPI2C_MasterNackDetectFlag)
		{
			return kStatus_LPI2C_Nak;
		}

		/* subAddress = 0x01, data = g_master_txBuff - write to slave.
	          start + slaveaddress(w) + subAddress + length of data buffer + data buffer + stop*/
		reVal = LPI2C_MasterSend(EXAMPLE_I2C_MASTER, &reg_address, LPI2C_REG_ADDRESS_LENGTH);

		/*
			if (reVal != kStatus_Success)
			{
				if (reVal == kStatus_LPI2C_Nak)
				{
					LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
				}
				return -1;
			}
		 */


		LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
		while (txCount)
		{
			LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
		}



		reVal = LPI2C_MasterSend(EXAMPLE_I2C_MASTER, &reg_data, LPI2C_DATA_LENGTH);
		if (reVal != kStatus_Success)
		{
			if (reVal == kStatus_LPI2C_Nak)
			{
				LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
			}
			return -1;
		}

		reVal = LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
		if (reVal != kStatus_Success)
		{
			return -1;
		}
	}

	/* Wait until the slave is ready for transmit, wait time depend on user's case.
	       Slave devices that need some time to process received byte or are not ready yet to
	       send the next byte, can pull the clock low to signal to the master that it should wait.*/
	for (uint32_t i = 0U; i < WAIT_TIME; i++)
	{
		__NOP();
	}


	return reVal;

}

uint16_t read_reg_bat_conn(uint8_t reg_address)
{
		uint16_t read_bat_conn_status;
		size_t txCount        = 0xFFU;
		status_t reVal        = kStatus_Fail;

		if (kStatus_Success == LPI2C_MasterStart(EXAMPLE_I2C_MASTER, LPI2C_MASTER_SLAVE_ADDR_7BIT, kLPI2C_Write))
		{
			/* Check master tx FIFO empty or not */
			LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
			while (txCount)
			{
				LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
			}
			/* Check communicate with slave successful or not */
			if (LPI2C_MasterGetStatusFlags(EXAMPLE_I2C_MASTER) & kLPI2C_MasterNackDetectFlag)
			{
				PRINTF("No ACK from slave\n");
				//			return kStatus_LPI2C_Nak;
			}

			/* subAddress = 0x01, data = g_master_txBuff - write to slave.
				          start + slaveaddress(w) + subAddress + length of data buffer + data buffer + stop*/
			reVal = LPI2C_MasterSend(EXAMPLE_I2C_MASTER, &reg_address, 1);
			if (reVal != kStatus_Success)
			{
				if (reVal == kStatus_LPI2C_Nak)
				{
					LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
				}
				PRINTF("Failed\n");
				return -1;
			}




			reVal = LPI2C_MasterRepeatedStart(EXAMPLE_I2C_MASTER, LPI2C_MASTER_SLAVE_ADDR_7BIT, kLPI2C_Read);
			if (reVal != kStatus_Success)
			{
				PRINTF("Failed\n");
				return -1;
			}

			reVal = LPI2C_MasterReceive(EXAMPLE_I2C_MASTER, &read_bat_conn_status, LPI2C_DATA_LENGTH);
			for (uint32_t i = 0U; i < WAIT_TIME; i++)
			{
				__NOP();
			}

			//		LPI2C_MasterStop(EXAMPLE_I2C_MASTER);


			if (reVal != kStatus_Success)
			{
				if (reVal == kStatus_LPI2C_Nak)
				{

					LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
				}
				PRINTF("Failed\n");
				return -1;
			}

			reVal = LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
			if (reVal != kStatus_Success)
			{
				PRINTF("Failed\n");
				return -1;
			}

		}


		return read_bat_conn_status;

}
uint16_t read_reg_bat_mon(uint8_t reg_address)
{


	uint16_t read_data;
	size_t txCount        = 0xFFU;
	status_t reVal        = kStatus_Fail;

	if (kStatus_Success == LPI2C_MasterStart(EXAMPLE_I2C_MASTER, LPI2C_MASTER_SLAVE_ADDR_7BIT, kLPI2C_Write))
	{
		/* Check master tx FIFO empty or not */
		LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
		while (txCount)
		{
			LPI2C_MasterGetFifoCounts(EXAMPLE_I2C_MASTER, NULL, &txCount);
		}
		/* Check communicate with slave successful or not */
		if (LPI2C_MasterGetStatusFlags(EXAMPLE_I2C_MASTER) & kLPI2C_MasterNackDetectFlag)
		{
			PRINTF("No ACK from slave\n");
			//			return kStatus_LPI2C_Nak;
		}

		/* subAddress = 0x01, data = g_master_txBuff - write to slave.
			          start + slaveaddress(w) + subAddress + length of data buffer + data buffer + stop*/
		reVal = LPI2C_MasterSend(EXAMPLE_I2C_MASTER, &reg_address, 1);
		if (reVal != kStatus_Success)
		{
			if (reVal == kStatus_LPI2C_Nak)
			{
				LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
			}
			PRINTF("Failed\n");
			return -1;
		}




		reVal = LPI2C_MasterRepeatedStart(EXAMPLE_I2C_MASTER, LPI2C_MASTER_SLAVE_ADDR_7BIT, kLPI2C_Read);
		if (reVal != kStatus_Success)
		{
			PRINTF("Failed\n");
			return -1;
		}

		reVal = LPI2C_MasterReceive(EXAMPLE_I2C_MASTER, &read_data, LPI2C_DATA_LENGTH);
		for (uint32_t i = 0U; i < WAIT_TIME; i++)
		{
			__NOP();
		}

		//		LPI2C_MasterStop(EXAMPLE_I2C_MASTER);


		if (reVal != kStatus_Success)
		{
			if (reVal == kStatus_LPI2C_Nak)
			{

				LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
			}
			PRINTF("Failed\n");
			return -1;
		}

		reVal = LPI2C_MasterStop(EXAMPLE_I2C_MASTER);
		if (reVal != kStatus_Success)
		{
			PRINTF("Failed\n");
			return -1;
		}

	}


	return read_data;


}





void WriteAndVerifyRegister (uint8_t RegisterAddress, uint16_t RegisterValueToWrite)
{
	uint8_t attempt=0;
	uint16_t RegisterValueRead;

	do {
		write_reg_bat_mon(RegisterAddress, RegisterValueToWrite);
		//Wait sometime
		for (uint32_t i = 0U; i < WAIT_TIME_CONFIG; i++)
		{
			__NOP();
		}
		RegisterValueRead = read_reg_bat_mon (RegisterAddress) ;
	}
	while(RegisterValueToWrite != RegisterValueRead && attempt++<3);
}



/*
 * Check for Power on Reset , If non zero, do the init procedure.
 */

uint16_t check_por(void)
{
	return (read_reg_bat_mon(MAX1726X_STATUS_REG) & 0x0002);
}


void init_battery_monitor(bat_mon_init *bat_mon_init_param, bool first_power_on)
{
	uint16_t reg_val;
	reg_val = check_por();

	if((reg_val != 0x0000)||(first_power_on == true))

	{
		// Wait until until FSTAT.DNR bit == 0
		while(read_reg_bat_mon(MAX1726X_FSTAT_REG) & 0x0001)
		{
			for (uint32_t i = 0U; i < WAIT_TIME_CONFIG; i++)
			{
				__NOP();
			}
		}


		// initialize configuration
		HibCFG = read_reg_bat_mon(MAX1726X_HIBCFG_REG) ; //Store original HibCFG value
		write_reg_bat_mon (MAX1726X_VFSOC0_QH0_LOCK_REG , 0x0090) ; // Exit Hibernate Mode step 1
		write_reg_bat_mon (MAX1726X_HIBCFG_REG , 0x0000) ; // Exit Hibernate Mode step 1
		write_reg_bat_mon (MAX1726X_VFSOC0_QH0_LOCK_REG , 0x0000) ; // Exit Hibernate Mode step 1

		write_reg_bat_mon (MAX1726X_DESIGNCAP_REG , bat_mon_init_param->DesignCap) ; // Write DesignCap
		write_reg_bat_mon (MAX1726X_ICHGTERM_REG , bat_mon_init_param->IchgTerm) ; // Write IchgTerm
		write_reg_bat_mon (MAX1726X_VEMPTY_REG , bat_mon_init_param->VEmpty) ; // Write VEmpty

		if(bat_mon_init_param->ChargeVoltage > 4.275)
			write_reg_bat_mon(MAX1726X_MODELCFG_REG , 0x8400) ; // Write ModelCFG
		else
			write_reg_bat_mon(MAX1726X_MODELCFG_REG , 0x8000) ; // Write ModelCFG

		//write_reg_bat_mon (MAX1726X_VCELL_REG , 0x0000) ;
		//Poll ModelCFG.Refresh(highest bit),
		//proceed to Step 3 when ModelCFG.Refresh=0
		while (read_reg_bat_mon(MAX1726X_MODELCFG_REG) & 0x8000)
		{
			for (uint32_t i = 0U; i < WAIT_TIME_CONFIG; i++)
			{
				__NOP();
			}
			//do not continue until ModelCFG.Refresh==0
		}


		write_reg_bat_mon (MAX1726X_HIBCFG_REG , HibCFG) ; // Restore Original HibCFG value
		for (uint32_t i = 0U; i < WAIT_TIME_CONFIG; i++)
		{
			__NOP();
		}

		// Complete initialization operation
		reg_val = check_por();
		WriteAndVerifyRegister(0x00, reg_val & 0xFFFD) ; //Write and Verify Status with POR bit Cleared

	}


}


status_t read_FuelGauge_results(bat_mon_init *bat_mon_init_param, fuelGaugeresultsHandle *fuelGaugeresults)
{
	if(check_por() != 0x0000)
	{
		init_battery_monitor(bat_mon_init_param, false);
		PRINTF("POR not done\n");
		return kStatus_Fail;
	}

	// Read REPCAP and REPSOC registers

	fuelGaugeresults->RepCap = read_reg_bat_mon(MAX1726X_REPCAP_REG) ; //Read RepCap
	fuelGaugeresults->RepSOC = read_reg_bat_mon(MAX1726X_REPSOC_REG) ; //Read RepSOC

	fuelGaugeresults->TTE = read_reg_bat_mon(MAX1726X_TTE_REG) ; //Read TTE

	return kStatus_Success;
}

