// LCD2004 and Pi Pico!
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);  // set the LCD address to 0x27 for a 16 chars and 2 line display

// Input pins to connect to the encoder
const uint8_t en1chA = 12;
const uint8_t en1chB = 11;
const uint8_t en2chA = 10;
const uint8_t en2chB = 9;
const uint8_t en3chA = 8;
const uint8_t en3chB = 7;
const uint8_t en4chA = 20;
const uint8_t en4chB = 19;
const uint8_t en5chA = 17;
const uint8_t en5chB = 16;

// Define leaf buttons for encoders
const uint8_t en1Pin = 18;
const uint8_t en2Pin = 21;
const uint8_t en3Pin = 22;
const uint8_t en4Pin = 26;
const uint8_t en5Pin = 27;
// Define LEDs
const uint8_t en1LED = 6;
const uint8_t en2LED = 3;
const uint8_t en3LED = 2;
const uint8_t en4LED = 1;
const uint8_t en5LED = 0;
//Define operator control buttons for forced reset
const uint8_t userPin1 = 13;
const uint8_t userPin2 = 14;
const uint8_t userPin3 = 15;

int16_t inputDelta = 0;             // Counts up or down depending which way the encoder is turned
bool printFlag = LOW;             // Flag to indicate that the value of inputDelta should be printed

// Declaring variables for interrupt routine
volatile bool userPin1State = false;
volatile bool userPin2State = false;
volatile bool userPin3State = false;

volatile bool en1PinState = false;
volatile bool en2PinState = false;
volatile bool en3PinState = false;
volatile bool en4PinState = false;
volatile bool en5PinState = false;

int16_t enData[5]; // Array of size 5 to have each encoder data seperately

// Temporary variables to store previous state of encoder
static int16_t enOld = 0;
int16_t inputDeltaOld = 0;

uint8_t mode = 0; // Mode selector for different conditions

bool resetFlag = 0; // Flag raised when buttons are pressed

void setup() {
  //Serial1.begin(115200);
  //Serial1.println("Testing...");

  lcd.init();                      // initialize the lcd
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("Testing...");
  /*
      // Print a message to the LCD.
      lcd.setCursor(3, 0);
      lcd.print("Hello, world!");
      lcd.setCursor(2, 1);
      lcd.print("LCD");
      lcd.setCursor(5, 2);
      lcd.print("Working");
      lcd.setCursor(2, 3);
      lcd.print("Correctly");
  */
  // Set pin mode for channel pins of encoders as input
  pinMode(en1chA, INPUT_PULLUP);
  pinMode(en1chB, INPUT_PULLUP);
  pinMode(en2chA, INPUT_PULLUP);
  pinMode(en2chB, INPUT_PULLUP);
  pinMode(en3chA, INPUT_PULLUP);
  pinMode(en3chB, INPUT_PULLUP);
  pinMode(en4chA, INPUT_PULLUP);
  pinMode(en4chB, INPUT_PULLUP);
  pinMode(en5chA, INPUT_PULLUP);
  pinMode(en5chB, INPUT_PULLUP);


  // Set pin mode for buttons as input attached to encoders
  pinMode(en1Pin, INPUT_PULLUP);
  pinMode(en2Pin, INPUT_PULLUP);
  pinMode(en3Pin, INPUT_PULLUP);
  pinMode(en4Pin, INPUT_PULLUP);
  pinMode(en5Pin, INPUT_PULLUP);

  // Set pin mode for user controlled buttons as input
  pinMode(userPin1, INPUT_PULLUP);
  pinMode(userPin2, INPUT_PULLUP);
  pinMode(userPin3, INPUT_PULLUP);


  // Create interrupt routines when buttons are pressed
  attachInterrupt(digitalPinToInterrupt(en1Pin), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(en2Pin), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(en3Pin), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(en4Pin), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(en5Pin), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(userPin1), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(userPin2), reset, CHANGE);
  attachInterrupt(digitalPinToInterrupt(userPin3), reset, CHANGE);

  //Setup LED pins as output
  pinMode(en1LED, OUTPUT);
  pinMode(en2LED, OUTPUT);
  pinMode(en3LED, OUTPUT);
  pinMode(en4LED, OUTPUT);
  pinMode(en5LED, OUTPUT);

  digitalWrite(en1LED, LOW);
  digitalWrite(en2LED, LOW);
  digitalWrite(en3LED, LOW);
  digitalWrite(en4LED, LOW);
  digitalWrite(en5LED, LOW);

}

