#include "DFRobot_RGBLCD1602.h"

#define ONOFF_PIN 0   // Define the pin connected to the switch
#define SWITCH_PIN 1  // Define the pin connected to the switch

const int R = 255;
const int G = 255;
const int B = 255;

int XrPin = 7;
int YrPin = 5;
int ZrPin = 3;

int XsPin = 6;
int YsPin = 4;
int ZsPin = 2;

int Xr_value = 0;
int Xs_value = 0;
int Yr_value = 0;
int Ys_value = 0;
int Zr_value = 0;
int Zs_value = 0;

int Xs_map_Val;
int Xs_const_Val;
int Xr_map_Val;
int X_const_Val;
String Gcode_X;

int Ys_map_Val;
int Ys_const_Val;
int Yr_map_Val;
int Yr_const_Val;
String Gcode_Y;

int Zr_map_Val;
int Zr_const_Val;
int Zr_calc_Val;
String Gcode_Z;

int Zs_map_Val;
int Zs_const_Val;
String Gcode_F;

int s0_Xr_value = 0;
int s0_Xs_value = 0;
//int s0_Xs_map_Val;
int s0_Xs_map_min_Val;
int s0_Xs_map_max_Val;
int s0_Xs_const_Val;
int s0_Xs_const_min_Val;
int s0_Xs_const_max_Val;
int s0_Xr_map_Val;
int s0_X_const_Val;
String s0_X;

int s0_Yr_value = 0;
int s0_Ys_value = 0;
//int s0_Ys_map_Val;
int s0_Ys_map_min_Val;
int s0_Ys_map_max_Val;
int s0_Ys_const_Val;
int s0_Ys_const_min_Val;
int s0_Ys_const_max_Val;
int s0_Yr_map_Val;
int s0_Yr_const_Val;
String s0_Y;

int s0_Zr_value = 0;
int s0_Zs_value = 0;
//int s0_Zs_map_Val;
int s0_Zs_map_min_Val;
int s0_Zs_map_max_Val;
int s0_Zs_const_Val;
int s0_Zs_const_min_Val;
int s0_Zs_const_max_Val;
int s0_Zr_map_Val;
int s0_Zr_const_Val;
String s0_Z;





int b_Xr_value;
int b_Xs_value;
int b_Xs_map_min_Val;
int b_Xs_const_min_Val;
int b_Xs_map_max_Val;
int b_Xs_const_max_Val;
int Xr_Current_Val = 0;  //creating a empty box for the value
int Xr_Previous_Val;
int Xr_Sub_Val;
String Baby_X;

int b_Yr_value;
int b_Ys_value;
int b_Ys_map_min_Val;
int b_Ys_const_min_Val;
int b_Ys_map_max_Val;
int b_Ys_const_max_Val;
int Yr_Current_Val = 0;  //creating a empty box for the value
int Yr_Previous_Val;
int Yr_Sub_Val;
String Baby_Y;

int b_Zr_value;
int b_Zs_value;
int b_Zs_map_min_Val;
int b_Zs_const_min_Val;
int b_Zs_map_max_Val;
int b_Zs_const_max_Val;
int Zr_Current_Val = 0;  //creating a empty box for the value
int Zr_Previous_Val;
int Zr_Sub_Val;
String Baby_Z;

DFRobot_RGBLCD1602 lcd(/*RGBAddr*/ 0x60, /*lcdCols*/ 16, /*lcdRows*/ 2);  //16 characters and 2 lines of show

bool SentGcode = false;
bool SetBaby = false;
bool InitialHome = false;
bool LastHome = true;

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);  // Initialize secondary serial communication (for inter-Arduino communication)
  delay(500);
  Serial.println("This is A - Board initialized");  // Debug message to PC
  Serial1.println("Hello from A!");                 // Initial message to other board
  //Serial1.println("G28");
  lcd.init();
  lcd.setRGB(R, G, B);
  lcd.print("Initialized");  // Print a message to the LCD.

  pinMode(XrPin, INPUT);
  pinMode(YrPin, INPUT);
  pinMode(ZrPin, INPUT);

  delay(1000);
  lcd.clear();

  pinMode(ONOFF_PIN, INPUT_PULLUP);   // Set pin as input with internal pull-up
  pinMode(SWITCH_PIN, INPUT_PULLUP);  // Set pin as input with internal pull-up
}

