#include <LiquidCrystal.h>
#include <Keypad.h>
#include <HX711_ADC.h> 
#include <Servo.h>
#include "pitches.h"

#define startupMode 1
#define autoFeederMode 2
#define manualFeederMode 3
#define maxFeed 750

// Global variables & Sensors
// For the buzzer
int nokia[] = {
  NOTE_E5, NOTE_D5, NOTE_FS4, NOTE_GS4, 
  NOTE_CS5, NOTE_B4, NOTE_D4, NOTE_E4, 
  NOTE_B4, NOTE_A4, NOTE_CS4, NOTE_E4,
  NOTE_A4
};

int durations[] = {
  8, 8, 4, 4,
  8, 8, 4, 4,
  8, 8, 4, 4,
  2
};
int mode = startupMode;
// LCD Parameters: (rs, enable, d4, d5, d6, d7)
LiquidCrystal lcd(A5, A4, A3, A2, A1, A0); 
// Keypad
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] =
  {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
  };
byte colPins[COLS] = {5, 4, 3, 2}; //column pins
byte rowPins[ROWS] = {9, 8, 7, 6}; //row pins
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);
char weightDisplay[4] = "000";
int weightTracker = 0;
int weightChosen = -1;
int weightValue = 0;
char timeDisplay[9] = "00/00/00";
int timeTracker = 0;
int timeChosen = -1;
int timeValue = 0;
int autoTimer = 0;
char timeDisplayManual[9] = "00/00/00";
int timeTrackerManual = 0;
int timeValueManual = 0;
int timeChosenManual = -1;
int manualTimer = 0;

const float lc_calibration = 1970.88;
const float lc_tare = 4861.2;

// Functions
void initialisationStage(char input);
void autoFeederStage(char input);
void manualFeederStage(char input);

Servo myservo;
HX711_ADC LoadCell(12, 13);

void setup() {
  Serial.begin(57600);

  // Initialise all sensors and actuators

  // LCD screen
  lcd.begin(16, 2);

  // Buzzer
  pinMode(10, OUTPUT);

  // Servo
  myservo.attach(11);

  // Load Cell
  LoadCell.begin();
  LoadCell.setCalFactor(lc_calibration);

 
}
void loop() {
  // If you click * on keypad, it will go back to startup mode
  char key = keypad.getKey();
  if (key == '*') {
    mode = startupMode;
  }
  // FSM
  if (mode == startupMode) {
    initialisationStage(key);
  }
  else if (mode == autoFeederMode) {
    autoFeederStage(key);
  }
  else if (mode == manualFeederMode) {
    manualFeederStage(key);
  }
}

void initialisationStage(char input) {
  //reset flags
  weightDisplay[4] = "000";
  weightTracker = 0;
  weightChosen = -1;
  timeDisplay[9] = "00/00/00";
  timeTracker = 0;
  timeChosen = -1;
  autoTimer = 0;
  timeDisplayManual[9] = "00/00/00";
  timeTrackerManual = 0;
  timeValueManual = 0;
  timeChosenManual = -1;
  manualTimer = 0;

  lcd.setCursor(0, 0);
  lcd.print("Select Mode:    ");
  lcd.setCursor(0, 1);
  lcd.print("1-Auto, 2-Manual");

  // Now get the mode
  if (input == '1') {
    mode = autoFeederMode;
  }
  else if (input == '2') {
    mode = manualFeederMode;
  }
}

