/*##############################################
                       _ _                  _
                      | | |                | |
   _ __   ___  ___  __| | | ___  __ _  __ _| |_ ___
  | '_ \ / _ \/ _ \/ _` | |/ _ \/ _` |/ _` | __/ _ \
  | | | |  __/  __/ (_| | |  __/ (_| | (_| | ||  __/
  |_| |_|\___|\___|\__,_|_|\___|\__, |\__,_|\__\___|
                               __/ |
                              |___/
   made by: Needlegate
   needlegate@gmail.com
   2019-01-05

   for Arduino ATmega 2560
*/
#include "Motors.h"
#include "Program.h"
#include <Braccio.h>
#include <Servo.h>
#define CLS "\033[2J"
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_rot;
Servo wrist_ver;
Servo gripper;
Motors Robot;
Program Prog;
int x_axis = A0;            //joystick pin
int y_axis = A1;            // joystick pin
int x_pos;
int y_pos;
byte axis_choice = 0;       //manual mode
int button1 = 30;
int button2 = 34;
int button3 = 38;
bool bt1_Tick;              //high or low every loop
bool bt2_Tick;              //      --II--
bool bt3_Tick;              //      --II--
bool lastBt1;
long last_bt1_highFlank;
long last_bt2_highFlank;
long last_bt3_highFlank;
bool lastBt2;
bool lastBt3;
byte lvl_one_ptr = 1;
byte final_lvl_1 = 0;
byte lvl_two_ptr = 0;
byte last_lvl_two_ptr;        //special for manual menu
byte final_lvl_2 = 0;
byte lvl_three_ptr = 0;
byte final_lvl_3 = 0;

byte program_amount;
byte program_choice;
bool program_running = false;
byte program_nodes;
byte current_node;

int lastCheck;
byte tick = 0;
long last_tick = 0;


void setup() {
  Serial.begin(57600);


  Serial.println("##################################################################");
  Serial.println("################ NEEDLEGATE ROBOT TOOL 1.0 #######################");
  Serial.println("##################################################################");
  Serial.println();
  Serial.println("robot starting up, stay away..");
  //  Braccio.begin(-70);
  pinMode(button1, INPUT);
  pinMode(button2, INPUT);
  pinMode(button3, INPUT);
}