void loop() {
  //Read from Printer & Write to PC
  if (Serial1.available()) {
    char data = Serial1.read();  // Read data from other board
    Serial.write(data);          // Send it to the PC
  }
  //Read from PC & Write to Printer
  if (Serial.available()) {
    char data = Serial.read();  // Read data from PC
    Serial1.write(data);        // Send it to the other board
  }
  //  ledOn(HIGH);
  //  ledOn(LOW);
  int InitialState = digitalRead(ONOFF_PIN);  // Read the state of the switch
  int switchState = digitalRead(SWITCH_PIN);  // Read the state of the switch

  while (digitalRead(ONOFF_PIN) == LOW) {
    if (InitialHome == false) {
      InitialHoming();
      lcd_Start();
      delay(100);
      InitialHome = true;
      LastHome = false;
    }
    while (digitalRead(SWITCH_PIN) == LOW && digitalRead(ONOFF_PIN) == LOW) {
      SentGcode = false;
      SetBaby = false;
      lcd.clear();
      val_to_gcode();  //reading and mapping val for gcode
      lcd_gcode();     //display gcode mode
      delay(100);
    }
    while (digitalRead(SWITCH_PIN) == HIGH && digitalRead(ONOFF_PIN) == LOW) {
      if (!SentGcode) {
        lcd.clear();
        Serial1_write_main();
        lcd_gcode_sent();
        delay(3000);
        SentGcode = true;
      } else {
        val_to_set0();
        while (!(s0_X == "0" && (s0_Y == "0") && (s0_Z == "0")) && (digitalRead(SWITCH_PIN) == HIGH) && digitalRead(ONOFF_PIN) == LOW) {
          lcd.clear();
          val_to_set0();  //reading and mapping val for babystep
          lcd_set0();     //display for making all value 0
          delay(100);
        }
        lcd.clear();
        lcd_lets();
        delay(3000);
        while ((digitalRead(SWITCH_PIN) == HIGH) && digitalRead(ONOFF_PIN) == LOW) {
          lcd.clear();
          val_to_baby();
          Serial1_write_baby();
          lcd_baby();
          delay(100);
        }
      }
    }
  }
  if (InitialState == HIGH) {
    if (LastHome == false) {
      InitialHome = false;
      LastHoming();
      lcd_byebye();
      LastHome = true;
    }
  }
}  //END OF LOOP
//###########################################################################
//reference
// void ledOn(bool state){
//   digitalWrite(LED, state);
// }
//#####################################################################################
//function (int needs to be declared before )
//###################################################################################
void InitialHoming() {
  Serial1.write("G28");
  Serial1.write("\n");
  Serial1.write("G0 X0 F0");
  Serial1.write("\n");
}
void lcd_Start() {
  lcd.clear();
  lcd.setRGB(R, 0, 0);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Homing");
  delay(3000);
  lcd.clear();
  lcd.setRGB(R, G, B);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Centering");
  delay(3000);
  lcd.clear();
}

void LastHoming() {
  Serial1.write("G28");
  Serial1.write("\n");
}
void lcd_byebye() {
  lcd.clear();
  lcd.setRGB(R, 0, 0);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Back Home");
  delay(4000);
  lcd.clear();
  lcd.setRGB(R, G, B);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Bye Bye");
  delay(3000);
  lcd.clear();
}


