#include "smbus.h"
#include "emac_support.h"

/** These global variables below used in POLLING mode - Slave device ----------------*/
uint8_t Master_Buf[BUFFER_SIZE];
BATTERY_DATA batteryData[55];
BATTERY_STATE bat_state;
uint32_t totalSMBUSErrors = 0;
uint16_t batteryTemp;
uint32_t humidity_temp;
uint16_t humidity;
bool bat_data_ready = false;
int bat_count = 0;
static int mode_poll;   /* Poll/Interrupt mode flag */
bool chgwd_enabled;
static uint32_t chgwd_last_reset;
uint32_t second_tick;

/*********************************************************************//**
 * @brief		Initialize buffer
 * @param[in]	type:
 * 				- 0: Initialize Master_Buf with increment value from 0
 * 					Fill all member in Slave_Buf with 0
 * 				- 1: Initialize Slave_Buf with increment value from 0
 * 					Fill all member in Master_Buf with 0
 * @return 		None
 **********************************************************************/
void Buffer_Init(uint8_t type)
{
	uint8_t i;

	if (type)
	{
		for (i = 0; i < BUFFER_SIZE; i++) {
			Master_Buf[i] = i;
		}
	}
	else
	{
		for (i = 0; i < BUFFER_SIZE; i++) {
			Master_Buf[i] = 0;
		}
	}
}

/* Set I2C mode to polling/interrupt */
void i2c_set_mode(I2C_ID_T id, int polling)
{
	if(!polling) {
		mode_poll &= ~(1 << id);
		Chip_I2C_SetMasterEventHandler(id, Chip_I2C_EventHandler);
		NVIC_EnableIRQ(id == I2C0 ? I2C0_IRQn : I2C1_IRQn);
	} else {
		DEBUGOUT("setting I2C%d to polling mode\n", (int)id);
		mode_poll |= 1 << id;
		NVIC_DisableIRQ(id == I2C0 ? I2C0_IRQn : I2C1_IRQn);
		Chip_I2C_SetMasterEventHandler(id, Chip_I2C_EventHandlerPolling);
	}
}

static void i2c_probe_slaves(I2C_ID_T i2c)
{
	int i;
	uint8_t ch[2];

	DEBUGOUT("\r\nProbing available I2C devices...\r\n");
	DEBUGOUT("\r\n     00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D 0E 0F");
	DEBUGOUT("\r\n====================================================");
	for (i = 0; i <= 0x7F; i++) {
		if (!(i & 0x0F)) DEBUGOUT("\r\n%02X  ", i >> 4);
		if (i <= 7 || i > 0x78) {
			DEBUGOUT("   ");
			continue;
		}
		/* Address 0x48 points to LM75AIM device which needs 2 bytes be read */
		if(Chip_I2C_MasterRead(i2c, i, ch, 1 + (i == 0x48)) > 0)
			DEBUGOUT(" %02X", i);
		else
			DEBUGOUT(" --");
	}
	DEBUGOUT("\r\n");
}

/* State machine handler for I2C0 and I2C1 */
static void i2c_state_handling(I2C_ID_T id)
{
	if (Chip_I2C_IsMasterActive(id)) {
		Chip_I2C_MasterStateHandler(id);
	} else {
		Chip_I2C_SlaveStateHandler(id);
	}
}

/**
 * @brief	I2C0 Interrupt handler
 * @return	None
 */
void I2C0_IRQHandler(void)
{
	i2c_state_handling(I2C0);
}


void RIT_IRQHandler(void)
{
	/* Clear interrupt */
	Chip_RIT_ClearInt(LPC_RITIMER);
	second_tick++;
}



void smbusReset()
{
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, 8);
	task_sleep(50);
	Chip_GPIO_SetPinOutHigh(LPC_GPIO, 0, 8);
	task_sleep(50);
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, 8);
	task_sleep(50);
}


