/*******************************************************************************
   Keypad-LCD_Interface
   Date: JUN-2017
   This project is to have an adjustable backstop to control the length of boards cut by a chop-saw.
   As of the current rev, it has an interface with the user via an LCD screen housed
   on an Aruino Mega board. It displays the current location, and interfaces with a
   keypad such that the user can input locations for the motor to move to.
   It updates the current location after each move command.
    _______________________________________________________
   ||Potential Commands                                   ||
   ||-----------------------------------------------------||
   ||  KEY PRESSED | Action Taken                         ||
   ||--------------+--------------------------------------||
   ||        START | Command move to Input Location       ||
   ||         STOP | Disables motor                       ||
   ||           F1 | Clears Entry                         ||
   ||           F2 | NOT USED                             ||
   ||           F3 | Command Move to Max Stroke Location  ||
   ||           F4 | Homes motor                          ||
   ||  Number Keys | Enter location value                 ||
   ||--------------+--------------------------------------||
 ******************************************************************************/

//Libraries for LCD Screen
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
//##include <utility/Adafruit_MCP23017.h>
//Keypad Library
#include <Keypad.h>

//+++++SCENARIO SPECIFIC VARIABLES+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//The following are config file variables, changeable within MSP. They must match the values in MSP
#define MinAltPulse  20                         //Min Alt Pulse             [ms]
#define InputResolution 6400                    //Pulses/Revolution
#define HomingSpeed 100.0                       //RPM
#define MovingSpeed 900.0                       //RPM

//#define MaxStroke 68.0                         //inches
#define MaxStroke 999                         //mm
//ThouPerPulse represents the physical distance the stage will move per pulse sent by the Arduino
//It is calculated based off of the Input Resolution defined in the config file in MSP (6400 pulses/rev)
// as well as the diameter of the pulley for the belt stage (1.379 in).
// Calculation: (1/resolution)*(pi*Diameter)*(unitconversion)
//     Example: (1/6400 [rev/pulse])*(pi*1.379 [in/rev])*(1000 [thou/in]) = 0.677 [thou/pulse]
#define ThouPerPulse 0.677                      //Physical Move Res.        [Thou/Pulse]

//+++++PIN ASSIGNMENTS++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//Motor Pin Assignments, set once, not to be changed.
#define PULLUP_PIN 44              //Pullup  Pin Location on Arduino
#define  E   46                    //Enable+ Pin Location on Arduino
#define  A   48                    //Inp_A+  Pin Location on Arduino
#define  B   50                    //Inp_B+  Pin Location on Arduino
#define  H   52                    //HLFB+   Pin Location on Arduino
#define GND1 47                    //Enable- ''     ''    ''   ''
#define GND2 49                    //Inp_A-  ''     ''    ''   ''
#define GND3 51                    //Inp_B-  ''     ''    ''   ''
#define GND4 53                    //HLFB-   ''     ''    ''   ''

//+++++KEYPAD SETUP++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
//#byte colPins[COLS] = {35, 34, 33, 32}; //connect to the row pinouts of the keypad
//#byte rowPins[ROWS] = {39, 38, 37, 36}; //connect to the column pinouts of the keypad

uint8_t colPins[COLS] = { 5, 4, 3, 2 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 9, 8, 7, 6 }; // Pins connected to R1, R2, R3, R4

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

//Note: on the keypad, pins 1-8 are connected to pins 32-39 on the Arduino, respectively.

#define START '*'   //Maps Character inputs to logical Button Names
#define STOP  '#'
#define F1    'A'
#define F2    'B'
#define F3    'C'
#define F4    'D'
#define NOKEY  0

//+++++LCD SCREEN SETUP+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// LCD Screen Setup
// include the library code:


// The shield uses the I2C SCL and SDA pins. On classic Arduinos
// this is Analog 4 and 5 so you can't use those for analogRead() anymore
// However, you can connect other I2C sensors to the I2C bus and share
// the I2C bus.
//# Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();
LiquidCrystal_I2C lcd(0x27, 16,2);

// These #defines make it easy to set the backlight color
#define RED 0x1
#define YELLOW 0x3
#define GREEN 0x2
#define TEAL 0x6
#define BLUE 0x4
#define VIOLET 0x5
#define WHITE 0x7
//variables for cursor handling
#define RMAX 2                      //Max # of Rows on LCD Display  
#define CMAX 16                     //Max # of Columns on Display   
int r = 0;                          //Current Cursor row Location
int c = 0;                          //Current Cursor Column Loc.

