#include "battery.h"
#include "ti/driverlib/dl_i2c.h"
#include "ti_msp_dl_config.h"
#include "src/peripherals/dac/dac.h"
#include "src/peripherals/adc/adc.h"
#include "src/interfaces/i2c_controller.h"
// we need the itnerface for the ADC_TARGET_BASE_ADDRESS constant
// refactor this somewhen to a general constants file
#include "src/peripherals/adc/adc_interface.h"
#include "src/config.h"

BatterySlot battery_slots[NUM_SLOTS];

static void set_dac(uint8_t slot, uint16_t value) {
    battery_slots[slot].dac_value = value;
    DAC_SingleWrite(slot, value);
}
static void set_pwm(uint8_t slot, uint16_t value) {
    battery_slots[slot].pwm_value = value;
    DL_TimerG_setCaptureCompareValue(battery_slots[0].timer, value, DL_TIMER_CC_1_INDEX);
}
static void batteryslots_disable(uint8_t slot) {
    if (battery_slots[slot].dac_value != 0) {
        set_dac(slot, 0);
    }
    if (battery_slots[slot].pwm_value != 0) {
        set_pwm(slot, 0);
    }
}

/*Initialize battery array and default parameters*/
static void batteryslots_init() {

    // initialize data structures
    battery_slots[0].timer = PWM_0_INST;
#if NUM_SLOTS == 4
    battery_slots[1].timer = PWM_1_INST;
    battery_slots[2].timer = PWM_2_INST;
    battery_slots[3].timer = PWM_3_INST;
#endif

    for(uint8_t i=0; i< NUM_SLOTS; i++){

        battery_slots[i].measurement.state = SLOT_STATE_OK;
        // convinience trick:
        // with that we can set *battery_slots[i].state = SLOT_STATE_* or SLOT_ERR_*
        // like e.g. *battery_slots[i].state = SLOT_ERR_OVERTEMPERATURE
        battery_slots[i].state = &battery_slots[i].measurement.state;

        battery_slots[i].measurement.voltage = 0;
        battery_slots[i].measurement.current = 0;
        battery_slots[i].measurement.temperature = 0;
        battery_slots[i].set_current = 0;

        set_pwm(i, 0);
    }

    
    if (!i2c_discover(DAC_TARGET_ADDRESS)) {
        // there is only 1 DAC for all 4 slots
        for(uint8_t i=0; i< NUM_SLOTS; i++) {
            *battery_slots[i].state = SLOT_ERR_NO_DAC;
        }
        // Error state - no I2C on bus - we cannot continue.
        return;
    }

    for(uint8_t i=0; i< NUM_SLOTS; i++){

        set_dac(i, 0);

        if (!i2c_discover(ADC_TARGET_BASE_ADDRESS+i)) {
            // this automatically translates to the other addresses
            *battery_slots[i].state = SLOT_ERR_NO_ADC1+i;
        }
    }
}
static void batteryslots_read_state(uint8_t slot) {
    /*
     * Strategy:
     * 1. updateADCReading calls the ADC function that does the control loop for getting the values
     * 2. the updateADCReading also calls internally the HAL to send the i2c commands,
     *    construct the configuration byte and calculate the values (voltage, current)
     * 3. the adc updates the battery slot value directly
     */
 
    // step 1: read channel 0 (voltage reading of the cell)
    uint16_t bare_voltage = read_adc_channel(slot, 0);
    battery_slots[slot].measurement.voltage = (100.0/56+1)*bare_voltage; // We have that voltage divider

    // DAC branch: we can calculate the current based on the shunt
    if (battery_slots[slot].set_current >= 0) {
        // read channel 1 (current reading on charge)
        bare_voltage = read_adc_channel(slot, 1);
        battery_slots[slot].measurement.current = bare_voltage*10/1000; // current comes in microvolts
#ifdef DEBUG_TRACE_CTRL
        printf("Slot %d voltage: %d mV and %d mA (dac shunt)\n", slot, battery_slots[slot].measurement.voltage, battery_slots[slot].measurement.current);
#endif
    } else {
        // we are in PWM mode, the shunt is on the high side
        // read channel 2 (voltage reading on 5V side)
        int16_t shunt_current = read_adc_channel(slot, 2)*10;

        // read channel 3 (current reading after step conversion, 5V side)
        int16_t hi_voltage = read_adc_channel(slot, 3)*100/56;

        // calculate the result
        battery_slots[slot].measurement.current = -1*shunt_current*hi_voltage/battery_slots[slot].measurement.voltage;
#ifdef DEBUG_TRACE_CTRL
        printf("Slot %d voltage: %d mV and %d mA (pwm shunt) (hi side voltage: %d mV, hi side current: %d mA)\n", slot, battery_slots[slot].measurement.voltage, battery_slots[slot].measurement.current, hi_voltage, shunt_current);
#endif
    }
}

