///////////////////////////////////////////////
//
//  XY Micro Combined Machine
//
//  Machine: 007 3018-400 Oil
//
//  08/08/2023
//
//  Encoder, cleaning, testing, display implemented,encoder debouncing,
//  inverse pump speed, pump motor speed ramp up, manual cycle signal
//  Testing encoder switch, motor control
//
//  Pin 2 - encoder CLK (number of forward steps)
//  Pin 3 - encoder DT
//  Pin 4 - GRBL M3 (start pump cycle)
//  Pin 5 - GRBL M4 (currently not used)
//  Pin 6 - encoder SW  (manual cycle)
//  Pin 7 - GRBL M9  (hopper feeder control)
//  Pin 8 - GRBL Cycle (pump cycle complete)
//  Pin 9 - motor output
//  Pin 10 - test SW in (start single pump cycle for testing)
//  Pin 11 - pump a4988 step
//  Pin 12 - pump a4988 dir
//  Pin A0 - potentiometer  (currently not used)
//  Pin A1 - cleaning in (spin pump stepper when ON)
//  Pin A2 - solenoid output  (hopper feeder)
//  Pin A4 - I2C SDA
//  Pin A5 - I2C SCL
//
///////////////////////////////////////////////

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#define TPWM 1000.0                               // PWM period in us
#define SIG_DURATION 100                          // Duration of Cycle signal to GRBL in ms
#define DEFAULT_NUM_STEPS 9900                    // Default number of pump forward steps
#define MIN_NUM_STEPS 1000                        // Minimal number of pump forward steps
#define MAX_NUM_STEPS 30000                       // Maximal number of pump forward steps
#define STEPPER_SPEED 30.0                        // Step delay in us
#define STEPPER_SPEED_CLEAN 30.0                  // Step delay in us for high speed
#define STEPPER_SPEED_RETRACT 30.0                // Step delay in us for reverse speed
#define STEPPER_SPEED_DELTA 20                    // Delta time for stepper speed ramp up
#define STEPPER_SPEED_MAXDELAY 100.0              // Start value of step delay
#define STEPS_BACK 250                            // Number of reverse steps for pump
#define WAIT_TIME_FINAL 3000                      // Waiting time in us (after cw rotation)
#define WAIT_TIME_LOOP  3000                      // Waiting time in us (after ccw rotation)
#define ENC_MULTI_STEP  100                       // Number of pump steps per encoder step
#define DEBOUNCE_DELAY_MS 10                      // Delay time for debouncing
#define GRAVITY_DELAY_MS 500                      // Gravity and activation delay time
#define MOTOR_DELAY 3                             // Duration of motor operation in seconds, with resoution of 0.5 seconds

const byte ledPin = LED_BUILTIN;                  //
const byte encClk = 2;                            // Encoder Clk
const byte encDt = 3;                             // Encoder Data
const byte grblM3 = 4;                            // GRBL M3 (input)
const byte grblM4 = 5;                            // GRBL M4 (input)
const byte encSw = 6;                             // Encoder switch
const byte grblM9 = 7;                            // GRBL M9 (input)
const byte sigOut = 8;                            // Signal when procedure complete (GRBL CYCLE)
const byte motRel = 9;                            // Motor relay
const byte testIn = 10;                           // Test input
const byte potIn = A0;                            // Potentiometer
const byte cleanIn = A1;                          // Clean input
const byte solRel = A2;                           // Solenoid relay

// I2C display
LiquidCrystal_I2C lcd(0x27, 16, 2);               // I2C address 0x27, 16 column and 2 rows

// a4988 pins
const byte STEPPin = 11;    // a4988 STEP
const byte DIRPin = 12;     // a4988 DIR

int stepCount = 0;          // count of pump steps made

int numSteps, encChanged=0, encSw_old;

unsigned long debounceTimer=0;

int motorCounter;

void setup() {
  Serial.begin(115200);
  numSteps=DEFAULT_NUM_STEPS;
  disp_config();
  pinMode(ledPin, OUTPUT);  
  pinMode(grblM3, INPUT);
  pinMode(grblM9, INPUT);
  pinMode(cleanIn, INPUT_PULLUP);
  pinMode(sigOut, OUTPUT);
  digitalWrite(ledPin, LOW);  
  digitalWrite(sigOut, LOW); // Cycle signal active HIGH  
  pinMode(STEPPin, OUTPUT);
  digitalWrite(STEPPin, LOW);
  pinMode(DIRPin, OUTPUT);
  digitalWrite(DIRPin, LOW);
  pinMode(solRel, OUTPUT);
  digitalWrite(solRel, LOW);
  pinMode(motRel, OUTPUT);
  digitalWrite(motRel, LOW);
  pinMode(encClk, INPUT);
  pinMode(encDt, INPUT);
  attachInterrupt(digitalPinToInterrupt(encClk), readEnc, RISING);
  pinMode(testIn, INPUT_PULLUP);
  pinMode(encSw, INPUT_PULLUP);
  encSw_old=digitalRead(encSw);
  Serial.println("XY Combined Oil Machine");
  Serial.print("-----------------------");
  Serial.println("");  
  lcd.init();               // Initialize the lcd
  lcd.backlight();
  lcd.clear();               
  lcd.setCursor(0, 0);       
  lcd.print("XY Combined Machine");
  lcd.setCursor(2, 1);         
  lcd.print("Amount="); 
  lcd.print(numSteps/10);

  // Initializing TIMER1 periodic interrupt
  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  OCR1A = 7812; // each 0.5 seconds
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS10);
  TCCR1B |= (1 << CS12);
  TIMSK1 |= (1 << OCIE1A);
  sei();
}

