// Define the software version
const char* SW_VERSION = "Slave SW 2.0.4 V2 Loopstart";
    //Gen sensing A5 send to python
    //variable naming clarified and some performance improvements by VS Code


// HW VARIANTS AND GENERATORS
//*********************************************

//#define V1
#define V2
//#define Diesel
//#define Remotestart
#define Loopstart

// ANALOG SIGNALS HANDLING
//*********************************************

const int V_in = A0; //Battery voltage
const int I_ref = A1; // Reference voltage of Hall current sensors 2,5V
const int I_1 = A2; // DC currentCurrentFactorDC sensor high range
const int I_2_low = A3; // DC current sensor low range (output multiplied by 50/Proto V1 or by 20/Proto V2)
const int GenOn = A5; //Read generator sensing relay 0=AC detected
const int NTC2 = A6; // NTC voltage

// Pins different in V1 and V2
const int I_3_low = A4; // AC current sensor 2 low range, multiplied by 2- THIS is used
  //In V1 this is not used, connected to connector via 1k resistor
const int I_4 = A7; // AC current sensor 2 
  //In V1 this is 48V measurement, normally at 0 V

// Current and voltage averaging is done in 3 phases; 20ms, 500ms, user defined e.g. 5 seconds
//************************************************************************************************

// 20ms sampling period will read analog interfaces
//int result_I1; //current high range
//int result_I2; //crrent low range
int Voltage = 0;
int CurrentDC = 1;
int CurrentAC = 1;
int PowerDC = 1;
long CurrentACpp = 1;
int GenStatus = 1; // Gen input

int CountCurrent = 1;
long SumCurrentDC = 0;    // store sum value here
long SumCurrentAC = 0;    // store sum value here

//500ms cycle between 20ms samples
unsigned long oldMillis = 0; //This is period start time
unsigned int Interval2 = 500; //This is a period length

//N sec. averaging intervall defined by user
unsigned long previousMillis = 0; //This is period start time
int Interval = 5; //This is a period length in seconds used for Graph. Can be set via UI.

 
// Below Current multiplier to trigger high power inverter On
// Python calculates the Current value based on AD value as follows: Current1*0.674/50 
// AD value multiplier and 50 divider due to 50 times anplifier in control board
// As invTrigger is given by user in Watts, we divide alvo by average voltage 13V
//long CurrentFactorDC = (-1 / (0.674/50) / 13);

#if defined(V2)
float CurrentFactorDC = 0.35; // Controller V2
const int I_low_amp = 20; //amplifier for I low current
#elif defined(V1)
float CurrentFactorDC = 0.15; // Controller V1
const int I_low_amp = 50;
#endif

// AC current probe gives xx V output at nominal current. E.g. 0,625 at 50A -->  0,0125 V/A
// if there are 4 rounds on current wire on probe, it will give 0,05 V/A
// Arduino ADC reference is 4,5V --> One bit will mean 4,5mV
//float CurrentFactorAC = (45 / 375 / 2 * 0.707 * 230);
float CurrentFactorAC = 2.99;

//MESSAGES FROM RASPBERRY
//****************************************************
const byte numChars = 32;
char receivedChars[numChars];
bool newData = false;
char messageFromPC[32] = {0};
unsigned int integerFromPC = 0;

//INVERTER CONTROLS
//****************************************************
unsigned int Load = 1; //User defines if Inverter1 is on (Load=1)
unsigned int HiPower = 0; //User defines if Inverter12 is on (HiPower=1)
unsigned int Power = 0; //Defines if Inverter12 is on (Power=1)

long InvStart = 4000;  //delay to wait for inverter2 to start, before Inverter select is activated
long InvHold = 10000; //delay in ms to keep high power inverter active after current goes back tolow
long InvTriggerAC = 200; //Current to activate high power inverter, this is set in Wats by user
signed int InvTriggerDC = -200; //Current to activate high power inverter, this is set in Wats by user

//static enum { PowerOff, LowPower, PowerUp, HighPower, Charge } state = PowerOff;

// ENGINE CONTROL
//****************************************************
unsigned int ChargeCommand = 0; //Generator started when 1

unsigned int Cranktime = 6000; //Time to keep starter on
unsigned int Glowtime = 6000; //Time to keep glow plug on

unsigned int StartOn = 3000; //Time to keep remotestart button on
unsigned int StopOn = 3000; //Time to keep remote stop button on

