/*
 * Behavior-Based Control
 */
#include <iostream>
#include "Myro.h"
using namespace std;

double cruiseSpeed = -0.5; // negative to move in sensor direction
double turnSpeed = 0.5;
int lightThresh = 250;

struct rec { //a recommendation
    bool active; //if actually making a recommendation
    double translateSpeed;
    double rotateSpeed;
};

rec make_rec (bool a, double t, double r) {
    rec R;
    R.active = a;
    R.translateSpeed = t;
    R.rotateSpeed = r;
    return R;
}

rec cruise () {
    // is always active, just move forward
    return make_rec (true, cruiseSpeed, 0);
}

rec avoid () {
    // see if there are any obstacles
    bool L = ! robot.getIR("left"); // L is true if obst. on left
    bool R = ! robot.getIR("right");// R is true if obst. on right
    if (L) {
    	cout << "obstacle on left\n";
        return make_rec (true, 0, -turnSpeed);
    } else if (R) {
    	cout << "obstacle on right\n";
        return make_rec (true, 0, turnSpeed);
    } else {
        return make_rec (false, 0, 0);
    }
}

rec seekLight () {
    int L = robot.getLight("left");
    int R = robot.getLight("right");
    cout << "L = " << L << " R = " << R << endl;

    if (L < lightThresh) {
    	cout << "seeking light on left\n";
        return make_rec (true, cruiseSpeed/2.0, turnSpeed);
    } else if (R < lightThresh) {
    	cout << "seeking light on right\n";
        return make_rec (true, cruiseSpeed/2.0, -turnSpeed);
    } else {
        return make_rec (false, 0, 0);
    }
}

rec arbitrate () {
    rec R;
    // priority-based behavioral control
    R = seekLight(); if (R.active) return R;
    R = avoid();     if (R.active) return R;
    R = cruise();    if (R.active) return R;
    return make_rec (false, 0, 0); // should never happen!
}


int main () {
    connect();

    while (true) {
        rec decision = arbitrate ();
        robot.move (decision.translateSpeed,
                    decision.rotateSpeed);
    }

    disconnect();
}