#include <Adafruit_NeoPixel.h>
#include <Servo.h>
const int neopixelPin = 2, numPixel = 1;
Adafruit_NeoPixel NeoPixel(numPixel, neopixelPin, NEO_GRB + NEO_KHZ800);
Servo servo;
const int servoPin = 8;
long duration1, duration2; int distance1, distance2; double space;
const int trig1 = 4, echo1 = 5, trig2 = 6, echo2 = 7;
const int camera = 3;
#define PI 3.1415926535897932384626433832795
int maxMistake = 10, maxSpeed = 7, car, carr;
unsigned long long time1;

void setup() {
  pinMode(trig1, OUTPUT);
  pinMode(trig2, OUTPUT);
  pinMode(camera, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(echo2, INPUT);
  servo.attach(servoPin);
  servo.write(0);
  NeoPixel.begin();
  brightness(96);
  blue(1);
  show();
  delay(100);
  digitalWrite(trig1, LOW);
  delayMicroseconds(2);
  digitalWrite(trig1, HIGH);
  delayMicroseconds(8);
  digitalWrite(trig1, LOW);
  duration1 = pulseIn(echo1, HIGH);
  distance1 = duration1*0.017175;
  double servoPoint = atan(100.00/distance1)*180/PI;
  servo.write(servoPoint);
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(8);
  digitalWrite(trig2, LOW);
  duration2 = pulseIn(echo2, HIGH);
  distance2 = duration2*0.017175;
  space = sqrt(sq(distance2) - sq(distance1)) + servoPoint/3.0 + int(servoPoint*100)%100/6.0;
  Serial.begin(115200);
}

void loop() {
  if (distance2 >= 399) {
    red(1);
    show();
    return;
  }
  green(1);
  show();
  digitalWrite(trig1, LOW);
  delayMicroseconds(2);
  digitalWrite(trig1, HIGH);
  delayMicroseconds(8);
  digitalWrite(trig1, LOW);
  duration1 = pulseIn(echo1, HIGH);
  int index1 = duration1*0.017175;
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(8);
  digitalWrite(trig2, LOW);
  duration2 = pulseIn(echo2, HIGH);
  int index2 = duration2*0.017175;
  if (index1 < distance1 - maxMistake) {
    carr = 1;
  } else {
    carr = 0;
  }
  if (index2 < distance2 - maxMistake && carr == 0) {
    car = 1;
    time1 = micros();
  }
  if ((car == 1) && (index1 < distance1 - maxMistake)) {
    double speed = space/(micros() - time1)*0.00036;
    if (speed > maxSpeed && micros() - time1 >= 50) {
      digitalWrite(camera, HIGH);
      yellow(1);
      show();
      delay(1000);
      digitalWrite(camera, LOW);
      green(1);
      show();
    }
    Serial.println(speed);
    car = 0;
  }
}

void red(int rd) {
  NeoPixel.setPixelColor(rd-1, 227, 12, 12);
}

void blue(int bl) {
  NeoPixel.setPixelColor(bl-1, 16, 69, 230);
}

void green(int gr) {
  NeoPixel.setPixelColor(gr-1, 62, 232, 10);
}

void yellow(int yl) {
  NeoPixel.setPixelColor(yl-1, 247, 244, 3);
}

void show() {
  NeoPixel.show();
}

void brightness(int b) {
  b = map(b, 0, 100, 0, 255);
  NeoPixel.setBrightness(b);
}