#include <string.h>
#include "smbus.h"
#include "utils.h"

CELL_VOLTAGES cell_voltages[55];

BATTERY_DATA batteryData[55];
BATTERY_STATE bat_state;
uint32_t totalSMBUSErrors = 0;
uint32_t unrec_smbus_errors = 0;
uint16_t batteryTemp;
uint32_t humidity_temp;
uint16_t humidity;
uint16_t voltages[5];

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;
static LED_STATE led_state;

int curmux, curchan;


/* time in ms for RIT interrupt */
#define RIT_INTERVAL	50

/* SMBus request retries */
#define	SMBUS_RETRIES	5

void set_status_led(void);


/* 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);
	}
}

/* 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)
{
	static int count = 0;
	
	/* Clear interrupt */
	Chip_RIT_ClearInt(LPC_RITIMER);

	count = (count + 1) % (1000 / RIT_INTERVAL);
	if (count == 0)
		second_tick++;
	
	set_status_led();
}

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

static __inline int
get_corrected_batnum(int batnum) {
	/* mux 6 is the 6 cell pack, so ignore channel 7 + 8 */
	if (batnum > 46)
		batnum += 2;
	/* mux 5 only has one pack attached to it, ignore remaining 7 ports */
	if (batnum > 40)
		batnum += 7;
	return batnum;
}

static __inline int
get_mux_addr(int batnum) {
	return get_corrected_batnum(batnum) / 8;
}

static __inline int
get_bat_addr(int batnum) {
	return get_corrected_batnum(batnum) % 8;
}

static int
setSmbusMux(int mux, int channel)
{
	int status;
	static int last_mux = -1;
	uint8_t zero = 0;
	uint8_t channelbits = (channel == -1 ? 0 : 1<<channel);
	
	/* clear old mux channels if we switch muxes */
	if (mux != last_mux) {
		Chip_I2C_MasterSend(I2C0, (uint8_t)((0xE0+ (last_mux<<1))>>1), &zero, 1);
		utils_block_sleep(1);
	}
	last_mux = mux;
	
	curmux = mux;
	curchan = channel;

	status = Chip_I2C_MasterSend(I2C0, (uint8_t)((0xE0+ (mux<<1))>>1),
	  &channelbits, 1);
	utils_block_sleep(2);
	return status;
}

int select_battery(int batnum) {
	return (setSmbusMux(get_mux_addr(batnum), get_bat_addr(batnum)));
}



int
smbus_read_reg(uint8_t addr, uint8_t reg, uint8_t *val, int count,
  bool ignore_errors)
{
	int i, res;
	bool succeeded = false;
	
	for (i = 1; i <= SMBUS_RETRIES; i++) {
		res = Chip_I2C_MasterCmdRead(I2C0, addr, reg, val, count);
		if (res != count) {
			if (ignore_errors)
				continue;

			totalSMBUSErrors++;
			utils_block_sleep(10 * i);
		} else {
			succeeded = true;
			break;
		}
	}
	
	if (ignore_errors)
		return (!succeeded);

	if (!succeeded) {
		unrec_smbus_errors++;
		for (i = 0; i < count; i++) *(val + i) = 0;
		return (1);
	}
	
	return (0);
}

void set_pack_state(int batnum, uint8_t state) {
	uint8_t buf[3];
	char strbuf[16];
	int i, res;
	int succeeded = false;
	uint16_t fetstate;
	int x = 0;

	/* if battery is in shutdown or going into shutdown, don't mess with it */
	if (batteryData[batnum].state == 3 ||
		batteryData[batnum].type == BATTYPE_UNKNOWN) {
		return;
	}

	if (!select_battery(batnum)) {
		totalSMBUSErrors++;
	}

	if (batteryData[batnum].type == BATTYPE_34) {
		/* if we're coming out of shutdown, allow some time for wake-up */
		if (batteryData[batnum].state == 3) {
			utils_block_sleep(20);
		}
		
		buf[0] = 0x2b;
		for (i = 1; i <= SMBUS_RETRIES; i++) {
			buf[1] = 0x97;
			buf[2] = 0x11;
			if ((res = Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, buf, 3)) != 3) {
				totalSMBUSErrors++;
				utils_block_sleep(10 * i);
				x += 1;
				continue;
			}
			utils_block_sleep(1);
			buf[1] = state ? 0x07 : 0x00;
			buf[2] = 0x00;
			if ((res = Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, buf, 3)) != 3) {
				totalSMBUSErrors++;
				utils_block_sleep(10 * i);
				x += 10;
				continue;
			}
			/*
			 * Sometimes the FETs do not get set even though the command
			 * succeeds. So test if the FET state is correct and repeat the
			 * command if necessary.
			 */
			if (smbus_read_reg(BATTERY_ADDR, 0x2b, (uint8_t*)&fetstate, 2,
			  false)) {
				totalSMBUSErrors++;
				utils_block_sleep(10 * i);
				x += 100;
				continue;
			}
			if ((fetstate == 0xffff) || (state && (fetstate & 3) != 3) || (!state && fetstate)) {
				sprintf(strbuf, "bat%d: fet\r\n", batnum);
				x += 1000;
				continue;
			}
			succeeded = true;
			break;
		}

		if (!succeeded)
			unrec_smbus_errors++;
		
	} else {
		buf[0] = 0x3f;
		buf[1] = 0x00;
		buf[2] = state;
		if (Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, buf, 3) != 3) {
			totalSMBUSErrors++;
		}
	}

	batteryData[batnum].state = state;
}