void val_to_gcode() {
  //## X value Rotational & Slide #####################################################################################
  Xr_value = analogRead(XrPin);  //reading Xr potentiometer
  Xs_value = analogRead(XsPin);  //reading Xr potentiometer

  Xs_map_Val = map(Xs_value, 15, 1023, 10, 12);  //converting value from potentiometer to value I want
  Xs_const_Val = constrain(Xs_map_Val, 10, 12);

  Xr_map_Val = map(Xr_value, 15, 1023, 0, Xs_const_Val);  //converting value from potentiometer to value I want
  X_const_Val = constrain(Xr_map_Val, 0, 12);

  Gcode_X = String(X_const_Val*10);
  //###############################################

  //## Y value Rotational & Slide #####################################################################################
  // Yr_value = analogRead(YrPin);  //reading Xr potentiometer
  // Ys_value = analogRead(YsPin);  //reading Xr potentiometer

  // Ys_map_Val = map(Ys_value, 15, 1023, 1, 100);  //converting value from potentiometer to value I want
  // Ys_const_Val = constrain(Ys_map_Val, 1, 100);

  // Yr_map_Val = map(Yr_value, 15, 1023, 0, Ys_const_Val);  //converting value from potentiometer to value I want
  // Yr_const_Val = constrain(Yr_map_Val, 0, 100);

  //Gcode_Y = String(Yr_const_Val*144);
  Yr_const_Val = Zr_calc_Val/2;
  Gcode_Y = String(Yr_const_Val*144);
  //###############################################

  //## Z value Rotational & Slide #####################################################################################
  Zr_value = analogRead(ZrPin);
  //reading Xr potentiometer
  Zr_map_Val = map(Zr_value, 15, 1023, 0, 100);
  Zr_const_Val = constrain(Zr_map_Val, 0, 100);  //converting value from potentiometer to value I want
  Zr_calc_Val = Zr_const_Val*2;
  Gcode_Z = String(Zr_calc_Val);
  //###############################################

  //## F value Rotational & Slide #####################################################################################
  Zs_value = analogRead(ZsPin);  //reading Xr potentiometer
  Zs_map_Val = map(Zs_value, 15, 1023, 0, 100);
  Zs_const_Val = constrain(Zs_map_Val, 0, 100);  //converting value from potentiometer to value I want
  Gcode_F = String(Zs_const_Val*10);
  //###############################################
}

void lcd_gcode() {
  lcd.setRGB(0, 0, B);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Ready");
  lcd.setCursor(11, 0);
  lcd.print("F");
  lcd.print(Gcode_F);
  lcd.setCursor(0, 1);
  lcd.print("X");
  lcd.print(Gcode_X);
  lcd.setCursor(4, 1);
  lcd.print(" Y");
  lcd.print(Yr_const_Val);
  lcd.print("R");
  lcd.setCursor(11, 1);
  lcd.print(" Z");
  lcd.print(Gcode_Z);
}

void lcd_gcode_sent() {
  lcd.setRGB(R, G, B);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Sent");
  lcd.setCursor(11, 0);
  lcd.print("F");
  lcd.print(Gcode_F);
  lcd.setCursor(0, 1);
  lcd.print("X");
  lcd.print(Gcode_X);
  lcd.setCursor(4, 1);
  lcd.print(" Y");
  lcd.print(Gcode_Y);
  lcd.print("R");
  lcd.setCursor(11, 1);
  lcd.print(" Z");
  lcd.print(Gcode_Z);
}

void Serial1_write_main() {
  Serial1.write("G0 X");
  Serial1.write(Gcode_X.c_str());  //sending the value as string
  Serial1.write(" Y");
  Serial1.write(Gcode_Y.c_str());  //sending the value as string
  Serial1.write(" Z");
  Serial1.write(Gcode_Z.c_str());  //sending the value as string
  Serial1.write(" F");
  Serial1.write(Gcode_F.c_str());  //sending the value as string
  Serial1.write("\n");

  //debug
  Serial.write("G0 X");
  Serial.write(Gcode_X.c_str());  //sending the value as string
  Serial.write(" Y");
  Serial.write(Gcode_Y.c_str());  //sending the value as string
  Serial.write(" Z");
  Serial.write(Gcode_Z.c_str());  //sending the value as string
  Serial.write(" F");
  Serial.write(Gcode_F.c_str());  //sending the value as string
  Serial.write("\n");
}