void loop() {
  while (digitalRead(grblM3)==HIGH | digitalRead(grblM9)==HIGH) {                                // Drop if M3 or M9 already HIGH
      disp_steps();
      if(digitalRead(cleanIn)==LOW) {
        cw_high_speed();
      }
      if(digitalRead(testIn)==LOW) {
        Serial.print("Starting test pump cycle...");
        Serial.println("");  
        cyclePump();
        delay(500);
      }
      if(digitalRead(encSw)==LOW & encSw_old==HIGH) {               // Manual cycle pulse to GRBL
        encSw_old=LOW;
        if((millis()-debounceTimer)>DEBOUNCE_DELAY_MS) {            // Encoder switch debouncing
          debounceTimer=millis();
          Serial.print("Sending manual cycle pulse to GRBL");
          Serial.println("");
          digitalWrite(sigOut, HIGH);                                  
          delay(SIG_DURATION);                                         
          digitalWrite(sigOut, LOW);
        }
      } else encSw_old=digitalRead(encSw);
  }
  Serial.println("...............................");
  Serial.println("Waiting M3 or M9 signal from GRBL...");
  while (digitalRead(grblM3)==LOW & digitalRead(grblM9)==LOW) {     // Wait for rising edge of M3 or M9
      disp_steps();
      if(digitalRead(cleanIn)==LOW) {
        cw_high_speed();
      }
      if(digitalRead(testIn)==LOW) {
        Serial.print("Starting test pump cycle...");
        Serial.println("");  
        cyclePump();
        delay(500);
      }
      if(digitalRead(encSw)==LOW & encSw_old==HIGH) {               // Manual cycle pulse to GRBL
        encSw_old=LOW;
        if((millis()-debounceTimer)>DEBOUNCE_DELAY_MS) {            // Encoder switch debouncing
          debounceTimer=millis();
          Serial.print("Sending manual cycle pulse to GRBL");
          Serial.println("");
          digitalWrite(sigOut, HIGH);                                  
          delay(SIG_DURATION);                                         
          digitalWrite(sigOut, LOW);
        }
      } else encSw_old=digitalRead(encSw);
  }
  if (digitalRead(grblM3)==HIGH) {
    cyclePump();
  }
  if (digitalRead(grblM9)==HIGH) {
    cycleHopper();
  }  
}

void disp_config() {
  Serial.println("Pump Stepper");
  Serial.println("--------------");
  Serial.println("Configuration:");
  Serial.print("TPWM= ");
  Serial.println(TPWM);
  Serial.print("DEFAULT_NUM_STEPS= ");
  Serial.println(DEFAULT_NUM_STEPS);
  Serial.print("MIN_NUM_STEPS= ");
  Serial.println(MIN_NUM_STEPS);
  Serial.print("MAX_NUM_STEPS= ");
  Serial.println(MAX_NUM_STEPS);
  Serial.print("STEPPER_SPEED= ");
  Serial.println(STEPPER_SPEED);
  Serial.print("STEPPER_SPEED_CLEAN= ");
  Serial.println(STEPPER_SPEED_CLEAN);
  Serial.print("SIG_DURATION= ");
  Serial.println(SIG_DURATION);
  Serial.print("WAIT_TIME_FINAL= ");
  Serial.println(WAIT_TIME_FINAL);
  Serial.print("WAIT_TIME_LOOP= ");
  Serial.println(WAIT_TIME_LOOP);
  Serial.print("STEPS_BACK= ");
  Serial.println(STEPS_BACK);
  Serial.println("");
}

void disp_steps() {
    if(encChanged) {
    lcd.clear();               
    lcd.setCursor(0, 0);       
    lcd.print("XY Combined Machine");
    lcd.setCursor(2, 1);         
    lcd.print("Amount="); 
    lcd.print(numSteps/10);
    //Serial.println(numSteps/10);
    encChanged=0;
  }
}

void readEnc() {
  if((millis()-debounceTimer)<DEBOUNCE_DELAY_MS) return;  // Encoder debouncing
  debounceTimer=millis();
  if(digitalRead(encDt)!=digitalRead(encClk)) {
    if(numSteps>MIN_NUM_STEPS) numSteps-=ENC_MULTI_STEP;     
  } else {
    if(numSteps<MAX_NUM_STEPS) numSteps+=ENC_MULTI_STEP; 
  }
  encChanged=1;
}