void allPacks(uint8_t state)
{
	int batNum;

	for (batNum = 0; batNum < 55; batNum++) {
		set_pack_state(batNum, state);
	}
	return;
}

uint32_t
get_smbus_errors()
{
	return totalSMBUSErrors;
}

uint32_t
get_unrec_smbus_errors()
{
	return unrec_smbus_errors;
}

uint16_t
get_battery_temp()
{
	return batteryTemp;
}

uint32_t
get_humidity_temp()
{
	return humidity_temp;
}

uint16_t
get_humidity()
{
	return humidity;
}

uint16_t*
get_voltages()
{
	return voltages;
}

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;
	}
}

int
get_charge_level(void)
{
	
	int i;
	float voltage = 0.0, level;
	int bat_count, bat_count_active = 0;
	
	bat_count = get_bat_count();
	
	for (i = 0; i < bat_count; i++) {
		voltage += (float)batteryData[i].stackVolt / 1000;
		if (batteryData[i].stackVolt)
			bat_count_active++;		
	}
	voltage /= bat_count_active;
	
	level = (voltage - VOLT_EMPTY) * 1000 / (VOLT_FULL - VOLT_EMPTY);
	if (level > 1000) level = 1000;
	return (level > 0 ? (int) level : 0); 
}

void
set_led_state(LED_STATE user_led_state)
{
	led_state = user_led_state;
}

#if defined(X41CELL)
void
set_status_led(void)
{
	static int count = 0;
	static LED_STATE last_mode = LED_SOLID_LEVEL;
	static LED_STATE current_mode = LED_SOLID_LEVEL;
	static int intensity;
	int color;
	int levels = 15; // number of intensity levels to cycle through
	int start = 5;   // number of cycles after which to start dimming
	int	pause = 2;   // number of cycles between min intensity and ramping back up
	
	count = (count + 1) % (start + 2*levels + 2*pause);
	
	switch (led_state) {
	// display battery charge level as solid color from green to red
	case LED_SOLID_LEVEL:
		if (current_mode == LED_SOLID_LEVEL && count % 10 != 0)
			break;
		color = get_charge_level();
		Board_LED_Color(1000 - color, color, 0);
		last_mode = current_mode = LED_SOLID_LEVEL;
		break;

  // pulse battery charge level
	case LED_PULSE_LEVEL:
		if (current_mode != LED_PULSE_LEVEL) {
			count = 0;
		}
		color = get_charge_level();
		if (count == 0) {
			intensity = levels + 1;
		} else if (count >= start && count < start + levels) {
			intensity--;
		} else if (count >= start + levels + pause && count < start + 2*levels + pause) {
			intensity++;
		}			
		Board_LED_Color(((1000 - color) * intensity) / levels, (color * intensity) / levels, 0);
		last_mode = current_mode = LED_PULSE_LEVEL;
		break;

	// blink blue once, then go back to previous mode
	case LED_BLINK:
		if (current_mode != LED_BLINK) {
			count = 0;
		}
		if (count >= 0 && count < 5) {
			Board_LED_Color(0, 0, 1000);
			current_mode = LED_BLINK;
		} else {
			count = 0;
			led_state = last_mode;
		}
		break;
	default:
		break;
	}
}

#elif defined(LINCOLN60)
void
set_status_led(void) {

	static int count = 0;
	static LED_STATE last_mode = LED_SOLID_LEVEL;
	static LED_STATE current_mode = LED_SOLID_LEVEL;
	int interval = 30;

	count = (count + 1) % interval;
	
	switch (led_state) {
	// green and amber led on
	case LED_SOLID_LEVEL:
		Board_LED_Set(LED_GREEN, 1);
		Board_LED_Set(LED_AMBER, 1);
		last_mode = current_mode = LED_SOLID_LEVEL;
		break;

	// pulse amber led to indicate charging
	case LED_PULSE_LEVEL:
		Board_LED_Set(LED_GREEN, 1);
		if (current_mode != LED_PULSE_LEVEL) {
			count = 0;
		}
		if (count < interval / 2) {
			Board_LED_Set(LED_AMBER, 1);
		} else {
			Board_LED_Set(LED_AMBER, 0);
		}
		last_mode = current_mode = LED_PULSE_LEVEL;
		break;

	// turn off green once, then go back to previous mode
	case LED_BLINK:
		if (current_mode != LED_BLINK) {
			count = 0;
		}
		if (count >= 0 && count < 5) {
			Board_LED_Set(LED_GREEN, 0);
			current_mode = LED_BLINK;
		} else {
			count = 0;
			led_state = last_mode;
		}
		break;
	default:
		break;
	}
}
#else
#error "Need to select board type (X41CELL or LINCOLN60)"
#endif