//+++++CODE SPECIFIC VARIABLES+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//Parameters used in the code. Will be changed later.
byte MotorState = 0;     //Motor State options are: DISABLED, HOMING, MOVING, IDLING
#define DISABLED 0
#define HOMING   1
#define MOVING   2
#define IDLING   3
char ButtonState = 0;
#define HLFBERROR 4
#define STROKE_EXCEEDED 5
//double DigitPlaceValue = 10; //Place value of input digit    (Ex: 20.000 = 2 * DigitPlaceValue = 2 * 10)
//double InputValue = 0.0;      //The current input value
//double DisplayValue = 0.0;    //The current displayed value
//double CurrentLocation = 0.0; //The current location value
int InputValue = 0;      //The current input value
int DisplayValue = 0;    //The current displayed value
int CurrentLocation = 0; //The current location value

unsigned long Timing = 0;
int HLFBTimer = 500;
#define HomingTimeout round(MaxStroke/HomingSpeed/ThouPerPulse*1000*60*1000/InputResolution*1.5)
#define MovingTimeout round(MaxStroke/MovingSpeed/ThouPerPulse*1000*60*1000/InputResolution*1.5)



//+++++SETUP+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void setup() {
  //Debugging output
  Serial.begin(9600);
  //Setup Pins as outputs, HLFB as input, set GND to LOW for Ground.
/*  pinMode(E, OUTPUT);
  pinMode(A, OUTPUT);
  pinMode(B, OUTPUT);
  pinMode(H, INPUT);              //Use a pullup resistor to limit the current flowing through HLFB
  pinMode(PULLUP_PIN, OUTPUT);    //The built in pullup resistor is too large. We used a 330 Ohm Resistor
  digitalWrite(PULLUP_PIN, HIGH); //Connecting H to 5V through a resistor (5V through the pullup_pin)
  pinMode(GND1, OUTPUT);
  pinMode(GND2, OUTPUT);
  pinMode(GND3, OUTPUT);
  pinMode(GND4, OUTPUT);
  digitalWrite(GND1, LOW);
  digitalWrite(GND2, LOW);
  digitalWrite(GND3, LOW);
  digitalWrite(GND4, LOW); */
  // set up the LCD's number of columns and rows:
  lcd.begin(CMAX, RMAX);
  lcd.blink();
  Screen(DISABLED);

}

void loop() {
  //The following Switch-Case structure determines the different behavior of the system in different motor states:
  // While disabled, the motor should only accept a Homing command
  // While homing, the motor should only accept a Disable command, with exceptions for homing complete and timeout
  // While moving, the motor should only accept a Disable command, with exceptions for moving complete and timeout
  // While idling, all buttons should be active. All numbered buttons will input values, and the function buttons will work as designed.
  switch (MotorState) {
    case DISABLED: {
        if (ButtonState == F4) {
          GoHome();
          MotorState = HOMING;
          Screen(HOMING);
        }
        break;
      }
    case HOMING: {
        if (ButtonState == STOP) {
          DisableMotor();
          MotorState = DISABLED;
          Message(DISABLED);  //Prints a message that The motor was commanded to disable
          Screen(DISABLED);
        }
        if (AllSystemsGo()) {
          MotorState = IDLING;
          Message(IDLING);  //Prints a homing complete message before switching to Idle State
          Screen(IDLING);
        }
        if (Timeout()) {
          DisableMotor();
          MotorState = DISABLED;
          Message(HOMING);  //Prints an Error Message due to Homing Timeout
          Screen(DISABLED);
        }
        break;
      }
    case MOVING: {
        if (ButtonState == STOP) {
          DisableMotor();
          MotorState = DISABLED;
          Message(DISABLED);  //Prints a message that The motor was commanded to disable
          Screen(DISABLED);
        }
        if (AllSystemsGo()) {
          //Move complete!
          MotorState = IDLING;
          Screen(IDLING);
        }
        if (Timeout()) {
          DisableMotor();
          MotorState = DISABLED;
          Message(MOVING);  //Prints an Error Message due to Moving Timeout
          Screen(DISABLED);


        }
        break;
      }
    case IDLING: {
        if (AllSystemsGo()) {
          Timing = millis();
        }
        if (millis() - Timing > HLFBTimer) {
          DisableMotor();
          MotorState = DISABLED;
          Message(HLFBERROR);
          Screen(DISABLED);
        }
        //this switch case structure handles the button inputs during the IDLING state.
        //START: Command Move, STOP: Disable Motor, F1: Clear Input, F4: Home Motor
        //Numbered Buttons handle input values
        switch (ButtonState) {
          case START: {
              if (InputValue > MaxStroke) {
                Message(STROKE_EXCEEDED);
                ResetInput();
                Screen(IDLING);
                break;
              }
              Screen(MOVING);
              MoveCommand(InputValue);
              MotorState = MOVING;
              break;
            }
          case STOP: {
              DisableMotor();
              MotorState = DISABLED;
              Message(DISABLED);  //Prints a message that The motor was commanded to disable
              Screen(DISABLED);
              break;
            }
          case F1: {
              ResetInput();
              Screen(IDLING);
              break;
            }
          case F2: {
              //possibly metric conversion
              break;
            }
          case F3: {
              //Go to Max Stroke Position
              Screen(MOVING);
              MoveCommand(MaxStroke);
              MotorState = MOVING;
              break;
            }
          case F4: {
              GoHome();
              MotorState = HOMING;
              Screen(HOMING);
              break;
            }
          case NOKEY: {
              //ignore
              break;
            }
          default: {
              ReadNumber(ButtonState);
              Screen(IDLING);
              break;
            }
        }
        break;
      }
  }
  ButtonState = keypad.getKey();

}