void val_to_set0() {
  //## X value Rotational & Slide #####################################################################################
  s0_Xr_value = analogRead(XrPin);  //reading Xr potentiometer
  s0_Xs_value = analogRead(XsPin);  //reading Xr potentiometer

  s0_Xs_map_min_Val = map(s0_Xs_value, 15, 1023, 0, -10);  //converting value from potentiometer to value I want
  s0_Xs_const_min_Val = constrain(s0_Xs_map_min_Val, -10, 0);
  s0_Xs_map_max_Val = map(s0_Xs_value, 15, 1023, 0, 10);  //converting value from potentiometer to value I want
  s0_Xs_const_max_Val = constrain(s0_Xs_map_max_Val, 0, 10);

  s0_Xr_map_Val = map(s0_Xr_value, 15, 1023, s0_Xs_const_min_Val, s0_Xs_const_max_Val);  //converting value from potentiometer to value I want
  s0_X_const_Val = constrain(s0_Xr_map_Val, -10, 10);

  s0_X = String(s0_X_const_Val);
  //###############################################

  //## Y value Rotational & Slide #####################################################################################
  s0_Yr_value = analogRead(YrPin);  //reading Xr potentiometer
  s0_Ys_value = analogRead(YsPin);  //reading Xr potentiometer

  s0_Ys_map_min_Val = map(s0_Ys_value, 15, 1023, 0, -10);  //converting value from potentiometer to value I want
  s0_Ys_const_min_Val = constrain(s0_Ys_map_min_Val, -10, 0);
  s0_Ys_map_max_Val = map(s0_Ys_value, 15, 1023, 0, 10);  //converting value from potentiometer to value I want
  s0_Ys_const_max_Val = constrain(s0_Ys_map_max_Val, 0, 10);

  s0_Yr_map_Val = map(s0_Yr_value, 15, 1023, s0_Ys_const_min_Val, s0_Ys_const_max_Val);  //converting value from potentiometer to value I want
  s0_Yr_const_Val = constrain(s0_Yr_map_Val, -10, 10);

  s0_Y = String(s0_Yr_const_Val);
  //###############################################

  //## Z value Rotational & Slide #####################################################################################
  s0_Zr_value = analogRead(ZrPin);  //reading Xr potentiometer
  s0_Zs_value = analogRead(ZsPin);  //reading Xr potentiometer

  s0_Zs_map_min_Val = map(s0_Zs_value, 15, 1023, 0, -10);  //converting value from potentiometer to value I want
  s0_Zs_const_min_Val = constrain(s0_Zs_map_min_Val, -10, 0);
  s0_Zs_map_max_Val = map(s0_Zs_value, 15, 1023, 0, 10);  //converting value from potentiometer to value I want
  s0_Zs_const_max_Val = constrain(s0_Zs_map_max_Val, 0, 10);

  s0_Zr_map_Val = map(s0_Zr_value, 15, 1023, s0_Zs_const_min_Val, s0_Zs_const_max_Val);  //converting value from potentiometer to value I want
  s0_Zr_const_Val = constrain(s0_Zr_map_Val, -10, 10);

  s0_Z = String(s0_Zr_const_Val);
  //###############################################
}

void lcd_set0() {
  lcd.setRGB(0, G, 0);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("Set All to Zero");
  lcd.setCursor(0, 1);
  lcd.print("X");
  lcd.print(s0_X);
  lcd.setCursor(4, 1);
  lcd.print(" Y");
  lcd.print(s0_Y);
  lcd.setCursor(11, 1);
  lcd.print(" Z");
  lcd.print(s0_Z);
}

void lcd_lets() {
  lcd.setRGB(R, G, B);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("DJ MODE");
  // lcd.setCursor(0, 1);
  // lcd.print("DJ or Dance");
}