void
shutdown_packs(void)
{
	int bat;
	uint8_t buf[3]; 
	
	for (bat = 0; bat < get_bat_count(); bat++) {
		/* only 3.4 Ah packs support shutdown */
		if (batteryData[bat].type != BATTYPE_34) continue;
		
		select_battery(bat);
		buf[0] = 0x00;
		buf[1] = 0x10;
		buf[2] = 0x00;		
		if (Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, buf, 3) != 3) {
			totalSMBUSErrors++;
		}
		utils_task_sleep(25);
		if (Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, buf, 3) != 3) {
			totalSMBUSErrors++;
		}
		batteryData[bat].state = 2;
	}
}

CELL_VOLTAGES *
get_cell_voltages(int batnum)
{
	int bat, first, count;
	uint8_t reg;
	uint8_t *cvolt;
	
	if (batnum == -1) {
		first = 0;
		count = get_bat_count();
	} else {
		first = batnum;
		count = 1;
	}
	
	for (bat = first; bat < first + count; bat++) {
		if (batteryData[bat].state == 3) continue;
		
		select_battery(bat);
		for (reg = 0; reg < 8; reg++) {
			cvolt = ((uint8_t *)&cell_voltages[bat]) + (reg * 2);
			smbus_read_reg(BATTERY_ADDR, 0x3f - reg, cvolt, 2, false);
		}
	}
	return cell_voltages;
}

int
get_time_to_full(void)
{
	int i;
	int sum = 0;
	uint16_t val;
	
	for (i = 0; i < 55; i++) {
		select_battery(i);
		smbus_read_reg(BATTERY_ADDR, 0x05, (uint8_t *)&val, 2, false);
		sum += val;
	}
	
	return (sum / 55);
}