static int setSmbusMux(int mux, uint8_t channel)
{
	int status;
	
	  Master_Buf[0] = channel;
		status = Chip_I2C_MasterSend(I2C0, (uint8_t)((0xE0+ (mux<<1))>>1),
		  Master_Buf, 1);
		vTaskDelay(5);
	  return status;
}

void allPacks(uint8_t state)
{
uint8_t battery, mux;
int batNum;
	
	//set all batteries to "on" state, so that they will respond to charge current request with non-zero
	for (mux = 0,batNum=0; mux <= 7; mux++) {
		for (battery = 1; battery <=8; battery++) {
			if (mux == 5 && battery > 1) continue;
      if (mux == 6 && battery >6) continue;
			
			//select the battery
			if (!setSmbusMux(mux, 1<<(battery-1))) {
				DEBUGOUT("smbus error in mux setup\r\n");
				totalSMBUSErrors++;
			}
			
			Master_Buf[0] = 0x3f;
			Master_Buf[1] = 0x00;
			Master_Buf[2] = state;
			if (Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, Master_Buf, 3) != 3) {
				totalSMBUSErrors++;
			}
			batteryData[batNum++].state = state;
		}
    
		msleep(2);

		//clear the mux channels
		if (!setSmbusMux(mux, 0)) {
			DEBUGOUT("error clearing mux\r\n");
			totalSMBUSErrors++;
		}
	}
}

uint32_t
get_smbus_errors()
{
	return totalSMBUSErrors;
}

uint16_t
get_battery_temp()
{
	return batteryTemp;
}

uint32_t
get_humidity_temp()
{
	return humidity_temp;
}

uint16_t
get_humidity()
{
	return humidity;
}

uint8_t
get_bat_count()
{
	if (!bat_data_ready) {
		return (0);
	}
	return bat_count;
}
	

void
chgwd_control(bool enabled)
{
	chgwd_enabled = enabled;
}


void
chgwd_reset()
{
	chgwd_last_reset = second_tick;
}


void
chgwd_check(void)
{
	if (!chgwd_enabled || bat_state != BATT_CHG) return;

	if (second_tick - chgwd_last_reset > CHGWD_TIMEOUT) {
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);
			allPacks(0);
			bat_state = BATT_OFF;
	}
}


void
update_battery_status(void)
{
	uint16_t batteryMode;
	uint16_t batteryStatus;
	int16_t maxCurrent = INT16_MIN;
	int16_t minCurrent = INT16_MAX;
	uint16_t maxVoltage = 0;
	uint16_t minVoltage = UINT16_MAX;
	uint16_t my_serialNo;
	uint16_t requestedChargeCurrent;
	uint8_t battery, mux;
	uint8_t totalPacksOn;
	char out_string[80];
	int batNum;
	
	maxCurrent = INT16_MIN;
	maxVoltage = 0;
	minCurrent = INT16_MAX;
	minVoltage = UINT16_MAX;
	
	for (mux = 0,batNum=0,totalPacksOn = 0; mux <= 7; mux++) {
		for (battery = 1; battery <=8; battery++) {
			batteryData[batNum].mux = mux;
			batteryData[batNum].channel = battery;
		
			//select the battery
			if (!setSmbusMux(mux, 1<<(battery-1))) {
//			DEBUGOUT("smbus error in mux setup\r\n");
				totalSMBUSErrors++;
			}

			if (mux == 5 && battery == 2) {
				if (Chip_I2C_MasterCmdRead(I2C0, (0x90>>1), 0x00, (uint8_t *)&batteryTemp, 2) != 2) {
//					sprintf(out_string, "error with T sensor\r\n");
//					DEBUGSTR(out_string);
					totalSMBUSErrors++;
				}
				batteryTemp = ((batteryTemp&0x00ff)<<8) | ((batteryTemp&0xff00)>>8);
				batteryTemp >>= 4;
				continue;
			}

			if (mux == 5 && battery == 8) {
				Chip_I2C_MasterSend(I2C0, (0x4e>>1), (uint8_t *)&humidity_temp, 1);
				task_sleep(50);
				if (Chip_I2C_MasterRead(I2C0, (0x4e>>1), (uint8_t *)&humidity_temp, 4) != 4) {
//					sprintf(out_string, "error reading humidity sensor\r\n");
//					DEBUGSTR(out_string);
					totalSMBUSErrors++;
				}
				humidity = humidity_temp & 0x0000ffff;
				humidity = ((humidity&0x00ff)<<8) | ((humidity&0xff00)>>8);
				humidity_temp = humidity_temp>>16;
				humidity_temp = ((humidity_temp&0x00ff)<<8) | ((humidity_temp&0xff00)>>8);
				humidity_temp >>= 2;
				continue;
			}

			if (mux == 5 && battery > 2) continue;
			if (mux == 6 && battery >6) continue;
			
			if (Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x03, (uint8_t *)&batteryMode, 2) != 2) {			
//				sprintf(out_string, "error\r\n");
//				DEBUGSTR(out_string);
				totalSMBUSErrors++;
			}

			batteryMode |= (1<<13)|(1<<14);
			Master_Buf[0] = 0x03; //request BatteryStatus()
			Master_Buf[1] = batteryMode & 0xff;
			Master_Buf[2] = (batteryMode & 0xff00) >> 8;
			(void)Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, Master_Buf, 3);
				
			(void)Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x16, (uint8_t *)&batteryData[batNum].status, 2);			
			batteryStatus = batteryData[batNum].status;
