/* File: mazeSolving.ch
   the NXT robot will solve a maze based on the line following technique */
#include <mindstorms.h>
#include <math.h>

/* define numbers of colors */
#define RED   5
#define WHITE 6

/* flags for situations the robot encounters */
#define ERROR       -1
#define NOBRANCH     0
#define LEFTBRANCH   1
#define RIGHTBRANCH  2
#define DEADEND      3
#define FINISH       4

CMindstorms robot;

double calibration = 7.2;
double radius = 2.8;
double trackwidth = 11.2;

int i;
int branchStatus;  // used to store the current status of brach the robot meets
int gTargetPower = 3;  // global variable of power that outputs when the difference between lightValueLeft and lightValueRight is 0

int lineFollowing();  // a self-defined function which performs line following 
int checkBranch();  // a self-defined function which checks the current status of branch

void turnLeft();  // a self-defined function that makes left turn
void turnRight();  // a self-defined function that makes right turn

robot.setSensorLight(PORT3, "Reflect");
robot.setSensorColor(PORT4, "Color");
robot.setSensorColor(PORT1, "Color");

/* a for-loop filtering out values read within the first three times because usually these readings are incorrect */
for (i = 0; i < 3; i++) { 
    branchStatus = checkBranch();
}

/* an infinite while-loop which keeps the robot running */
while (branchStatus != ERROR) {
    branchStatus = lineFollowing();
    
    /* if-statements to determine which one of the situations the robot faces  */
    if (branchStatus == LEFTBRANCH) {
        robot.setJointPower(PORT3, gTargetPower);
        robot.setJointPower(PORT2, gTargetPower);
        robot.driveDistance(calibration, radius);
        turnLeft();
    }
    else if (branchStatus == RIGHTBRANCH) {
        robot.setJointPower(PORT3, gTargetPower);
        robot.setJointPower(PORT2, gTargetPower);
        robot.driveDistance(calibration, radius);
        turnRight();
    }
    else if (branchStatus == DEADEND) {
        turnRight();
    }
    else if (branchStatus == FINISH) {
        robot.holdJoints();
        break;
    }
}

int lineFollowing() {
    double offset;  // how much to subtract from a raw light reading to convert it to an error value
    double black = 30,  // light reading for color black
           white = 40;  // light reading for color white
    double error;  // it tells us how far off the line's edge we are
    double Kp = 0.14;  // proportional constant
    double turn;  // output of the P controller
    double targetPower = 10;  // the power level of both motors when the error is 0 and the robot goes straight forward
    
    int branchStatus;
    int lightValue;
    int powerLeft,
        powerRight;

    offset = (white + black)/2;

    branchStatus = checkBranch();
    /* an while-loop which keeps the robot performing line following when there is no branch arround */
    while (branchStatus == NOBRANCH) {
        robot.getSensorLight(PORT3, lightValue);
        
        error = lightValue - offset;
        
        turn = Kp*error;
        
        powerLeft = round(targetPower + turn);
        powerRight = round(targetPower - turn);
        
        robot.setJointPower(PORT3, powerLeft);
        robot.setJointPower(PORT2, powerRight);
        
        robot.moveForeverNB();
        
        branchStatus = checkBranch();
    }

    robot.holdJoints();  // stop motors
    
    return branchStatus;
}

int checkBranch() {
    int colorValueLeft,  // value of the left color sensor reading
        lightValueCenter,  // value of the center light sensor reading
        colorValueRight;  // value of the right color sensor reading
    
    robot.getSensorColor(PORT4, colorValueLeft);
    robot.getSensorLight(PORT3, lightValueCenter);
    robot.getSensorColor(PORT1, colorValueRight);
    
    if (colorValueLeft == RED || colorValueRight == RED) {  // the robot has arrived the finish line
        return FINISH;
    }
    else if (colorValueRight != WHITE) {  // right turn always comes first then comes left turn so we check if there a right branch ahead first
        return RIGHTBRANCH;
    }
    else if (lightValueCenter < 60) {  // moving straight forward also comes first then the left turn so we know that if there is no right branch ahead then it must be a straight trial or a left branch
        return NOBRANCH;
    }
    else if (colorValueLeft != WHITE) {  // if there is no straight trial ahead then it goes to check if there is a left turn 
        return LEFTBRANCH;
    }
    else {  // if none of the situations above is met then we got only the situation of a dead end left
        return DEADEND;
    }
    
    return ERROR;  // if sensors are reading wrong values
}

void turnLeft() {
    int lightValue = 100;

    robot.turnLeft(45, radius, trackwidth);

    robot.setJointPower(PORT3, -gTargetPower);
    robot.setJointPower(PORT2, gTargetPower);

    robot.moveForeverNB();
    
    robot.waitUntil(PORT3, "<", 40);  // the motors stop not until the reading from light sensor is less than 40
    robot.holdJoints();
}

void turnRight() {
    int lightValue = 100;

    robot.turnRight(45, radius, trackwidth);

    robot.setJointPower(PORT3, gTargetPower);
    robot.setJointPower(PORT2, -gTargetPower);

    robot.moveForeverNB();

    robot.waitUntil(PORT3, "<", 60); // the motors stop not until the reading from light sensor is less than 60
    robot.holdJoints();
}