void autoFeederStage(char input) {
  // Start by letting user get the food weight
  // Keep track of user input
  if (weightChosen == -1) {
    lcd.setCursor(0, 0);
    lcd.print("Choose Weight:  ");
    lcd.setCursor(0, 1);
    lcd.print("grams: ");
    lcd.print(weightDisplay);
    lcd.print("      ");

    if (input >= '0' && input <= '9') {
      if (weightTracker == 3) {
        weightTracker = 0;
      }
      weightDisplay[weightTracker] = input;
      weightTracker++;
    }

    // User presses enter
    if (input == '#') {
      weightValue = (weightDisplay[2] - '0') + 
      10 * (weightDisplay[1] - '0') +
      100 * (weightDisplay[0] - '0');

      // If weight selected is valid
      if (weightValue >= 0 && weightValue <= maxFeed) {
        weightChosen = 1;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Weight Chosen:");
        lcd.setCursor(0, 1);
        lcd.print(weightValue);
        lcd.print(" grams");
        delay(3000);
      }

      // If weight selected is invalid, reset everything
      else {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("INVALID      ");
        lcd.setCursor(0, 1);
        lcd.print("weight chosen");
        delay(3000);
        weightDisplay[4] = {0};
        weightTracker = 0;
        weightChosen = -1;
      }
      input = '\0';
    }
  }

  // Now choose time interval
  if (weightChosen == 1 && timeChosen == -1) {
    lcd.setCursor(0, 0);
    lcd.print("Choose Time:  ");
    lcd.setCursor(0, 1);
    lcd.setCursor(0, 1);
    lcd.print("h/m/s: ");
    lcd.print(timeDisplay);
    lcd.print("  ");

    if (input >= '0' && input <= '9') {
      if (timeTracker == 9) {
        timeTracker = 0;
      }
      timeDisplay[timeTracker] = input;
      timeTracker++;
      if (timeTracker == 2 || timeTracker == 5) {
        timeTracker++;
      }
    }

    // User presses enter (conver from hh/mm/ss to seconds)
    if (input == '#') {
      timeValue = (timeDisplay[7] - '0') + 
      10 * (timeDisplay[6] - '0') +
      60 * (timeDisplay[4] - '0') +
      600 * (timeDisplay[3] - '0') +
      3600 * (timeDisplay[1] - '0') +
      36000 * (timeDisplay[0] - '0');

      timeChosen = 1;
      lcd.setCursor(0, 0);
      lcd.print("Time Chosen:");
      lcd.setCursor(0, 1);
      lcd.print(timeDisplay);
      lcd.print(" (h/m/s)");
      delay(3000);
      lcd.clear();
      input = '\0';
    }
  }
  
  // Now actually do the automatic feed
  if (weightChosen == 1 && timeChosen == 1) {
    lcd.setCursor(0, 0);
    lcd.print(weightValue);
    lcd.print("g   ");
    lcd.print(timeDisplay);
    lcd.setCursor(0, 1);
    lcd.print("Press '#' to ok");
    
    if (input == '#') {
      autoTimer = timeValue;
      while (mode != startupMode) {
      float FoodWeight=0.0;

        for (autoTimer=timeValue; autoTimer>0 && mode != startupMode;autoTimer--){
          lcd.setCursor(0, 0);
          lcd.print("Dispense Time:  ");
          lcd.setCursor(0, 1);
          lcd.print(autoTimer);
          lcd.print(" secs          ");
          for (int i = 1; i < 1000; i++) {
            LoadCell.update();
            delay(1);
             // The following is to break out the while loop
            char breakKey =  keypad.getKey();
            if (breakKey == '*') {
              mode = startupMode;
              //initialisationStage(breakKey);
              break;
            }
          }
        }

        // reset the time so that it loops forever
        if (autoTimer == 0 && mode != startupMode) {
          //autoTimer = timeValue;
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Dispensing...  ");

          FoodWeight = auto_DispenseFood(weightValue);

          for (int i = 1; i < 200; i++) {
            LoadCell.update();
            FoodWeight = lc_weight();
            delay(1);

          }

          lcd.clear();
          lcd.setCursor(0,0);
          lcd.print("Finished!");
          lcd.setCursor(0,1);
          lcd.print("Weight (g): ");
          lcd.print(FoodWeight);
          
          for (int i=0; i < 3000; i++){
            LoadCell.update();
            //FoodWeight = lc_weight();
            delay(1);
             // The following is to break out the while loop
            char breakKey =  keypad.getKey();
            if (breakKey == '*') {
              mode = startupMode;
              //initialisationStage(breakKey);
              break;
            }
          }
          autoTimer = timeValue;

           // The following is to break out the while loop
          char breakKey =  keypad.getKey();
          if (breakKey == '*') {
            mode = startupMode;
            //initialisationStage(breakKey);
            break;
          }

        }

        // The following is to break out the while loop
        char breakKey =  keypad.getKey();
        if (breakKey == '*') {
          mode = startupMode;
          //initialisationStage(breakKey);
          break;
        }

      }
    }
  }
}