static void batteryslots_adjust_current(uint8_t slot) {

#ifdef DEBUG_TRACE_CTRL
    printf("Ctrl: Checking to adjust battery current. Set current: %d mA, measured current: %d mA\n",
        battery_slots[slot].set_current,
        battery_slots[slot].measurement.current
        );
#endif

    if (battery_slots[slot].set_current > 0) {
        // positive current -> charge (with DAC)

        if (battery_slots[slot].pwm_value != 0) {
            // seems like we switched from a charging before
            // -> disable DAC before getting active
#ifdef DEBUG_CTRL
            printf("Detected active pwm. Value: %d. Disabling.", battery_slots[slot].pwm_value);
#endif
            set_pwm(slot, 0);
        }

        if (battery_slots[slot].set_current + BATTERY_CURRENT_THRESHOLD < battery_slots[slot].measurement.current) {
            // we are outside of the tolerance band
            // exceeded to the upper limit
            // -> update dac value, decrease the voltage
            if (battery_slots[slot].dac_value-1 >= 0) {
#ifdef DEBUG_TRACE_CTRL
                printf("Ctrl: Decreasing DAC to %d\n", battery_slots[slot].dac_value-1);
#endif
                set_dac(slot, --battery_slots[slot].dac_value);
            }
            else {
                // we want to give more current, but we can't ?!
#ifdef DEBUG_CTRL
                printf("Ctrl: Unable to decrease DAC to %d\n", battery_slots[slot].dac_value-1);
#endif
                *battery_slots[slot].state = SLOT_WARN_LOWER_DAC_NOT_POSSIBLE;
            }
        } else if (battery_slots[slot].set_current - BATTERY_CURRENT_THRESHOLD > battery_slots[slot].measurement.current) {
            // we are outside of the tolerance band
            // exceeded to the upplowerer limit
            // -> update dac value, increase the voltage
            if (battery_slots[slot].dac_value+1 <= MAX_DAC_VALUE) {
#ifdef DEBUG_TRACE_CTRL
                printf("Ctrl: Increasing DAC to %d\n", battery_slots[slot].dac_value+1);
#endif
                set_dac(slot, ++battery_slots[slot].dac_value);
            }
            else {
                // we want to give more current, but we can't ?!
#ifdef DEBUG_CTRL
                printf("Ctrl: Unable to increase DAC to %d\n", battery_slots[slot].dac_value+1);
#endif
                *battery_slots[slot].state = SLOT_WARN_HIGHER_DAC_NOT_POSSIBLE;
            }
        }
        // no else statement here: we are ok, since we are in the tolerance measure
    } else if (battery_slots[slot].set_current < 0) {
        // negative current -> discharge (with PWM)
        if (battery_slots[slot].dac_value != 0) {
            // seems like we switched from a charging before
            // -> disable DAC before getting active
#ifdef DEBUG_CTRL
            printf("Detected active dac. Value: %d. Disabling.", battery_slots[slot].dac_value);
#endif
            set_dac(slot, 0);
        }

        if (battery_slots[slot].set_current + BATTERY_CURRENT_THRESHOLD > battery_slots[slot].measurement.current) {
            // we are outside of the tolerance band
            // exceeded to the upper limit
            // -> update pwm value, decrease the voltage
            
            // @todo debugging & validation: ensure that this directive works
#ifdef DEBUG_CTRL
            printf("timer count: %d\n", DL_Timer_getTimerCount(battery_slots[0].timer));
#endif

            if (battery_slots[slot].pwm_value+1 <= DL_Timer_getTimerCount(battery_slots[0].timer)) {
                // pwm is inverse to the DAC since dragging more current means more negative
#ifdef DEBUG_TRACE_CTRL
                printf("Ctrl: Increasing PWM to %d\n", battery_slots[slot].pwm_value+1);
#endif
                set_pwm(slot, ++battery_slots[slot].pwm_value);
            }
            else {
                // we want to give more current, but we can't ?!
#ifdef DEBUG_CTRL
                printf("Ctrl: Unable to increase PWM to %d\n", battery_slots[slot].pwm_value+1);
#endif
                *battery_slots[slot].state = SLOT_WARN_HIGHER_PWM_NOT_POSSIBLE;
            }
        } else if (battery_slots[slot].set_current - BATTERY_CURRENT_THRESHOLD < battery_slots[slot].measurement.current) {
            // we are outside of the tolerance band
            // exceeded to the upplowerer limit
            // -> update pwm value, increase the voltage
            if (battery_slots[slot].pwm_value-1 >= 0) {
#ifdef DEBUG_TRACE_CTRL
                printf("Ctrl: Decreasing PWM to %d\n", battery_slots[slot].pwm_value-1);
#endif
                set_pwm(slot, --battery_slots[slot].pwm_value);
            }
            else {
                // we want to give more current, but we can't ?!
#ifdef DEBUG_CTRL
                printf("Ctrl: Unable to decrease PWM to %d\n", battery_slots[slot].pwm_value-1);
#endif
                *battery_slots[slot].state = SLOT_WARN_LOWER_PWM_NOT_POSSIBLE;
            }
        }
    } else {
        // we have 0 -> stop charging and discharging
#ifdef DEBUG_CTRL
        printf("0 current -> Disabling slot.\n");
#endif
        batteryslots_disable(slot);
    }
}

BatterySlotManager battery_slotmgr = {
    .init = batteryslots_init,
    .read_state = batteryslots_read_state,
    .adjust_current = batteryslots_adjust_current,
    .disable = batteryslots_disable
};