//Code by Graham Gier, USC ID# 7227272017

#include <Myro.h>
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <cstdlib>

using namespace std;
double ambient;

double normalize(int v) {
    if (v>ambient)
    {
        v=ambient;
    }
return 1.0-v/ambient;
}

void alive()
{
    ambient = (robot.getLight("left") +
               robot.getLight("center") +
               robot.getLight("right")) / 3.0;
    // Braitenberg vehicle#1: Alive
     while (true) {
        int L = robot.getLight("center");
        robot.forward(normalize(L)); }
    disconnect();
}

void coward()
{
    ambient = (robot.getLight("left") +
               robot.getLight("center") +
               robot.getLight("right")) / 3.0;
    while (true) {
        int L = robot.getLight("left");
        int R = robot.getLight("right"); robot.motors(normalize(L), normalize(R));
    }
    disconnect();
}

void aggressive()
{
    ambient = (robot.getLight("left") +
               robot.getLight("center") +
               robot.getLight("right")) / 3.0;
    while (true) {
        int R = robot.getLight("right");
        int L = robot.getLight("left"); robot.motors(normalize(R), normalize(L));
    }
    disconnect();
}

void indecisive()
{
    ambient = (robot.getLight("left") +
               robot.getLight("center") +
               robot.getLight("right")) / 3.0;
  while(true)
  {
      if (robot.getLight("center") <= ambient)
          robot.forward(1, 1);
      else robot.backward(1,1);
  }
    
}

int main()
{
    connect("/dev/tty.Fluke2-0221-Fluke2");
    cout<<"choose a behavior"<<endl;
    cout<<"alive= 1"<<endl;
    cout<<"coward= 2"<<endl;
    cout << "aggressive= 3"<< endl;
    cout << "indecisive= 4" << endl;
    int input;
    cin >> input;
    if (input == 1)   alive();
    else if (input ==2) coward();
    else if (input == 3) aggressive();
    else if (input == 4) indecisive();
        disconnect();
        return 0;
    }