void loop() {
  bt1_Tick = false;
  bt2_Tick = false;
  bt3_Tick = false;
  check_button_highFlank();

  menus();
  if (!program_running) {
    if (final_lvl_1 == 1) { // manual mode
      manual();
      move_robot();
    }
    if (final_lvl_1 == 2) { //programming
      manual();
      move_robot();
      menu_logic();
    }
    if (final_lvl_1 == 3) { // load program
      menu_logic();

    }
    if (final_lvl_1 == 4) { //Erase memory
      print_menu();
      menu_logic();
    }

    if (update_screen()) { // only updates if something changed (not perfect)
      print_screen();
    }

  } else {
    menu_logic();
    if (program_running) {
      if ( run_program(program_choice, program_nodes)) {
        Serial.println("program done");
        delay(1000);
      } else {
        print_screen();
        Serial.print("running next node: ");
        Serial.println(program_nodes);
      }
    }

  }
}
boolean run_program(byte program, byte nodes) {

  if (current_node > nodes) {
    program_running = false;
    Serial.println("program completed");
    return true;
  }

  bool moving_to_node = true;
  byte actual_pos[7];
  byte goal_pos[7];
  for (int i = 1; i < 7; i++) {
    actual_pos[i] = Robot.get_motor(i); // get actual pos
    goal_pos[i] = Prog.get_axis(program, current_node, i); //error <<... node 255?

  }
  byte completed = 0;
  for (int i = 1; i < 7; i++) {
    if (actual_pos[i] > goal_pos[i]) {
      Robot.decrement(i);
    } else if (actual_pos[i] < goal_pos[i]) {
      Robot.increment(i);
    } else {
      completed++;
    }
  }
  /// just 1 degree per lap


  if (completed >= 6) {
    current_node++;
  }

  moving_to_node = false;

  // move_auto(m1, m2, m3, m4, m5, m6);
  return false;
}
boolean update_screen() {
  long this_tick = millis();
  if (this_tick - last_tick > 1000) {
    tick += 1;
    last_tick = this_tick;
  }
  int a = digitalRead(button1);
  int b = digitalRead(button2);
  int c = digitalRead(button3);
  int tempx = analogRead(x_axis);
  int tempy = analogRead(y_axis);
  tempx = map(tempx, 0, 1023, 0, 100);
  tempy = map(tempy, 0, 1023, 0, 100);
  int r = 0;
  for (int i = 1; i < 7; i++) {
    int R =  Robot.get_motor(i);
    r = r + R;
  }
  int checkSum = a + b + c + r + tick;
  if (checkSum != lastCheck) {
    lastCheck = a + b + c + r + tick;
    return true;
  }
  lastCheck = a + b + c + r + tick;
  return false;
}
void manual() {     /// manually run robot. menu 1 /2

  x_pos = analogRead(x_axis);
  y_pos = analogRead(y_axis);
  long currentMillis = millis();

  if (y_pos > 490 && y_pos < 520) { //return if joystick is untouched
    return;
  }
  boolean increment;

  if (y_pos > 520) {    // determine if we want to increment or not
    increment = true;
  } else if (y_pos < 490) {
    increment = false;
  }

  if (increment) {         //increment robot pos for axis of choice
    int value = map(y_pos, 521, 1023, 1, 3);
    long thisMove = millis();
    long lastMove = Robot.get_lastMove(axis_choice);
    boolean timeToMove = false;
    if (thisMove - lastMove > 50 / value) {
      if (Robot.increment(axis_choice)) {
        //Serial.println(Robot.get_motor(axis_choice));
      }
    }
  }

  if (!increment) {         //decrement robot pos for axis of choice
    int value = map(y_pos, 490, 0, 1, 3);
    long thisMove = millis();
    long lastMove = Robot.get_lastMove(axis_choice);
    boolean timeToMove = false;
    if (thisMove - lastMove > 50 / value) {
      if (Robot.decrement(axis_choice)) {
        ;
      }
    }
  }

}
boolean save_node() {  //send the axis positions from Robot class to Prog class
  byte temp_axis[6];
  for (int i = 1; i < 7; i++) {
    temp_axis[i - 1] = Robot.get_motor(i);
  }
  if (Prog.set_node(temp_axis[0], temp_axis[1], temp_axis[2], temp_axis[3],
                    temp_axis[4], temp_axis[5])) {
    return true;
  } else {

    return false;
  }
}
boolean save_program() { // let programmer class know that the program is done
  Prog.set_end();

}

void move_robot() {
  byte m1 = Robot.get_motor(1);
  byte m2 = Robot.get_motor(2);
  byte m3 = Robot.get_motor(3);
  byte m4 = Robot.get_motor(4);
  byte m5 = Robot.get_motor(5);
  byte m6 = Robot.get_motor(6);
  Braccio.ServoMovement(3, m1, m2, m3, m4, m5, m6);
  //delay(5);;

}

