/* J Paewai NOV 2022,
  PART 1
  This electronic circuit is a basic grey code to decimal converter.
  The dip switch is in place of a Omron E6CP-AG5C-C absolute Encoder. The encoder is an open collector device.
  By virtue of the open collector the output is therefore inverted to the true encoder state.
  The external electronics invert the input and covert the encoders GRAY code output to base 10 decimal.

  Initially I had the absolute encoder directly connected to the Arduino but found that at medium speeds the
  microprocessor was spending too long converting the encoder output and failing to stop the motor at the required place.
  The circuit diagram reduces the computational work load by pre coverting the encoder GRAY code to base 10 decimal.

  The Arduino processor is now able to stop the motor at the right pace.
  and factor in a 5° ramp up and down to the desired runn speed of the motor.
  The code below still performs a grey code conversion for the purposes of demonstration but once the real encoder is installed into the circuit the
  encoder read comman reduce to just one instruction "encoder=PINA"

  Summary
  Reads Omron E6CP-AG5C-C Encoder., Inverts the read as the encoder outputs Open collector
   Converts the Gray code read to Decimal to display on an LCD
   256 Bit encloder is equal to (360/255=1.4118) so 1.41 degrees per bit change

   Encoder Wiring
   Black-Ground
   Red-12v Power
   Brown -Bit 0
   Orange-Bit 1
   Yellow-Bit 2
   Green-Bit 3
   Blue-Bit 4
   Purple-Bit 5
   Grey-Bit 6
   White-Bit 7

  Stepper Setup
  setting max current of the stepper
  current Current Limit = VRef x 2.5
  so CurrentofStepper/2.5 = REF Volts


  PART 2 of the project switch from a geared motor to a stepper motor
  Add keyboard with the following functions:-

  A = Direction toggle CCW/CW
  B = Input stop angle 0-255
  C = Peform a reset (Return motor to an angle of 0 degrees going Clockwise)
  D = Execute
  * = Emergency stop
  # = reserved for a future function
*/
//#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))

#include <Keypad.h>
#include <LiquidCrystal.h>
#define dirPin 5
#define stepPin 6
#define speedPin A6 
#define StepDelayNS 2000 
  
/* Controlling the Step size
  LLL=Full Step
  HLL=1/2  Step
  LHL=1/4  Step
  HHL=1/6  Step
  HHH=1/16 Step
*/

byte MSR[] = {53, 52, 51};
boolean stepDir;    // Direction of travel

// Configure / Define LCD settings
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

// Setup Keypad
const byte KEYPAD_ROWS = 4;
const byte KEYPAD_COLS = 4;
byte rowPins[KEYPAD_ROWS] = {A0, A1, A2, A3};
byte colPins[KEYPAD_COLS] = {13, 4, 3, 2};
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  {'1', '2', '3', 'a'},
  {'4', '5', '6', 'b'},
  {'7', '8', '9', 'c'},
  {'*', '0', '#', 'd'}
};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, KEYPAD_ROWS, KEYPAD_COLS);

// Create Degree symbol for the LCD
byte degreeSym[] = {
  B00110,
  B01001,
  B01001,
  B00110,
  B00000,
  B00000,
  B00000,
  B00000
};

// Setting the Step rate
void setMSR(byte p) { //0-5 - Quick & dirty setup to be better programmed
  switch (p)
  {
    case 1: digitalWrite(MSR[0], LOW);  digitalWrite(MSR[1], LOW);  digitalWrite(MSR[2], LOW); break;
    case 2: digitalWrite(MSR[0], HIGH); digitalWrite(MSR[1], LOW);  digitalWrite(MSR[2], LOW); break;
    case 3: digitalWrite(MSR[0], LOW);  digitalWrite(MSR[1], HIGH); digitalWrite(MSR[2], LOW); break;
    case 4: digitalWrite(MSR[0], HIGH); digitalWrite(MSR[1], HIGH); digitalWrite(MSR[2], LOW); break;
    case 5: digitalWrite(MSR[0], HIGH); digitalWrite(MSR[1], HIGH); digitalWrite(MSR[2], HIGH); break;
    default: digitalWrite(MSR[0], LOW); digitalWrite(MSR[1], LOW);  digitalWrite(MSR[2], LOW); break;
  }
}

void setDir(byte dir)  { // // Setting the rotation direction - CW=0, CCW=1 - Toggle value 0 or 1
  digitalWrite(dirPin, dir);
}

byte readDir(void) { // Returns A4988 Dir direction setting - CW=0, CCW=1
  return digitalRead(dirPin);
}

void spinMotorto(byte stopPos, int speed) {// Run the stepper until the spcified stop point is reached as determined by the encoder
 while ( stopPos != readEncoder() );
  setMSR(determineMSR(readEncoder(),stopPos) );
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(2000);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(2000);
}

