#define BLYNK_PRINT Serial
#include <BlynkSimpleEsp8266.h>
#include <cmath>

char auth[] = "auth";
char ssid[] = "ssid";
char pass[] = "path";

int PWMA = D5;
int AIN2 = D7;
int AIN1 = D6;
int STBY = D8;
int PWMB = D0;
int BIN1 = D2;
int BIN2 = D1;
int ISON = D4;
int CONN = D3;

int speedMA = 0;
int directionMA = 0;
int speedMB = 0;
int directionMB = 0;

int radius = 0;
int angle = 0;

BLYNK_WRITE(V1)
{
  radius = Radius(param[0].asInt(), param[1].asInt());
  angle = Angle(param[0].asInt(), param[1].asInt());
}

void setup()
{
  pinMode(D0, OUTPUT);
  pinMode(D1, OUTPUT);
  pinMode(D2, OUTPUT);
  pinMode(D3, OUTPUT);
  pinMode(D4, OUTPUT);
  pinMode(D5, OUTPUT);
  pinMode(D6, OUTPUT);
  pinMode(D7, OUTPUT);
  pinMode(D8, OUTPUT);
  digitalWrite(ISON, HIGH);
  Serial.begin(9600);
  Blynk.begin(auth, ssid, pass, IPAddress(192, 168, 1, 8));
}

void loop()
{
  Blynk.run();

  if (Blynk.connected()) {
    digitalWrite(CONN, HIGH);
  } else {
    digitalWrite(CONN, LOW);
  }

  Serial.println(analogRead(A0));

  return;

  if (radius > 200) {

    if (angle >= 45 && angle < 135 ) {

      forwardMA(radius);
      forwardMB(radius);

    } else if (angle >= 135 && angle < 225 ) {

      forwardMA(radius);
      reverseMB(radius);

    } else if (angle >= 225 && angle < 315 ) {

      reverseMA(radius);
      reverseMB(radius);

    } else if (angle >= 315 || angle > 45 ) {

      reverseMA(radius);
      forwardMB(radius);

    }

  } else {

    stopMAB();
  }

  Serial.println();

}

void forwardMA(int speed) {

  digitalWrite(STBY, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  analogWrite(PWMA, speed);
  Serial.print("MA Forward Speed: ");
  Serial.print(speed);
}

void reverseMA(int speed) {

  digitalWrite(STBY, HIGH);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  analogWrite(PWMA, speed);
  Serial.print("MA Reverse Speed: ");
  Serial.print(speed);
}

void forwardMB(int speed) {

  digitalWrite(STBY, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMB, speed);
  Serial.print(" MB Forward Speed: ");
  Serial.print(speed);
}

void reverseMB(int speed) {

  digitalWrite(STBY, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMB, speed);
  Serial.print(" MB Reverse Speed: ");
  Serial.print(speed);
}

void stopMAB() {

  digitalWrite(STBY, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
  Serial.print("MAB Stop");

}

int Radius(int x, int y)
{
  int rad;

  rad = sqrt((pow(x, 2)) + (pow(y, 2)));

  if (rad < 1023) {
    return rad;
  } else {
    return 1023;
  }
}
int Angle(int x, int y)
{
  float Thta;

  if (y >= 0) {

    Thta = PI / 2 - atan(x / (y + 0.000001));

  } else {

    Thta = 3 * PI / 2 - atan(x / (y + 0.000001));

  }

  int deg = Thta * 57.2957795;

  return deg;
}