/*
 * Define the physical ports used on the handy board so they can
 * be easily changed whenever the wires are moved or whatever, and
 * makes the code easier to understand.
 */
#define LEFT_MOTOR_PORT 1
#define RIGHT_MOTOR_PORT 3
#define LEFT_REFLECTIVE_PORT 6
#define RIGHT_REFLECTIVE_PORT 5

/*
 * The value above which a reading from a reflective
 * sensor would be considered black.
 */
#define BLACK_LEVEL 150

/*
 * Some times used for turning and backing off
 * that should be easily changed whenever the battery
 * power changes...
 */
#define RIGHT_ANGLE_TURN_SLEEP 1.2
#define ALIGN_BACKOFF_SLEEP_TIME 0.1

/*
 * These values are added to the motor power
 * when the robot is going straight.
 */
#define LEFT_MOTOR_ADJUST 0
#define RIGHT_MOTOR_ADJUST 0

/* Some Current State Variables */
int stopPressed;
int leftMotorPower;
int rightMotorPower;

int leftIsBlack;
int rightIsBlack;

int encodersEnabled;

/**
 * Returns whether the stop button has been pressed
 * since the last time 'stopPressed' was set to false.
 */
int shouldStop() {
    if (stop_button()) {
        stopPressed = 1;
    }
    return stopPressed;
}

/**
 * Causes the robot to go as straight as it can.  When a
 * wheel has not gone as far as the other, it will be given
 * more power, while the other is given less power.  Plus,
 * a steady increase in power, 'accel', is given to the
 * wheels to cause a minimum of slipping.
 *
 * speed - the max POWER (not really speed) given to the motors.
 * accel - how fast the wheels are sped up from their starting
 *         motor powers to the specified 'speed' power.
 * correctionAccel - how much the motors react to a difference
 *                   in wheel encoder measurments, in terms of
 *                   power * difference.
 * ticks - the number of ticks that the encoders should count
 *         before returning from this function.
 */
void goStraight(int speed, int accel, int correctionAccel, int ticks, int checkForTape) {
    
    int leftTick;   // the tick count of the left wheel encoder
    int rightTick;  // the tick count of the right wheel encoder
    int diffCount;  // the difference between the left and right encoders
    
    int leftSpeed;  // the power applied to the left motor at each iteration
    int rightSpeed; // the power applied to the right motor at each iteration
    
    if (!encodersEnabled) {
        
        enable_encoder(0);  // right wheel encoder...
        enable_encoder(1);  // left wheel encoder...
        
        reset_encoder(0);   // reset the encoders to zero values
        reset_encoder(1);   // not sure if needed, don't care...
    }
    
    leftIsBlack = rightIsBlack = 0;
    
    while (!shouldStop()) {
        
        if (checkForTape) {
            if (analog(LEFT_REFLECTIVE_PORT) > BLACK_LEVEL) {
                leftIsBlack = 1;
                if (analog(RIGHT_REFLECTIVE_PORT) > BLACK_LEVEL) {
                    rightIsBlack = 1;
                } else {
                    rightIsBlack = 0;
                }
                return;
            } else {
                if (analog(RIGHT_REFLECTIVE_PORT) > BLACK_LEVEL) {
                    // left is not, already checked...
                    rightIsBlack = 1;
                    leftIsBlack = 0;
                    return;
                }
            }
        }
        
        /*
         * Attempt steady acceleration of wheels
         */
        // left wheel accel...
        if (leftMotorPower < speed) {
            leftMotorPower += accel;
            if (leftMotorPower > speed) {
                leftMotorPower = speed;
            }
        } else {
            if (leftMotorPower > speed) {
                leftMotorPower -= accel;
                if (leftMotorPower < speed) {
                    leftMotorPower = speed;
                }
            }
        }
        // right wheel accel...
        if (rightMotorPower < speed) {
            rightMotorPower += accel;
            if (rightMotorPower > speed) {
                rightMotorPower = speed;
            }
        } else {
            if (rightMotorPower > speed) {
                rightMotorPower -= accel;
                if (rightMotorPower < speed) {
                    rightMotorPower = speed;
                }
            }
        }
        
        /*
         * Read the encoders to see how fast the wheels are
         * going, and to try to correct any deviations.
         */
        leftTick = read_encoder(1);
        rightTick = read_encoder(0);
        if (leftTick >= ticks && rightTick >= ticks) {
            break;
        }
        diffCount = rightTick - leftTick;
        /*
         * diffCount is positive when the right wheel is going faster,
         * and negative when the left wheel is going faster, and zero
         * when they are going the same speed.
         */
        leftSpeed = leftMotorPower + correctionAccel * diffCount + LEFT_MOTOR_ADJUST;
        rightSpeed = rightMotorPower - correctionAccel * diffCount + RIGHT_MOTOR_ADJUST;
        
        //if (diffCount > 1 || diffCount < -1) {
        //    if (diffCount > 0) {
        //        tone(800.0, 0.05);
        //    }
        //    if (diffCount < 0) {
        //        tone(400.0, 0.05);
        //    }
        //} else {
        //    diffCount = 0;
        //}
        
        /*
         * Make sure correction power levels are not
         * too high or too low.
         */
        if (leftSpeed > 100) {
            leftSpeed = 100;
        } else {
            if (leftSpeed < 0) {
                leftSpeed = 0;
            }
        }
        if (rightSpeed > 100) {
            rightSpeed = 100;
        } else {
            if (rightSpeed < 0) {
                rightSpeed = 0;
            }
        }
        
        /*
         * Apply guessed power correction levels to motors...
         */
        motor(LEFT_MOTOR_PORT, leftSpeed);
        motor(RIGHT_MOTOR_PORT, rightSpeed);
        
        //printf("[%d::%d]\n", read_encoder(1), read_encoder(0));
        sleep(0.020);
        
    }
    
    /*
     * Make sure to disable encoders before returning...
     */
    if (!encodersEnabled) {
        disable_encoder(1);
        disable_encoder(0);
    }
}