void loop() {
  // Read encoder's channels data

  bool en1chAstate = digitalRead(en1chA);
  bool en1chBstate = digitalRead(en1chB);
  bool en2chAstate = digitalRead(en2chA);
  bool en2chBstate = digitalRead(en2chB);
  bool en3chAstate = digitalRead(en3chA);
  bool en3chBstate = digitalRead(en3chB);
  bool en4chAstate = digitalRead(en4chA);
  bool en4chBstate = digitalRead(en4chB);
  bool en5chAstate = digitalRead(en5chA);
  bool en5chBstate = digitalRead(en5chB);

  switch (resetFlag) {
    case 0:
      if ((!en1chAstate && !en2chAstate) || (!en1chBstate && !en2chBstate)) { // condition 1 when both encoders 1 & 2 are running simultaneously
        readEncoder(en1chA, en1chB); //read any encoder either 1 or 2
        mode = 0;
        printDelta(0);
      } else if ((!en3chAstate && !en4chAstate) || (!en3chBstate && !en4chBstate)) { // condition 1 when both encoders 3 & 4 are running simultaneously
        readEncoder(en3chA, en3chB); //read any encoder either 3 or 4
        mode = 0;
        printDelta(0);
      } else if ((!en1chAstate) != (!en2chAstate) && (!en1chAstate)) { // condition 2 when encoder 1 is only running
        readEncoder(en1chA, en1chB);
        mode = 1;
        printDelta(0);
      }

      else if ((!en1chAstate) != (!en2chAstate) && (!en2chAstate)) { // condition 2 when encoder 2 is only running
        readEncoder(en2chA, en2chB);
        mode = 1;
        printDelta(1);
      }

      else if ((!en3chAstate) != (!en4chAstate) && (!en3chAstate)) { // condition 2 when encoder 3 is only running
        readEncoder(en3chA, en3chB);
        mode = 1;
        printDelta(2);
      }

      else if ((!en3chAstate) != (!en4chAstate) && (!en4chAstate)) { // condition 2 when encoder 4 is only running
        readEncoder(en4chA, en4chB);
        mode = 1;
        printDelta(3);
      }
      else if ((!en5chAstate)) { // condition 3 when encoder 5 is only running
        readEncoder(en5chA, en5chB);
        mode = 2;
        printDelta(4);
      }
      // Testing
      // readEncoder(en5chA, en5chB);
      // printDelta();
      break;
    case 1:
      resetFlag = 0;
      resetDisp ();
      break;
  }
}

void readEncoder(uint8_t chA, uint8_t chB) {
  while (!printFlag) {
    static uint8_t state = 0;
    bool chAstate = digitalRead(chA);
    bool chBstate = digitalRead(chB);

    switch (state) {
      case 0:                         // Idle state, encoder not turning
        if (!chBstate) {            // Turn clockwise and chB goes low first
          state = 1;
        } else if (!chAstate) {      // Turn anticlockwise and chA goes low first
          state = 4;
        }
        break;
      // Clockwise rotation
      case 1:
        if (!chAstate) {             // Continue clockwise and chA will go low after chB
          state = 2;
        }
        break;
      case 2:
        if (chBstate) {             // Turn further and chB will go high first
          state = 3;
        }
        break;
      case 3:
        if (chBstate && chAstate) {  // Both chB and chA now high as the encoder completes one step clockwise
          state = 0;
          ++inputDelta;
          printFlag = true;
        }
        break;
      // Anticlockwise rotation
      case 4:                         // As for clockwise but with chB and chaA roles reversed
        if (!chBstate) {
          state = 5;
        }
        break;
      case 5:
        if (chAstate) {
          state = 6;
        }
        break;
      case 6:
        if (chBstate && chAstate) {
          state = 0;
          --inputDelta;
          printFlag = true;
        }
        break;
    }
  }
}
// Function for counter, storage, distance, angle and display for encoder output
void printDelta(uint8_t x) {
  if (printFlag) {
    printFlag = false;
    if (x != enOld) { // continue counting previous encoder values from where left off after changing encoder
      inputDelta = enData [x] + (inputDelta - inputDeltaOld);
    }
    enData [x] = inputDelta;
    // Serial1.println(inputDelta);
    lcd.setCursor(0, 0);
    lcd.print("Encoder: "); lcd.print(x + 1);
    lcd.setCursor(0, 1);
    lcd.print("Counter: "); lcd.print("    ");
    lcd.setCursor(0, 1);
    lcd.print("Counter: "); lcd.print(enData [x]);
    lcd.setCursor(0, 2);
    lcd.print("Distance: "); lcd.print("     ");
    lcd.setCursor(0, 2);
    lcd.print("Distance: "); lcd.print(distCalc (enData [x]));
    if (mode == 0) { //dont show angle line and show additional line for synchronous encoder
      lcd.setCursor(0, 3);
      lcd.print("          "); lcd.print("     ");
      lcd.setCursor(0, 3);
      enData [x+1] = enData [x]; // copy data of encoder 1 to 2 (or copy data of encoder 3 to 4)
      lcd.print("Distance: "); lcd.print(distCalc (enData [x+1]));
    }
    if (mode == 1) { // if condition 2 calculate angle as well
      lcd.setCursor(0, 3);
      lcd.print("   Angle: "); lcd.print("     ");
      lcd.setCursor(0, 3);
      lcd.print("   Angle: "); lcd.print(angleCalc (enData [x]));
    }
    if (mode == 2) { // if condition 3 dont show angle
      lcd.setCursor(0, 3);
      lcd.print("          "); lcd.print("     ");
    }
    enOld = x;
    inputDeltaOld = inputDelta;
  }
}