//Relay output pin definitions, 1 = relay active
#if defined(V1) 
const byte Inverter1 = 2, Inverter2 = 3, InverterSel = 4, PowerRelay = 7, StartRelay = 6, SolenoidRelay = 5, GlowRelay = 8, GenRelay = 12; //Output pins, GenRelay not used
#elif defined (V2)
const byte Inverter1 = 2, Inverter2 = 3, InverterSel = 4, PowerRelay = 7, StartRelay = 6, SolenoidRelay = 5, GenRelay = 8; //Output pins GlowRelay = 8 in proto 1
const byte GreenLed = 12,  RedLed = 13, Button = 11;
#endif



//SETUP
//*****************************************************
void setup()
{

  delay(2000); // let Raspberry USB settle

  Serial.begin(9600);
  unsigned long start = millis();
  while (!Serial && millis() - start < 5000) delay(50);
  if (Serial) Serial.println(SW_VERSION);

    analogReference(EXTERNAL); 
    pinMode(A5, INPUT_PULLUP); //Read generator sensing relay 
    pinMode(2, OUTPUT);
    pinMode(3, OUTPUT);    
    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);
    pinMode(6, OUTPUT);
    pinMode(7, OUTPUT);
    pinMode(8, OUTPUT); //Generator connect
    pinMode(12, OUTPUT);
    pinMode(13, OUTPUT);
    pinMode(11, INPUT);
    digitalWrite(2, LOW);
    digitalWrite(3, LOW);      
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);    
    digitalWrite(6, LOW);
    digitalWrite(7, LOW);
    digitalWrite(8, LOW);
    digitalWrite(12, HIGH);
    digitalWrite(13, LOW);
} 

#if defined(Remotestart) & defined(V2)
#include "RemoteMotorControl_V2.h"

#elif defined(Remotestart) & defined(V1)
#include "RemoteMotorControl_V1.h"

#elif defined(Loopstart) & defined(V2) 
#include "LoopMotorControl_V2.h"

#elif defined(Diesel) & defined(V1)
#include "DieselMotorControl_V1.h" 

#else
#error "Unsupported HW"
#endif

// MAIN LOOP
//****************************************************************
//****************************************************************
void loop()
{

     //Check if there is new Command data from Raspi
    recvWithStartEndMarkers();

      if (newData == true)
      {
          parseData();      //split data to string, integer, float

          if (strcmp(messageFromPC, "Charge") == 0)
          {
            ChargeCommand = integerFromPC;
            Serial.print(F("ChargeCommand "));
            Serial.println(ChargeCommand);
          }
                    
          else if (strcmp(messageFromPC, "Load") == 0)
          {
            Load = integerFromPC;
            Serial.print(F("Load --> "));
            Serial.println(Load);
          }
                              
          else if (strcmp(messageFromPC, "HiPower") == 0)
          {
            HiPower = integerFromPC;
            Serial.print(F("HiPower --> "));
            Serial.println(HiPower);
          }
          
          //else if (strcmp(messageFromPC, "Servo") == 0)
  
          else if (strcmp(messageFromPC, "GraphT") == 0)
          {
            Interval = (integerFromPC);

          }  
          else if (strcmp(messageFromPC, "InvTrigger") == 0)
          {
            InvTriggerAC = integerFromPC;
            InvTriggerDC = (integerFromPC * (-1));
            Serial.print(InvTriggerAC);
          }
          else if (strcmp(messageFromPC, "InvStart") == 0)
          {
            InvStart = (integerFromPC * 1000);
          }            
          else if (strcmp(messageFromPC, "InvHold") == 0)
          {
            InvHold = (integerFromPC * 60000);
          }  
          else if (strcmp(messageFromPC, "Cranktime") == 0)
          {
            Cranktime = (integerFromPC * 1000);
          }            
          else if (strcmp(messageFromPC, "Glowtime") == 0)
          {
            Glowtime = (integerFromPC * 1000);
          }           
          newData = false;
         
      }  
    MotorState();


    // Analog values are avaraged in 3 phases: After every Interval2 (500ms), averaging is done over Period (20ms)
    // When user defined Interval (5...10sec) is over,average over this period is calculated
    if ((millis() - oldMillis) >= Interval2)
    {
      //Read currentS and voltage on 20ms long sample
      CurrentDC = getCurr();
      
      //This will define state of inverters based on currents and is time critical function
      //because it is good to start High power inverter asap if load gets high
      InverterState();
      
      oldMillis = millis(); // Remember the time 
      CountCurrent = (CountCurrent + 1);
      SumCurrentDC = (SumCurrentDC + CurrentDC);
      SumCurrentAC = (SumCurrentAC + CurrentAC);

      //Serial.println(SumCurrentDC / CountCurrent);
    }
  
 
    //If user defined period between readings is full, calculate cumulative value
    if ((millis() - previousMillis)/1000 >= Interval)
    {
      CurrentDC = (SumCurrentDC / CountCurrent);
      CurrentAC = (SumCurrentAC / CountCurrent);
      GenStatus = digitalRead(GenOn);

      Serial.print("Graph1 ");
      Serial.print(Voltage);
      Serial.print(" ");
      Serial.print(CurrentDC);
      Serial.print(" ");
      Serial.println(GenStatus);

      //Serial.print(" ");
      //Serial.println(CurrentAC);
           
      previousMillis = millis(); // Remember the time
      CountCurrent = 0;
      SumCurrentDC = 0;
      SumCurrentAC = 0;
    }        

 
}
//****************************************************************************************