/**
 * Aligns the robot with a black line in front of it.
 *
 * param - inc - the amount to increment the motor power while it stays on
 *               either light or dark surfaces for more than one iteration.
 * param - maxForwardSpeed - the maximum speed that the robot will go while
 *                           searching for the black line in front of it.
 * param - maxBackSpeed - the maximum speed that the robot will go while
 *                        backing off from a black line that it has
 *                        encountered.
 */
void align2(int inc, int maxForwardSpeed, int maxBackSpeed) {
    
    int leftWasBlack = leftIsBlack = analog(LEFT_REFLECTIVE_PORT) > BLACK_LEVEL;
    int rightWasBlack = rightIsBlack = analog(RIGHT_REFLECTIVE_PORT) > BLACK_LEVEL;
    
    int leftWasBlackCount = 0;
    int rightWasBlackCount = 0;
    
    int leftSpeed;
    int rightSpeed;
    
    while (1) {
        
        leftIsBlack = analog(LEFT_REFLECTIVE_PORT) > BLACK_LEVEL;        
        rightIsBlack = analog(RIGHT_REFLECTIVE_PORT) > BLACK_LEVEL;
        
        if (leftIsBlack) {
            if (leftWasBlack) {
                leftWasBlackCount++;
                // make left go backwards...
                leftSpeed = 2 * leftWasBlackCount * inc + 30;
                if (leftSpeed > maxBackSpeed) {
                    leftSpeed = maxBackSpeed;
                }
                motor(LEFT_MOTOR_PORT, -leftSpeed);
            } else {
                leftWasBlackCount = 0;
                // check if rightIsBlack and rightWasBlack...
                if (rightIsBlack && !rightWasBlack) {
                    break;
                } else {
                    // go backwards some...
                    motor(LEFT_MOTOR_PORT, -maxBackSpeed);
                }
            }
        } else {
            // left is white...
            // move forward some...
            if (leftWasBlack) {
                leftWasBlackCount = 0;
                // just move forward ever so slightly...
                motor(LEFT_MOTOR_PORT, 10);
            } else {
                // it has been on white for more than one iter...
                // go forward more...
                leftWasBlackCount--;
                leftSpeed = (-leftWasBlackCount) * inc + 10;
                if (leftSpeed > maxForwardSpeed) {
                    leftSpeed = maxForwardSpeed;
                }
                motor(LEFT_MOTOR_PORT, leftSpeed);
            }
        }
        leftWasBlack = leftIsBlack;
        
        if (rightIsBlack) {
            if (rightWasBlack) {
                rightWasBlackCount++;
                // make right go backwards...
                rightSpeed = 2 * rightWasBlackCount * inc + 30;
                if (rightSpeed > maxBackSpeed) {
                    rightSpeed = maxBackSpeed;
                }
                motor(RIGHT_MOTOR_PORT, -rightSpeed);
            } else {
                rightWasBlackCount = 0;
                // check if leftIsBlack and leftWasBlack...
                if (leftIsBlack && !leftWasBlack) {
                    break;
                } else {
                    // go backwards some...
                    motor(RIGHT_MOTOR_PORT, -maxBackSpeed);
                }
            }
        } else {
            // right is white...
            // move forward some...
            if (rightWasBlack) {
                rightWasBlackCount = 0;
                // just move forward ever so slightly...
                motor(RIGHT_MOTOR_PORT, 10);
            } else {
                // it has been on white for more than one iter...
                // go forward more...
                rightWasBlackCount--;
                rightSpeed = -rightWasBlackCount * 5 + 10;
                if (rightSpeed > maxForwardSpeed) {
                    rightSpeed = maxForwardSpeed;
                }
                motor(RIGHT_MOTOR_PORT, rightSpeed);
            }
        }
        rightWasBlack = rightIsBlack;
        
        sleep(0.01);
    }
}

