#include <SPI.h>
#include <WiFi.h>
#include <Servo.h>
#include <WiFiUdp.h>

char ssid[] = "";                    //    name
char pass[] = "";
int PORT = 6969;

bool F = 0, L = 0, R = 0;

// Sensor setup
int echoPin = 2;
int initPin = 3;
long pulseTime = 0;
long distance = 0;

Servo myServo;
int servoPin = 9;
int pos = 0;

// Motor setup
int in[] = {0, 4, 5, 6, 7};

// Controling Setup
void process(int t){
    switch (t){
        case 1:
            F = true;
            break;
        case 2:
            F = false;
            break;
        case 3:
            L = true;
            break;
        case 4:
            L = false;
            break;
        case 5:
            R = true;
            break;
        case 6:
            R = false;
            break;
        default:
            Serial.println("Command invalid");
    }
}

int speed = 100;

void MoveFoward(int iMotor){
    if (iMotor == 1){
        digitalWrite(in[1],HIGH);
        analogWrite(in[2],255-speed);
    } else {
        digitalWrite(in[4],LOW);
        analogWrite(in[3],speed);
    }
}

void MoveBackward(int iMotor){
    if (iMotor == 1){
        digitalWrite(in[1], LOW);
        analogWrite(in[2], speed);
    } else {
        digitalWrite(in[4], HIGH);
        analogWrite(in[3],255 - speed);
    }
}

void MoveFoward(){
    MoveFoward(1);
    MoveFoward(2);
}

void MoveBackward(){
    MoveBackward(1);
    MoveBackward(2);
}

void TurnLeft(){
    MoveBackward(1);
    MoveFoward(2);
}

void TurnRight(){
    MoveBackward(2);
    MoveFoward(1);
}
void StopMotor(int iMotor){
  if (iMotor == 1){
    digitalWrite(in[1],LOW);
    analogWrite(in[2],0);
  }
  else{
    digitalWrite(in[4],HIGH);
    analogWrite(in[3],255);
  }
}

void StopMotors(){
    StopMotor(1);
    StopMotor(2);
}

void DistanceCalculate(){
    digitalWrite(initPin,LOW);
    digitalWrite(initPin,HIGH);
    delayMicroseconds(10);
    digitalWrite(initPin,LOW);
    pulseTime = pulseIn(echoPin,HIGH);
    distance = pulseTime/58;
    delay(10);
}

void ServoTurn(Servo svr, int pos){
    svr.write(pos);
}

void ServoSweep(int delayTime){
    for (pos = 0; pos < 180; pos += 1){
        ServoTurn(myServo,pos);
        delay(delayTime);
    }

    for (pos = 180; pos > 0; pos -= 1){
        ServoTurn(myServo,pos);
        delay(delayTime);
    }
}

// Server Setup

int keyIndex = 0;                        // network key Index number (needed only for WEP)
int status = WL_IDLE_STATUS;
WiFiServer server(PORT);
bool alreadyConnected = false; // whether or not the client was connected previously

void setup() {
    Serial.begin(9600);

    myServo.attach(servoPin); // Servo attach
    // Sensor in-out
    pinMode(initPin,OUTPUT);
    pinMode(echoPin,INPUT);

    for (int i = 1 ; i <= 4 ; i += 1) pinMode(in[i],OUTPUT); // Motor

    //while (!Serial) {}
    if (WiFi.status() == WL_NO_SHIELD) {
        Serial.println("WiFi shield not present");
        while(true);
    }

    String fv = WiFi.firmwareVersion();
    if (fv != "1.1.0") Serial.println("Firmware outdated");

    // attempt to connect to Wifi network:
    while (status != WL_CONNECTED) {
        Serial.print("Attempting to connect to SSID: ");
        Serial.println(ssid);
        // Connect to WPA/WPA2 network
        status = WiFi.begin(ssid, pass);

        delay(5000);
    }
    server.begin();
    printWifiStatus();
 }


void loop() {
    WiFiClient client = server.available();
   DistanceCalculate();
    if (client) { // first byte
        if (!alreadyConnected) {
            client.flush();
            client.print("xxx");
            Serial.println("New client");
            alreadyConnected = true;
        }
        if (client.available() > 0) {
            // read incoming bytes from the client
            int thisChar = client.read();
            Serial.println(thisChar);
            process(thisChar);
            // echo back to the client
            Serial.println("Nhan cmnr");
            server.write(thisChar);
            Serial.println("Gui cmnr");
        }
        if (L) TurnLeft();
        else if (R) TurnRight();
        if (F) MoveFoward();

        if (!L && !R && !F) StopMotors();
        if ((distance < 200 && distance > 2)){
          int x[] = {7, distance, pos};
          for (int i = 0; i < 3; ++i) server.write(x[i]);
        }
    }
}


void printWifiStatus() {
    Serial.print("SSID: ");
    Serial.println(WiFi.SSID());

    IPAddress ip = WiFi.localIP();
    Serial.print("IP Address: ");
    Serial.println(ip);

    long rssi = WiFi.RSSI();
    Serial.print("signal strength (RSSI):");
    Serial.print(rssi);
    Serial.println(" dBm");
}