void DisableMotor() {
  //Disables the motor by lowering the voltage on the Enable+ Pin.
//#  digitalWrite(E, LOW);
}

void GoHome() {
  //Commands homing by toggling the Enable+ pin off and then on.
  //Resets all location values (utilizes ResetInput() for InputValue and DisplayValue)
  Timing = millis();
//#  digitalWrite(E, LOW);
//#  delay(MinAltPulse * 3 + 5);
//#  digitalWrite(E, HIGH);
  CurrentLocation = 0;
  DisplayValue = 0;
  ResetInput();
}

void MoveCommand(double Location) {
  //Checks ASG in case motor is in error state
  //Sends a # of pulses to the motor equivalent to the distance necessary to achieve the desired location
  //Does book keeping with location values

  Timing = millis();
  Serial.print("timing = ");
  Serial.println(Timing);
  DisplayValue = Location;                             //immediately updates the stored display value
  double Distance = Location - CurrentLocation;     //calculates the distance to move
  long Pulses = round(Distance / ThouPerPulse * 1000);        //pulses to send
  double MoveDistance = Pulses * ThouPerPulse / 1000;        //actual move distance commanded to the motor [in]
  CurrentLocation = CurrentLocation + MoveDistance;                   //update current location
  //Pulses = Pulses * -1; //Uncomment this line if the motor is moving the wrong direction.
  if (Pulses < 0) {
    //digitalWrite(A, HIGH);
    Pulses = Pulses * -1;
  }
  /*else {
    digitalWrite(A, LOW);
  }
  for (int i = 1; i <= Pulses; i++) {
    digitalWrite(B, HIGH);
    digitalWrite(B, LOW);
  }*/
  Serial.print("time when pulses complete = ");
  Serial.println(millis());
  ResetInput();
}

void ReadNumber(char NumberChar) {
  //Reads the digit of the button pressed and adjusts the Input Value to represent that input.
  
  lcd.setCursor(c, r);
  int Digit = NumberChar - 48; //The ASCII characters 0-9 hold a value of 48 greater than their numerical value
  
  if ( InputValue > 99 ) {             // If the 3 Chars are already used, we need to drop one.
    String tempNumber = String(InputValue);   // Converts and Interger to a String
    tempNumber.remove(0, 1);                  // Removes the first Char of the String (i.e. 120 -> 20)
    InputValue = tempNumber.toInt();          // Converts the String back to Integer
  }
  
  InputValue = InputValue * 10;          // Make the last value move down the Chars (i.e. 2  -> 20)
  InputValue = InputValue + Digit;       // Add the new value to existing value (i.e. 20 -> 26  - assuming new button pressed was 6)

}