//			sprintf(out_string,"0x%x\r\n", batteryStatus);
//			DEBUGSTR(out_string);
			
			(void)Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x09, (uint8_t *)&batteryData[batNum].stackVolt, 2);
			(void)Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x0f, (uint8_t *)&batteryData[batNum].capacity_mAh, 2);
			(void)Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x17, (uint8_t *)&batteryData[batNum].cycleCnt, 2);

			//request Serial Number
			if (Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x1c, (uint8_t *)&batteryData[batNum].serialNo, 2) != 2) {
//				sprintf(out_string, "error requesting serial number\r\n");
//				DEBUGSTR(out_string);
				totalSMBUSErrors++;
			}
				
			my_serialNo = batteryData[batNum].serialNo;
//			sprintf(out_string,"serial no: %d\r\n", my_serialNo);
//			DEBUGSTR(out_string);

			//request Current
			if (Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x0a, (uint8_t *)&batteryData[batNum].current, 2) != 2) {
//				sprintf(out_string, "error requesting current\r\n");
//				DEBUGSTR(out_string);
				totalSMBUSErrors++;
			}
		  
			sprintf(out_string, "current:%d\r\n", batteryData[batNum].current);
//			DEBUGSTR(out_string);				
			
			if (batteryData[batNum].current > maxCurrent) maxCurrent = batteryData[batNum].current;
			if (batteryData[batNum].stackVolt > maxVoltage) maxVoltage = batteryData[batNum].stackVolt;
			if (batteryData[batNum].current <  minCurrent) minCurrent = batteryData[batNum].current;
			if (batteryData[batNum].stackVolt < minVoltage) minVoltage = batteryData[batNum].stackVolt;

			//request requestedChargingCurrent
			if (Chip_I2C_MasterCmdRead(I2C0, BATTERY_ADDR, 0x14, (uint8_t *)&requestedChargeCurrent, 2) != 2) {
//				sprintf(out_string, "error requesting charge current\r\n");
//				DEBUGSTR(out_string);
				totalSMBUSErrors++;
			}
			
			//check individual pack status, turn off if needed
			if ((batteryData[batNum].status & (0x0800/*|0x4000*/)) || (batteryData[batNum].state == 0)) {
				Master_Buf[0] = 0x3f; Master_Buf[1] = 0x00; Master_Buf[2] = 0x00;
				if (Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, Master_Buf, 3) != 3) {
//					sprintf(out_string, "error setting battery state\r\n");
//					DEBUGSTR(out_string);
					totalSMBUSErrors++;
				}
				batteryData[batNum].state = 0;
			} else {
				totalPacksOn++;
			}
			batNum++;
		}

		//clear the mux channels
		if (!setSmbusMux(mux, 0x00)) {
//			DEBUGOUT("error clearing mux\r\n");
			totalSMBUSErrors++;
		}
	}

	if (totalPacksOn <= 25 && bat_state == BATT_DISCH) {
		printf("turning off pack because on count is %d\n", totalPacksOn);
		Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
		Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);
		allPacks(0);
		bat_state = BATT_OFF;
	}

