/*
 * Behavior-Based Control
 */

#include "Myro.h"

double cruiseSpeed = 0.8;
double turnSpeed = 0.8;
int lightThresh = 80;

struct rec {
    bool active;
    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) {
        return make_rec (true, 0, -turnSpeed);
    } else if (R) {
        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");

    if (L < lightThresh) {
        return make_rec (true, cruiseSpeed/2.0, turnSpeed);
    } else if (R < lightThresh) {
        return make_rec (true, cruiseSpeed/2.0, -turnSpeed);
    } else {
        return make_rec (false, 0, 0);
    }
}

rec arbitrate () {
    rec R;
    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();
}