void ResetInput() {
  //clears the input value
  InputValue = 0;
  //DigitPlaceValue = 10;
  c = 11;
  r = 1;
}

void Message(byte Reason) {
  //Prints a message such as an error message or a move done message
  //Accepts inputs of DISABLED, HOMING, MOVING, IDLING, or HLFBERROR
  switch (Reason) {
      lcd.noBlink();
    case DISABLED: {
        //Motor commanded to disable
        lcd.setBacklight(YELLOW);
        ScreenWrite(" E-STOP PRESSED ", " MOTOR DISABLED ");
        break;
      }
    case HOMING: {
        //Homing Timeout Error
        lcd.setBacklight(RED);
        ScreenWrite(" HOMING TIMEOUT ", " MOTOR DISABLED ");
        break;
      }
    case MOVING: {
        //Moving Timeout Error
        lcd.setBacklight(RED);
        ScreenWrite(" MOVING TIMEOUT ", " MOTOR DISABLED ");
        break;
      }
    case IDLING: {
        //Homing Complete, switch to idling state
        lcd.setBacklight(GREEN);
        ScreenWrite("HOMING COMPLETE:", "READY FOR INPUT ");
        break;
      }
    case HLFBERROR: {
        //HLFB Output lost, disabling motor
        lcd.setBacklight(RED);
        ScreenWrite("HLFB OUTPUT LOST", " MOTOR DISABLED ");
        break;
      }
    case STROKE_EXCEEDED: {
        lcd.setBacklight(YELLOW);
        ScreenWrite("COMMAND EXCEEDS ", "   MAX STROKE   ");
        break;
      }
  }
  delay(2000);
}

void Screen(byte State) {
  //displays the default screen for the given Motor State
  lcd.noBlink();
  switch (State) {
    case DISABLED: {
        lcd.setBacklight(YELLOW);
        ScreenWrite(" ** DISABLED ** ", "  F4 TO ENABLE  ");
        break;
      }
    case HOMING: {
        lcd.setBacklight(VIOLET);
        ScreenWrite(" *** HOMING *** ", "                ");
        break;
      }
    case MOVING: {
        lcd.setBacklight(BLUE);
        ScreenWrite(" *** MOVING *** ", "                ");
        break;
      }
    case IDLING: {
        //Print the Base screen
        lcd.clear(); 
        lcd.setBacklight(WHITE);
        lcd.setCursor(0, 0);
        lcd.print("CURRENT:  ");
        lcd.setCursor(0, 1);
        lcd.print("INPUT  :  ");
        lcd.setCursor(12, 0);
        lcd.print(" MM ");
        lcd.setCursor(12, 1);
        lcd.print(" MM ");
        //Now Print the display value
        lcd.setCursor(9, 0);
        if (DisplayValue < 100){
          lcd.print("0");
        }
        if (DisplayValue < 10){
          lcd.print("0");
        }
        lcd.print(DisplayValue);
        //Now print the input value
        lcd.setCursor(9, 1);
        if (InputValue < 100){
          lcd.print("0");
        }
        if (InputValue < 10){
          lcd.print("0");
        }
        lcd.print(InputValue);
        lcd.setCursor(c, r);
        lcd.blink();
        break;
      }

  }

}

void ScreenWrite(const char Line1[16], const char Line2[16]) {
  //Writes two lines to the LCD, defined as 16 characters long, equal to the width of the screen
  //Smaller strings will not fill up the entire screen,
  //Pad with spaces for empty space.
  lcd.setCursor(0, 0);
  lcd.print(Line1);
  lcd.setCursor(0, 1);
  lcd.print(Line2);
}

boolean AllSystemsGo() {
  //checks HLFB to see if it is asserted or deasserted.
  //Since we're using a pull-up resistor, this is backwards from the signal read on the H pin
  //boolean Asserted = !digitalRead(H);
  boolean Asserted = true;
  return Asserted;
}

boolean Timeout() {
  //Checks to see if timeout has been exceeded based on the motor state
  boolean Exceeded = false;
  switch (MotorState) {
    case HOMING: {
        if (millis() - Timing > HomingTimeout) {
          Exceeded = true;
        }
        break;
      }
    case MOVING: {
        if (millis() - Timing > MovingTimeout) {
          Exceeded = true;
        }
        break;
      }
  }
  return Exceeded;
}