void menus() {
  ////////////return button 3
  if (bt3_Tick) { //return 1 level
    if (lvl_one_ptr > 0 && lvl_two_ptr == 0 && lvl_three_ptr == 0) {
      final_lvl_1 = 0;
    }
    else if (lvl_two_ptr > 0 && lvl_three_ptr == 0) {
      lvl_two_ptr = 0;    //return to level 1
      final_lvl_2 = 0;
    } else if (lvl_three_ptr > 0) {
      lvl_three_ptr = 0; //return to level 2
      final_lvl_3 = 0;
    }
  }

  ////////cycle menu level - button 2
  if (bt2_Tick) { //cycle menus
    if (final_lvl_2 > 0) { //we are in level 3
      lvl_three_ptr = (lvl_three_ptr + 1) % 4;
      if (lvl_three_ptr == 0) //make sure we dont get zero
        lvl_three_ptr = 1;
      /////////////////////////////////////////////
    } else if (final_lvl_1 > 0 && final_lvl_3 == 0) { // lvl 2
      if (final_lvl_1 == 3) {
        lvl_two_ptr = (lvl_two_ptr + 1) % 4;
        if (lvl_two_ptr == 0) //make sure we dont get zero
          lvl_two_ptr = 1;
      } else if (final_lvl_1 == 2) { // special - create program
        axis_choice = (axis_choice + 1) % 7;
        if (axis_choice == 0) {
          axis_choice = 1;
        }
      }
      ///////// end special create menu
      else if (final_lvl_1 == 1) { //special - manual menu
        axis_choice = (axis_choice + 1) % 7;
        if (axis_choice == 0) {
          axis_choice = 1;
        }
      }
      ////// end special manual menu
    } else {
      lvl_one_ptr = (lvl_one_ptr + 1) % 5;        //lvl 1
      if (lvl_one_ptr == 0)
        lvl_one_ptr = 1;
    }
  }
  //////chose menu - button 1
  if (bt1_Tick) {  //select button 1
    byte temp;

    if (lvl_three_ptr > 0) {
      final_lvl_3 = lvl_three_ptr;

    } else if (lvl_two_ptr > 0 && lvl_three_ptr == 0) {
      final_lvl_2 = lvl_two_ptr;

    } else if (lvl_one_ptr > 0 && lvl_two_ptr == 0 && lvl_three_ptr == 0) {
      final_lvl_1 = lvl_one_ptr;
      if (final_lvl_1 == 3) {                  //special for loading menu
        program_amount = 0;
        bool check_program = true;
        while (check_program) {
          temp = Prog.get_program_list();
          if (temp == 254) {
            program_amount++;
          }
          if (temp == 0) {
            check_program = false;
          }
        }
      }
    }
  }

}


