#include "AsyncUDP.h"
#include "WiFi.h"
#include <Wire.h>
#include <ZX_Sensor.h>


const char * ssid = "Module-325-00";  /// MODULE TERMS
const char * password = "zirokits";

bool WifiConnected;
AsyncUDP udp;
int cntr = 0;
int trigPin = 18;    // Trigger
int echoPin = 19;    // Echo
int trigPin2 = 17;    // Trigger
int echoPin2 = 16;    // Echo
double duration, inches;
boolean wall = false;
double duration2, inches2;
boolean wall2 = false;
const int ZX_ADDR = 0x10;    // ZX Sensor I2C address
uint8_t x_pos;
uint8_t x[3];
int difference[2];
ZX_Sensor zx_sensor = ZX_Sensor(ZX_ADDR);
int gesture = 1;
int temp_gesture = 0;
const int NONE = 0, START = 1, STOP = 2;

void setup() {
  uint8_t ver = 0;

  //Serial Port begin
  Serial.begin (19200);

  // Initialize ZX Sensor (configure I2C and read model ID)
  if ( zx_sensor.init() ) {
    Serial.println("ZX Sensor initialization complete");
  } else {
    Serial.println("Something went wrong during ZX Sensor init!");
  }

  // Read the model version number and ensure the library will work
  ver = zx_sensor.getModelVersion();
  if ( ver == ZX_ERROR ) {
    Serial.println("Error reading model version number");
  } else {
    Serial.print("Model version: ");
    Serial.println(ver);
  }
  if ( ver != ZX_MODEL_VER ) {
    Serial.print("Model version needs to be ");
    Serial.print(ZX_MODEL_VER);
    Serial.print(" to work with this library. Stopping.");
    Serial.print(ver);
  }

  // Read the register map version and ensure the library will work
  ver = zx_sensor.getRegMapVersion();
  if ( ver == ZX_ERROR ) {
    Serial.println("Error reading register map version number");
  } else {
    Serial.print("Register Map Version: ");
    Serial.println(ver);
  }
  if ( ver != ZX_REG_MAP_VER ) {
    Serial.print("Register map version needs to be ");
    Serial.print(ZX_REG_MAP_VER);
    Serial.print(" to work with this library. Stopping.");
  }

  //Try to connect to the Wifi 10 times
  while (WifiConnected == false && cntr < 10) {
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
    if (WiFi.waitForConnectResult() != WL_CONNECTED) {
      WifiConnected = false;
      Serial.println("WiFi Failed");
    }
    cntr++;
  }
  if (udp.listen(2325)) {
    Serial.print("UDP Listening on IP: ");
    WifiConnected = true;
    Serial.println(WiFi.localIP());
    udp.onPacket([](AsyncUDPPacket packet) {
      Serial.print("UDP Packet Type: ");
      Serial.print(packet.isBroadcast() ? "Broadcast" : packet.isMulticast() ? "Multicast" : "Unicast");
      Serial.print(", From: ");
      Serial.print(packet.remoteIP());
      Serial.print(":");
      Serial.print(packet.remotePort());
      Serial.print(", To: ");
      Serial.print(packet.localIP());
      Serial.print(":");
      Serial.print(packet.localPort());
      Serial.print(", Length: ");
      Serial.print(packet.length());
      Serial.print(", Data: ");
      Serial.write(packet.data(), packet.length());
      Serial.println();
      //reply to the client
      packet.printf("Got %u bytes of data", packet.length());
      pinMode(trigPin, OUTPUT);
      pinMode(echoPin, INPUT);
      pinMode(trigPin2, OUTPUT);
      pinMode(echoPin2, INPUT);
    });
  }
}

void loop() {
  GestureLoop();
  if (gesture == START) {
    UltraSonicLoop();
    if (!(wall || wall2)) {
      Forward();
    } else {
      Turn();
    }
  } else {
    udp.broadcast("{""pType"":7,""m_T"":[0,0,0,0,0,0,0],""m_M"":[0,0,0,0,0,0,0]}");
  }
  delay(50);
}

void UltraSonicLoop() {
  //Send a low pulse before the high pulse to cancel the majority of the white noise
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  //Read the input from the high pulse only to avoid unnecessary noise
  duration = pulseIn(echoPin, HIGH);

  digitalWrite(trigPin2, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);

  duration2 = pulseIn(echoPin2, HIGH);

  //Conversion from time to inches based off the speed of sound in air
  inches = (duration / 2) * 0.01351417;
  inches2 = (duration2 / 2) * 0.01351417;

  //Check for a wall less than 7 inches away
  (inches <= 7 /*&& inches != 0*/) ? wall = true : wall = false;
  (inches2 <= 7 /*&& inches2 != 0*/) ? wall2 = true : wall2 = false;
}

void Forward() {
  udp.broadcast("{""pType"":7,""m_T"":[55,55,0,0,0,0,0],""m_M"":[2,1,0,0,0,0,0]}");
}

void Turn() {
  if (wall && wall2) {
    //if there is a wall  on both sides go backwards
    udp.broadcast("{""pType"":7,""m_T"":[55,55,0,0,0,0,0],""m_M"":[2,1,0,0,0,0,0]}");
  } else if (wall) {
    //If there is a wall on the right turn left to avoid it
    udp.broadcast("{""pType"":7,""m_T"":[55,55,0,0,0,0,0],""m_M"":[2,2,0,0,0,0,0]}");
  } else if (wall2) {
    //If there is a wall on the left turn right to avoid it
    udp.broadcast("{""pType"":7,""m_T"":[55,55,0,0,0,0,0],""m_M"":[1,1,0,0,0,0,0]}");
  }
}

void GestureLoop() {
  if ( zx_sensor.gestureAvailable() ) {
    temp_gesture = zx_sensor.readGesture();
    if (temp_gesture == RIGHT_SWIPE) {
      //Sensor positioned so a forward swipe will start it
      gesture = START;
      temp_gesture = 0;
    } else if (temp_gesture == LEFT_SWIPE) {
      //Sensor positioned so a backward swipe will stop it
      gesture = STOP;
      temp_gesture = 0;
    }
  }
}