#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0X27,16,2); //SCL A5 SDA A4

#define LASER           12
#define COOL            A3          // Quatj tan nhiet
#define limit_SW1       9           // Cong tac hanh trinh truc X
#define limit_SW2       10          // Cong tac hanh trinh truc Y
#define upButton        4
#define downButton      7
#define selectButton    13
#define ARC_CW          (1)         // Cung chieu kim dong ho
#define ARC_CCW         (-1)        // Nguoc chieu kim dong ho
#define MM_PER_SEGMENT  (10)        // Độ dài mong muốn mỗi đoạn để xấp xỉ ( ap dung cho noi suy cung tron use bresenham algorithm)

#define STEPS_PER_TURN (200)        // depends on your stepper motor most are 200.
#define MIN_STEP_DELAY (50.0)      // in microsecands thời gian trễ giữa các bước
#define MAX_FEEDRATE   (1000000.0/MIN_STEP_DELAY)
#define MIN_FEEDRATE   (0.1)
#define MAX_BUF        (64)         // What is the longest message Arduino can store?

int times = 0;                      // Bien bat tat laser kiem tra M3 hay M5
float power = 0;                    // Bien check Cong suat laser: 0 - 255
int laseron = 255;                  //lowers servo/activates laser
int laseroff = 0;                   //raises servo/de-activates laser
int laserlevel = 255;               // saves current laser state
int menu = 1;

char  serialBuffer[64];             // where we store the message until we get a newline
int   sofar;                        // how much is in the serialBuffer
float px, py;                       //  toa do hien tai cua X va Y
float fr = 0;                       // human version
long  step_delay;                   // machine version
char mode_abs=1;                    // absolute mode?// Doc gia tri value xu li tai ham process_command

void m1step(int dir) {
  digitalWrite(8,LOW);
  digitalWrite(5,dir==1? HIGH:LOW);
  digitalWrite(2,HIGH);
  digitalWrite(2,LOW);
}

void m2step(int dir) {
  digitalWrite(8,LOW);
  digitalWrite(6,dir==1?HIGH:LOW);
  digitalWrite(3,HIGH);
  digitalWrite(3,LOW);
}

void disable() {
  digitalWrite(8,HIGH);             // Muc low bat module bat dong co len , muc High tat module di
  digitalWrite(8,HIGH);
}

