#include <ACB_ARM.h>
#include <WiFi.h>

#define FIRMWARE_VERSION "ACB_ARM V2.0"

ACB_ARM ARM;

// WiFi user name and password
const char* ssid = "Robot_Arm";
const char* password = "12345678";

int Chassis_input, Shoulder_input, Elbow_input, Claws_input;
int Chassis_slide, Shoulder_slide, Elbow_slide, Claws_slide;
int PTPX,PTPY,PTPZ;
bool record = false;

void setup() {    // initialize
  
  Serial.begin(115200);  // set the baud rate to 115200

  ARM.Chassis_angle_adjust(8);  // Default 0
  ARM.Slight_adjust(0,2);   // Default 0,0
  ARM.ARM_init(5,16,17,18); // The parameters are four Servo pins
  ARM.JoyStick_init(32,33,34,35,36,39); // Joystick initialization

  WiFi.setTxPower(WIFI_POWER_19_5dBm);  // Set the transmit power of WiFi
  WiFi.mode(WIFI_AP); // Set WiFi mode to AP (Access Point) mode
  WiFi.softAP(ssid, password, 9); // Set the WiFi hotspot name and password, and enable AP mode
  IPAddress myIP = WiFi.softAPIP(); // Obtain the IP address of the AP mode
  Serial.print("AP IP address: ");
  Serial.println(myIP); // address: 192.168.4.1
  
  ARM.startWebServer();
}

void loop() {
  //---------------------JoyStick---------------------
  handleJoystick();

  //---------------------  W E B  --------------------
  // NULL
  if (ARM.val == 3){

  }

  // button
  else if (ARM.val == 1){
    ARM.web_claw_x();
  } else if (ARM.val == 2){
    ARM.web_claw_y();
  } else if (ARM.val == 4){
    ARM.web_turn_x();
  } else if (ARM.val == 5){
    ARM.web_turn_y();
  } else if (ARM.val == 6){
    ARM.web_lower_arm_x();
  } else if (ARM.val == 7){
    ARM.web_lower_arm_y();
  } else if (ARM.val == 8){
    ARM.web_upper_arm_x();
  } else if (ARM.val == 9){
    ARM.web_upper_arm_y();
  }

  // Slide input
  else if (ARM.val == 21) {         // chassis
    Chassis_input = ARM.Chassis_Silde_Angle;
    ARM.ChassisCmd(Chassis_input);  
    ARM.chassis_angle = Chassis_input;
  } else if (ARM.val == 22) {         // shoulder
    Shoulder_input = ARM.Shoulder_Silde_Angle;
    ARM.ShoulderCmd(Shoulder_input);  
    ARM.shoulder_angle = Shoulder_input;
  } else if (ARM.val == 23) {         // elbow
    Elbow_input = ARM.Elbow_Silde_Angle;
    ARM.ElbowCmd(Elbow_input);  
    ARM.elbow_angle = Elbow_input;
  } else if (ARM.val == 24) {         // claws
    ARM.Speed(50);
    Claws_input = ARM.Claws_Silde_Angle;
    ARM.ClawsCmd(Claws_input); 
    ARM.claws_angle = Claws_input; 
  } 

  // Silde control
  else if (ARM.val == 25) {         // chassis
    Chassis_slide = ARM.Chassis_Silde_Angle;
    ARM.JoyChassisCmd(Chassis_slide);  
    ARM.chassis_angle = Chassis_slide;
  } else if (ARM.val == 26) {         // shoulder
    Shoulder_slide = ARM.Shoulder_Silde_Angle;
    ARM.JoyShoulderCmd(Shoulder_slide);  
    ARM.shoulder_angle = Shoulder_slide;
  } else if (ARM.val == 27) {         // elbow
    Elbow_slide = ARM.Elbow_Silde_Angle;
    ARM.JoyElbowCmd(Elbow_slide);  
    ARM.elbow_angle = Elbow_slide;
  } else if (ARM.val == 28) {         // claws
    Claws_slide = ARM.Claws_Silde_Angle;
    ARM.JoyClawsCmd(Claws_slide);  
    ARM.claws_angle = Claws_slide; 
  } 

  // Mode control
  else if (ARM.val == 31 && record) {         // save
    ARM.saveState();
    delay(200);
    ARM.val = 3;
  } else if (ARM.val == 32) {         // end Record
    record = true;
  } else if (ARM.val == 33) {         // start Record
    record = false;
  } else if (ARM.val == 34 && !record) {         // Run
    ARM.executeStates();
    delay(200);
  } else if (ARM.val == 55 && !record) {         // Run
    ARM.executeStates();
    delay(200);
  } else if (ARM.val == 35 && !record) {         // Reset
    ARM.clearSavedStates();
    delay(200);
    ARM.val = 3;
  } 

  // Mode select 1-6
  else if (ARM.val == 40) {           // 0
    ARM.mode = 0;
  } else if (ARM.val == 41) {         // 1
    ARM.mode = 1;
  } else if (ARM.val == 42) {         // 2
    ARM.mode = 2;
  } else if (ARM.val == 43) {         // 3
    ARM.mode = 3;
  } else if (ARM.val == 44) {         // 4
    ARM.mode = 4;
  } else if (ARM.val == 45) {         // 5
    ARM.mode = 5;
  } else if (ARM.val == 46) {         // 6
    ARM.mode = 6;
  }
  
  // Spatial coordinate
  else if (ARM.val == 51) {         // x
    PTPX = ARM.PTP_X;
  } else if (ARM.val == 52) {         // y
    PTPY = ARM.PTP_Y;
  } else if (ARM.val == 53) {         // z
    PTPZ = ARM.PTP_Z;
  } else if (ARM.val == 54) {         // ptp
    ARM.PtpCmd(PTPX,PTPY,PTPZ);
    delay(100);
    ARM.val = 3;
  }

}

void handleJoystick() {
  ARM.get_JoyStick();

  if (ARM.JoyY1 < 50) {  // chassis left
    ARM.chassis_angle = ARM.chassis_angle + 1;
    ARM.JoyChassisCmd(ARM.chassis_angle);
  }

  if (ARM.JoyY1 > 3500) { // chassis right
    ARM.chassis_angle = ARM.chassis_angle - 1;
    ARM.JoyChassisCmd(ARM.chassis_angle);
  }

  if (ARM.JoyX1 < 50) {  // Shoulder down
    ARM.shoulder_angle = ARM.shoulder_angle + 1;
    ARM.JoyShoulderCmd(ARM.shoulder_angle);
  }

  if (ARM.JoyX1 > 4000) {  // Shoulder up
    ARM.shoulder_angle = ARM.shoulder_angle - 1;
    ARM.JoyShoulderCmd(ARM.shoulder_angle);
  }

  if (ARM.JoyX2 < 50) { // Elbow up 
    ARM.elbow_angle = ARM.elbow_angle + 1;
    ARM.JoyElbowCmd(ARM.elbow_angle);
  }
  
  if (ARM.JoyX2 > 4000) { // Elbow dwon 
    ARM.elbow_angle = ARM.elbow_angle - 1;
    ARM.JoyElbowCmd(ARM.elbow_angle);
  }

  if (ARM.JoyY2 > 4000) { // Claws open
    ARM.claws_angle = ARM.claws_angle + 1;
    ARM.JoyClawsCmd(ARM.claws_angle);
  }

  if (ARM.JoyY2 < 50) {  // Claws close
    ARM.claws_angle = ARM.claws_angle - 1;
    ARM.JoyClawsCmd(ARM.claws_angle);
  }
}


