#include <Wire.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <SparkFun_VL53L1X.h>

const char * networkName = "SSID";
const char * networkPswd = "PASS";
const char * udpAddress = "192.168.1.76";
const int udpPort = 6000;
SFEVL53L1X distanceSensor;
WiFiUDP udp;
boolean connected = false;

#define       MOTOR_PHASE_A           26
#define       MOTOR_PHASE_B           25
#define       MOTOR_PHASE_C           33
#define       MOTOR_PHASE_D           32
#define       HOMING_PIN              35
unsigned long starttime;
unsigned long endtime;
float  rotationtime;
int count = 0;
int flag = 0;

int circle = 2048 * 3.25;

char *buffer1;
char buffer2[20];
int distance;
int angle;

int step();

void setup() {
  Wire.begin();
  if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
  {
    Serial.println("Sensor online!");
  }

  connectToWiFi(networkName, networkPswd);
   Serial2.begin(9600);
  Serial.begin(9600);
  Serial.println("Setup");
  pinMode(MOTOR_PHASE_A, OUTPUT);
  pinMode(MOTOR_PHASE_B, OUTPUT);
  pinMode(MOTOR_PHASE_C, OUTPUT);
  pinMode(MOTOR_PHASE_D, OUTPUT);
  pinMode(HOMING_PIN, INPUT);
  starttime = millis();
  buffer1 = (char*) malloc((sizeof(char) + sizeof(unsigned long)) * sizeof(char) + 1);
  while (digitalRead(HOMING_PIN) == HIGH)step();
  while (digitalRead(HOMING_PIN) == LOW) {
    step();
  }
  delay(2000);
}

void loop() {
  //if(count%circle==0)delay(5000);
  for(count=0;count<circle-1;){
      count += step();

  if (count % 18 == 0) {
    distanceSensor.startRanging();
    distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
    distanceSensor.stopRanging();
    if (connected) {
      //Send a packet
      udp.beginPacket(udpAddress, udpPort);
      angle = ceil(count * 0.054086538);
      sprintf(buffer2, "%d,%f",angle,(float)distance);
      udp.printf("%s",buffer2);
      udp.endPacket();
    }
    Serial.print("ANGLE:");
    Serial.print(angle);
    Serial.print(" DISTANCE:");
    Serial.println(distance/10);
    Serial.println(buffer2);
  }

}

  //  if(digitalRead(HOMING_PIN)==LOW)
  //        Serial.println("LOW");
  //  else
  //  Serial.println("HIGH");
  count = count % circle-1;


     sprintf (buffer1, "%c%lu",'F',2500);
     Serial2.print(buffer1);
     delay(5000);

  /*
      sprintf (buffer1, "%c%lu",'L',200);
    Serial2.print(buffer1);
    delay(2000);

      sprintf (buffer1, "%c%lu",'F',200);
    Serial2.print(buffer1);
      delay(2000);
      sprintf (buffer1, "%c%lu",'R',200);
    Serial2.print(buffer1);
       delay(2000);*/


  /*


      if(count==2048*3.25){
        endtime=millis();
        rotationtime=(endtime-starttime)/1000;
        if(flag==0){
              Serial.print("It took:");
        Serial.print(rotationtime);
        Serial.println("seconds");
        flag++;
        }
      }
      count=count%2048*3.25; */


}


int step() {
  static int count;

  switch (count) {
    case 0:
      {
        digitalWrite(MOTOR_PHASE_D, HIGH);
        digitalWrite(MOTOR_PHASE_C, LOW);
        digitalWrite(MOTOR_PHASE_B, LOW);
        digitalWrite(MOTOR_PHASE_A, LOW);
      }
      break;
    case 1:
      {
        digitalWrite(MOTOR_PHASE_D, LOW);
        digitalWrite(MOTOR_PHASE_C, HIGH);
        digitalWrite(MOTOR_PHASE_B, LOW);
        digitalWrite(MOTOR_PHASE_A, LOW);
      }
      break;
    case 2:
      {
        digitalWrite(MOTOR_PHASE_D, LOW);
        digitalWrite(MOTOR_PHASE_C, LOW);
        digitalWrite(MOTOR_PHASE_B, HIGH);
        digitalWrite(MOTOR_PHASE_A, LOW);
      }
      break;
    case 3:
      {
        digitalWrite(MOTOR_PHASE_D, LOW);
        digitalWrite(MOTOR_PHASE_C, LOW);
        digitalWrite(MOTOR_PHASE_B, LOW);
        digitalWrite(MOTOR_PHASE_A, HIGH);
      }
      break;


  }
  delay(8);
  count++;
  count = count % 4;

  return 1;
}



void connectToWiFi(const char * ssid, const char * pwd) {
  Serial.println("Connecting to WiFi network: " + String(ssid));

  // delete old config
  WiFi.disconnect(true);
  //register event handler
  WiFi.onEvent(WiFiEvent);

  //Initiate connection
  WiFi.begin(ssid, pwd);

  Serial.println("Waiting for WIFI connection...");
}

//wifi event handler
void WiFiEvent(WiFiEvent_t event) {
  switch (event) {
    case SYSTEM_EVENT_STA_GOT_IP:
      //When connected set
      Serial.print("WiFi connected! IP address: ");
      Serial.println(WiFi.localIP());
      //initializes the UDP state
      //This initializes the transfer buffer
      udp.begin(WiFi.localIP(), udpPort);
      connected = true;
      break;
    case SYSTEM_EVENT_STA_DISCONNECTED:
      Serial.println("WiFi lost connection");
      connected = false;
      break;
    default: break;
  }
}