// Declare Libraries


#include <Stepper.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <MovingAverageFloat.h>
#include <Encoder.h>


// Declare variables and Instances

int P5Input_FunctionModeOff = 0;
int P6Input_FunctionModeDISTILL = 0;
int P7Input_FunctionModeFILTER = 0;
int P34Input_DripSensor = 0;
const int P36Output_SSR = 12;
int P38Input_AutoModePB = 0;
int P32Input_EncoderPB = 0;
int FunctionMode = 0;
int AutoMode = 0;
int EncoderPositionInt = 0;

float SSR_Setpoint_Percent = 0.0;
float SSROnStateTime = 0.0;
float SSROutputPercent = 0.0;
unsigned long SSRTimerCurrentTime;
unsigned long SSRTimerRunningTime;
unsigned long SSRTimerStartTime;
unsigned long CurrentTime;
unsigned long FunctionModeOFF_ONS_CurrentTime;
unsigned long AlarmStartTime;
float LastDripTime;
float PrevDripTime;
unsigned long DripSense_CaptureTime;
float AvgDripsSec = 0.0;
float DripPerSecond;
int AlarmTimerReady = 1;
float StalledDripCheck;

int FunctionModeOFF_ONS_Ready = 1;
unsigned long FunctionModeDistill_ONS_CurrentTime;
int FunctionModeDistill_ONS_Ready = 1;
unsigned long FunctionModeFilter_ONS_CurrentTime;
int FunctionModeFilter_ONS_Ready = 1;
unsigned long ScreenRefresh_ONS_CurrentTime;
int ScreenRefresh_ONS_Ready = 1;
unsigned long DripSense_ONS_CurrentTime;
int DripSense_ONS_Ready = 1;
unsigned long AutoMode_ONS_CurrentTime;
int AutoMode_ONS_Ready = 1;
unsigned long AutoLoop_ONS_CurrentTime;
int AutoLoop_ONS_Ready = 1;
float DripSetPoint = 0.0;

//  Define instances of Library classes.
Stepper BFGStepper(3060, 8, 10, 9, 11);  // Steps/rev, Pins 8,9,10,11
LiquidCrystal_I2C LCD1 = LiquidCrystal_I2C(0x27, 20, 4);
Encoder Encoder1(2, 3);  // Use pins 2,3 as these are interrupt pins.
MovingAverageFloat <4> AvgDripsSecond;


// SETUP LOOP 

void setup() {
  // Note to wire the Reset Input through a NC momentary PB to ground.  This hard resets controller.
  //  Serial.begin(9600); // remove after testing.
  Wire.begin();  // Begin I2C transfer on serial bus
  pinMode (5, INPUT_PULLUP);  // Function Mode Off
  pinMode (6, INPUT_PULLUP);  // Function Mode DISTILL
  pinMode (7, INPUT_PULLUP);  // Function Mode FILTER
  pinMode (9, OUTPUT);        // set Pin 9 as PUL (TO STEPPER)
  pinMode (8, OUTPUT);        // set Pin 8 as DIR (TO STEPPER)
  pinMode (32,INPUT_PULLUP);  // set pin 32 as AutoReseet Pushbutton (Encoder)
  pinMode (34, INPUT);		 //  Drip Sensor
  pinMode (36, OUTPUT);       // Output PWM to SSR
  pinMode (38, INPUT_PULLUP);  // Auto Mode Pushbutton
  pinMode (40, OUTPUT);      // Alarm Output to Relay or Device
 
  FunctionMode = 0;
  AutoMode = 0;
  LCD1.init();
  LCD1.backlight();
  Encoder1.write(0);
}

