#include "cc_cv_charging.h"
#include "battery.h"
#include "cc_cc_charging.h"
#include "dac.h"
#include <cstdint>
#include <stdio.h>

#define MAX_VOLTAGE_MV (4200)
#define MIN_VOLTAGE_MV (3000)
#define CC_CURRENT_LIMIT_MA (500)
#define CUTOFF_CURRENT_MA (50)
#define MAX_CYCLES (500)
// if the battery is deeply discharged then 50mA of trickle charge is given for a set timer
#define TRICKLE_CHARGE_CURRENT_MA (50)
#define TRICKLE_CHARGE_VOLTAGE_MV (2100)
#define TRICKLE_CHARGE_TIMEOUT_MS (5000) //5 mseconds
//Pre Charge
#define BATTERY_CAPACITY_MAH (2000)
#define PRE_CHARGE_CURRENT_MA (BATTERY_CAPACITY_MAH/ 10)

static ChargingState charging_state= STATE_PRE_CHARGE;
static uint16_t cycle_count= 0;
static uint32_t pre_charge_start_time= 0;

void CC_CV_UpdateChargingState(uint8_t slot_id){
    uint16_t batt_voltage= batteries[slot_id].voltage;
    int16_t batt_current= batteries[slot_id].current;
    static uint16_t trickle_timeer= 0;

    if(batt_voltage < TRICKLE_CHARGE_VOLTAGE_MV){
        charging_state= STATE_TRICKLE_CHARGE;
        trickle_timer= 0;
    }
    else if(charging_state== STATE_TRICKLE_CHARGE){
        trickle_timer ++;

        if(trickle_timer >= TRICKLE_CHARGE_TIMEOUT_MS){
            charging_state = STATE_ERROR;
        }
    }
    else if(batt_voltage < MIN_VOLTAGE_MV){
        charging_state= STATE_PRE_CHARGE;
    }

    else if(batt_voltage >= MIN_VOLTAGE_MV && batt_voltage < MAX_VOLTAGE_MV - BATTERY_THRESHOLD){
        charging_state= STATE_CC_CHARGING;
    }

    else if(batt_voltage >= MAX_VOLTAGE_MV - BATTERY_THRESHOLD){
        charging_state= STATE_CV_CHARGING;
    }

    else if(charging_state == STATE_CV_CHARGING && batt_current <= CUTOFF_CURRENT_MA + BATTERY_THRESHOLD){

        if(cycle_count < MAX_CYCLES){
            charging_state= STATE_DISCHARGING;
        }
        else{
            charging_state = STATE_FINAL_DISCHARGE;
        }
        
    }

    else if(charging_state == STATE_DISCHARGING && batt_voltage <= MIN_VOLTAGE_MV + BATTERY_THRESHOLD) {
        charging_state= STATE_CC_CHARGING;      
    }
  
    else{
        charging_state= STATE_ERROR;
    }
}

void CC_CV_ControlCharging(uint8 slot_id){
    
    CC_CV_UpdateChargingState(slot_id){
    
    switch(charging_state){
        case STATE_TRICKLE_CHARGE:
            DAC_fastWrite(TRICKLE_CHARGE_CURRENT_MA * 1000);
            printf("[CC-CV] Trickle Charging: Slot %d at %d mA.\n", slot_id, TRICKLE_CHARGE_CURRENT_MA);
            break;
        
        case STATE_PRE_CHARGE:
            DAC_fastWrite(PRE_CHARGE_CURRENT_MA * 1000);
            printf("[CC-CV] Pre Charging: Slot %d at %d mA.\n", slot_id, PRE_CHARGE_CURRENT_MA);
            break;
        
        case STATE_CC_CHARGING:
            DAC_fastWrite(CC_CURRENT_LIMIT_MA * 1000):
            printf("[CC-CV] CC Charging: Slot %d at %d mA.\n", slot_id, CC_CURRENT_LIMIT_MA);
            break;
        
        case STATE_CV_CHARGING:
            DAC_fastWrite(MAX_VOLTAGE_MV);
            printf("[CC-CV] CV Charging: Slot %d at %d mA.\n", slot_id, PRE_CHARGE_CURRENT_MA);
            break;
        default:
            break;
        }
    }
}