#include <Wire.h>

#include <EEPROM.h>

#include <LiquidCrystal_I2C.h> //FARNELL  2748653

int speed = 1;       //stepper speed
int Lval = 0;     
int Tval = 0;     
int Auto = 5;         //auto select switch 
int AutoLed = 6;      //auto select led
int LoadMax = 4;      //load max switch
int LoadMin = 3;      //load min switch
int LoadDir = 8;      //load stepper direction
int LoadStep = 7;     //load step 
int RemLoadPos = A7;
int MagV1 = A0;       //mag V1
int MagV2 = A1;       //mag V2
int TuneMax = 2;      //tune max switch
int TuneMin = 13;      //tune min switch
int TuneDir = 12;     //tune stepper direction
int TuneStep = 11;    //tune step
int RemTunePos = A6;
int PhaseV1 = A2;     //phase V1
int PhaseV2 = A3;     //phase V2

int reset = 10;
int enable =9;      //load and tune stepper enable
boolean (Mag)=false;  //mag flag
boolean (Phase)=false;  //phase flag
int CapMax =200;
int CapMin =0;
int Counter;        //EEPROM address counter
int LoadHi;         //EEPROM
int LoadLow;        //EEPROM
int LoadStepCount;  //load step counter
int TuneHi;         //EEPROM
int TuneLow;        //EEPROM
int TuneStepCount;  //tune step counter
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 20, 4);

void setup() {
  // put your setup code here, to run once:
pinMode(LoadMax, INPUT_PULLUP);
pinMode(LoadMin, INPUT_PULLUP);
pinMode(LoadDir, OUTPUT);
pinMode(LoadStep, OUTPUT);
pinMode(RemLoadPos, INPUT);
pinMode(MagV1, INPUT);
pinMode(MagV2, INPUT);
pinMode(TuneMax, INPUT_PULLUP);
pinMode(TuneMin, INPUT_PULLUP);
pinMode(TuneDir, OUTPUT);
pinMode(TuneStep, OUTPUT);
pinMode(RemTunePos, INPUT);
pinMode(PhaseV1, INPUT); 
pinMode(PhaseV2, INPUT);
pinMode(reset, OUTPUT);
pinMode(enable, OUTPUT);
pinMode(Auto, INPUT_PULLUP);
pinMode(AutoLed, OUTPUT);
pinMode(SDA, INPUT);
pinMode(SCL, INPUT);

  lcd.init();
  lcd.backlight(); 
  lcd.setCursor(1, 2); 
  lcd.print("M1        P1"); 
  lcd.setCursor(1, 3);
  lcd.print("M2        P2");
  lcd.setCursor(2, 0); 
  lcd.print("LOAD      TUNE"); 
  lcd.setCursor(1, 1);
  lcd.print("C%        C%"); 

  UpdateLoad();
  UpdateTune();
  
 
  
Serial.begin(115200);

  }

void loop() 
{

if (digitalRead(Auto)==LOW)               //Test Auto switch if LOW do the match
{
   digitalWrite(enable, HIGH);
  AutoMatch();
  }

if (digitalRead(Auto)==HIGH)               //Test manual switch if high goto manual
{
    digitalWrite(enable, HIGH);
  RTB();
}
if ((MagV1 && MagV2 && PhaseV1 && PhaseV2) == 0)
RTB();

}


     
     
  
void AutoMatch()
{
  digitalWrite(AutoLed,HIGH);             //Turn on Auto led
  digitalWrite(enable, LOW);              //Enable Stepper motors
if (analogRead(MagV1)>analogRead(MagV2))  
{
  Mag=false;
  if (LoadStepCount < CapMax)
  {
  LoadCw();
  }  
  }
  
if (analogRead(MagV1)<analogRead(MagV2))
{
  Mag=false;
  if (LoadStepCount > CapMin)
  {
  LoadCcw();
  }  
}
 
if (analogRead(PhaseV1)>analogRead(PhaseV2))
{
  Phase=false;
  if (TuneStepCount < CapMax)
  {
  TuneCw(); 
  }
}
  
if (analogRead(PhaseV1)<analogRead(PhaseV2))
{
  Phase=false;
  if (TuneStepCount > CapMin)
  {
  TuneCcw(); 
  }
}

  if (analogRead(MagV1)==analogRead(MagV2))
  {Mag=true;}
  if (analogRead(PhaseV1)==analogRead(PhaseV2))
  {Phase=true;}
 if (Mag&&Phase)
 {
 digitalWrite(enable, HIGH);             //Disable Stepper motors}
 }
                                            //UPDATE DISPLAY
}

      
void LoadCw(){
   digitalWrite(LoadDir, LOW);
   digitalWrite(LoadStep, HIGH);
      delay(speed);
      digitalWrite(LoadStep,LOW);
      delay(speed);
       LoadStepCount = LoadStepCount + 1;
      UpdateLoad();                           //update display  
      }

