#include <mindstorms.h>
#include <math.h>

CMindstorms robot;

double ultraValueLeft,  // value of the left ultrasonic sensor reading
       ultraValueRight;  // value of the right ultrasonic sensor reading
double powerFactor,  // the ration we want to multiply to the difference between the value of ultraValueLeft and ultraValueRight
       powerFactorMax = 0.5,  // the adjustable desired maximum powerFactor
       powerFactorMin = 0;  // the minimum powerFactor
double ultraValueMax = 60,  // the maximum possible ultrasonic sensor reading
       ultraValueMin = 0;  // the minimum possible ultrasonic sensor reading
double turnLeft,  // correction value of left motor power
       turnRight;  // correction value of right motor power
double targetPower = 10;  // the power level of both motors when the error is 0 and the robot goes straight forward

int powerLeft,
    powerRight;

robot.setSensorUltrasonic(PORT2, "Centimeter");
robot.setSensorUltrasonic(PORT3, "Centimeter");
/* an infinite while-loop which keeps the following robot to track the leading robot */
while (1) {
    robot.getSensorUltrasonic(PORT2, ultraValueLeft);
    robot.getSensorUltrasonic(PORT3, ultraValueRight);
    /* set the left ultrasonic sensor reading to 60 if it is larger than 60 */
    if (ultraValueLeft > 60) {
        ultraValueLeft = 60;
    }
    /* set the right ultrasonic sensor reading to 60 if it is larger than 60 */
    if (ultraValueRight > 60) {
        ultraValueRight = 60;
    }
    
    powerFactor = (powerFactorMax-powerFactorMin)*((ultraValueMax-ultraValueMin) - abs(ultraValueLeft-ultraValueRight))/(ultraValueMax-ultraValueMin);
    /* check out if either of the ultrasonic sensor reading is less than 20 which indicates the following robot is too close to the leading robot */
    if (ultraValueLeft > 20 && ultraValueRight > 20) {
        /* the two equations below would dirve the following robot keeps moving toward the leading robot */
        turnLeft  = (ultraValueLeft - ultraValueRight)*powerFactor;
        turnRight = (ultraValueRight - ultraValueLeft)*powerFactor;
        
        powerLeft = round(targetPower + turnLeft);
        powerRight = round(targetPower + turnRight);
        
        robot.setJointPower(JOINT2, powerLeft);
        robot.setJointPower(JOINT3, powerRight);
    }
    else {
        /* the two equations below would make the following robot get out the way of the leading robot until either of the ultrasonic sesnsor readings is less than 20 */
        turnLeft  = (ultraValueRight - ultraValueLeft)*powerFactor;
        turnRight = (ultraValueLeft - ultraValueRight)*powerFactor;
            
        powerLeft = round(targetPower + turnLeft);
        powerRight = round(targetPower + turnRight);
        /* set the motor power with negative values so that the robot moves backward */
        robot.setJointPower(JOINT2, -powerLeft);
        robot.setJointPower(JOINT3, -powerRight);
    }
    
    robot.moveForeverNB();
}