// include the library code:
#include <LiquidCrystal.h>
#include <PS2Keyboard.h>
#include <FlexyStepper.h>

// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

//create bullet character
byte bullet[8]=
{
  B00000,
  B00000,
  B00000,
  B01110,
  B01110,
  B00000,
  B00000,
  B00000,
};

const int DataPin = 13; //orange to yellow
const int IRQpin =  3; //blue to black


//stepper pins
//const char step_pin =A2; //red
//const char dir_pin = A5; //yellow

FlexyStepper stepper;
const int MOTOR_STEP_PIN = A2;
const int MOTOR_DIRECTION_PIN = A5;
const int LIMIT_SWITCH_PIN = 9;
const float steps_per_rotation = 400;
const float gear_teeth = 15;
const float rack_pitch = .1;
const float fudge_fact = 1/1.002506;
const float lin_dist_per_rotation = gear_teeth * rack_pitch * fudge_fact; 
const float steps_per_in = steps_per_rotation/lin_dist_per_rotation;
const float in_per_step = lin_dist_per_rotation/steps_per_rotation;

PS2Keyboard keyboard;

int mode = 1, step_value;
float int_value;
float abs_destination = 0, inc_dist=0, abs_location_value=0;
String abs_destination_disp = "0.0", inc_dist_disp="0.0", abs_location_value_disp="0.0";
int  cursor_pos_row,  cursor_pos_col;
String mode_display_value, value_display_value;
float abs_pos_display_value;
boolean int_neg = false;


void setup()
{
  pinMode(MOTOR_STEP_PIN, OUTPUT);
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);
  pinMode(LIMIT_SWITCH_PIN, INPUT);

  
  // for debug
  Serial.begin(9600);
  
  //create bullet character
  lcd.createChar(0,bullet);
  
  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);

  
  keyboard.begin(DataPin, IRQpin);

  lcd.setCursor(0,0);
  lcd.blink();

  stepper.connectToPins(MOTOR_STEP_PIN, MOTOR_DIRECTION_PIN);

  stepper.setSpeedInStepsPerSecond(6000);
  stepper.setAccelerationInStepsPerSecondPerSecond(6000);

update_display();
mode_display();
  
}

void loop()
{


  //if (Serial.available())  
  if (keyboard.available())
  {
    on_keyboard();

  }
  
//delay(1000);
//stepper.moveRelativeInSteps(10);
}



void update_display()
{
  lcd.clear();  //clear
  //
  lcd.setCursor(0,0);
  lcd.print(">");
  //lcd.write(byte(0));
  lcd.setCursor(1,0);
  lcd.print(mode_display_value);
  
  lcd.setCursor(0,1);
  lcd.print(">");
  //lcd.write(byte(0));
  lcd.setCursor(1,1);
  lcd.print(value_display_value);
  
  lcd.setCursor(9,0);
  lcd.print(">");
  //lcd.write(byte(0));
  lcd.setCursor(10,0);

  abs_pos_display_value = stepper.getCurrentPositionInSteps() * in_per_step;
  lcd.print(-abs_pos_display_value);
  
  lcd.setCursor(9,1);
  lcd.print(">");
  //lcd.write(byte(0));
  lcd.setCursor(10,1);
  lcd.print("Go");
  lcd.setCursor(9,1);
  cursor_pos_row = 1;
  cursor_pos_col = 9;   
  
}