void reset() {

  en1PinState = digitalRead(en1Pin);
  en2PinState = digitalRead(en2Pin);
  en3PinState = digitalRead(en3Pin);
  en4PinState = digitalRead(en4Pin);
  en5PinState = digitalRead(en5Pin);

  userPin1State = digitalRead(userPin1);
  userPin2State = digitalRead(userPin2);
  userPin3State = digitalRead(userPin3);


  if (!en1PinState) {
    digitalWrite(en1LED, HIGH);
    enData [0] = 0;
    resetFlag = 1;
  } else if (en1PinState) {
    digitalWrite(en1LED, LOW);
  }
  if (!en2PinState) {
    digitalWrite(en2LED, HIGH);
    enData [1] = 0;
    resetFlag = 1;
  } else if (en2PinState) {
    digitalWrite(en2LED, LOW);
  }
  if (!en3PinState) {
    digitalWrite(en3LED, HIGH);
    enData [2] = 0;
    resetFlag = 1;
  } else if (en3PinState) {
    digitalWrite(en3LED, LOW);
  }
  if (!en4PinState) {
    digitalWrite(en4LED, HIGH);
    enData [3] = 0;
    resetFlag = 1;
  } else if (en4PinState) {
    digitalWrite(en4LED, LOW);
  }
  if (!en5PinState) {
    digitalWrite(en5LED, HIGH);
    enData [4] = 0;
    resetFlag = 1;
  } else if (en5PinState) {
    digitalWrite(en5LED, LOW);
  }

  if (!userPin1State) {
    digitalWrite(en1LED, HIGH);
    digitalWrite(en2LED, HIGH);
    enData [0] = 0;
    enData [1] = 0;

  }
  if (!userPin2State) {
    digitalWrite(en3LED, HIGH);
    digitalWrite(en4LED, HIGH);
    enData [2] = 0;
    enData [3] = 0;
  }
  if (!userPin3State) {
    digitalWrite(en5LED, HIGH);
    enData [4] = 0;
  }

  inputDelta = 0;
  printFlag = true;
}

float distCalc(int16_t encoderCount) {
  //return encoderCount * (1.25 / 200) / (0.0393701); //inch conversion
  return encoderCount * (1.25 / 200); //1.25 = mm/rev, 200 = cnt/rev
}

float intersectionCalc(float enDist) {
  // define the first circle as the circle created by the linkage connecting the gun to theta axis (x-A)^2 + (y-B)^2 = Ro^2
  float A = enDist; //The program should monitor the pulses from encoder 1 and output an angle relative to the home position
  const uint16_t B = 0; //mm delta Y of the circle center from the theta axis. constant
  const float Ro = 61.47; //mm radius of first circle, equal to linkage length. constant
  //define the second cricle as the crircle created by the gun pivoting about the spray tip. (x-C)^2 + (y-D)^2 = Rt^2
  const float C = 121.8074; //mm delta X of the spray tip compared to x axis. constant. measured in CAD from home position
  const float D = 146.1; //mm delta Y of the spray tip compared to theta axis. constant. measured in CAD
  const float Rt = 131.7; //mm length from spray tip to linkage connection. measured in CAD

  // Determine distance between circle centers
  int S = sqrt(pow((C - A), 2) + pow((D - B), 2));
  // Determine the area of the triangle S/Ro/Rt using Heron's formula (area from 3 side lengths) and use that to find X,Y locations of intersections
  // relative to the "home" position
  int Ah = 1 / 4 * sqrt((S + Ro + Rt) * (S + Ro - Rt) * (S - Ro + Rt) * (-S + Ro + Rt));
  // Given the system, we need only to solve for the two possibilities of the X location and return the maximum
  // (the lower number is useless as it's a state the system will never be in)

  float X1 = ((A + C) / 2) + ((C - A) * (pow(Ro, 2) - pow(Rt, 2)) / (2 * pow(S, 2))) + (2 * ((B - D) / pow(S, 2)) * Ah);
  float X2 = ((A + C) / 2) + ((C - A) * (pow(Ro, 2) - pow(Rt, 2)) / (2 * pow(S, 2))) - (2 * ((B - D) / pow(S, 2)) * Ah);

  return (X1 > X2) ? X1 : X2;
}

float angleCalc(float enDist) {
  float X = intersectionCalc(enDist);
  float angle = asin((121.8074 - X) / 131.7); //only needs to be shown on screen to 1 decimal place.
  return angle;
}

void resetDisp () {
  lcd.setCursor(0, 2);
  lcd.print("Distance: "); lcd.print("     ");
  lcd.setCursor(0, 2);
  lcd.print("Distance: "); lcd.print(0);
  if (mode == 0 || mode == 2) { //dont show angle line if condition 1 or 3
    lcd.setCursor(0, 3);
    lcd.print("          "); lcd.print("     ");
  }
  if (mode == 1) { // if condition 2 show angle as 0
    lcd.setCursor(0, 3);
    lcd.print("   Angle: "); lcd.print("     ");
    lcd.setCursor(0, 3);
    lcd.print("   Angle: "); lcd.print(0);
  }
}