// FollowTV
// Vije Miller

#include "Servo.h"

const int atpin = 2;
const int aepin = 3;
const int btpin = 4;
const int bepin = 5;
const int ctpin = 6;
const int cepin = 7;

// For Cycle
const int num = 15;
int i = 0;
// Limit Distance
const int limit = 5500;
int hold = 500;

long adur, atot, afix;
long bdur, btot, bfix;
long cdur, ctot, cfix;

Servo lfs; // Left Front Speaker
const int lfsout = 69;
const int lfsin = 92;

Servo mon; // Monitor
const int monl = 145;
const int monc = 91;
const int monr = 45;

Servo rfs; // Right Front Speaker
const int rfsin = 63;
const int rfsout = 78;

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

  pinMode(atpin, OUTPUT);
  pinMode(aepin, INPUT);
  pinMode(btpin, OUTPUT);
  pinMode(bepin, INPUT);
  pinMode(ctpin, OUTPUT);
  pinMode(cepin, INPUT);

  lfs.attach(8);
  mon.attach(9);
  rfs.attach(10);

  digitalWrite(atpin, LOW);
  digitalWrite(btpin, LOW);
  digitalWrite(ctpin, LOW);

  lfs.write(lfsin);
  mon.write(monc);
  rfs.write(rfsin);

}

void loop() {

  atot = 0;
  for (i=0; i <= num; i++){
    digitalWrite(atpin, HIGH);
    delayMicroseconds(10);
    digitalWrite(atpin, LOW);
    adur = pulseIn(aepin, HIGH);
    if (adur > limit) {
      adur = limit;
    }
    atot = atot + adur;
  }
  afix = atot / num;

  btot = 0;
  for (i=0; i <= num; i++){
    digitalWrite(btpin, HIGH);
    delayMicroseconds(10);
    digitalWrite(btpin, LOW);
    bdur = pulseIn(bepin, HIGH);
    if (bdur > limit) {
      bdur = limit;
    }
    btot = btot + bdur;
  }
  bfix = btot / num;

  ctot = 0;
  for (i=0; i <= num; i++){
    digitalWrite(ctpin, HIGH);
    delayMicroseconds(10);
    digitalWrite(ctpin, LOW);
    cdur = pulseIn(cepin, HIGH);
    if (cdur > limit) {
      cdur = limit;
    }
    ctot = ctot + cdur;
  }
  cfix = ctot / num;

  Serial.print(afix);
  Serial.print("  ");
  Serial.print(bfix);
  Serial.print("  ");
  Serial.println(cfix);

  if (afix < bfix && afix < cfix) {
    // Position 2
    lfs.write(lfsout);
    mon.write(monl);
    rfs.write(rfsin);
  } 
  else if (bfix < afix && bfix < cfix) {
    // Position 1 and 3
    lfs.write(lfsin);
    mon.write(monc);
    rfs.write(rfsin);
  } 
  else if (cfix < afix && cfix < bfix) {
    // Position 4
    lfs.write(lfsin);
    mon.write(monr);
    rfs.write(rfsout);
  } 
  else if (afix == limit && bfix == limit && cfix == limit) {
    // Position Open
    lfs.write(lfsout);
    mon.write(monc);
    rfs.write(rfsout);
  }

  delay(hold);
}