#include "../global_data/defines.h"
#include <Preferences.h>
#include <Elog.h>
#include "Wire.h"
#include "../global_data/global_data.h"


#ifdef USE_INA226
#include "INA226.h"
INA226 ina_sensor(0x40);
#else
#include <INA233.h>
INA233 ina_sensor(0x40);
#endif
#include <tools/log.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

extern Preferences prefs;
extern WaterData water_data;
extern ActiveErrors active_errors;
extern SensorData shunt_data;

// Calibration variables
float zero_value = 0.03; // Measured shunt voltage with nothing connected, used to fix measuring offset

void init_sensor(){
    ina_sensor.begin(33, 32);
    #ifdef USE_INA226
        ina_sensor.setMaxCurrentShunt(0.02, 4, false);
        ina_sensor.setBusVoltageConversionTime(7);
        ina_sensor.setShuntVoltageConversionTime(7);
        ina_sensor.setAverage(4);
    #else
        ina_sensor.reset();
        ina_sensor.setShuntVoltageConversionTime(conversion_time_8244uS);
        ina_sensor.setBusVoltageConversionTime(conversion_time_8244uS);
        ina_sensor.setAveragingMode(averages_128);
    #endif
}

void read_sensor_task(void* parameter)
{
    while (true) {
        // Get Values from sensor

        String chip_id = ina_sensor.get_device_model();
        LOG(DEBUG, "Chip Model: %s", chip_id.c_str());

        float bus_voltage = ina_sensor.getBusVoltage();
        float shunt_voltage = ina_sensor.getShuntVoltage_mV() - zero_value;

        LOG(DEBUG, "RAW Shunt voltage: %F mV", ina_sensor.getShuntVoltage_mV());

        float shunt_current = shunt_voltage / RESISTOR_VALUE;
        
        // Get values from storage
        float sensor_range = prefs.getFloat(level_sensor_range_key, 200);
        float max_water_level = prefs.getFloat(water_level_max_key, sensor_range);
        float min_water_level = prefs.getFloat(water_level_min_key, 0);
        float max_liters = prefs.getFloat(water_volume_key, 10000.);
        
        float mA_per_cm = (20. - 4.) / (sensor_range);

        // Get mA over 0cm/4mA for max/min water level
        float min_water_level_mA_over_zero = (min_water_level * mA_per_cm);
        float max_water_level_mA_over_zero = (max_water_level * mA_per_cm);

        // Levels which represent raw sensor value, with 4mA added
        float min_water_level_mA = 4 + min_water_level_mA_over_zero;
        float max_water_level_mA = 4 + max_water_level_mA_over_zero;

        LOG(DEBUG, "max_water_level_mA: %F", max_water_level_mA);
        LOG(DEBUG, "min_water_level_mA_over_zero: %F", min_water_level_mA_over_zero);

        // Current over the 0 level of the water
        float shunt_current_over_zero = shunt_current - min_water_level_mA;

        // cm over zero water level
        float cm_over_zero = shunt_current_over_zero / mA_per_cm;

        // Raw unrounded percentage in decimal
        float percentage_raw = (shunt_current_over_zero / (max_water_level_mA_over_zero - min_water_level_mA_over_zero));
        float percentage_rounded = round(percentage_raw * 10000) / 100;

        // Tank volume in liters
        float liters_raw = max_liters * percentage_raw;
        int liters = round(liters_raw);

        active_errors.current_low = shunt_current < 3.8;
        active_errors.current_high = shunt_current > 20.2;
        active_errors.voltage_low = bus_voltage < 23;
        active_errors.voltage_high = bus_voltage > 25;
        LOG(DEBUG, "Shunt current: %F", shunt_current);
        LOG(DEBUG, "Shunt voltage: %F", shunt_voltage);
        LOG(DEBUG, "Bus voltage: %F", bus_voltage);
        LOG(DEBUG, "cm_over_zero: %F", cm_over_zero);

        shunt_data.bus_voltage =  bus_voltage;
        shunt_data.shunt_voltage = shunt_voltage;
        shunt_data.shunt_current = shunt_current;

        water_data.level = cm_over_zero;
        water_data.liters = liters;
        water_data.percentage = percentage_rounded;

        delay(20000);
    }
}