#include <Arduino.h>
#include "LARS.h"
#include <WiFi.h>
#include <ESPUI.h>

const char* ssid = "Lars";

#define TIME_INTERVAL 5000

/*
#define FORWARD 'f'
#define LEFT 'l'
#define STAND 's'
#define RIGHT 'r'
#define BACKWARD 'b'
#define GO 'g'
#define RIGHT_FRONT 'c'
#define RIGHT_BACK 'e'
#define LEFT_FRONT 'd'
#define LEFT_BACK 'h'
*/


LARS robot;

bool state = true;



boolean walk_forward = false;
boolean walk_backward = false;
boolean hello = false;
boolean turn_left = false;
boolean turn_right = false;
boolean moonwalk = false;
boolean audience = false;
boolean omni = false;
boolean omni_left = false;
boolean dance = false;
boolean upDown = false;
boolean PushUp = false;

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_AP);
  WiFi.setHostname(ssid);
  WiFi.softAP(ssid);
  //WiFi.softAP(ssid, password);
  Serial.println("");
  Serial.print("IP address: ");
  Serial.println(WiFi.softAPIP());


  ESPUI.pad("Walking Control", true, &MegaMaxim);
  ESPUI.button("Wave  Button", &AudienceButton);
  ESPUI.button("MoonWalk  Button", &BeerButton);
  ESPUI.button("Dance  Button", &BeerButton3);
  ESPUI.button("PushUp  Button", &BeerButton4);
  ESPUI.button("upDown  Button", &BeerButton5);
  ESPUI.button("Slow Turn Right", &BeerButton6);
  ESPUI.button("Slow Turn left", &BeerButton7);

  ESPUI.begin("Crabby Control");
  
  robot.init();
  delay(1000);
}

void AudienceButton(int id, int type) {
  if (type == B_DOWN) {
    audience = true;
  }
  else {
    audience = false;
  }
}


void BeerButton(int id, int type) {
  if (type == B_DOWN) {
    moonwalk = true;
  }
  else {
    moonwalk = false;
  }
}

void BeerButton3(int id, int type) {
  if (type == B_DOWN) {
    dance = true;
  }
  else {
    dance = false;
  }
}
void BeerButton4(int id, int type) {
  if (type == B_DOWN) {
    PushUp = true;
  }
  else {
    PushUp = false;
  }
}
void BeerButton5(int id, int type) {
  if (type == B_DOWN) {
    upDown = true;
  }
  else {
    upDown = false;
  }
}
void BeerButton6(int id, int type) {
  if (type == B_DOWN) {
    omni = true;
  }
  else {
    omni = false;
  }
}

void BeerButton7(int id, int type) {
  if (type == B_DOWN) {
    omni_left = true;
  }
  else {
    omni_left = false;
  }
}

void MegaMaxim (int id, int value) {
  switch (value) {
    case P_LEFT_DOWN:
      turn_left = true;
      break;
    case P_LEFT_UP:
      turn_left = false;
      break;
    case P_RIGHT_DOWN:
      turn_right = true;
      break;
    case P_RIGHT_UP:
      turn_right = false;
      break;
    case P_FOR_DOWN:
      walk_forward = true;
      break;
    case P_FOR_UP:
      walk_forward = false;
      break;
    case P_BACK_DOWN:
      walk_backward = true;
      break;
    case P_BACK_UP:
      walk_backward = false;
      break;
    case P_CENTER_DOWN:
      hello = true;
      break;
    case P_CENTER_UP:
      hello = false;
      break;
  }
}



void loop() {

if (walk_forward) robot.walk();
else if (walk_backward) robot.walk(0);
else if (hello) robot.hello();
else if (turn_left) robot.turnL(1, 550);
else if (turn_right) robot.turnR(1, 550);
else if (moonwalk) robot.moonwalkL(1, 5000);
else if (audience) robot.wave();
else if (dance) robot.dance(1, 600);
else if (omni) robot.omniWalk(1, 600, true, 1);
else if (omni_left) robot.omniWalk(1, 600, false, 1);
else if (PushUp) robot.pushUp(1, 600);
else if (upDown) robot.upDown(1, 5000);
else robot.home();

}

/*
void serialEvent() {
  char tmp = -1;
  boolean taken = false;

  while (Serial.available()) {
    state = !state;
 //   analogWrite(LED_PIN, 128 * state); //flashing LED indicating cmd received
    tmp = Serial.read (); //Serial.println(cmd);
   // not_gaits(tmp);
    taken = gaits(tmp);
    if (taken) cmd = tmp;
  }
}


boolean  gaits(char cmd) {
  bool taken = true;
  switch (cmd) {
    case GO:
      robot.walk();
      break;
    case FORWARD:
      robot.hello();
      break;
    case BACKWARD:
      //robot.run(0);
      robot.walk(0);
      break;
    case RIGHT:
      robot.turnR(1, 550);
      break;
    case LEFT:
      robot.turnL(1, 550);
      break;
    case RIGHT_FRONT:
      robot.turnR(1, 550);
      robot.walk();
      break;
    case RIGHT_BACK:
      robot.turnR(1, 550);
      robot.walk(0);
      break;
    case LEFT_FRONT:
      robot.turnL(1, 550);
      robot.walk();
      break;
    case LEFT_BACK:
      robot.turnL(1, 550);
      robot.walk(0);
      break;
    case STAND:
      robot.home();
      break;
    default:
      taken = false;
  }
  return taken;
}
*/