int InverterState()
{ 
  
  static enum { PowerOff, LowPower, PowerUp, HighPower, Charge } state = PowerOff;
  static int GenStatus; // Generator sensing input
  GenStatus = digitalRead(GenOn);
  static uint32_t InverterTimer = 1000; //Timer for inverter state changes
  //Serial.print(InvTriggerDC);
  //Serial.print("  ");
  //Serial.print(CurrentDC);
  //Serial.print("State ");
  //Serial.println(state);
  Power = 0;
  PowerDC = (CurrentDC * CurrentFactorDC);
  //Serial.print("DC power ");
  //Serial.print(PowerDC);
  //Serial.print("  Inv trigger  ");
  //Serial.println(InvTriggerDC);
  
  if (PowerDC <= InvTriggerDC) //Load current over trigger limit
  {
      Power = 1;
  }
  if (CurrentAC >= InvTriggerAC) //Load current over trigger limit
  {
      Power = 1;
  }
  if (HiPower == 1) //High power command from Raspi
  {
      Power = 1;
  }
  if (GenStatus == 0) // AC voltage detected from generator
  {
      Power = 1;

      //Serial.print("GenStatus: "); 
      //Serial.println(digitalRead(GenStatus));
  }

  switch (state)
    {
            
    case PowerOff:     
      if (Load == 1) //Load command to activate Inverter1
      {
        state = LowPower;
        Serial.println("Go to LowPower");
        digitalWrite(Inverter1, 1); 
        digitalWrite(Inverter2, 0); 
        break;
      }      
      break;     

    case LowPower:
      if (Load == 0) //Load command to deactivate Inverter1
      {       
        state = PowerOff;
        Serial.println("Go to PowerOff1");
        digitalWrite(Inverter1, 0); 
        digitalWrite(Inverter2, 0); 
        break;
      }


      if (GenStatus == 0) //AC voltage detected from generator
      {
        state = PowerUp;
        Serial.println("PowerUpCharge2");
        InverterTimer = millis();
        digitalWrite(Inverter1, 1); 
        digitalWrite(Inverter2, 1); 
        break;
      }
      
      //Load current over trigger limit
      if (Power == 1)
      {
        state = PowerUp;
        Serial.println("PowerUp");
        InverterTimer = millis();
        digitalWrite(Inverter2, 1); 
        break;
      }  
              
      if (ChargeCommand == 1) //Charge command
      {
        state = PowerUp;
        //Serial.println("PowerUpCharge1");
        InverterTimer = millis();
        digitalWrite(Inverter1, 1); 
        digitalWrite(Inverter2, 1); 
        break;
      }
      break;

    // Delay after inverter power on
    case PowerUp:
      if ((millis() - InverterTimer) > InvStart)
      {
        state = HighPower;
        Serial.println("HighPower");
        InverterTimer = millis();
        digitalWrite(InverterSel, 1);
        if (GenStatus == 0)
        {
          digitalWrite(GenRelay, 1); 
          //Serial.println("GenConnect relay ON1");
        }
        break;   
      }
      break;
    
    //Stay here until current is < InTrigger over invHold time  
    case HighPower:
      //Serial.println(PowerDC);   
     
      if (Load == 0) //Load command to deactivate Inverter1
      {       
        state = PowerOff;
        Serial.println("Go to PowerOff2");
        digitalWrite(Inverter1, 0); 
        digitalWrite(Inverter2, 0); 
        digitalWrite(InverterSel, 0);
        digitalWrite(GenRelay, 0);
        break;
      }     
      if (GenStatus == 0) //AC voltage detected from generator
        {
         digitalWrite(GenRelay, 1); //Activate solenoid to connect AC input
         InverterTimer = millis(); 
         //Serial.println("GenConnect relay ON2");
         break;  
        } 
                   
      //Discharge currect remains
      if (Power == 1)
         {
           InverterTimer = millis();  
         }
      else if (ChargeCommand == 1) //Charging ongoing
         {
          //Serial.println(ChargeCommand);
          InverterTimer = millis();
         }
      
      if ((millis() - InverterTimer) > (InvHold))
      {
        state = LowPower;
        Serial.println("Go to LowPower");
        digitalWrite(InverterSel, 0);
        digitalWrite(Inverter1, 1);
        digitalWrite(Inverter2, 0);
        digitalWrite(GenRelay, 0);
        break;
      }
        //InverterTimer = millis();
        break;   

    //default:
      //state = LowPower;
      //break;
    }
  return state;
} 