/**
 * The main function of this program.  This waits for the start
 * button to be pushed, over and over again until the robot is
 * turned off.  Origonally, this funcion was meant to operate
 * with other functions in such a way that when the stop button
 * was pressed, the robot would stop and reset along with the
 * start function.  But this was not the case as our group
 * quickly ran out of time.
 */
void main(void) {
    
    int count = 0;
    
    while (!start_button());
    
    while (count < 12) {
        
        encodersEnabled = 0;
        
        printf("lap %d, sqare %d\n", count/4 + 1, count % 4 + 1);
        
        count++;
        
        leftIsBlack = rightIsBlack = 0;
        
        //  alignWithBlackTape(50);
        align2(2, 40, 90);
        ao();
        rightMotorPower = 0;
        leftMotorPower = 0;
        sleep(0.5);
        
        enable_encoder(0);
        enable_encoder(1);
        reset_encoder(0);
        reset_encoder(1);
        encodersEnabled = 1;
        
        // gently roll out of sqare...
        goStraight(80, 5, 10, 300, 0);
        
        // go to next sqare...
        goStraight(80, 10, 10, 2600, 1);
        
        disable_encoder(0);
        disable_encoder(1);
        encodersEnabled = 0;
        
        ao();
        leftMotorPower = 0;
        rightMotorPower = 0;
        
        if (leftIsBlack && rightIsBlack) {
            // both sensors got to black line...
            
            // jump into square...
            motor(LEFT_MOTOR_PORT, 100);
            motor(RIGHT_MOTOR_PORT, 100);
            sleep(0.7);
            ao();
            
            // turn left 90...
            motor(LEFT_MOTOR_PORT, -100);
            motor(RIGHT_MOTOR_PORT, 100);
            sleep(RIGHT_ANGLE_TURN_SLEEP);
            ao();
            
            // go backwards some in case it turned over the tape...
            motor(LEFT_MOTOR_PORT, -100);
            motor(RIGHT_MOTOR_PORT, -100);
            sleep(0.15);
            ao();
            
        } else {
            if (leftIsBlack) {
                // jump into square...
                motor(LEFT_MOTOR_PORT, 100);
                motor(RIGHT_MOTOR_PORT, 100);
                sleep(0.7);
                ao();
                
                // turn left 90...
                motor(LEFT_MOTOR_PORT, -100);
                motor(RIGHT_MOTOR_PORT, 100);
                sleep(RIGHT_ANGLE_TURN_SLEEP);
                ao();    
                
                motor(LEFT_MOTOR_PORT, 100);
                motor(RIGHT_MOTOR_PORT, 100);
                sleep(0.3);
                ao();
            } else {
                if (rightIsBlack) {
                    // hit left corner of square...
                    
                    motor(LEFT_MOTOR_PORT, -100);
                    motor(RIGHT_MOTOR_PORT, -100);
                    sleep(0.8);
                    
                    motor(LEFT_MOTOR_PORT, 100);
                    motor(RIGHT_MOTOR_PORT, -100);
                    sleep(RIGHT_ANGLE_TURN_SLEEP/4.0);
                    
                    align2(2, 50, 90);
                    
                    // jump into square...
                    motor(LEFT_MOTOR_PORT, 100);
                    motor(RIGHT_MOTOR_PORT, 100);
                    sleep(0.2);
                    
                    // then was just a little bit off,
                    // so turn 90...
                    
                    motor(LEFT_MOTOR_PORT, 100);
                    motor(RIGHT_MOTOR_PORT, 100);
                    sleep(0.7);
                    
                    motor(LEFT_MOTOR_PORT, -100);
                    motor(RIGHT_MOTOR_PORT, 100);
                    
                    sleep(RIGHT_ANGLE_TURN_SLEEP);
                    ao();
                } else {
                    // goStraight went stopped after counting the
                    // specfied number of ticks, and no black tape
                    // was found...
                    
                    motor(LEFT_MOTOR_PORT, -100);
                    motor(RIGHT_MOTOR_PORT, 100);
                    sleep(RIGHT_ANGLE_TURN_SLEEP);
                    ao();
                    sleep(0.5);
                    
                    //alignWithBlackTape(50);
                    align2(2, 50, 90);
                    
                    motor(LEFT_MOTOR_PORT, 100);
                    motor(RIGHT_MOTOR_PORT, 100);
                    sleep(0.3);
                    
                    ao();
                    rightMotorPower = leftMotorPower = 0;
                }
            }
            // let the robot stop...
            sleep(0.5);
        }       
    }
    if (!shouldStop()) {
        align2(2, 50, 90);
    }
    while (!shouldStop()) {
        beep();
    }
}