/*
 * In this module we are going to implement the main process functions
 * for FCHG.
 */

#include "fchg/fchg_env.h"
#include "fchg/fchg_func_i.h"
#include "rv/rv_general.h"
#include "rvf/rvf_api.h"
#include "rvm/rvm_use_id_list.h"
#include "abb/abb.h"
#include <string.h>
#include <stdio.h>

extern UINT16 madc_vbat_2_physical(UINT16 adc_val);
extern UINT16 madc_vbat_inverse(UINT16 mv);

void pwr_init_discharge(void)
{
	pwr_ctrl->curr_disch_thresh = 0;
}

static void handle_discharge(void)
{
	UINT16 i;
	char trace[64];

	/* first we need to find the current threshold we are at */
	i = pwr_ctrl->curr_disch_thresh;
	/* is there one below? */
	if (++i == pwr_ctrl->nb_thresholds)
		return;
	/* are we crossing it? */
	if (pwr_ctrl->batt_mv >= pwr_ctrl->batt_thresholds[i].bat_voltage)
		return;
	/* yes, we crossed it - see if we fell even further down */
	while (i < pwr_ctrl->nb_thresholds &&
	       pwr_ctrl->batt_mv < pwr_ctrl->batt_thresholds[i].bat_voltage)
		i++;
	/* the last one was it */
	i--;
	pwr_ctrl->curr_disch_thresh = i;
	sprintf(trace, "Battery fell through %u%% mark",
		pwr_ctrl->batt_thresholds[i].remain_capa);
	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
			RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
}

static void start_i2v_cal(void)
{
	UINT16 bciconf;

	rvf_send_trace("Calibrating i2v offset", 22, NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
	pwr_ctrl->state = FCHG_STATE_I2V_CAL_2;
	bciconf = ABB_Read_Register_on_page(PAGE1, BCICONF);
	bciconf &= 0x3E0;
	bciconf |= pwr_ctrl->config.bciconf;
	ABB_Write_Register_on_page(PAGE1, BCICONF, bciconf);
	/*
	 * Set the CHDISPA bit and start the zero calibration routine
	 * of the I to V converter
	 */
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0010);
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0019);
}

static void start_ci_charging(void)
{
	rvf_send_trace("Start CI charging", 17, NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
	pwr_ctrl->state = FCHG_STATE_CI_CHARGING;
	/* Select constant current charging. The charger is disabled */
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0002);
	/* Program the DAC with the constant current value */
	ABB_Write_Register_on_page(PAGE0, CHGREG,
			pwr_ctrl->config.ci_current + pwr_ctrl->i2v_offset);
	/* Enable the charger */
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0003);
	/* The total charging time starts now */
	pwr_ctrl->start_time = rvf_get_tick_count();
}

