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

const char * networkName = "OTEfd57e0";
const char * networkPswd = "akup8407";
const char * udpAddress = "192.168.1.2";
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;

int count = 0;
int flag = 0;

int circle = 2048 * 3.25;

char *buffer1;
char *buffer2;
float distance;
float x,y;
int angle;
int max_angle,max_d,f_d;
int flag_lastTurn[2];

int step();

void setup() {
  flag_lastTurn[0]=0;
  flag_lastTurn[1]=1;
  Wire.begin();

  if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
  {
    Serial.println("Sensor online!");
  }
  //distanceSensor.setDistanceModeShort();//Sets it to max 1.3 works better with diffrent ambient lights
  distanceSensor.setTimingBudgetInMs(120);
  distanceSensor.setIntermeasurementPeriod(150);
  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);
  buffer2 = (char*) malloc((sizeof(int)+(3*sizeof(float)) ) * sizeof(char) + 1);
  while (digitalRead(HOMING_PIN) == HIGH)step();
  while (digitalRead(HOMING_PIN) == LOW) {
    step();
  }
  x=0;
  y=0;
}

void loop() {
   sprintf (buffer1, "%c%lu",'F',1000);
   Serial2.print(buffer1);
    max_angle=-1;
  max_d=-1;
  f_d=0;
   //if(count%circle==0)delay(5000);
   for(count=0;count<circle-1;){
       count += step();

   if (count % 18 == 0) {
     delay(150);
     distanceSensor.startRanging();
     distanceSensor.stopRanging();
     while(!distanceSensor.checkForDataReady()){};
     distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
     if(distanceSensor.getRangeStatus()==0){
       if (connected) {
         //Send a packet
         udp.beginPacket(udpAddress, udpPort);
         angle = ceil(count * 0.054086538);
         sprintf(buffer2, "%d,%f,%f,%f",angle,distance/10,x*10,y*10);
         udp.printf("%s",buffer2);
         udp.endPacket();
       }
       if(distance>max_d){
        max_d=distance;
        max_angle=angle;
        }
     }
     if(count==0){
      if(distance>18||distanceSensor.getRangeStatus()==4){
         f_d=1;
        }
      }
     // Serial.print("ANGLE:");
     // Serial.print(angle);
     // Serial.print(" DISTANCE:");
     // Serial.println(distance/10);
     // Serial.println(buffer2);
   }

 }
   count = count % circle-1;
   if(f_d!=1){
   		if(max_angle>180){
   		  sprintf (buffer1, "%c%lu",'L',850);
       	  Serial2.print(buffer1);
   		}
   		else
   		{
   		  sprintf (buffer1, "%c%lu",'R',850);
       	  Serial2.print(buffer1);
   		}
   }
   delay(1000);
   /*
   if(f_d!=1){
    if(max_angle>180){
       sprintf (buffer1, "%c%lu",'L',850);
       Serial2.print(buffer1);
       delay(500);
       if(flag_lastTurn[0]==1){
        switch(flag_lastTurn[1]){
            case 1:
                  // x-=12.5;
                    break;
            case 0:
                  x+=12.5;
                    break;
         }
        }
       else
       //y-=12.5;
       sprintf (buffer1, "%c%lu",'F',1000);
       Serial2.print(buffer1);
      }
     else{
       sprintf (buffer1, "%c%lu",'R',850);
       Serial2.print(buffer1);
       delay(500);
              if(flag_lastTurn[0]==1){
        switch(flag_lastTurn[1]){
            case 1:
                   x+=12.5;
                    break;
            case 0:
                 // x-=12.5;
                    break;
         }
        }
       else
       y+=12.5;
      }
       sprintf (buffer1, "%c%lu",'F',1000);
       Serial2.print(buffer1);
   }
   else{
       sprintf (buffer1, "%c%lu",'F',1000);
       Serial2.print(buffer1);
       x+=12.5;
       flag_lastTurn[0]=0;
       flag_lastTurn[1]=0;
   }*/

}


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;
  }
}