void manualFeederStage(char input) {

  if (timeChosenManual == -1) {
    lcd.setCursor(0, 0);
    lcd.print("Reminder Time:  ");
    lcd.setCursor(0, 1);
    lcd.setCursor(0, 1);
    lcd.print("h/m/s: ");
    lcd.print(timeDisplayManual);
    lcd.print("  ");
    if (input >= '0' && input <= '9') {
      if (timeTrackerManual == 9) {
        timeTrackerManual = 0;
      }
      timeDisplayManual[timeTrackerManual] = input;
      timeTrackerManual++;
      if (timeTrackerManual == 2 || timeTrackerManual == 5) {
        timeTrackerManual++;
      }
    }

    // User presses enter (conver from hh/mm/ss to seconds)
    if (input == '#') {
      timeValueManual = (timeDisplayManual[7] - '0') + 
      10 * (timeDisplayManual[6] - '0') +
      60 * (timeDisplayManual[4] - '0') +
      600 * (timeDisplayManual[3] - '0') +
      3600 * (timeDisplayManual[1] - '0') +
      36000 * (timeDisplayManual[0] - '0');

      timeChosenManual = 1;
    }
  }
  else {
    lcd.setCursor(0, 0);
    lcd.print(timeDisplayManual);
    lcd.print(" (h/m/s)");
    lcd.setCursor(0, 1);
    lcd.print("Press '#' to ok");

    if (input == '#') {
      manualTimer = timeValueManual;
      lcd.setCursor(0, 0);
      lcd.print("Time:  ");
      lcd.print(manualTimer);
      lcd.print(" secs    ");

      // Now dispense in intervals
      while (mode != startupMode) {
        for (manualTimer; manualTimer > 0 && mode != startupMode; manualTimer--) {
            lcd.setCursor(0, 0);
            if (manualTimer % 4 == 0) {
              lcd.print("Manual Feed...    ");
            }
            else if (manualTimer % 4 == 1) {
              lcd.print("Manual Feed..    ");
            }
            else if (manualTimer % 4 == 2) {
              lcd.print("Manual Feed.     ");
            }
            else if (manualTimer % 4 == 3) {
              lcd.print("Manual Feed     ");
            }
            lcd.setCursor(0, 1);
            lcd.print("Time:  ");
            lcd.print(manualTimer);
            lcd.print(" secs    ");

            for (int i = 1; i < 1000; i++) {
              char breakKey = keypad.getKey();
              if (breakKey == '*') {
                mode = startupMode;
                initialisationStage(breakKey);
                break;
              }
              delay(1);
            }

        }

        // reset the time so that it loops forever
        if (manualTimer == 0) {
          manualTimer = timeValueManual;
          int manual_isBuzzing = 1;
          lcd.clear();

          while(manual_isBuzzing) { // buzzing UI
            lcd.setCursor(0,0);
            lcd.print("Feeding Time!");
            lcd.setCursor(0,1);
            lcd.print("'#' To Stop!");
            int size = sizeof(durations) / sizeof(int);
            for (int note = 0; note < size && keypad.getKey() != '#'; note++) {
              int duration = 1000 / durations[note];
              tone(10, nokia[note], duration);
              int pause = duration * 1.30;
              delay(pause);

              char breakKey = keypad.getKey();
              if (breakKey == '#') {
                manual_isBuzzing = 0;
                break;
              }
            
            //stop the tone playing:
              noTone(40);
              if (keypad.getKey() == '#') {
                manual_isBuzzing = 0;
                break;
              }
            }

            char breakKey = keypad.getKey();
            if (breakKey == '#') {
              manual_isBuzzing = 0;
              break;
            }
            
          }

          int manual_weighFood = 1;
          lcd.clear();
          float FeederWeight = 0;

          for (int i = 0; i < 500; i++) {
            LoadCell.update();
            FeederWeight = lc_weight();
            delay(2);
          }

          while(manual_weighFood) {
            lcd.setCursor(0,0);
            lcd.print("# To Reset Timer");
            lcd.setCursor(0,1);
            lcd.print("Weight (g): ");

            while(1) {
              LoadCell.update();
              FeederWeight = lc_weight();
              lcd.setCursor(12,1);
              lcd.print(FeederWeight);
              delay(50);

              char breakKey =  keypad.getKey();
              if (breakKey == '#') {
                manual_weighFood = 0;
                break;
              }
              
            }

            float FeederWeight = 0;
            for (int i = 0; i < 1000; i++) {
              LoadCell.update();
              FeederWeight = lc_weight();
              delay(1);
            }

            lcd.clear();
            lcd.setCursor(0,0);
            lcd.print("Final Weight (g):");
            lcd.setCursor(0,1);
            lcd.print(FeederWeight);
            delay(3000);

            myservo.write(0);
            
          }

        }
        // The following is to break out the while loop
        char breakKey =  keypad.getKey();
        if (breakKey == '*') {
          mode = startupMode;
          initialisationStage(breakKey);
          break;
        }
      }
    }
  }
}