char on_keyboard()
{
    // read the next key
    //char c = Serial.read();
    char c = keyboard.read();
    
    // check for some of the special keys
    if (c == PS2_ENTER)
    {
      if (cursor_pos_row == 1 && cursor_pos_col == 9)
      {
        on_go();
        //Serial.println("on_go_ok");
      }
      else
      {
        //on_enter(); ??might not be needed
      }
    }
    
    else if (c == 65) //(c == PS2_F1) 
    {
      mode=1;
      mode_display();
      
    }

    else if (c == 66) //(c == PS2_F2) 
    {
      mode=2;
      mode_display();
    }

    else if (c == 67) //(c == PS2_F3) 
    {
      mode=3;
      mode_display();
    }

    else if (c == 68) //(c == PS2_F4) 
    {
      mode=4;
      mode_display();
    }

    else if (c == PS2_PAGEUP) 
    {
      //code to scroll through modes
      mode-=1;
      if (mode < 1){mode = 1;}
      mode_display();
      //Serial.print("[PgUp]");Serial.println(mode);
      //lcd.print("[PgUP]");
    }
    
    else if (c == PS2_PAGEDOWN) 
    {
      //code to scroll through modes
      mode+=1;
      if (mode > 4){mode = 4;}
      mode_display();
      //Serial.print("[PgDn]");Serial.println(mode);
      //lcd.print("[PgDn]");
    }
    
//if number, go straight to value entry:

    if ((c > 47 && c < 58) || c == 45 || c==46)//then its a ASCII number or negative or point
    {
      if (c==45)//negative
      {
        int_neg = true; //will update to negative later
        value_display_value = "-";
        lcd.setCursor(1,1);
        lcd.print("        ");
        lcd.setCursor(1,1);
        lcd.print(value_display_value);
       }
       else if (c==42) //point
       {
        value_display_value = "0.";
        lcd.setCursor(1,1);
        lcd.print("        ");
        lcd.setCursor(1,1);
        lcd.print(value_display_value);        
       }
      
      else
      {
          value_display_value = c;
          lcd.setCursor(1,1);
          lcd.print("        ");
          lcd.setCursor(1,1);
          lcd.print(value_display_value);
      }
      do
      {
        //if (Serial.available())
        if (keyboard.available())
        {
          //c = Serial.read();
          c = keyboard.read();
          if ((c > 47 && c < 58) || c == PS2_BACKSPACE || c==46) //number or backspace or point
          {
            if (c==PS2_BACKSPACE)//backspace
            {
              value_display_value = value_display_value.substring(0,value_display_value.length()-1);
              lcd.setCursor(1,1);
              lcd.print("        ");
              lcd.setCursor(1,1);
              lcd.print(value_display_value);
            }
            else //number or point
            {
            value_display_value = value_display_value + c;
            lcd.setCursor(1,1);
            lcd.print(value_display_value);
            }
          }

        }
      } while (c != PS2_ENTER);

    if (int_neg == true){value_display_value=value_display_value.substring(1);}//remove negative sign for float conversion
    int_value = value_display_value.toFloat();//convert to float
    if (int_neg == true)//change float to negative and update string
    {
      int_value*=-1;
      int_neg = false;
      value_display_value = "-" + value_display_value;
    }
    
    value_store();//distributes the value appropriately.
    //Serial.println(int_value);
    setCursor_track(1,9);
     
    }


//ignore all other inputs
    
    else 
    {
      //return c;
      // otherwise, just print all normal characters
      //Serial.print(c);
    }
  }  

void mode_display()
{
  switch(mode)
  {
    case 1:
      mode_display_value = "Abs Loc";
      int_value = abs_destination;
      value_display_value = abs_destination_disp;
      update_display();
      setCursor_track(1,9);
     break;
    
    case 2:
      mode_display_value = "Inc Move";
      int_value = inc_dist;
      value_display_value = inc_dist_disp;
      update_display();
      setCursor_track(1,9);
    break;
    
    case 3:
      mode_display_value = "Set Abs";
      int_value = abs_location_value;
      value_display_value = abs_location_value_disp;
      update_display();
      setCursor_track(1,9);
    break;
    
    case 4:
      mode_display_value = "Home";
      update_display();
      setCursor_track(1,9);
    break;
  }
  
}

void value_store()
{
  switch(mode)
  {
    case 1:
      abs_destination = int_value;
      abs_destination_disp = value_display_value;
    break;
    
    case 2:
      inc_dist = int_value;
      inc_dist_disp = value_display_value;
    break;
    
    case 3:
      abs_location_value = int_value; 
      abs_location_value_disp = value_display_value;
    break;
    
    case 4:
      //value doesn't mean anything
    break;
  }
  
}


  void setCursor_track(int row, int col)
  {
  lcd.setCursor(col,row);
  cursor_pos_row = row;
  cursor_pos_col = col;    
  }

  void on_go()
  {
 
    switch(mode)
    {
      case 1:
      //absolute, difference between current and value, move there, update display
      stepper.moveToPositionInSteps(-abs_destination * steps_per_in);
      update_display();
      break;
      
      case 2:
      //incremental, move, update display
      stepper.moveRelativeInSteps(-inc_dist* steps_per_in);
      update_display();
      break;
      
      case 3:
      //set absolute, set it, update display
      stepper.setCurrentPositionInSteps(-abs_location_value* steps_per_in);
      update_display();
      break;
      
      case 4:
      //home, homing routine, update display
      stepper.moveToHomeInSteps(-1,   100, 1000, LIMIT_SWITCH_PIN);
      update_display();
      break;
    }
  }