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


#ifdef USE_INA226s
INA226 ina_sensor(0x40);
#else
INA233 ina_sensor(0x40);
#endif

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.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

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

        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.verbose("max_water_level_mA: %F", max_water_level_mA);
        Log.verbose("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*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.verbose("Shunt current: %F", shunt_current);
        Log.verbose("Shunt voltage: %F", shunt_voltage);
        Log.verbose("Bus voltage: %F", bus_voltage);
        Log.verbose("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);
    }
}