byte determineMSR(byte current, byte destination){   // Sets the MSR value for the A4988 based on rules for distance left to travel
 byte result;
 byte diff;
 if (current > destination ) // ensure we have positive value to calculate with 
 {   
   diff = current - destination;
 }else
  {
   diff = destination - current;  
  }
 // Gap calculation Rules
 // Rules diff > 30 max step
 // between 29-20 1/2 step
 // between 19-10 1/4 step
 // between 9-4   1/8 step
 // between 4-destination 1/16 step

 if (diff > 30 ) {
  result = 1;                       // Full Step
  } else {
          if(diff > 20 && diff < 30) {
           result = 2;                    // 1/2
          }  else {
              if(diff > 10 && diff < 19) {
                 result = 3;                 // 1/4 
              } else {
                 if(diff > 4 && diff < 10) {
                  result = 4;              //1/8 Step
                } else {
                   if(diff < 4) {
                    result = 5;          //  1/16 Step
                  }
                }
              }
          }
       }   
 return result;
 }   

void showStartupMessage() {  // Display Startup Banner
  lcd.setCursor(0, 0);
  lcd.print("Encoder CTRL");
  lcd.setCursor(0, 1);
  lcd.print("J Paewai - V.99");
  delay(3000);
}

void StepReset(void) {      // Perfrom Homming to the 0 encoder position, using closest router
  if(readEncoder() > 127){   // for resetting determine closest return to zero point and set direction
    setDir(0);  // CW
  }else{
    setDir(1);   // CCW
  }
  spinMotorto(0, 2000);  // spin up motor go to destination 0 at the speed set}
}

byte readEncoder(void) {
  volatile byte encoder = PINA;        // Read without inverting - when external XOR gates are used
  volatile byte rotation bitRead(encoder, 7);
  for ( int i = 6; i >= 0; i--) {      // GRAY to BCD Conversion - Not needed when external decoder used just here for demo
    rotation = (rotation << 1) | (bitRead(encoder, i) ^ (rotation & 0x1)); // XOR each bit
  }
  return rotation; 
 }

void lcdPrintStatus(void) { // Display setting to be executed
  byte value = readEncoder();
  lcd.setCursor(0, 3); lcd.print("EN:"); lcd.setCursor(4, 3); lcd.print(value, DEC);
  lcd.setCursor(10, 3); lcd.print("A: "); lcd.setCursor(14, 3); lcd.print(round(value * 1.412), 0);
  lcd.setCursor(18, 3);
  if (readDir() == 0) {
    lcd.print("CW ");
  } else {
    lcd.print("CCW");
  }

  // lcd.write(0); not working
}

int inputDigits(String menuTitle, byte digits, String acceptedKeys) { // Keyboard input of the 0-255 angle to move to
  
  lcd.setCursor(0,0);lcd.print(menuTitle);
  lcd.setCursor(0,1);lcd.print("Select-");
  lcd.setCursor(7,1);lcd.print(acceptedKeys);
  lcd.setCursor(5,2);lcd.print("[___]");
   lcd.setCursor(6, 2);
  String result = "";
  while (result.length() < digits) {
    char key = keypad.getKey();
    if (key >= '0' && key <= '9') {
      lcd.print(key);
      result = result +- key;
    }
  }
  return result.toInt();
}

String inputCharacters (String menyTitle, byte digits){  // inputer A-D, * & # return as string

}

void menuA(void) {
  byte result = inputDigits("SELECT, 0=CW or 1=CCW",1,"0 or 1");
  while(1);
 }

void menuB(void) {
  byte result = inputDigits("3-Digit Angle",3,"1-9");
  spinMotorto(result, 2000);
  while(1);
}

void menuC(void) {
  lcd.setCursor(0,0);lcd.print("Reset in progress   ");
  lcd.setCursor(0,1);lcd.print("Please wait...      ");
  StepReset();
}

void menuD(void) {
  lcd.setCursor(0,0);lcd.print("Rotation in progress");
  lcd.setCursor(0,1);lcd.print("Please wait...      ");
  spinMotorto(0, 2000);  // to be worked on
}

void menuStar(void) {
  lcd.print("Star Menu");
  delay(2000);
}

void menuHash(void) {
  lcd.print("Hash Menu");
  delay(2000);
}

byte speed(void) {
  analogRead(speed);
  return map(speed,0,1023,0,255);
}

void setup() {
  Serial.begin(9600);
  pinMode(MSR[0], OUTPUT);
  pinMode(MSR[2], OUTPUT);
  pinMode(MSR[3], OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(speedPin, INPUT);
  // Create Custom LCD Character - Degree Symbol
  lcd.createChar(0, degreeSym);
  lcd.begin(20, 4);                                  // set up the LCD's number of columns and rows:
  
  /*for(int x=22;x<30;x++) pinMode(x,INPUT_PULLUP);  external electronics do this. Re instate
                                                     when directly connecting the incoder to MCU
  */
  showStartupMessage();
}

void loop()
{
  char menuKey = inputDigits("Main Menu",1,"A-D,# or *");  // to be fixed
  
  switch (menuKey)
  {
    case 'a': menuA(); break;
    case 'b': menuB(); break;
    case 'c': menuC(); break;
    case 'd': menuD(); break;
    case '#': menuHash(); break;
    case '*': menuStar(); break;
  }
  lcdPrintStatus();
}