void val_to_baby() {
  //X-axis BabyStep
  b_Xr_value = analogRead(XrPin);  //reading Xr potentiometer
  b_Xs_value = analogRead(XsPin);
  Xr_Previous_Val = Xr_Current_Val;  //Overwriting Previous value with current value. this needs to be

  b_Xs_map_min_Val = map(b_Xs_value, 15, 1023, 0, -10);  //converting value from potentiometer to value I want
  b_Xs_const_min_Val = constrain(b_Xs_map_min_Val, -10, 0);
  b_Xs_map_max_Val = map(b_Xs_value, 15, 1023, 0, 10);  //converting value from potentiometer to value I want
  b_Xs_const_max_Val = constrain(b_Xs_map_max_Val, 0, 10);

  Xr_Current_Val = map(b_Xr_value, 0, 1023, b_Xs_const_min_Val, b_Xs_const_max_Val);  //converting value from potentiometer to value I want
  Xr_Sub_Val = Xr_Current_Val - Xr_Previous_Val;                                      //introducing difference between Current value and previous value
  Baby_X = String(Xr_Sub_Val);                                                        //making the value as a string

  //Y-axis BabyStep
  b_Yr_value = analogRead(YrPin);  //reading Xr potentiometer
  b_Ys_value = analogRead(YsPin);
  Yr_Previous_Val = Yr_Current_Val;  //Overwriting Previous value with current value. this needs to be

  b_Ys_map_min_Val = map(b_Ys_value, 15, 1023, 0, -10);  //converting value from potentiometer to value I want
  b_Ys_const_min_Val = constrain(b_Ys_map_min_Val, -10, 0);
  b_Ys_map_max_Val = map(b_Ys_value, 15, 1023, 0, 10);  //converting value from potentiometer to value I want
  b_Ys_const_max_Val = constrain(b_Ys_map_max_Val, 0, 10);

  Yr_Current_Val = map(b_Yr_value, 0, 1023, b_Ys_const_min_Val, b_Ys_const_max_Val);  //converting value from potentiometer to value I want
  Yr_Sub_Val = Yr_Current_Val - Yr_Previous_Val;                                      //introducing difference between Current value and previous value
  Baby_Y = String(Yr_Sub_Val);                                                        //making the value as a string

  //Z-axis BabyStep
  b_Zr_value = analogRead(ZrPin);  //reading Xr potentiometer
  b_Zs_value = analogRead(ZsPin);
  Zr_Previous_Val = Zr_Current_Val;  //Overwriting Previous value with current value. this needs to be before the substraction(?)

  b_Zs_map_min_Val = map(b_Zs_value, 15, 1023, 0, -10);  //converting value from potentiometer to value I want
  b_Zs_const_min_Val = constrain(b_Zs_map_min_Val, -10, 0);
  b_Zs_map_max_Val = map(b_Zs_value, 15, 1023, 0, 10);  //converting value from potentiometer to value I want
  b_Zs_const_max_Val = constrain(b_Zs_map_max_Val, 0, 10);

  Zr_Current_Val = map(b_Zr_value, 0, 1023, b_Zs_const_min_Val, b_Zs_const_max_Val);  //converting value from potentiometer to value I want
  Zr_Sub_Val = Zr_Current_Val - Zr_Previous_Val;                                      //introducing difference between Current value and previous value
  Baby_Z = String(Zr_Sub_Val);                                                        //making the value as a string
}

void Serial1_write_baby() {
  if (Xr_Sub_Val != 0) {  //only send this message when the value is not 0
    Serial1.write("M290 X");
    Serial1.write(Baby_X.c_str());  //sending the value as string
    Serial1.write("\n");
  }
  if (Yr_Sub_Val != 0) {  //only send this message when the value is not 0
    Serial1.write("M290 Y");
    Serial1.write(Baby_Y.c_str());  //sending the value as string
    Serial1.write("\n");
  }
  if (Zr_Sub_Val != 0) {  //only send this message when the value is not 0
    Serial1.write("M290 Z");
    Serial1.write(Baby_Z.c_str());  //sending the value as string
    Serial1.write("\n");
  }
}

void lcd_baby() {
  lcd.setRGB(R, 100, 0);  // Set R, G, B Value
  lcd.setCursor(0, 0);
  lcd.print("DJ:Twist & Slide");
  lcd.setCursor(0, 1);
  lcd.print("X");
  lcd.print(Xr_Current_Val);
  lcd.setCursor(4, 1);
  lcd.print(" Y");
  lcd.print(Yr_Current_Val);
  lcd.setCursor(11, 1);
  lcd.print(" Z");
  lcd.print(Zr_Current_Val);
}