void setup_controller() {
  // put your setup code here, to run once:
  pinMode(8,OUTPUT);
  pinMode(2,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(LASER, OUTPUT);      // PIN_LASER = 12
  pinMode(limit_SW1, INPUT);       //lim1
  pinMode(limit_SW2, INPUT);       //lim2
  pinMode(COOL,OUTPUT);
  pinMode(upButton, INPUT_PULLUP);
  pinMode(downButton, INPUT_PULLUP);
  pinMode(selectButton, INPUT_PULLUP);
  digitalWrite(LASER, LOW);     // ban dau off Laser
}

void Homing(){
  //move to enstop on x and y and stop, set the position to x=0 y=0
  int XsensorValue = digitalRead(limit_SW1);
  int YsensorValue = digitalRead(limit_SW2);
  while(XsensorValue != HIGH || YsensorValue != HIGH){
    if(XsensorValue != HIGH)
    {
      m1step(-1);
      pause(step_delay);
      XsensorValue = digitalRead(limit_SW1);
      Serial.println("Limit_X\r\n");
      ready();
    }
    if(YsensorValue != HIGH)
    {
      m2step(-1);             // quay nghich lai
      pause(step_delay);
      YsensorValue = digitalRead(limit_SW2);
      Serial.println("Limit_Y\r\n");
      ready();
    }
  }
    px = 0;
    py = 0;
    Serial.println("Homing Complete");
}
void pause(long ms)                 // Ham tao thoi gian delay
{
  delay(ms/1000);                   // Doi sang giay
  delayMicroseconds(ms%1000);       // delayMicroseconds doesn't work for values > ~16k.
}
void feedrate(float nfr) {
  if(fr==nfr) return;  //  check F_new the same with F_old ?

  if(nfr>MAX_FEEDRATE || nfr<MIN_FEEDRATE) {  // don't allow crazy feed rates
    Serial.print(F("New feedrate must be greater than "));
    Serial.print(MIN_FEEDRATE);
    Serial.print(F("steps/s and less than "));
    Serial.print(MAX_FEEDRATE);
    Serial.println(F("steps/s."));
    return;
  }
  step_delay = 1000000.0/nfr;
  fr = nfr;
}
void LaserPower (int mypower){
  if (laserlevel != mypower)
  {       // tat roi tat lai k dc hoac bat roi ma bat lai cung k dc
    if (mypower == laseron){
      digitalWrite(LASER, HIGH); // turn laser on
      laserlevel = mypower;
    } 
    else if(mypower == laseroff) {
      digitalWrite(LASER, LOW); // turn laser off
      laserlevel = mypower;
    }
  }
}

void position(float npx,float npy) {
  // here is a good place to add sanity tests
  px=npx;
  py=npy;

}

/**
 * Uses bresenham's line algorithm to move both motors
 * @input newx the destination x position
 * @input newy the destination y position
 **/
void line(float newx,float newy) {
  long i;
  long over= 0;
  long dx  = newx-px;
  long dy  = newy-py;
  int dirx = dx>0?1:-1;
  int diry = dy>0?1:-1; 
  dx = abs(dx);
  dy = abs(dy);
  if(dx>dy) {
    over = dx/2;
   for(i=0; i<dx*80; ++i) {
      displayValuesOnLCD(px, py, fr);
      monitorbuttons();
      m1step(dirx);
      over += dy;
      if(over>=dx) {
        over -= dx;
        m2step(diry);
      }
      pause(step_delay);
    }
  } else {
    over = dy/2;
    for(i=0; i<dy*800; ++i) {
      displayValuesOnLCD(px, py, fr);
      m2step(diry);
      over += dx;
      if(over >= dy) {
        over -= dy;
        m1step(dirx); 
      }
      pause(step_delay);
    }
  }
  px = newx;
  py = newy;
}

float BBatan(float thisdy,float thisdx) {
  float a = atan2(thisdy,thisdx);
  if(a<0) a = (PI*2.0)+a;
  return a;
}
// This method assumes the limits have already been checked.
// This method assumes the start and end radius match.
// This method assumes arcs are not >180 degrees (PI radians)
// cx/cy - center of circle (I, J)   tam cung tron
// x/y - end position (X, Y)  diem cuoi 
// dir - ARC_CW or ARC_CCW to control direction of arc
void arc(float cx,float cy,float x,float y,float dir) 
{
  float dx = px - cx;                           // Tinh do lech cua tam va toa do ban dau theo X va Y
  float dy = py - cy;
  float radius=sqrt((dx*dx)+(dy*dy));           // Tinh ban kinh tam cung tron   

  float angle1=BBatan(dy,dx);                   // find angle of arc (sweep) tinh goc theta
  float angle2=BBatan(y-cy,x-cx);
  float theta=angle2-angle1;

  if(dir>0 && theta<0) angle2+=2*PI;            // kiem tra noi suy CW hay CCW
  else if(dir<0 && theta>0) angle1+=2*PI;
  theta=angle2-angle1;

  float len = abs(theta) * radius;
  int i, segments = ceil( len / MM_PER_SEGMENT );
  float nx, ny, angle3, scale;
  for(i=0;i<segments;++i) 
  {
    scale = ((float)i)/((float)segments);       // interpolate around the arc
    angle3 = ( theta * scale ) + angle1;
    nx = cx + cos(angle3) * radius;
    ny = cy + sin(angle3) * radius;
    line(nx,ny);                               // send it to the planner
  }
  //Serial.println("drwing final line");
  line(x,y);
}

//  Ham doc string va tra ve gia tri
float parseNumber(char code,float val) {  
  char *ptr=serialBuffer;                     // start at the beginning of buffer
  while((long)ptr > 1 && (*ptr) && (long)ptr < (long)serialBuffer+sofar) {  // walk to the end
    if(*ptr==code) {  // if you find code on your walk,
      return atof(ptr+1);  // convert the digits that follow into a float and return it
    }
    ptr=strchr(ptr,' ')+1;  // take a step from here to the letter after the next space
  }
  return val;  // end reached, nothing found, return default val.
}

// Khong can thiet van duoc
void output(const char *code,float val) {
  Serial.print(code);
  Serial.println(val);
}

  //print the current position, feedrate, and absolute mode.
void where() {
  output("X",px);
  output("Y",py);
  output("F",fr);
  Serial.println(mode_abs?"ABS":"REL");
} 
void processCommand() {
  int cmd = parseNumber('G',-1);
  switch(cmd) {
  case  0:
  case  1: { // line
    feedrate(parseNumber('F',fr));
    line( parseNumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
          parseNumber('Y',(mode_abs?py:0)) + (mode_abs?0:py) );
    break;
    }
  case 2:
  case 3: {  // arc
      feedrate(parseNumber('F',fr));
      arc(parseNumber('I',(mode_abs?px:0)) + (mode_abs?0:px),
          parseNumber('J',(mode_abs?py:0)) + (mode_abs?0:py),
          parseNumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
          parseNumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
          (cmd==2) ? -1 : 1);
      break;
    }
  case  4:  pause(parseNumber('P',0)*1000);  break;  // dwell
  case 90:  mode_abs=1;  break;  // absolute mode
  case 91:  mode_abs=0;  break;  // relative mode
  case 92:  // set logical position
    position( parseNumber('X',0),
              parseNumber('Y',0) );
    break;
  default:  break;
  }

  cmd = parseNumber('M',-1);
  switch(cmd) {
  case 3: 
      LaserPower(laseron);
      break;
  case 5:
      LaserPower(laseroff);
      break;
  case 18:  // disable motors
    disable();
    break;
  case 114:  where();  break;
  default:  break;
  }
}

  // Hien thi ra man hinh
void ready() {
  sofar=0;  // clear input buffer
  Serial.print(F(">>"));  // signal ready to receive input
}

void displayValuesOnLCD(float x, float y, int fr) {
  lcd.setCursor(0, 0);
  lcd.print("X: ");
  lcd.print(x);  // In giá trị x lên LCD

  lcd.setCursor(9, 0);
  lcd.print("Y: ");
  lcd.print(y);  // In giá trị y lên LCD

  lcd.setCursor(0, 1);
  lcd.println("F:");
  lcd.print(int(fr));  // In giá trị y lên LCD
  lcd.print("        ");
}

void updateMenu() {
  switch (menu) {
    case 0:
      menu = 1;
      break;
    case 1:
      lcd.clear();
      lcd.print("> HOME");
      lcd.setCursor(0, 1);
      lcd.print(" READ CARD");
      break;
    case 2:
      lcd.clear();
      lcd.print(" HOME");
      lcd.setCursor(0, 1);
      lcd.print(">READ CRAD");
      break;
    case 3:
      lcd.clear();
      lcd.print(">CONTROL X Y");
      lcd.setCursor(0, 1);
      lcd.print(" LASER ENGAVING");
      break;
    case 4:
      lcd.clear();
      lcd.print(" CONTROL X Y");
      lcd.setCursor(0, 1);
      lcd.print(">LASER ENGAVING");
      break;
    case 5:
      menu = 4;
      break;
  }
}

void executeAction() {
  switch (menu) {
    case 1:
      action1();
      break;
    case 2:
      action2();
      break;
    case 3:
      action3();
      break;
    case 4:
      action4();
      break;
  }
}
void action1() {
  lcd.clear();
  // Su kien 1
  //lcd.print(">Executing #1");
  displayValuesOnLCD(px, py, fr);
  Homing();
  delay(1500);
}
void action2() {
  lcd.clear();
  lcd.print(">Executing #2");
  delay(1500);
}
void action3() {
  lcd.clear();
  lcd.print(">Executing #3");
  delay(1500);
}
void action4() {
  lcd.clear();
  // LASER ENGAVING
  //lcd.print(">Executing #4");

  delay(1500);
}

void monitorbuttons(){                          // Check nut nhan
  if (!digitalRead(downButton)){
    menu++;
    updateMenu();
    delay(100);
    while (!digitalRead(downButton));
  }
  if (!digitalRead(upButton)){
    menu--;
    updateMenu();
    delay(100);
    while(!digitalRead(upButton));
  }
  if (!digitalRead(selectButton)){
    executeAction();
    updateMenu();
    delay(100);
    while (!digitalRead(selectButton));
  }
}
void nut(){

}
void monitorserial(){
  while(Serial.available() > 0) {  // if something is available
    char c=Serial.read();  // get it
    Serial.print(c);  // repeat it back so I know you got the message
    if(sofar<MAX_BUF-1) serialBuffer[sofar++]=c;  // store it
    if((c=='\n') || (c == '\r')) {
      // entire message received
      serialBuffer[sofar]=0;  // end the buffer so string functions work right
      Serial.print(F("\r\n"));  // echo a return character for humans
      processCommand();  // do something with the command
      ready();
      Serial.println("ok");
    }
  }
}
void setup() {
  Serial.begin(9600);  // open coms
  lcd.init();                    
  lcd.backlight();
  setup_controller();
  updateMenu();
  position(0,0);  // set staring position
  feedrate(MAX_FEEDRATE);  // set default speed
  ready();
}
void loop() {
  // put your main code here, to run repeatedly:
    //displayValuesOnLCD(px, py, fr);
    monitorserial();            // Xu ly tung dong code gui ve
    monitorbuttons();           // Xu ly nut nhan su kien
}