void print_screen() {
  delay(5);
  Serial.print("\033[2J"); // clear screen
  // print_logo();
  print_axis();
  Serial.println("");
  Serial.print(lvl_one_ptr); Serial.println(final_lvl_1);
  Serial.print(lvl_two_ptr); Serial.println(final_lvl_2);
  Serial.print(lvl_three_ptr); Serial.println(final_lvl_3);
  print_menu();

}
void print_menu() {
  char arrow = '<';
  char spear = '-';
  Serial.println();
  ///main menu
  if (final_lvl_1 == 0) {
    Serial.println("__MAIN MENU__");
    Serial.print("* manual drive   ");
    if (lvl_one_ptr == 1) {
      Serial.print(arrow);
      Serial.print(spear); Serial.println(spear);
    } else {
      Serial.println();
    }
    Serial.print("* create program ");
    if (lvl_one_ptr == 2) {
      Serial.print(arrow);
      Serial.print(spear); Serial.println(spear);
    } else {
      Serial.println();
    }
    Serial.print("* load program   ");
    if (lvl_one_ptr == 3) {
      Serial.print(arrow);
      Serial.print(spear); Serial.println(spear);
    } else {
      Serial.println();
    }
    Serial.print("* format EEPROM   ");
    if (lvl_one_ptr == 4) {
      Serial.print(arrow);
      Serial.print(spear); Serial.println(spear);
    } else {
      Serial.println();
    }
  }
  // level 2 manual menu
  //manual menu
  if (final_lvl_2 == 0 && final_lvl_1 == 1) {
    Serial.println("__MANUAL MENU__");
    Serial.println("manual drive");
    Serial.print("axis choosed: "); Serial.println(axis_choice);

  }
  // create program menu
  if (final_lvl_2 == 0 && final_lvl_1 == 2) {
    Serial.println("_Create Program_");
    Serial.println("manual drive - save node by clicking bt1");
    Serial.println(", hold bt1 2 secs to save program");
    Serial.print("axis choosed: "); Serial.println(axis_choice);
  }
  // list programs om EEPROM
  if (final_lvl_2 == 0 && final_lvl_1 == 3) {
    Serial.print("number of saved programs: ");
    Serial.println(program_amount);
    Serial.print("load program: "); Serial.println(program_choice);
  }
  /// erase EEPROM menu
  if (final_lvl_2 == 0 && final_lvl_1 == 4) {
    Serial.print(CLS);
    Serial.println("WARNING");
    Serial.println("this will erase all stored data on EEPROM");
    Serial.println("press 1 to ERASE or press 3 to go back to menu");
    Serial.println("continue?");
    delay(100);
  }
}
////////////////////////
////LOGIC//////////////
//////////////////////
void menu_logic() {
  byte temp = digitalRead(button1);   /// for reset for save button
  long currentMillis = millis();
  /// Erase menu
  if (final_lvl_2 == 0 && final_lvl_1 == 4) {
    bool choice = false;
    while (!choice) {

      if (get_bt3_highFlank()) {
        final_lvl_1 = 0;
        choice = true;
      }
      if (get_bt1_highFlank()) {
        final_lvl_1 = 0;
        Prog.format_EEPROM();
        program_amount = 0;
        choice = true;
      }
    }
  }
  /// programming menu
  if (final_lvl_1 == 2 && lvl_one_ptr == 2) { //prog menu
    if (bt1_Tick) {     ///set node
      if (save_node()) {
        Serial.println("saving node in program");
      } else {
        Serial.println("failed to save node");
      }

      delay(300);
    } else if (temp == 1 && currentMillis - last_bt1_highFlank > 2000 && currentMillis - last_bt1_highFlank < 3000) {
      Serial.println("Saving program");
      Prog.set_end();
      last_bt1_highFlank = millis();
      final_lvl_1 == 0;
      lvl_one_ptr == 0;
    }

  }

  /// load program
  if (final_lvl_1 == 3 && lvl_one_ptr == 3) {
    if (bt2_Tick) {
      program_choice = (program_choice + 1) % ( program_amount + 1);
      if (program_choice == 0) {
        program_choice = 1;
      }
    }
    if (bt1_Tick && program_choice != 0) {
      program_nodes =  Prog.get_program_node_amount(program_choice);

      program_running = true;
      current_node = 2;                           ///<<<<----------2 because of error in program class

    }
    if (bt3_Tick) {
      program_nodes = 0;
      program_running = false;
      program_choice = 0;
    }
  }

}
void print_axis() {
  Serial.println("axis angles:");
  Serial.print("M1"); Serial.print("  M2");
  Serial.print("  M3"); Serial.print("   M4"); Serial.print("   M5");
  Serial.println("  M6");
  Serial.print(Robot.get_motor(1)); Serial.print("  ");
  Serial.print(Robot.get_motor(2)); Serial.print("  ");
  Serial.print(Robot.get_motor(3)); Serial.print("  ");
  Serial.print(Robot.get_motor(4)); Serial.print("  ");
  Serial.print(Robot.get_motor(5)); Serial.print("  ");
  Serial.print(Robot.get_motor(6)); Serial.println("  ");
}
void print_logo() {
  Serial.print("\033[2J"); // clear screen
  Serial.println("##################################################################");
  Serial.println("################ NEEDLEGATE ROBOT TOOL 1.0 #######################");
  Serial.println("##################################################################");
  Serial.println();
  delay(1000);
}
boolean get_bt1_lowFlank() {
  bool b1 = digitalRead(button1);
  // delay(5);;
  if (!b1 && lastBt1) { // low flank
    lastBt1 = b1;
    return true;
  }
  lastBt1 = b1;
  return false;
}
boolean get_bt1_highFlank() {
  bool b1 = digitalRead(button1);
  // delay(5);;
  if (b1 && !lastBt1) { // high flank
    lastBt1 = b1;
    last_bt1_highFlank = millis();
    return true;
  }
  lastBt1 = b1;
  return false;
}
boolean get_bt2_highFlank() {
  bool b2 = digitalRead(button2);
  // delay(5);;
  if (b2 && !lastBt2) { // high flank
    lastBt2 = b2;
    return true;
  }
  lastBt2 = b2;
  return false;
}
boolean get_bt3_highFlank() {
  bool b3 = digitalRead(button3);
  // delay(5);;
  if (b3 && !lastBt3) { // high flank
    lastBt3 = b3;
    return true;
  }
  lastBt3 = b3;
  return false;
}
void check_button_highFlank() {
  if (get_bt1_highFlank()) {
    last_bt1_highFlank = millis();
    bt1_Tick = true;
  }
  if (get_bt2_highFlank()) {
    last_bt2_highFlank = millis();
    bt2_Tick = true;
  }
  if (get_bt3_highFlank()) {
    last_bt3_highFlank = millis();
    bt3_Tick = true;
  }
}