// INA_Command-Bistable_PWM-Control
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// BLOT Arthur 14/08/2023
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This code is a compilation of the code INA and the code that give command to the Bistable
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <Wire.h>
#include <Adafruit_INA219.h>

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// VARIABLE 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// INA
Adafruit_INA219 ina219WIND(0x40);   // Create an INA219 objectp
Adafruit_INA219 ina219SOLAR(0x41);  // Create an INA219 object
Adafruit_INA219 ina219IN(0x44);     // Create an INA219 object
Adafruit_INA219 ina219OUT(0x45);     // Create an INA219 object

// Read buttons
int Solar_PWR = 10;         // Change to the digital pin number connected to the solar power relay
int Green_PWR = 11;         // Change to the digital pin number connected to the wind power relay
int Wind_PWR = 12;          // Change to the digital pin number connected to the charge relay after the solar or wind battery
// int Wind_discharge = 9;     // Change to the digital pin number connected to the discharge relay after the wind battery
int Output_PWR = 2; // Change to the digital pin number connected to the ouput relay

// Button state
int Solar_State;
int Green_State;
int Wind_State;
// int Resistance_State;
int Out_State;

// WINDTURBINE AND LIGHT CONTROL
// Wind set up
int B1B = 3; // PWM Command
int B1A = 7; // Digital command
// Light set up
int A1B = 6; // PWM Command
int A1A = 5; // PWM Command

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// SETUP
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void setup() {
  Wire.begin(); // No argument, starting connection between Arduino and SDA/SCL track. 
  Serial.begin(115200); // INA baud rate.
  ClearShell();
  delay(1000);
  Serial.println("Hello Green");

  // Button state
  Solar_State = ButtonSetup(Solar_PWR, Solar_State); // Set pin as OUTPUT
  Green_State = ButtonSetup(Green_PWR, Green_State);
  Wind_State = ButtonSetup(Wind_PWR, Wind_State);
  Out_State = ButtonSetup(Output_PWR, Out_State);

  // INA
  ina219WIND.begin();
  ina219SOLAR.begin();
  ina219IN.begin();
  ina219OUT.begin();
  Serial.println("Initialization done");

  // You can adjust the calibration value to match your specific INA219 module
  ina219WIND.setCalibration_32V_2A ();  // 32V, 2A range
  ina219SOLAR.setCalibration_32V_2A ();  // 32V, 2A range
  ina219IN.setCalibration_32V_2A ();  // 32V, 2A range
  ina219OUT.setCalibration_32V_2A ();  // 32V, 2A range
  Serial.println("Calibration done");

  // Initialisation of the control motor driver
  InitControl();
  
  Serial.println("Ready to use");
  Serial.println("/////////////////////////////////////");
  Serial.print("\n");
  delay(1000); // Wait for 1 s before collecting datas. 
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// LOOP
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void loop() {
  // Starting date & time
  String THEDATE = ArduinoDateToDisplayDate(__DATE__);
  String THETIME = ArduinoElapsedTime(__TIME__);

  // DATA
  float shuntVoltage_mV_WIND = 0;
  float busVoltage_V_WIND = 0;
  float voltage_V_WIND = 0;
  float current_A_WIND = 0;
  float power_W_WIND = 0;

  float shuntVoltage_mV_SOLAR = 0;
  float busVoltage_V_SOLAR = 0;
  float voltage_V_SOLAR = 0;
  float current_A_SOLAR = 0;
  float power_W_SOLAR = 0;

  float shuntVoltage_mV_IN = 0;
  float busVoltage_V_IN = 0;
  float voltage_V_IN = 0;
  float current_A_IN = 0;
  float power_W_IN = 0;

  float shuntVoltage_mV_OUT = 0;
  float busVoltage_V_OUT = 0;
  float voltage_V_OUT = 0;
  float current_A_OUT = 0;
  float power_W_OUT = 0;
  
  // BUTTON STATE
  Serial.println(THEDATE+" "+THETIME+","+"PRODSS"+","+Solar_State+","+"NULL"); // PRODuction Solar State // 0 = charge, 1 = discharge
  Serial.println(THEDATE+" "+THETIME+","+"PRODGS"+","+Green_State+","+"NULL"); // PRODuction Green State 0 = solar, 1 = wind
  Serial.println(THEDATE+" "+THETIME+","+"PRODWS"+","+Wind_State+","+"NULL"); // PRODuction Wind State 0 = charge, 1 = discharge
  // Serial.println(THEDATE+" "+THETIME+","+"PRODDS"+","+Wind_discharge+","+"NULL"); // PRODuction Discharge State 0 = disconnected, 1 = discharge 
  Serial.println(THEDATE+" "+THETIME+","+"CONSS"+","+Out_State+","+"NULL"); // CONSumption State 0 = load on grid, 1 = renawable
  delay(100);

  // WIND
  shuntVoltage_mV_WIND = ina219WIND.getShuntVoltage_mV();
  busVoltage_V_WIND = ina219WIND.getBusVoltage_V();
  voltage_V_WIND = busVoltage_V_WIND + (shuntVoltage_mV_WIND / 1000);
  current_A_WIND =   0.001 * ina219WIND.getCurrent_mA();
  power_W_WIND = current_A_WIND * voltage_V_WIND;
  Serial.println(THEDATE+" "+THETIME+","+"PRODWV"+","+voltage_V_WIND+","+"NULL"); // PRODuction Wind Voltage
  Serial.println(THEDATE+" "+THETIME+","+"PRODWC"+","+current_A_WIND+","+"NULL"); // PRODuction Wind Current
  Serial.println(THEDATE+" "+THETIME+","+"PRODWP"+","+power_W_WIND+","+"NULL"); // PRODuction Wind Power
  delay(100);

  // SOLAR  
  shuntVoltage_mV_SOLAR = ina219SOLAR.getShuntVoltage_mV();
  busVoltage_V_SOLAR = ina219SOLAR.getBusVoltage_V();
  voltage_V_SOLAR = busVoltage_V_SOLAR + (shuntVoltage_mV_SOLAR / 1000);
  current_A_SOLAR = 0.001 * ina219SOLAR.getCurrent_mA();
  power_W_SOLAR = current_A_SOLAR * voltage_V_SOLAR;
  Serial.println(THEDATE+" "+THETIME+","+"PRODSV"+","+voltage_V_SOLAR+","+"NULL"); // PRODuction Solar Voltage
  Serial.println(THEDATE+" "+THETIME+","+"PRODSC"+","+current_A_SOLAR+","+"NULL"); // PRODuction Solar Current
  Serial.println(THEDATE+" "+THETIME+","+"PRODSP"+","+power_W_SOLAR+","+"NULL"); // PRODuction Solar Power
  delay(100);

  // IN  
  shuntVoltage_mV_IN = ina219IN.getShuntVoltage_mV();
  busVoltage_V_IN = ina219IN.getBusVoltage_V();
  voltage_V_IN = busVoltage_V_IN + (shuntVoltage_mV_IN / 1000);
  current_A_IN = 0.001 * ina219IN.getCurrent_mA();
  power_W_IN = current_A_IN * voltage_V_IN;
  Serial.println(THEDATE+" "+THETIME+","+"INPUTV"+","+voltage_V_IN+","+"NULL"); // INPUT Voltage
  Serial.println(THEDATE+" "+THETIME+","+"INPUTC"+","+current_A_IN+","+"NULL"); // INPUT Current
  Serial.println(THEDATE+" "+THETIME+","+"INPUTP"+","+power_W_IN+","+"NULL"); // INPUT Power
  delay(100);

  // OUT
  shuntVoltage_mV_OUT = ina219OUT.getShuntVoltage_mV();
  busVoltage_V_OUT = ina219OUT.getBusVoltage_V();
  voltage_V_OUT = busVoltage_V_OUT + (shuntVoltage_mV_OUT / 1000);
  current_A_OUT = 0.001 * ina219OUT.getCurrent_mA();
  power_W_OUT = current_A_OUT * voltage_V_OUT;
  Serial.println(THEDATE+" "+THETIME+","+"CONSV"+","+voltage_V_OUT+","+"NULL"); // CONSumption Voltage
  Serial.println(THEDATE+" "+THETIME+","+"CONSC"+","+current_A_OUT+","+"NULL"); // CONSumption Current
  Serial.println(THEDATE+" "+THETIME+","+"CONSP"+","+power_W_OUT+","+"NULL"); // CONSumption Power
  delay(100);

  // MODE SELECTION
  int cmd = Serial.parseInt();
  if (cmd != 0) {
    switch (cmd) {
      ////////////////
      // MANUAL MODE//
      ////////////////
      case 1: // Solar_State
        Solar_State = ButtonSwitch(Solar_PWR, Solar_State);
        break;
      case 2: // Wind_State
        Wind_State = ButtonSwitch(Wind_PWR, Wind_State);
        break;
      case 3: // Green_State
        Green_State = ButtonSwitch(Green_PWR, Green_State);
        break;
      case 4: // Out_State
        Out_State = ButtonSwitch(Output_PWR, Out_State);
        break;
      ///////////////////
      // AUTOMATIC MODE//
      ///////////////////
      case 5: // wind charge, solar charge, no load powered by the grid.
        if (voltage_V_WIND >= 13.9) { // Wind charge
          Wind_State = ButtonSwitch(Wind_PWR, Wind_State);
        }
        if (voltage_V_SOLAR >= 13.9) { // Solar charge
          Solar_State = ButtonSwitch(Solar_PWR, Solar_State);
        }
        if (voltage_V_OUT >= 20 && voltage_V_WIND < 13.9 && voltage_V_SOLAR < 13.9) { // out on renawable
          Out_State = ButtonSwitch(Output_PWR, Out_State);
        }
        break;
      case 6: // Wind charge, solar charge, load powered by the grid.
        if (voltage_V_WIND >= 13.9) { // Wind charge
          Wind_State = ButtonSwitch(Wind_PWR, Wind_State);
        }
        if (voltage_V_SOLAR >= 13.9) { // Solar charge
          Solar_State = ButtonSwitch(Solar_PWR, Solar_State);
        }
        if (voltage_V_OUT < 20 && voltage_V_WIND < 13.9 && voltage_V_SOLAR < 13.9) { // out on renawable
          Out_State = ButtonSwitch(Output_PWR, Out_State);
        }
        break; 
      case 7: // Wind charge, solar discharge, load by solar.
        // Set up
        if (voltage_V_WIND >= 13.9) { // Wind charge
          Wind_State = ButtonSwitch(Wind_PWR, Wind_State);
        }
        if (voltage_V_SOLAR >= 13.9) { // Solar charge
          Solar_State = ButtonSwitch(Solar_PWR, Solar_State);
        }
        if (voltage_V_OUT >= 20 && voltage_V_WIND < 13.9 && voltage_V_SOLAR < 13.9) { // out on renawable
          Out_State = ButtonSwitch(Output_PWR, Out_State);
        }
        // Both battery is charging, Output is on renawable energy.
        delay(100);
        Solar_State = ButtonSwitch(Solar_PWR, Solar_State);
        if (voltage_V_OUT < 20) {
          Green_State = ButtonSwitch(Green_PWR, Green_State);
        }
        // Solar battery is discharging and green state is taking the current to the output.
        break; 
      case 8: // Wind discharge, solar charge, load by wind.
        // Set up
        if (voltage_V_WIND >= 13.9) { // Wind charge
          Wind_State = ButtonSwitch(Wind_PWR, Wind_State);
        }
        if (voltage_V_SOLAR >= 13.9) { // Solar charge
          Solar_State = ButtonSwitch(Solar_PWR, Solar_State);
        }
        if (voltage_V_OUT >= 20 && voltage_V_WIND < 13.9 && voltage_V_SOLAR < 13.9) { // out on renawable
          Out_State = ButtonSwitch(Output_PWR, Out_State);
        }
        // Both battery is charging, Output is on renawable energy.
        delay(100);
        Wind_State = ButtonSwitch(Wind_PWR, Wind_State);
        if (voltage_V_OUT < 20) {
          Green_State = ButtonSwitch(Green_PWR, Green_State);
        }
        // Wind battery is discharging and green state is taking the current to the output.
        break;
      case 11:
        // Serial.println("You selected mode 11, there is no wind.");
        analogWrite(B1B,0); 
        break;
      case 12:
        // Serial.println("You selected mode 12, wind is very low.");
        analogWrite(B1B, 50); // Real desired output (wind turbine wasn't turning with less than 150/255 with 5 V / 0.1 A on stabilized alimentation.)
        break;
      case 13:
        // Serial.println("You selected mode 13, wind is low.");
        analogWrite(B1B, 100);
        break;
      case 14:
        // Serial.println("You selected mode 14, wind is medium.");
        analogWrite(B1B, 150); 
        break;
      case 15:
        // Serial.println("You selected mode 15, wind is high.");
        analogWrite(B1B, 180); 
        break;
      case 16:
        // Serial.println("You selected mode 16, wind is very high.");
        analogWrite(B1B, 190); 
        break;
      case 21:
        // Serial.println("You selected mode 21, there is no sun.");
        analogWrite(A1B,0); 
        break;
      case 22:
        // Serial.println("You selected mode 22, sun irradiation are very low.");
        analogWrite(A1B, 200); // Acceleration to launch the motor, strong input. 
        delay(100); // Short delay before slowing down
        analogWrite(A1B, 150); // Real desired output (wind turbine wasn't turning with less than 150/255 with 5 V / 0.1 A on stabilized alimentation.)
        break;
      case 23:
        // Serial.println("You selected mode 23, sun irradiation are low.");
        analogWrite(A1B, 200); // same as above
        delay(100);
        analogWrite(A1B, 160); 
        break;
      case 24:
        // Serial.println("You selected mode 24, sun irradiation are medium.");
        analogWrite(A1B, 200); // same as above
        delay(100);
        analogWrite(A1B, 170); 
        break;
      case 25:
        // Serial.println("You selected mode 25, sun irradiation are high.");
        analogWrite(A1B, 180); 
        break;
      case 26:
        // Serial.println("You selected mode 26, sun irradiation are very high.");
        analogWrite(A1B, 190); 
        break; 
      default:
        Serial.println("Wrong command");  
    }
  }
  delay(500);
  Serial.print("\n");
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// FUNCTION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
String ArduinoDateToDisplayDate(char const *time) {
  char s_month[5];
  int month, day, year;
  static const char month_names[] = "JanFebMarAprMayJunJulAugSepOctNovDec";

  sscanf(time, "%s %d %d", s_month, &day, &year);

  month = (strstr(month_names, s_month) - month_names) / 3 + 1;

  String monthText = month < 10 ? "0" + String(month) : String(month);
  String dayText = day < 10 ? "0" + String(day) : String(day);

  return String(year) + "-" + monthText + "-" + dayText;
}

String ArduinoElapsedTime(char const *time) {
  long milli = millis();  // Time elapsed since the start of the program

  int hour, minute, second;
  sscanf(time, "%d:%d:%d", &hour, &minute, &second);
  int sec = (second + (milli / 1000)) % 60;
  int min = (minute + ((second + (milli / 1000)) / 60)) % 60;
  int hr = (hour + (minute + ((second + (milli / 1000)) / 60)) / 60) % 24;

  String hourText = hr < 10 ? "0" + String(hr) : String(hr);
  String minuteText = min < 10 ? "0" + String(min) : String(min);
  String secondText = sec < 10 ? "0" + String(sec) : String(sec);

  return hourText + ":" + minuteText + ":" + secondText;
}

void ClearShell() {
  for (int i = 0; i<10; i++) {
    Serial.println("\n");
  }
}

int ButtonSetup(int PIN, int STATE) {
  pinMode(PIN, OUTPUT);
  digitalWrite(PIN, LOW);
  return STATE = 0; 
}

int ButtonSwitch(int PIN, int button_state) { // As the pin is set up on low at the begining 
  if (button_state == 0) {
    digitalWrite(PIN, HIGH);
    delay(100); 
    digitalWrite(PIN, LOW);
    delay(100);
    return button_state = 1; 
  }
  if (button_state == 1) {
    digitalWrite(PIN, HIGH);
    delay(100); 
    digitalWrite(PIN, LOW);
    delay(100);
    return button_state = 0; 
  }   
}

void InitControl() {
  // Wind
  pinMode(B1A, OUTPUT);
  digitalWrite(B1A, LOW);
  pinMode(B1B, OUTPUT);
  analogWrite(B1B,0);
  // Light
  pinMode(A1A, OUTPUT);
  digitalWrite(A1A, LOW);
  pinMode(A1B, OUTPUT);
  analogWrite(A1B,0); 
}