/*
  ESP8266 Test 
  This is a TEST program to use WEMOS D1 Mini chip to control movement of the wheels
  Without WIFI or BLYNK  
  */
// Template ID, Device Name and Auth Token are provided by the Blynk.Cloud
// See the Device Info tab, or Template settings
#define BLYNK_TEMPLATE_ID           "TMPLxiEgDKTj"
#define BLYNK_DEVICE_NAME           "Quickstart Device"
#define BLYNK_AUTH_TOKEN            "NUp13hOHFLdRvrkPsoGcf2Rm03d4aC2Q"

// Comment this out to disable prints and save space
#define BLYNK_PRINT Serial

#include <Servo.h>
#include <ESP8266WiFi.h>
#include <DNSServer.h>
#include <ESP8266WebServer.h>
#include <EEPROM.h>
#include <BlynkSimpleEsp8266.h>

// Need this to match how we connect WEMOSD1 Mini ESP8266 to 
//https://create.arduino.cc/projecthub/lightthedreams/mecanum-wheel-omni-direction-wifi-rc-car-01a883?ref=part&ref_id=11332&offset=1
//output to motor
// Need this to match how we connect WEMOSD1 Mini ESP8266 to 
#define ENB D2      //ENB
#define MOTORB_1 D4 //IN3
#define MOTORB_2 D5 //IN4
#define MOTORA_1 D7 //IN1
#define MOTORA_2 D6 //IN2
#define ENA D8      //ENA
#define WEMOSD1MINI_D0 16
 


//Configure the below lines
// Your WiFi credentials.
// Set password to "" for open networks.
char ssid[] = "Rock";
char pass[] = "brandonisthebest";
char auth[] = BLYNK_AUTH_TOKEN;

double move_y = 0;
double turn = 0;

Servo forklift;
//End Configuration

void setup() {
   Serial.begin(9600);
  Serial.println("Getting in the setup"); 
  pinMode(LED_BUILTIN, OUTPUT);     // Initialize the LED_BUILTIN pin as an output

  pinMode(ENA, OUTPUT); // Enable Speed
  pinMode(MOTORA_1, OUTPUT); // Direction
  pinMode(MOTORA_2, OUTPUT); // Direction
  pinMode(ENB, OUTPUT); // Enable Speed
  pinMode(MOTORB_1, OUTPUT); // Direction
  pinMode(MOTORB_2, OUTPUT); // Direction

 
  digitalWrite(ENA,LOW);
  digitalWrite(ENB,LOW);

  //Set D0 to Output and low
  pinMode (WEMOSD1MINI_D0, OUTPUT);
  digitalWrite (WEMOSD1MINI_D0, LOW);
  // Connect Blynk
  Blynk.begin(auth, ssid, pass);
  //Sync with values saved since last sleep
  delay(500);  
  Serial.println("Syncing with Blynk");    
  delay(3000);
  Serial.println("Done!");

  digitalWrite(LED_BUILTIN, HIGH);
}

// Main code
void loop() {
  Blynk.run();
  move(move_y, turn);
}

BLYNK_WRITE(V8) {
  turn = param.asDouble();
}

BLYNK_WRITE(V9) {
  Serial.println("y");
  move_y = param.asDouble();
  Serial.println(move_y);
}


void move(double fwd, double turn){

    if (turn != 0) {
        if (turn >0) {
          MotorLeft(); 
        } else {
          MotorRight();
        }
    } else if (fwd != 0) {
      // turn by turn
       if (fwd>0) {
         MotorFwd();
       } else {
         MotorBack();
       }
    } else {
       MotorStop();
    }
}

void MotorFwd() {
  Serial.println("Moving forward");
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(MOTORA_1,HIGH);
  digitalWrite(MOTORB_1,LOW);
  digitalWrite(MOTORA_2,LOW);
  digitalWrite(MOTORB_2,HIGH);
}

// Motor Control BACKWARD
void MotorBack() {
  Serial.println("Backing up");
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(MOTORA_1,LOW);
  digitalWrite(MOTORA_2,HIGH);
  digitalWrite(MOTORB_1,HIGH);
  digitalWrite(MOTORB_2,LOW);
}

// Motor Control LEFT
void MotorRight() {
  Serial.println("Turning right");
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(MOTORA_1,LOW);
  digitalWrite(MOTORA_2,HIGH);
  digitalWrite(MOTORB_1,LOW);
  digitalWrite(MOTORB_2,HIGH);
}

// Motor Control RIGHT
void MotorLeft() {
  Serial.println("Turning left");
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(MOTORA_1,HIGH);
  digitalWrite(MOTORA_2,LOW);
  digitalWrite(MOTORB_1,HIGH);
  digitalWrite(MOTORB_2,LOW);
}

void MotorStop() {
  Serial.println("Stopping motor");
  digitalWrite(ENA,LOW);
  digitalWrite(ENB,LOW);
  digitalWrite(MOTORA_1,LOW);
  digitalWrite(MOTORA_2,LOW);
  digitalWrite(MOTORB_1,LOW);
  digitalWrite(MOTORB_2,LOW);
}