#include <ESP8266WiFi.h>
#include <WebSocketsServer.h>
#include <ESP8266WebServer.h>
#include <ESP8266mDNS.h>
#include "FS.h"

volatile int estado = HIGH;

volatile int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
volatile int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;

unsigned long loop_timer;
unsigned char Dir_m_left=D5;
unsigned char Step_m_left=D6;
unsigned char Dir_m_right=D3;
unsigned char Step_m_right=D4;
bool FWD=HIGH;
bool BWD=LOW;
 volatile int valor=0;

int descI=0;
int descR=0;


extern "C" {
#include "user_interface.h"
}

os_timer_t myTimer;

bool tickOccured;

// start of timerCallback
void timerCallback(void *pArg) {
  throttle_counter_left_motor ++;                                           //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
      if(throttle_counter_left_motor > throttle_left_motor_memory){             //If the number of loops is larger then the throttle_left_motor_memory variable
          throttle_counter_left_motor = 0;                                        //Reset the throttle_counter_left_motor variable
          throttle_left_motor_memory = throttle_left_motor;                       //Load the next throttle_left_motor variable
    if(throttle_left_motor_memory < 0){                                     //If the throttle_left_motor_memory is negative
          digitalWrite(Dir_m_left,BWD);                                                //Set output 3 low to reverse the direction of the stepper controller
          throttle_left_motor_memory *= -1;                                     //Invert the throttle_left_motor_memory variable
    }
    else digitalWrite(Dir_m_left,FWD);                                               //Set output 3 high for a forward direction of the stepper motor
  }
  else if(throttle_counter_left_motor == 1)digitalWrite(Step_m_left,HIGH);             //Set output 2 high to create a pulse for the stepper controller
  else if(throttle_counter_left_motor == 2)digitalWrite(Step_m_left,LOW);             //Set output 2 low because the pulse only has to last for 20us

  //right motor pulse calculations
  throttle_counter_right_motor ++;                                          //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
  if(throttle_counter_right_motor > throttle_right_motor_memory){           //If the number of loops is larger then the throttle_right_motor_memory variable
    throttle_counter_right_motor = 0;                                       //Reset the throttle_counter_right_motor variable
    throttle_right_motor_memory = throttle_right_motor;                     //Load the next throttle_right_motor variable
    if(throttle_right_motor_memory < 0){                                    //If the throttle_right_motor_memory is negative
      digitalWrite(Dir_m_right,BWD);                                                     //Set output 5 low to reverse the direction of the stepper controller
      throttle_right_motor_memory *= -1;                                    //Invert the throttle_right_motor_memory variable
    }
    else digitalWrite(Dir_m_right,FWD);;                                               //Set output 5 high for a forward direction of the stepper motor
  }
  else if(throttle_counter_right_motor == 1)digitalWrite(Step_m_right,HIGH);             //Set output 4 high to create a pulse for the stepper controller
  else if(throttle_counter_right_motor == 2)digitalWrite(Step_m_right,LOW);


} // End of timerCallback



void user_init(void) {

      os_timer_setfn(&myTimer, timerCallback, NULL);
      os_timer_arm(&myTimer, 1, true);
} // End of user_init

const char* ssid = "mdchaparror";
const char* password = "un260874";
int contconexion = 0;


ESP8266WebServer server = ESP8266WebServer(80);

void setup() {
 pinMode(Dir_m_left,OUTPUT);
 pinMode(Step_m_left,OUTPUT);
 pinMode(Dir_m_right,OUTPUT);
 pinMode(Step_m_right,OUTPUT);
 //digitalWrite(Dir_m_left,HIGH);


 Serial.begin(115200);
 SPIFFS.begin();


 Serial.println("");
 Serial.println("--------------------------");
 Serial.println("ESP8266 Motor Test");
 Serial.println("--------------------------");

 user_init();



 WiFi.mode(WIFI_STA); //para que no inicie el SoftAP en el modo normal
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED and contconexion <50) { //Cuenta hasta 50 si no se puede conectar lo cancela
    ++contconexion;
    delay(500);
    Serial.print(".");
  }
  if (contconexion <50) {

      Serial.println("");
      Serial.println("WiFi conectado");
      Serial.println(WiFi.localIP());
  }
  else {
      Serial.println("");
      Serial.println("Error de conexion");
  }


  if(MDNS.begin("motores")) {
    Serial.println("MDNS responder started");
  }

  // handle index
  server.on("/", []() {
      File index = SPIFFS.open("/index.html", "r");
    server.streamFile(index, "text/html");
    index.close();
  });

  server.on("/stick", []() {                  // Recibe los datos (dx y dy)
    String dx=server.arg("dx");
    String dy=server.arg("dy");
    server.send(200);
    int x = dx.toInt();                       // Convierte los datos de tipo String a Int
    int y = dy.toInt();


  valor = map(abs(y),0,255,20,3);
  if (y==0)valor=0;
  if(y<0) valor*=-1;

if(x>100){
  descI =3;
  descR =-3;
}
if(x<-100) {
 descI =-3;
 descR =3;

}
if(x<=100 && x>=-100) {
 descI =0;
 descR =0;

}


  throttle_left_motor = valor+descI;
  throttle_right_motor = valor+descR;

Serial.println(valor);

   // Serial.print(x);  Serial.print("  ");  Serial.println(y);
  });

  server.begin();

  // Add service to MDNS
  MDNS.addService("http", "tcp", 80);
  MDNS.addService("ws", "tcp", 81);

}

void loop() {


  server.handleClient();
  // throttle_left_motor = 10;
  //throttle_right_motor = 10;
  while(loop_timer > micros());
  loop_timer += 4000;

}