// Pump cycle on M3 signal from GRBL
void cyclePump() {
    noInterrupts();
    Serial.print("Number of steps: ");
    Serial.println(numSteps);
    Serial.println("Starting pump cycle...");
    digitalWrite(ledPin, HIGH);
    Serial.println("Moving the pump forward...");
    // Start moving forward
    cw();
    // Waiting after moving forward (some small delay necessary before changing to ccw)
    delay(WAIT_TIME_FINAL);
    Serial.println("Moving the pump back predefined number of steps...");
    // Start moving the pump back back
    ccw();
    // Wait time before Cycle pulse to GRBL (if necessary)
    delay(WAIT_TIME_LOOP);                                           // Waiting after ccw rotation before pulse to GRBL
    digitalWrite(sigOut, HIGH);                                      // Cycle pulse to GRBL
    delay(SIG_DURATION);                                             // when procedure complete
    digitalWrite(sigOut, LOW);
    digitalWrite(ledPin, LOW);
    Serial.println("Pump stepper cycle finished.");
    interrupts();
}

// Hopper cycle on M9 signal from GRBL
void cycleHopper() {
    noInterrupts();
    Serial.println("Starting hopper cycle...");
    digitalWrite(ledPin, HIGH);
    Serial.println("Activating solenoid...");
    // Activating solenoid
    digitalWrite(solRel, HIGH);
    Serial.println("Activating motor...");
    // Activating motor
    motorCounter=MOTOR_DELAY*2; // If TIMER1 interrupt each 0.5 seconds
    digitalWrite(motRel, HIGH);
    // Waiting after solenoid switched on
    delay(GRAVITY_DELAY_MS);
    Serial.println("Dectivating solenoid...");
    digitalWrite(solRel, LOW);
    // Wait time before Cycle pulse to GRBL (if necessary)
    delay(WAIT_TIME_LOOP);                                           // Waiting before pulse to GRBL
    digitalWrite(sigOut, HIGH);                                      // Cycle pulse to GRBL
    delay(SIG_DURATION);                                             // when procedure complete
    digitalWrite(sigOut, LOW);
    digitalWrite(ledPin, LOW);
    Serial.println("Hopper stepper cycle finished.");
    interrupts();
}

// Moving the pump stepper in forward direction
void cw()
{
  digitalWrite(DIRPin, LOW);
  stepCount=0;
  int stepperDelay=STEPPER_SPEED_MAXDELAY;
  while (stepCount < numSteps) {
    digitalWrite(STEPPin, HIGH);
    delayMicroseconds(stepperDelay);
    digitalWrite(STEPPin, LOW);
    delayMicroseconds(stepperDelay);
    stepCount++;
    if(stepperDelay>STEPPER_SPEED) {
      stepperDelay-=STEPPER_SPEED_DELTA;
    }
  }
}

// Moving the pump stepper in backward direction
void ccw()
{
  digitalWrite(DIRPin, HIGH);
  stepCount=0;
// *** Comment if ramp up for reverse movement not needed  
  int stepperDelay=STEPPER_SPEED_MAXDELAY;
// Uncomment if ramp up for reverse movement not needed
//  int stepperDelay=STEPPER_SPEED_RETRACT;
// End off ***
  while (stepCount < STEPS_BACK) {
    digitalWrite(STEPPin, HIGH);
    delayMicroseconds(stepperDelay);
    digitalWrite(STEPPin, LOW);
    delayMicroseconds(stepperDelay);
    stepCount++;
// *** Comment if ramp up for reverse movement not needed    
    if(stepperDelay>STEPPER_SPEED_RETRACT) {
      stepperDelay-=STEPPER_SPEED_DELTA;
    }
// End of ***    
  }
}

// Moving the pump stepper indefinitely in forward direction for cleaning
void cw_high_speed()
{
  Serial.println("Cleaning started...");
  noInterrupts();
  stepCount=0;
  int stepperDelay=STEPPER_SPEED_MAXDELAY;
  digitalWrite(DIRPin, LOW);
  while (digitalRead(cleanIn)==LOW) {
    digitalWrite(STEPPin, HIGH);
    delayMicroseconds(stepperDelay);
    digitalWrite(STEPPin, LOW);
    delayMicroseconds(stepperDelay);
    if(stepperDelay>STEPPER_SPEED_CLEAN) {
      stepperDelay-=STEPPER_SPEED_DELTA;
    }
  }
  interrupts();
  Serial.println("Cleaning stopped...");
}

// Executed periodically, each 0.5 seconds
ISR(TIMER1_COMPA_vect)
{
  if(motorCounter>0) {
    digitalWrite(motRel, HIGH);
    motorCounter--;
  } else {
    digitalWrite(motRel, LOW);
  }
}