void loop() {

CurrentTime = millis();
P5Input_FunctionModeOff = !digitalRead(5);
P6Input_FunctionModeDISTILL = !digitalRead(6);
P7Input_FunctionModeFILTER = !digitalRead(7);
P38Input_AutoModePB = !digitalRead(38);
P34Input_DripSensor = digitalRead(34);
P32Input_EncoderPB = !digitalRead(32);  // Encoder PB

// ENCODER LOGIC
// Depending on the Function Mode, encoder values must be scaled to float and clamped accordingly.
// Ex. Heat setpoint is 0.0-100.0, Drip setpoint is 0.0-10.0, so each "tick" corresponds to 0.1 increments.
// The read function uses only integers, so use a left most clamp of 0, and a right most of 1000.
// Divide by 10 (for heating), and 100 for filter or drip setpoint.

EncoderPositionInt = Encoder1.read();  // Clamp Encoder between 0 and 1000
  if (EncoderPositionInt < 0) {
    EncoderPositionInt = 0;
    Encoder1.write(0);
  }
  if (EncoderPositionInt > 1000) {
    EncoderPositionInt = 1000;
    Encoder1.write(1000);
  }



//  FUNCTION MODE SELECT LOGIC ///

if ((P5Input_FunctionModeOff==1) && (FunctionModeOFF_ONS_Ready == 1)) {
  FunctionMode = 0;                      // Function = OFF
  AutoMode = 0;
  FunctionModeOFF_ONS_CurrentTime = millis(); 
  FunctionModeOFF_ONS_Ready = 0;
  LCD1.clear();
  LCD1.setCursor(0,0);
  LCD1.print("SYSTEM OFF SELECTED");
  LCD1.setCursor(0,2);
  LCD1.print("SELECT MODE TO");
  LCD1.setCursor(0,3);
  LCD1.print("CONTINUE ..");
}
if ((CurrentTime - FunctionModeOFF_ONS_CurrentTime >= 500) && (P5Input_FunctionModeOff == 0))  {
  FunctionModeOFF_ONS_Ready = 1;
}    

if ((P6Input_FunctionModeDISTILL==1) && (FunctionModeDistill_ONS_Ready == 1)) {
  FunctionMode = 1;                      // Function = OFF
  AutoMode == 0;
  LCD1.clear();
  LCD1.setCursor(0,0);
  LCD1.print("DISTILL");
  LCD1.setCursor(11,0);
  LCD1.print("Mode:");
  LCD1.setCursor(0,1);
  LCD1.print("Avg Drip Rate:");
  LCD1.setCursor(18,1);
  LCD1.print("/s");
  LCD1.setCursor(0,2);
  LCD1.print("Last:");
  LCD1.setCursor(8,2);
  LCD1.print("sec");
  LCD1.setCursor(14,2);
  LCD1.print("SP:");
  LCD1.setCursor(0,3);
  LCD1.print("SSR Out:");
  FunctionModeDistill_ONS_Ready = 0;
  FunctionModeDistill_ONS_CurrentTime = millis(); 
}

if ((CurrentTime - FunctionModeDistill_ONS_CurrentTime >= 500) && (P6Input_FunctionModeDISTILL == 0))  {
  FunctionModeDistill_ONS_Ready = 1;
}

if ((P7Input_FunctionModeFILTER==1) && (FunctionModeFilter_ONS_Ready == 1)) {
  FunctionMode = 2;                      // Function = OFF
  AutoMode = 0;
  LCD1.clear();
  LCD1.setCursor(0,0);
  LCD1.print("FILTER");
  LCD1.setCursor(11,0);
  LCD1.print("Mode:");
  LCD1.setCursor(0,1);
  LCD1.print("Avg Drip Rate:");
  LCD1.setCursor(18,1);
  LCD1.print("/s");
  LCD1.setCursor(0,2);
  LCD1.print("Last:");
  LCD1.setCursor(8,2);
  LCD1.print("sec");
  LCD1.setCursor(13,2);
  LCD1.print("SP:");
  FunctionModeFilter_ONS_Ready = 0;
  FunctionModeFilter_ONS_CurrentTime = millis(); 
}

if ((CurrentTime - FunctionModeFilter_ONS_CurrentTime >= 500) && (P7Input_FunctionModeFILTER == 0))  {
  FunctionModeFilter_ONS_Ready = 1;
}


//  DISTILL MODE LOGIC (MANUAL OR AUTO MODE)
if ((FunctionMode==1) && (AutoMode==0)) {   // Distill Mode in MANUAL
  SSR_Setpoint_Percent = (EncoderPositionInt / 10.0);  // ** SSR Setpoint does not require the Encoder PB in manual mode
}  
if ((FunctionMode==1) && (AutoMode==1)) {   // Distill Mode in AUTO
// SSR_SetPoint_Percent now set by logic, not by encoder (i.e. EncoderPositionInt)  
// The encoder position is set by the AutoMode ONS to the Avg Drip rate for Setpoint.
// This can be adjusted for a new Avg drip rate setpoint.
// Check is SSR_Setpoint_Percent altered when the switch to auto mode is made??  Don't think so.. proceed.
  //  Set a timed continous loop here to check the SP to drip rate varience and adjust as required.
  DripSetPoint = (EncoderPositionInt/100.0);
    
  if (AutoLoop_ONS_Ready == 1) {
    AutoLoop_ONS_Ready = 0;
    AutoLoop_ONS_CurrentTime = millis();
    if ((DripSetPoint - AvgDripsSecond.get()) > 0.2) {
      SSR_Setpoint_Percent = (SSR_Setpoint_Percent + 0.5);
    }
    if ((DripSetPoint - AvgDripsSecond.get()) < -0.2) {
      SSR_Setpoint_Percent = (SSR_Setpoint_Percent - 0.5);
    }
    if (SSR_Setpoint_Percent <=0) {
      SSR_Setpoint_Percent = 0.0;
    }
    if (SSR_Setpoint_Percent >=100) {
      SSR_Setpoint_Percent = 100.0;
    }
  }
    if ((CurrentTime - AutoLoop_ONS_CurrentTime) >=10000) {
      AutoLoop_ONS_Ready = 1;
    }
  

}  
  // Logic To Handle the SSR output
  SSRTimerCurrentTime = millis();
  SSRTimerRunningTime = (SSRTimerCurrentTime - SSRTimerStartTime);
  SSROnStateTime = (SSR_Setpoint_Percent/100.0)*(1670.0);   // Max of 1.67 second period.
  SSROutputPercent = SSR_Setpoint_Percent;
  if (SSRTimerRunningTime <= SSROnStateTime)  {
    digitalWrite (P36Output_SSR, HIGH);}
  else {
    digitalWrite (P36Output_SSR, LOW);
  }
// If duty cyle is expired, reset
if (SSRTimerRunningTime > 1670)  {
  SSRTimerStartTime = millis();
}



  //  Refresh the screen every XXX ms to save resources
  //  Use if statements on the print statements if any differences in Functionmode.
if ((ScreenRefresh_ONS_Ready == 1) && (FunctionMode==1) || (FunctionMode==2)) {
  ScreenRefresh_ONS_CurrentTime = millis(); 
  ScreenRefresh_ONS_Ready = 0;
  if (AutoMode == 1) {
    LCD1.setCursor(16,0);
    LCD1.print("AUTO"); }
  else {
    LCD1.setCursor(16,0);
    LCD1.print("MAN ");
  }
  LCD1.setCursor(15,1);
  LCD1.print(AvgDripsSecond.get(), 1);
  LCD1.setCursor(5,2);
  LCD1.print(LastDripTime, 1);
  LCD1.setCursor(17,2);
  LCD1.print(DripSetPoint, 1);
  LCD1.setCursor(10,3);
  
  if (FunctionMode==1) {
  LCD1.print(SSROutputPercent, 1);
  LCD1.setCursor(15,3);
  LCD1.print("%");
  }
  }
if ((CurrentTime - ScreenRefresh_ONS_CurrentTime >= 500))   {
  ScreenRefresh_ONS_Ready = 1;
}


// DRIP SENSOR LOGIC - Runs continuously on a oneshot regardless of function mode.
if ((P34Input_DripSensor==1) && (DripSense_ONS_Ready == 1)) {
  DripSense_ONS_CurrentTime = millis();
  DripSense_ONS_Ready = 0;
  LastDripTime = (CurrentTime - DripSense_CaptureTime) / 1000.0;
    if (LastDripTime >= 9.0)  {
    LastDripTime = 9.0;}
  DripPerSecond = (1.0 / LastDripTime);
  AvgDripsSecond.add(DripPerSecond);
  DripSense_CaptureTime = millis();
}
if ((CurrentTime - DripSense_ONS_CurrentTime >= 300) && (P34Input_DripSensor == 0))  {
  DripSense_ONS_Ready = 1;
  /// ******* NEED TO TEST ON REAL HARDWARE - LOWER THIS TIMER AS MUCH AS POSSIBLE ******
}


// AUTO MODE LOGIC - Verify a valid Function Mode and ONS Auto mode.
// Capture the existing drip rate for either Distill or Filter Mode.
if ((P38Input_AutoModePB == 1)  && (AutoMode_ONS_Ready == 1)) {
  AutoMode_ONS_CurrentTime  = millis();
  AutoMode_ONS_Ready = 0;
  if (AutoMode == 0) {
    DripSetPoint = AvgDripsSecond.get();
    AutoMode = 1;
    Encoder1.write((AvgDripsSecond.get() * 100));
  }
  else {
    EncoderPositionInt = (SSR_Setpoint_Percent * 10.0);
    Encoder1.write(EncoderPositionInt);
    DripSetPoint = 0.0;
    AutoMode = 0;
  }
}
if ((CurrentTime - AutoMode_ONS_CurrentTime >= 1000) && (P38Input_AutoModePB == 0))  {
  AutoMode_ONS_Ready = 1;
}

    // ALARM LOGIC - OUTPUT TO HORN RELAY OR OTHER DEVICE
    // What do we want to trip an alarm?  High last drip for xx secs? High SSR output?
    if (((FunctionMode==1) || (FunctionMode==2)) && (AutoMode==1)) {
      if (AlarmTimerReady == 1) {
        StalledDripCheck = LastDripTime;
        AlarmStartTime = millis();
        AlarmTimerReady = 0;
      }
      if (StalledDripCheck == LastDripTime) {
        if (CurrentTime - AlarmStartTime >= 30000)  {
          digitalWrite (40, HIGH);  // Alarm Output
        }   
        } else {
              digitalWrite (40, LOW);
              AlarmTimerReady = 1;
        }

    } else {
        digitalWrite (40, LOW);
        AlarmTimerReady = 1;
    }
}