static void start_cv_charging(void)
{
	UINT16 code;

	rvf_send_trace("Start CV charging", 17, NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
	pwr_ctrl->state = FCHG_STATE_CV_CHARGING;
	/* Select constant voltage charging. The charger is disabled */
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
	/* figure out the DAC code */
	code = madc_vbat_inverse(pwr_ctrl->config.cv_init_set);
	rvf_send_trace("Voltage (DAC code) ", 19, code,
			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
	/* Program the DAC with the constant voltage value */
	ABB_Write_Register_on_page(PAGE0, CHGREG, code);
	/* Enable the charger */
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0001);
	/* CV control loop state init */
	pwr_ctrl->cv_dac_init = code;
	pwr_ctrl->cv_dac_curr = code;
	pwr_ctrl->cv_high_vbat_count = 0;
	pwr_ctrl->cv_low_vbat_count = 0;
	/* Ichg averaging state init */
	pwr_ctrl->ichg_fill_level = 0;
	pwr_ctrl->ichg_ring_ptr = 0;
	pwr_ctrl->ichg_low_count = 0;
}

static void start_charge_condition_met(void)
{
	rvf_send_trace("Charge start condition met", 26, NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
	if (pwr_ctrl->config.bciconf)
		start_i2v_cal();
	else {
		pwr_ctrl->i2v_offset = 0;
		start_ci_charging();
	}
}

static void ci_progress_trace(UINT16 ichg)
{
	char trace[64];

	sprintf(trace, "CI charging: Vbat=%u Ichg=%u i2v=%u",
		pwr_ctrl->batt_mv, ichg, pwr_ctrl->i2v_offset);
	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
}

static int cv_ichg_process(UINT16 ichg_new)
{
	UINT16 ichg_clip, ichg_entry;
	UINT32 ichg_accum;
	UINT16 i;
	char trace[64];

	if (pwr_ctrl->ichg_fill_level < ICHG_AVG_WINDOW)
		pwr_ctrl->ichg_avg_buf[pwr_ctrl->ichg_fill_level++] = ichg_new;
	else {
		ichg_clip = pwr_ctrl->ichg_average +
				pwr_ctrl->config.ichg_max_spike;
		if (ichg_new > ichg_clip)
			ichg_entry = ichg_clip;
		else
			ichg_entry = ichg_new;
		pwr_ctrl->ichg_avg_buf[pwr_ctrl->ichg_ring_ptr++] = ichg_entry;
		if (pwr_ctrl->ichg_ring_ptr >= ICHG_AVG_WINDOW)
			pwr_ctrl->ichg_ring_ptr = 0;
	}
	ichg_accum = 0;
	for (i = 0; i < pwr_ctrl->ichg_fill_level; i++)
		ichg_accum += pwr_ctrl->ichg_avg_buf[i];
	pwr_ctrl->ichg_average = ichg_accum / pwr_ctrl->ichg_fill_level;
	sprintf(trace, "CV charging: Vbat=%u Ichg=%u Ichg_avg=%u i2v=%u",
		pwr_ctrl->batt_mv, ichg_new, pwr_ctrl->ichg_average,
		pwr_ctrl->i2v_offset);
	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
	if (pwr_ctrl->ichg_average >
	    (pwr_ctrl->config.end_current + pwr_ctrl->i2v_offset)) {
		pwr_ctrl->ichg_low_count = 0;
		return 0;
	}
	pwr_ctrl->ichg_low_count++;
	if (pwr_ctrl->ichg_low_count < pwr_ctrl->config.ichg_samples_needed)
		return 0;
	rvf_send_trace("Stopping charge by low current condition", 40,
			NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
	pwr_init_discharge();
	pwr_ctrl->state = FCHG_STATE_READY_TO_RECHARGE;
	return 1;
}

static int overvoltage_end_charge_check(void)
{
	if (pwr_ctrl->batt_mv < pwr_ctrl->config.overvoltage)
		return 0;
	if (pwr_ctrl->cv_dac_curr !=
	    (pwr_ctrl->cv_dac_init - pwr_ctrl->config.cv_dac_max_decr))
		return 0;
	rvf_send_trace("Stopping charge by overvoltage condition", 40,
			NULL_PARAM, RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
	pwr_init_discharge();
	pwr_ctrl->state = FCHG_STATE_READY_TO_RECHARGE;
	return 1;
}

static void cv_ctrl_loop_high_check(void)
{
	if (pwr_ctrl->batt_mv < pwr_ctrl->config.cv_ctrl_loop_high) {
		pwr_ctrl->cv_high_vbat_count = 0;
		return;
	}
	pwr_ctrl->cv_high_vbat_count++;
	if (pwr_ctrl->cv_high_vbat_count < pwr_ctrl->config.cv_samples_needed)
		return;
	if (pwr_ctrl->cv_dac_curr ==
	    (pwr_ctrl->cv_dac_init - pwr_ctrl->config.cv_dac_max_decr))
		return;
	pwr_ctrl->cv_dac_curr--;
	ABB_Write_Register_on_page(PAGE0, CHGREG, pwr_ctrl->cv_dac_curr);
	rvf_send_trace("Sub CV DAC", 10, pwr_ctrl->cv_dac_curr,
			RV_TRACE_LEVEL_DEBUG_MEDIUM, FCHG_USE_ID);
	pwr_ctrl->cv_high_vbat_count = 0;
}

static void cv_ctrl_loop_low_check(void)
{
	if (pwr_ctrl->batt_mv >= pwr_ctrl->config.cv_ctrl_loop_low) {
		pwr_ctrl->cv_low_vbat_count = 0;
		return;
	}
	pwr_ctrl->cv_low_vbat_count++;
	if (pwr_ctrl->cv_low_vbat_count < pwr_ctrl->config.cv_samples_needed)
		return;
	if (pwr_ctrl->cv_dac_curr ==
	    (pwr_ctrl->cv_dac_init + pwr_ctrl->config.cv_dac_max_incr))
		return;
	pwr_ctrl->cv_dac_curr++;
	ABB_Write_Register_on_page(PAGE0, CHGREG, pwr_ctrl->cv_dac_curr);
	rvf_send_trace("Add CV DAC", 10, pwr_ctrl->cv_dac_curr,
			RV_TRACE_LEVEL_DEBUG_MEDIUM, FCHG_USE_ID);
	pwr_ctrl->cv_low_vbat_count = 0;
}

static int charging_time_limit_check(void)
{
	if ((rvf_get_tick_count() - pwr_ctrl->start_time) <
	    RVF_SECS_TO_TICKS(pwr_ctrl->config.charge_time_limit))
		return 0;
	rvf_send_trace("Stopping charge by time exceeded condition", 42,
			NULL_PARAM, RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
	pwr_init_discharge();
	pwr_ctrl->state = FCHG_STATE_RECHARGE_TIMER;
	pwr_ctrl->start_time = rvf_get_tick_count();
	return 1;
}

void pwr_process_adc(struct pwr_adc_ind_s *msg)
{
	pwr_ctrl->batt_mv = madc_vbat_2_physical(msg->data[0]);

	switch (pwr_ctrl->state) {
	case FCHG_STATE_NO_EXT_PWR:
	case FCHG_STATE_PWR_PLUG_TIMER:
	case FCHG_STATE_NO_CHARGING:
		handle_discharge();
		return;
	case FCHG_STATE_READY_TO_CHARGE:
		handle_discharge();
		if (!(msg->data[9] & CHGPRES)) {
			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
			return;
		}
		if (pwr_ctrl->batt_mv < pwr_ctrl->config.start_thresh)
			start_charge_condition_met();
		return;
	case FCHG_STATE_READY_TO_RECHARGE:
		handle_discharge();
		if (!(msg->data[9] & CHGPRES)) {
			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
			return;
		}
		if (pwr_ctrl->batt_mv < pwr_ctrl->config.restart_thresh)
			start_charge_condition_met();
		return;
	case FCHG_STATE_I2V_CAL_1:
		if (!(msg->data[9] & CHGPRES)) {
			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
			return;
		}
		if (pwr_ctrl->config.bciconf)
			start_i2v_cal();
		else {
			pwr_ctrl->i2v_offset = 0;
			start_ci_charging();
		}
		return;
	case FCHG_STATE_I2V_CAL_2:
		pwr_ctrl->i2v_offset = msg->data[2];
		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
		rvf_send_trace("i2v offset (MADC code) ", 23,
				pwr_ctrl->i2v_offset,
				RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
		if (!(msg->data[9] & CHGPRES)) {
			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
			return;
		}
		start_ci_charging();
		return;
	case FCHG_STATE_CI_CHARGING:
		ci_progress_trace(msg->data[2]);
		if (!(msg->data[9] & CHGPRES)) {
			ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
			return;
		}
		if (charging_time_limit_check())
			return;
		if (pwr_ctrl->batt_mv >= pwr_ctrl->config.ci2cv_thresh)
			start_cv_charging();
		return;
	case FCHG_STATE_CV_CHARGING:
		if (!(msg->data[9] & CHGPRES)) {
			ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
			return;
		}
		if (cv_ichg_process(msg->data[2]))
			return;
		if (overvoltage_end_charge_check())
			return;
		if (charging_time_limit_check())
			return;
		cv_ctrl_loop_high_check();
		cv_ctrl_loop_low_check();
		return;
	case FCHG_STATE_RECHARGE_TIMER:
		handle_discharge();
		if ((rvf_get_tick_count() - pwr_ctrl->start_time) <
		    RVF_SECS_TO_TICKS(pwr_ctrl->config.recharge_delay))
			return;
		rvf_send_trace("Restart time met, allowing new charging", 39,
				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
				FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_READY_TO_RECHARGE;
		return;
	default:
		rvf_send_trace("Invalid state in pwr_process_adc()", 32,
				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
				FCHG_USE_ID);
	}
}

void pwr_handle_timer(void)
{
	if (pwr_ctrl->state != FCHG_STATE_PWR_PLUG_TIMER)
		return;
	rvf_send_trace("Timer expired, ready to charge", 30, NULL_PARAM,
			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
	pwr_ctrl->state = FCHG_STATE_READY_TO_CHARGE;
}

void pwr_charger_plug(void)
{
	if (pwr_ctrl->state != FCHG_STATE_NO_EXT_PWR) {
		rvf_send_trace("Charger plug event in unexpected state", 38,
				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
				FCHG_USE_ID);
		return;
	}
	if (!pwr_ctrl->config_present) {
		rvf_send_trace(
			"Charger plugged in, but no config: won't charge", 47,
			NULL_PARAM, RV_TRACE_LEVEL_ERROR, FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_NO_CHARGING;
		return;
	}
	if (pwr_ctrl->config.start_delay) {
		rvf_send_trace("Charger plug, starting timer", 28, NULL_PARAM,
				RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
		rvf_start_timer(FCHG_TIMER,
				RVF_MS_TO_TICKS(pwr_ctrl->config.start_delay),
				FALSE);
		pwr_ctrl->state = FCHG_STATE_PWR_PLUG_TIMER;
	} else {
		rvf_send_trace("Charger plug, ready to charge", 29, NULL_PARAM,
				RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_READY_TO_CHARGE;
	}
}

void pwr_charger_unplug(void)
{
	switch (pwr_ctrl->state) {
	case FCHG_STATE_NO_EXT_PWR:
		rvf_send_trace("Charger unplug, already handled", 31,
				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_LOW,
				FCHG_USE_ID);
		/* nothing to do */
		return;
	case FCHG_STATE_PWR_PLUG_TIMER:
	case FCHG_STATE_READY_TO_CHARGE:
	case FCHG_STATE_READY_TO_RECHARGE:
	case FCHG_STATE_I2V_CAL_1:
	case FCHG_STATE_RECHARGE_TIMER:
	case FCHG_STATE_NO_CHARGING:
		rvf_send_trace("Charger unplug", 14, NULL_PARAM,
				RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
		return;
	case FCHG_STATE_I2V_CAL_2:
	case FCHG_STATE_CI_CHARGING:
	case FCHG_STATE_CV_CHARGING:
		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
		rvf_send_trace("Charger unplug, charging stopped", 32,
				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
				FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
		pwr_init_discharge();
		return;
	default:
		rvf_send_trace("Invalid state in pwr_charger_unplug()", 35,
				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
				FCHG_USE_ID);
	}
}

void pwr_charge_start_req(void)
{
	switch (pwr_ctrl->state) {
	case FCHG_STATE_NO_EXT_PWR:
		rvf_send_trace("Cannot charge without a power source", 36,
				NULL_PARAM, RV_TRACE_LEVEL_ERROR, FCHG_USE_ID);
		return;
	case FCHG_STATE_NO_CHARGING:
		if (!pwr_ctrl->config_present) {
			rvf_send_trace("No config set, cannot charge", 28,
					NULL_PARAM, RV_TRACE_LEVEL_ERROR,
					FCHG_USE_ID);
			return;
		}
		/* FALL THRU */
	case FCHG_STATE_PWR_PLUG_TIMER:
	case FCHG_STATE_READY_TO_CHARGE:
	case FCHG_STATE_READY_TO_RECHARGE:
	case FCHG_STATE_RECHARGE_TIMER:
		rvf_send_trace("Starting charge on user request", 31,
				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
				FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_I2V_CAL_1;
		return;
	case FCHG_STATE_I2V_CAL_1:
	case FCHG_STATE_I2V_CAL_2:
	case FCHG_STATE_CI_CHARGING:
	case FCHG_STATE_CV_CHARGING:
		rvf_send_trace(
			"Charging already in progress, start request ignored",
			51, NULL_PARAM, RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
		return;
	default:
		rvf_send_trace("Invalid state in pwr_charge_start_req()", 37,
				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
				FCHG_USE_ID);
	}
}

void pwr_charge_stop_req(void)
{
	switch (pwr_ctrl->state) {
	case FCHG_STATE_NO_EXT_PWR:
	case FCHG_STATE_NO_CHARGING:
		/* nothing to do */
		return;
	case FCHG_STATE_PWR_PLUG_TIMER:
	case FCHG_STATE_READY_TO_CHARGE:
	case FCHG_STATE_READY_TO_RECHARGE:
	case FCHG_STATE_I2V_CAL_1:
	case FCHG_STATE_RECHARGE_TIMER:
		rvf_send_trace("Charging disabled by user request", 33,
				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
				FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_NO_CHARGING;
		return;
	case FCHG_STATE_I2V_CAL_2:
	case FCHG_STATE_CI_CHARGING:
	case FCHG_STATE_CV_CHARGING:
		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
		rvf_send_trace("Charging stopped by user request", 32,
				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
				FCHG_USE_ID);
		pwr_ctrl->state = FCHG_STATE_NO_CHARGING;
		pwr_init_discharge();
		return;
	default:
		rvf_send_trace("Invalid state in pwr_charge_stop_req()", 36,
				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
				FCHG_USE_ID);
	}
}