//This reads current over 20ms and averages it, samples are needed because AC current is calculated
//****************************************************************
int getCurr()
{
    float result;
    int maxValue = 0;             // store max value here
    int minValue = 1024;          // store min value here
  
    int resultRef;
    int readValue;                 //value read from the sensor
    int readRef;                   //value read from the sensor
    long SumValue_I1 = 0;    // store sum value here
    long SumValue_I2 = 0;
    long SumValueRef = 0;
    long SumValueV = 0;
    long result_I1;
    long result_I2;

    long Period = 20;     // Time of samples in ms
    int Count = 0;                  // Nr of sanmples

    uint32_t start_time = millis();
    while ((millis() - start_time) < Period) //sample period
    {  
       //AC CURRENT MEASUREMENT TO FIND MIN AND MAX
       readValue = analogRead(I_3_low);
       //Serial.println(readValue);
       // see if you have a new maxValue
       if (readValue > maxValue) 
       { 
           /*record the maximum sensor value*/
           maxValue = readValue;
       }
       if (readValue < minValue) 
       {
           /*record the minimum sensor value*/
           minValue = readValue;
       }
        
      // add do sum
      SumValue_I1 = (SumValue_I1 + analogRead(I_1));
      SumValue_I2 = (SumValue_I2 + analogRead(I_2_low));
      SumValueRef = (SumValueRef + analogRead(I_ref));
      SumValueV = (SumValueV + analogRead(V_in));
      //Serial.println(analogRead(V_in));
      
      Count = (Count + 1); 
    }
    // average
    result_I1 = (SumValue_I1 / Count);
    result_I2 = (SumValue_I2 / Count);
    resultRef = (SumValueRef / Count);
    Voltage = (SumValueV / Count);

    CurrentAC = (maxValue - minValue) * CurrentFactorAC;
    //Serial.println(CurrentFactorAC);  
       
    //Proto 1: I_low_amp is 50, in V2 it is 20
    result_I1 = I_low_amp * (result_I1 - resultRef);
    result_I2 = (result_I2 - resultRef);
    
    //Serial.print(result_I1);
    //Serial.println(I_low_amp);
    //Serial.println(result_I2);
    
    if (fabs(result_I2) > 200)
    {
      return (result_I1);
    }
    else 
      return result_I2;
} 

// READ Receiving several characters from the Serial Monitor
//****************************************************************
void recvWithStartEndMarkers()
{
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;

    // if (Serial.available() > 0) {
    while (Serial.available() > 0 && newData == false)
    {
        rc = Serial.read();

        if (recvInProgress == true)
        {
            if (rc != endMarker)
            {
                receivedChars[ndx] = rc;
                ndx++;
                if (ndx >= numChars)
                {
                    ndx = numChars - 1;
                }
            }
            else
            {
                receivedChars[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }
        else if (rc == startMarker)
        {
            recvInProgress = true;
        }
    }
}


// Parse received data into String, Integer and Float
//****************************************************************

float floatFromPC = 0.0;
char recvChar;
char endMarker = '>';//----------------------------------------------------------------

void parseData()
{
    // split the data into its parts

    char *strtokIndx; // this is used by strtok() as an index

    strtokIndx = strtok(receivedChars, " "); // get the first part - the string
    strcpy(messageFromPC, strtokIndx);       // copy it to messageFromPC

    strtokIndx = strtok(NULL, ",");   // this continues where the previous call left off
    integerFromPC = atoi(strtokIndx); // convert this part to an integer

    strtokIndx = strtok(NULL, ",");
    floatFromPC = atof(strtokIndx); // convert this part to a float
}