#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <avr/interrupt.h>

//LCD Pins
#define LCDI2C_ADDR 0x27
#define LCDI2C_COLS 20
#define LCDI2C_ROWS 4

//Motor Pins
#define MOT_DIRPIN A2
#define MOT_STEPPIN 10

//Motor control constats
#define STEPS_DEGREES 0.1125
#define SCREW_PITCH_MM 8
#define STEP_DISTANCE_MM ((STEPS_DEGREES*SCREW_PITCH_MM)/360)

//Define keypad constants
#define KEYPAD_ROWS 4
#define KEYPAD_COLS 4
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

//Define input parameters
#define PARAM_COUNT 4
#define PARAM_XLOC 12
//param0 = Acceleration in mm/s2
//param1 = velocity in mm/s
//param2 = Distance in mm
//param3 = Direction (0 or 1)
int params[PARAM_COUNT] = {0,0,0,0};
int param_ind = 0;
int digit_ind = 0;

uint8_t col_ios[KEYPAD_COLS] = { 5, 4, 3, 2 }; // Pins connected to C1, C2, C3, C4
uint8_t row_ios[KEYPAD_ROWS] = { 9, 8, 7, 6 }; // Pins connected to R1, R2, R3, R4


LiquidCrystal_I2C alcd(LCDI2C_ADDR, LCDI2C_COLS, LCDI2C_ROWS);
Keypad keypad = Keypad(makeKeymap(keys), row_ios, col_ios, KEYPAD_ROWS, KEYPAD_COLS);


int num_steps = 0;
int step_count = 0;

ISR(TIMER1_OVF_vect){
  if(step_count == num_steps)
    TCCR1B&=0xF8;
  else
    ++step_count;
}

void initialize_motor_tick(){
  TCNT1H = 0x00;
  TCNT1L = 0x00;

  TCCR1A = 0x22;

  ICR1H = 0x03;
  ICR1L = 0x20;

  OCR1BH = 0x01;
  OCR1BL = 0x90;

  TIMSK1|=0x01;

  TCCR1B = 0x18; //Dont start timer yet


}

void setup() {
  alcd.init();
  alcd.backlight();
  pinMode(MOT_DIRPIN, OUTPUT);
  pinMode(MOT_STEPPIN, OUTPUT);
  
  //Set motor direction clockwise by default
  digitalWrite(MOT_DIRPIN, LOW);
  digitalWrite(MOT_STEPPIN, LOW);

  //initialize serial port
  Serial.begin(115200);

  memset(params,0,sizeof(int)*PARAM_COUNT);
  param_ind = 0;
  digit_ind = 0;

  // Print inputs
  alcd.setCursor(0, 0);
  alcd.print("Acc(mm/s2):");
  alcd.setCursor(0, 1);
  alcd.print("Vel(mm/s) :");
  alcd.setCursor(0, 2);
  alcd.print("Dis(mm)   :");
  alcd.setCursor(0, 3);
  alcd.print("Dir       :");
  for(int i=0;i<PARAM_COUNT;i++){
    alcd.setCursor(PARAM_XLOC,i);
    if(i== (PARAM_COUNT-1))
      alcd.print('B');
    else
      alcd.print('0');
  }
  alcd.setCursor(12,0);
  alcd.blink();

  initialize_motor_tick();
  sei();

}

void loop() {
  char key = keypad.getKey();

  if (key != NO_KEY) {
    Serial.println(key);
    if(key == '*'){
      param_ind = (param_ind+1)%PARAM_COUNT;
      alcd.setCursor(PARAM_XLOC, param_ind);
      digit_ind = 0;
    }
    else if((key>='0') && (key<='9')){
      if((param_ind<3) && (digit_ind<3)){
        if(digit_ind == 0) 
          params[param_ind] = 0;
        params[param_ind] = 
          params[param_ind]*10 
          + (key-'0');
        alcd.setCursor(PARAM_XLOC,param_ind);
        alcd.print("   ");
        alcd.setCursor(PARAM_XLOC,param_ind);
        alcd.print(params[param_ind]);
        Serial.println(params[param_ind]);
        if(params[param_ind]!=0)
          digit_ind=(digit_ind+1)%3;
        else
          alcd.setCursor(PARAM_XLOC,param_ind);
      }
    }
    else if((key == 'A')||(key == 'B')){
      if(param_ind == 3){
        params[param_ind] = (key=='A')?0:1;
        alcd.setCursor(PARAM_XLOC,param_ind);
        alcd.print(key);
        Serial.println(params[param_ind]);
        alcd.setCursor(PARAM_XLOC,param_ind);
      }
    }
    else if(key == '#'){
      if(params[0]>400){
        params[0]=400;
        Serial.println("Edge case: Acc. greater than 400 mm/s2 (handled).");
      }
      if(params[1]>100){
        params[1]=100;
        Serial.println("Edge case: Vel. greater than 100 mm/s (handled).");
      }
      Serial.print("Acc(mm/s2): ");
      Serial.println(params[0]);
      Serial.print("Vel(mm/s): ");
      Serial.println(params[1]);
      Serial.print("Dis(mm): ");
      Serial.println(params[2]);
      Serial.print("Dir: ");
      Serial.println(params[3]==0?"B - CW":"B - CC");

      float v = params[1];
      float acc = params[0];
      float d = params[2];
      float p1 = (v*v)/(2*acc);
      
      if(2*p1<d)
        Serial.println("Trapezoidal motion..");
      else
        Serial.println("Edge case: Triangular motion..");

    }
  }
  
}