void LoadCcw(){
   digitalWrite(LoadDir, HIGH);
   digitalWrite(LoadStep, HIGH);
      delay(speed);
      digitalWrite(LoadStep,LOW);      
      delay(speed);
       LoadStepCount = LoadStepCount - 1;
      UpdateLoad();                           //update display  
      }

void TuneCw(){
    digitalWrite(TuneDir, LOW);
    digitalWrite(TuneStep, HIGH);
      delay(speed);
      digitalWrite(TuneStep, LOW);      
      delay(speed);
      TuneStepCount = TuneStepCount + 1;
      UpdateTune();                           //update display  
      }

void TuneCcw(){
    digitalWrite(TuneDir, HIGH);
    digitalWrite(TuneStep, HIGH);
      delay(speed);
      digitalWrite(TuneStep, LOW);      
      delay(speed);
      TuneStepCount = TuneStepCount - 1;
      UpdateTune();                           //update display  
      }



void RTB(){
    digitalWrite(enable, LOW);              //Enable Stepper motors
while (LoadStepCount || TuneStepCount >0)
{
if (LoadStepCount > 0)
{LoadCcw();}
if (TuneStepCount > 0)
{TuneCcw();}
}
  digitalWrite(enable, HIGH);              //Disable Stepper motors
}
void Manual() {
  digitalWrite(AutoLed,LOW);             //Turn off Auto led
  digitalWrite(enable, LOW);              //Enable Stepper motors
  if (digitalRead(LoadMax)==LOW && LoadStepCount < CapMax  )  
{
 if (LoadStepCount >CapMin )
  LoadCw();
  }

 if (digitalRead(LoadMin)==LOW && LoadStepCount < CapMax  )  
{
 if (LoadStepCount >CapMin )
  LoadCcw();
  }

 if (digitalRead(TuneMax)==LOW && TuneStepCount < CapMax  )  
{
 if (TuneStepCount > CapMin )
  TuneCw();
  }

 if (digitalRead(TuneMin)==LOW && TuneStepCount < CapMax  )  
{
 if (TuneStepCount > CapMin )
  TuneCcw();
  } 
  }  

void UpdateLoad()
{
  lcd.setCursor(4, 1); // Set the cursor on the eigth column and first row.
  lcd.print(LoadStepCount); // Print the string "LOAD"
  lcd.print("   ");
  lcd.setCursor(4, 2); // Set the cursor on the eigth column and first row.
  lcd.print(analogRead(MagV1)); // Print the string "LOAD"
  lcd.print("   ");
  lcd.setCursor(4, 3); //Set the cursor on the eigth column and the second row 
  lcd.print(analogRead(MagV2));
  lcd.print("   ");
}

void UpdateTune()
{
  lcd.setCursor(14, 1); //Set the cursor on the eigth column and the second row 
  lcd.print(TuneStepCount);
  lcd.print("   ");
  lcd.setCursor(14, 2); // Set the cursor on the eigth column and first row.
  lcd.print(analogRead(PhaseV1)); // Print the string "LOAD"
  lcd.print("   ");
  lcd.setCursor(14, 3); //Set the cursor on the eigth column and the second row
  lcd.print(analogRead(PhaseV2));
  lcd.print("   ");
}