void
update_battery_status(void)
{
	int batnum, i;
	char count;
	bool no_comms;
	static int turn_off_packs;
	
	uint8_t i2c_buf[4];
	uint8_t volt[2];
	uint16_t battery_mode;
	uint16_t design_capacity;
	uint8_t totalPacksOn = 0;

	int16_t maxCurrent = INT16_MIN;
	int16_t minCurrent = INT16_MAX;
	uint16_t maxVoltage = 0;
	uint16_t minVoltage = UINT16_MAX;

	for (batnum = 0; batnum < 55; batnum++) {
		if (!select_battery(batnum)) {
			totalSMBUSErrors++;
		}

		/* 
		 * When packs are in shutdown they don't respond to I2C request anymore,
		 * just check one reg to see if we are in shutdown.
		 * Note: Dead/missing packs will be considered to be in shutdown too.
		 * Note: sometimes registers falsely read 0xffff 
		 */		
		count = 0;
		no_comms = false;
		do {
			if (smbus_read_reg(BATTERY_ADDR, 0x16,
			  (uint8_t *)&batteryData[batnum].status, 2, true)) {
				batteryData[batnum].status = 0;			  
				batteryData[batnum].state = 3;
				batteryData[batnum].stackVolt = 0;
				batteryData[batnum].capacity_mAh = 0;
				batteryData[batnum].cycleCnt = 0;
				batteryData[batnum].current = 0;
				no_comms = true;
			}
		} while (batteryData[batnum].status == 0xffff && count++ < 5);
		
		if (no_comms)
			continue;

		/* 
		 * This is the first run, query stuff we only have to read once.
		 * However, when packs have to get woken up from shutdown we might miss
		 * the first round so run this also if there is indication that it has
		 * not been run successfully on this pack.
		 */
		if (!bat_data_ready || batteryData[batnum].stackVolt == 0) {

			(void) smbus_read_reg(BATTERY_ADDR, 0x03, (uint8_t *)&battery_mode,
			  2, false);

			battery_mode |= (1<<13)|(1<<14);
			i2c_buf[0] = 0x03;
			i2c_buf[1] = battery_mode & 0xff;
			i2c_buf[2] = (battery_mode & 0xff00) >> 8;
			(void )Chip_I2C_MasterSend(I2C0, BATTERY_ADDR, i2c_buf, 3);

			(void) smbus_read_reg(BATTERY_ADDR, 0x1c,
			  (uint8_t *)&batteryData[batnum].serialNo, 2, false);
			
			if (!smbus_read_reg(BATTERY_ADDR, 0x18,
			  (uint8_t *)&design_capacity, 2, false)) {
				if (design_capacity == 3400)
					batteryData[batnum].type = BATTYPE_34;
				else
					batteryData[batnum].type = BATTYPE_29;
			}
			/* turn battery off so it's in a known state */
			set_pack_state(batnum, 0);

		}

		(void) smbus_read_reg(BATTERY_ADDR, 0x09,
		  (uint8_t *)&batteryData[batnum].stackVolt, 2, false);
		(void) smbus_read_reg(BATTERY_ADDR, 0x0f,
		  (uint8_t *)&batteryData[batnum].capacity_mAh, 2, false);
		(void) smbus_read_reg(BATTERY_ADDR, 0x17,
		  (uint8_t *)&batteryData[batnum].cycleCnt, 2, false);
		(void) smbus_read_reg(BATTERY_ADDR, 0x0a,
		  (uint8_t *)&batteryData[batnum].current, 2, false);
		
		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;

		//turn individual pack on
		if (batteryData[batnum].state == 0 && batteryData[batnum].flags & PACK_ON) {
			set_pack_state(batnum, 1);
			batteryData[batnum].state = 1;
			batteryData[batnum].flags &= ~PACK_ON;
		}

		/*
		 * Check individual pack status, turn off if needed.
		 * However, if were about to shut the stack down soon, don't turn off
		 * single packs anymore.
		 */
		if (bat_state == BATT_DISCH && turn_off_packs)
			continue;

		if ((bat_state == BATT_DISCH && batteryData[batnum].status & 0x0800) || (batteryData[batnum].flags & PACK_OFF)) {
			set_pack_state(batnum, 0);
			batteryData[batnum].state = 0;
			batteryData[batnum].flags &= ~PACK_OFF;
		} else {
			totalPacksOn++;
		}
	}
	
	// once we get down to 25 packs, wait for 60 seconds then turn stack off
	if (totalPacksOn <= 25 && bat_state == BATT_DISCH) {
		if (!turn_off_packs)
			turn_off_packs = second_tick + 60;
	} else {
		turn_off_packs = 0;
	}

	if (turn_off_packs && second_tick > turn_off_packs) {
		printf("turning off stack 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;
	}
	
	/* query temp sensor */
	if (!setSmbusMux(5, 1)) {
		totalSMBUSErrors++;
	} else {
		(void) smbus_read_reg((0x90>>1), 0x00, (uint8_t *)&batteryTemp, 2, false);
		batteryTemp = ((batteryTemp&0x00ff)<<8) | ((batteryTemp&0xff00)>>8);
		batteryTemp >>= 4;
	}
	
	/* query humidity sensor */
	if (!setSmbusMux(5, 7)) {
		totalSMBUSErrors++;
	} else {
		Chip_I2C_MasterSend(I2C0, (0x4e>>1), (uint8_t *)&humidity_temp, 1);
		utils_task_sleep(50);
		if (Chip_I2C_MasterRead(I2C0, (0x4e>>1), (uint8_t *)&humidity_temp, 4) != 4) {
			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;
	}

	/* get voltages and currents */
	for (i = 0; i < 5; i++) {		
		if (!setSmbusMux(5, i + 2)) {
			totalSMBUSErrors++;
			continue;
		}
		if (Chip_I2C_MasterRead(I2C0, (0x48), volt, 2) != 2) {
			totalSMBUSErrors++;
			voltages[i] = 9999;
			continue;
		}
		voltages[i] = volt[0] << 8 | volt[1];
	}

	bat_data_ready = true;
	bat_count = batnum;
}


void
init_batteries(void)
{
	uint8_t mux;

	smbusReset();
	memset(batteryData, 0, sizeof(BATTERY_DATA)*55);

	/* reset the mux's */
	for (mux = 0; mux <= 7; mux++) {
		(void)setSmbusMux(mux, -1);
	}
	
	
	/* Initialize RITimer */
	Chip_RIT_Init(LPC_RITIMER);
	/* Configure RIT for a 100ms interrupt tick rate */
	Chip_RIT_SetTimerInterval(LPC_RITIMER, RIT_INTERVAL);
	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 */
	Chip_GPIO_SetPinDIROutput(LPC_GPIO, 0, 9); /* GPIO - AUX@_DISCHG_EN */
	
	/* 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);
	Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, AUX2_ENABLE_PIN);
	bat_state = BATT_OFF;
	
	// Initialize Slave I2C peripheral
	Board_I2C_Init(I2C0);
	Chip_I2C_Init(I2C0);
	Chip_I2C_SetClockRate(I2C0 , 80000);
	i2c_set_mode(I2C0, I2C_INTERRUPT);
	
}