float auto_DispenseFood(float auto_FoodWeight) { // takes weight as input and dispenses food from feeder

  float FeederWeight = 0;
  static boolean newDataReady = 0;

  for (int i = 0; i < 100; i++) {
    LoadCell.update();
    FeederWeight = lc_weight();
    delay(1);
  }

  if (FeederWeight > auto_FoodWeight) return FeederWeight; 
  myservo.write(0);

  while (FeederWeight < (auto_FoodWeight - 50)) { // gross food control
    static boolean newDataReady = 0;
    if (LoadCell.update()) newDataReady = true;
    FeederWeight = lc_weight();
    Serial.println("Nearly there");
    Serial.println(FeederWeight);

    lcd.setCursor(0,1); // continuously print out weight
    lcd.print("Weight(g): ");
    lcd.print(FeederWeight);

    myservo.write(180);
    for (int i = 0; i < 180 && FeederWeight < auto_FoodWeight - 50; i++) {
      LoadCell.update();
      FeederWeight = lc_weight();
      delay(1);
    }

    myservo.write(120);
    for (int i = 0; i < 180 && FeederWeight < auto_FoodWeight - 50; i++) {
      LoadCell.update();
      FeederWeight = lc_weight();
      delay(1);
    }

  }

    myservo.write(0);
    for (int i = 0; i < 500; i++) {
      LoadCell.update();
      FeederWeight = lc_weight();
      lcd.setCursor(0,1); // continuously print out weight
      lcd.print("Weight(g): ");
      lcd.print(FeederWeight);
      delay(1);     
    }

  while (FeederWeight < auto_FoodWeight - 5) { // fine food control
    static boolean newDataReady = 0;
    if (LoadCell.update()) newDataReady = true;
    FeederWeight = lc_weight();
    Serial.println("Nearly there");
    Serial.println(FeederWeight);

    lcd.setCursor(0,1); // continuously print out weight
    lcd.print("Weight(g): ");
    lcd.print(FeederWeight);

    myservo.write(150);
    for (int i = 0; i < 90 && FeederWeight < auto_FoodWeight - 5; i++) {
      LoadCell.update();
      FeederWeight = lc_weight();
      delay(1);
    }
    myservo.write(120);
    for (int i = 0; i < 90 && FeederWeight < auto_FoodWeight - 5; i++) {
      LoadCell.update();
      FeederWeight = lc_weight();
      delay(1);
    }
  }

  for (int i = 0; i < 500; i++) {
      LoadCell.update();
      FeederWeight = lc_weight();
      delay(1);
    }

  return FeederWeight;
}

float lc_weight() {
  float i = LoadCell.getData();
  float lc_weight = i - lc_tare;
  return lc_weight;

}

void playSound() {
  int size = sizeof(durations) / sizeof(int);
  for (int note = 0; note < size && keypad.getKey() != '#'; note++) {
    int duration = 1000 / durations[note];
    tone(10, nokia[note], duration);
    int pause = duration * 1.30;
    delay(pause);

    char breakKey = keypad.getKey();
    if (breakKey == '#') {
      break;
    }
            
    //stop the tone playing:
    noTone(20);
    if (keypad.getKey() == '#') {
      break;
    }

  }
}
