/*#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#include <RTClib.h>
#include <Arduino.h>
#include <ESP8266WiFi.h>
#include <DFRobot_QMC5883.h>

#define PORT 10001
#define SERIAL_RATE 115200
#define SSID "Vyn"
#define PASSWD "test123321"
#define LOCATION "\x19\x19\x2A\x01\x31\x10\x18\x01"
#define VERSION "040101"


WiFiClient Client;
WiFiServer Server(PORT);

Servo servo;
RTC_DS1307 rtc;
DFRobot_QMC5883 compass(&Wire, 0x0D);

String recv();
void send(String str);
void reply(String str);
void printDate(DateTime date);
void printTime(DateTime time);
void syncDateTime(String str);


int16_t mx, my, mz;

void setup() {
  Serial.begin(SERIAL_RATE);
  WiFi.begin(SSID, PASSWD);

  while (!Serial);

  Serial.print("Conectando em SSID: ");
  Serial.print(SSID);

  while (WiFi.status() != WL_CONNECTED) {
    delay(250);
    Serial.print(".");
  }
  Serial.println();

  Serial.print("Conectado! Endereço: ");
  Serial.println(WiFi.localIP());

  Server.begin();
  Serial.println("Servidor iniciado!");

  while (!compass.begin()) {
    Serial.println("Could not find a valid 5883 sensor, check wiring!");
    delay(500);
  }

  if (!rtc.begin()) {
    Serial.println("Erro iniciando RTC.");
    while(1);
  }

  if (!rtc.isrunning()) {
    Serial.print("RTC não está rodando. Setando o tempo em: ");
    Serial.print(__DATE__);
    Serial.print(" ");
    Serial.println(__TIME__);
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }

  if(compass.isQMC()) {
    Serial.println("Initialize QMC5883");
    compass.setRange(QMC5883_RANGE_8GA);
    Serial.print("compass range is:");
    Serial.println(compass.getRange());

    compass.setMeasurementMode(QMC5883_CONTINOUS);
    Serial.print("compass measurement mode is:");
    Serial.println(compass.getMeasurementMode());

    compass.setDataRate(QMC5883_DATARATE_200HZ);
    Serial.print("compass data rate is:");
    Serial.println(compass.getDataRate());

    compass.setSamples(QMC5883_SAMPLES_8);
    Serial.print("compass samples is:");
    Serial.println(compass.getSamples());
  }

  pinMode(12, INPUT);
  pinMode(14, INPUT);

  DateTime now = rtc.now();
  printDate(now);
  printTime(now);
  pinMode(2, OUTPUT);

  servo.attach(2, 500, 2500);
}

void printDate(DateTime date) {
  Serial.print(date.day());
  Serial.print("/");
  Serial.print(date.month(), DEC);
  Serial.print("/");
  Serial.print(date.year(), DEC);
  Serial.print(" (");
  Serial.print(date.dayOfTheWeek(), DEC);
  Serial.println(")");
}

void printTime(DateTime time) {
  Serial.print(time.hour(), DEC);
  Serial.print(":");
  Serial.print(time.minute(), DEC);
  Serial.print(":");
  Serial.println(time.second(), DEC);
}

bool moving = true;
bool aligned = true;
uint8_t dst = 0;
uint8_t tz = 252;
String loc = "34AB0500,12CE0500";

void loop() {
  float declinationAngle = (-20-(15.0/60.0))/(180/PI);

  compass.setDeclinationAngle(declinationAngle);
  sVector_t mag = compass.readRaw();
  compass.getHeadingDegrees();
  Serial.print("X:");
  Serial.print(mag.XAxis);
  Serial.print(" Y:");
  Serial.print(mag.YAxis);
  Serial.print(" Z:");
  Serial.println(mag.ZAxis);
  Serial.print("Degress = ");
  Serial.println(mag.HeadingDegress);
  
  delay(250);

  Client = Server.accept();

  if (Client) {
    while (Client.connected()) {
      moving = digitalRead(12);
      aligned = digitalRead(14);
      Client.flush();

      String str = recv();

      //Empty response
      if (str.startsWith("Ç")) {
        continue;
      }

      ///HANDSHAKE
      //Ping
      if (str.startsWith("K")) {
        reply(str.substring(1));
      }
      //Firmware version
      if (str.startsWith("V")) {
        reply(VERSION);
      }
      //Location
      if (str.startsWith("w")) {
        reply(LOCATION);
      }
      //Tracking mode
      if (str.startsWith("t")) {
        String tmp = "F";
        tmp.setCharAt(0, '1');

        reply(tmp);
      }
      //Device version
      if (str.startsWith("P\x01")) {
        String tmp = "FF";
        tmp.setCharAt(0, '2');
        tmp.setCharAt(1, '3');

        reply(tmp);
      }
      //Mount model
      if (str.startsWith("m")) {
        String tmp = "F";
        tmp.setCharAt(0, 4);

        reply(tmp);
      }
      //Failed
      if (str.startsWith("##")) {
        Client.stop();
      }

      ///STATUS
      //GOTO?
      if (str.startsWith("L")) {
        reply(moving ? "1" : "0");
      }
      //Aligned?
      if (str.startsWith("J")) {
        String tmp = "F";
        tmp.setCharAt(0, aligned ? 1 : '0');
        
        reply(tmp);
      }
      if (str.startsWith("M")) {
        reply("");
      }
      

      ///SYNC
      //Position
      if (str.startsWith("W")) {
        //TODO: store pos
        reply("");
      }
      //Hour and date
      if (str.startsWith("H")) {
        syncDateTime(str.substring(1));
        reply("");
      }
      if (str.startsWith("h")) {
        DateTime now = rtc.now();
        String datetime = "FFFFFFFF";

        datetime.setCharAt(0, now.hour());
        datetime.setCharAt(1, now.minute());
        datetime.setCharAt(2, now.second());
        datetime.setCharAt(3, now.month());
        datetime.setCharAt(4, now.day());
        datetime.setCharAt(5, now.year()-2000);
        datetime.setCharAt(6, tz);
        datetime.setCharAt(7, dst);

        datetime.replace("\n", "\x10");

        reply(datetime);
      }
      //Precise RA/DEC
      if (str.startsWith("e")) {
        reply(loc);
      }
      if (str.startsWith("r")) {
        loc = str.substring(1);
        String rightAscencion = loc.substring(0, 8);
        Serial.println(rightAscencion);
        String decimation = loc.substring(9);
        Serial.println(decimation);

        double ra = strtoul(rightAscencion.c_str(), NULL, 16);
        ra = ra/4294967296;
        ra = ra*360;

        double dec = strtoul(decimation.c_str(), NULL, 16);
        dec = dec/4294967296;
        dec = dec*360;
        
        

        reply("");
      }
    }

    if (!Client.connected()) {
      Serial.println("Cliente desconectado :(");
      Client.stop();
    }
  }
}


void syncDateTime(String str) {
  tz = str.charAt(6);
  dst  = str.charAt(7);

  uint8_t hr = str.charAt(0);
  uint8_t mn = str.charAt(1);
  uint8_t sc = str.charAt(2);
  uint8_t mo = str.charAt(3);
  uint8_t dy = str.charAt(4);
  uint16_t y = str.charAt(5);

  hr = dst ? hr-1 : hr;

  DateTime dt = DateTime(
    y, mo, dy,
    hr, mn, sc
  );

  if (!dt.isValid()) {
    char temp[] = "DD/MM/YY hh:mm:ss";

    Serial.print("Data recebida é invalida! Impossível sincronizar: ");
    Serial.println(dt.toString(temp));
    return;
  }

  rtc.adjust(dt);
  dt = rtc.now();

  Serial.println("Tempo ajustado!");
  printDate(dt);
  printTime(dt);
}

String recv() {
  String str = "";

  while (Client.available()) {
    str += (char)(Client.read());
  }

  if (str.length() == 0) {
    str = "Ç";
  } else {
    Serial.print("R: ");
    Serial.println(str);
  }

  return str;
}

void reply(String str) {
  Client.flush();
  Serial.print("S: ");
  Serial.println(str += "#");
  byte n = Client.print(str.c_str());
  Serial.print("S: ");
  Serial.print(n);
  Serial.println("b");
}*/
