#include <Servo.h>

Servo myservo;

int Echo = A4;
int Trig = A5;

#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11

#define SERVO_PIN 3

bool modoAutomatico = true;
char comandoBT = 0;

const int InputNodes = 3;
const int HiddenNodes = 4;
const int OutputNodes = 4;

int i, j;
double Accum;
double Hidden[HiddenNodes];
double Output[OutputNodes];

float HiddenWeights[3][4] = {
 {1.8991509, -0.47694725, -0.64836902, -0.38609165},
 {-0.28186109, 4.0406957, 3.2291858, -2.8943011},
 {0.33406508, -1.40161144, 1.35800539, -0.98141597}
};

float OutputWeights[4][4] = {
 {1.13607229, 1.54602394, 1.61946122, 1.88190666},
 {-1.5469665, 1.39519307, 0.19393826, 0.30992504},
 {-0.77559824, 0.93908086, 2.08625107, -1.12294842},
 {-1.23570903, 0.85839302, 0.72470207, 0.97628527}
};

unsigned long previousMillis = 0;
const long interval = 25;
int grados_servo = 90;
bool clockwise = true;

void stopMotores() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

int Distance_test() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  return pulseIn(Echo, HIGH) / 58;
}

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

  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  myservo.attach(SERVO_PIN);
  myservo.write(90);

  stopMotores();
}

void loop() {
  if (Serial1.available()) {
    comandoBT = Serial1.read();

    if (comandoBT == 'A') modoAutomatico = true;
    if (comandoBT == 'M') modoAutomatico = false;

    if (!modoAutomatico) controlManual(comandoBT);
  }

  if (modoAutomatico) {
    controlAutomatico();
  }
}

void controlManual(char cmd) {
  analogWrite(ENA, 120);
  analogWrite(ENB, 120);

  if (cmd == 'F') { IN1HIGH(); IN3HIGH(); }
  if (cmd == 'B') { IN2HIGH(); IN4HIGH(); }
  if (cmd == 'L') { IN2HIGH(); IN3HIGH(); }
  if (cmd == 'R') { IN1HIGH(); IN4HIGH(); }
  if (cmd == 'S') stopMotores();
}

void IN1HIGH(){ digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW); }
void IN2HIGH(){ digitalWrite(IN1,LOW); digitalWrite(IN2,HIGH); }
void IN3HIGH(){ digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW); }
void IN4HIGH(){ digitalWrite(IN3,LOW); digitalWrite(IN4,HIGH); }

void controlAutomatico() {
  int distancia = Distance_test();
  if (distancia < 25) {
    digitalWrite(IN2, HIGH);
    digitalWrite(IN4, HIGH);
    analogWrite(ENA, 150);
    analogWrite(ENB, 150);
    delay(300);
  } else {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN3, HIGH);
    analogWrite(ENA, 120);
    analogWrite(ENB, 120);
  }
}