//		sprintf(out_string, "max current %d\r\n", maxCurrent);
//		DEBUGSTR(out_string);
//		sprintf(out_string, "max voltage %d\r\n", maxVoltage);
//		DEBUGSTR(out_string);
//		sprintf(out_string, "min current %d\r\n", minCurrent);
//		DEBUGSTR(out_string);
//		sprintf(out_string, "min voltage %d\r\n", minVoltage);
//		DEBUGSTR(out_string);
//  	sprintf(out_string, "smbus errors %d\r\n", totalSMBUSErrors);
//		DEBUGSTR(out_string);	
		
	bat_count = batNum;
	bat_data_ready = true;
}


void
init_batteries(void)
{
	uint8_t mux;

	smbusReset();

	/* Initialize buffer */
	Buffer_Init(1);
	memset(batteryData, 0, sizeof(BATTERY_DATA)*55);

	/* reset the mux's */
	for (mux = 0; mux <= 7; mux++) {
		(void)setSmbusMux(mux, 0);
	}
	
	
	 /* Initialize RITimer */
	Chip_RIT_Init(LPC_RITIMER);
	/* Configure RIT for a 1s interrupt tick rate */
	Chip_RIT_SetTimerInterval(LPC_RITIMER, 1000);
	NVIC_EnableIRQ(RITIMER_IRQn);
	
	/* enable charge watchdog */
	chgwd_control(true);
	chgwd_reset();
	second_tick = 0;
	
}


void
smbus_setup(void)
{

	/*initialize on-chip smbus hardware*/	

	/* set up digital outputs*/
	Chip_GPIO_SetPinDIROutput(LPC_GPIO, 0, 4); /* GPIO - 12V off */
	Chip_GPIO_SetPinDIROutput(LPC_GPIO, 0, 5); /* GPIO - CHG_ENABLE */
	Chip_GPIO_SetPinDIROutput(LPC_GPIO, 0, 6); /* GPIO - DISCH_ENABLE */
	Chip_GPIO_SetPinDIROutput(LPC_GPIO, 0, 7); /* GPIO - AUX_DISCHG_EN */
	Chip_GPIO_SetPinDIROutput(LPC_GPIO, 0, 8); /* GPIO - Battery Reset */
	
	/* LED0 is used for the link status, on = PHY cable detected */
	/* Initial LED state is off to show an unconnected cable state */
	Board_LED_Set(0, false);

	/*initialize on-chip smbus hardware*/	

	//to drive the high side switches, use PINSEL_PINMODE_NORMAL, PINSEL_PINMODE_PULLUP. SetValue turns switch on, ClearValue turns switch off.
	//toggle discharge enable line to off
	
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, INSTR_PWR_ENABLE_PIN);
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, AUX_ENABLE_PIN);
	bat_state = BATT_OFF;
	
	// Initialize Slave I2C peripheral
	Board_I2C_Init(I2C0);
	Chip_I2C_Init(I2C0);
	Chip_I2C_SetClockRate(I2C0 , 100000);
	i2c_set_mode(I2C0, I2C_INTERRUPT);
}