File content as of revision 45:8b0bee6baf38:

#include "mbed.h"

#include "irpair.h"
#include "main.h"
#include "motor.h"

#include <stdlib.h>
#include <stack>          // std::stack
#include <utility>      // std::pair, std::make_pair

#include "ITG3200.h"
#include "stm32f4xx.h"
#include "QEI.h"

/*LEFT/RIGHT BOTH WALLS
0.34        0.12

LEFT/RIGHT LEFT WALL GONE
0.02        0.11

LEFT/RIGHT RIGTH WALL GONE
0.33        0.008

LEFT/RIGHT BOTH WALLS GONE
0.02        0.008

LEFT/RIGHT CLOSE TO RIGHT WALL
0.14        0.47

LEFT/RIGHT CLOSE TO LEFT WALL
0.89        0.05

FRONT IRS NEAR WALL (STOPPING DISTANCE)
0.41        0.49

FRONT IRS NO WALL AHEAD (FOR ATLEAST 1 FULL CELL)
0.07        0.06

*/

#define IP_CONSTANT 1.6
#define II_CONSTANT 0.00001
#define ID_CONSTANT 0.24

void pidOnEncoders();

void moveForwardEncoder(double num)
{
    int count0;
    int count1;
    count0 = encoder0.getPulses();
    count1 = encoder1.getPulses();
    int initial1 = count1;
    int initial0 = count0;
    int diff = count0 - count1;
    double kp = 0.00022;
    double kd = 0.00020;
    int prev = 0;

    double speed0 = WHEEL_SPEED;
    double speed1 = WHEEL_SPEED;

    receiverOneReading = IRP_1.getSamples(100);
    receiverFourReading = IRP_4.getSamples(100);
    right_motor.move(speed0);
    left_motor.move(speed1);
    wait_ms(100);

    double distance_to_go = (oneCellCount)*num;

    while(  ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {

        if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) {
            left_motor.move( speed0/900 );
            right_motor.move( speed1/900 );
        } else {
            left_motor.move(speed0);
            right_motor.move(speed1);
            wait_us(16000);
            pidOnEncoders();
        }

        receiverOneReading = IRP_1.getSamples(100);
        receiverFourReading = IRP_4.getSamples(100);
    }

    right_motor.brake();
    left_motor.brake();
    return;
}

void encoderAfterTurn(double num)
{
    int count0;
    int count1;
    count0 = encoder0.getPulses();
    count1 = encoder1.getPulses();
    int initial1 = count1;
    int initial0 = count0;
    int diff = count0 - count1;
    double kp = 0.00008;
    double kd = 0.0000;
    int prev = 0;

    double speed0 = WHEEL_SPEED; // left wheel
    double speed1 = WHEEL_SPEED;
    receiverOneReading = IRP_1.getSamples(100);
    receiverFourReading = IRP_4.getSamples(100);

    right_motor.move(speed0);
    left_motor.move(speed1);
    wait_us(100000);

    double distance_to_go = (oneCellCount)*num;

    while(  ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {

        if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) {
            left_motor.move( speed0/900 );
            right_motor.move( speed1/900 );
        } else {
            left_motor.move(speed0);
            right_motor.move(speed1);
            wait_us(16000);
            pidOnEncoders();
        }
        receiverOneReading = IRP_1.getSamples(100);
        receiverFourReading = IRP_4.getSamples(100);
    }

    right_motor.brake();
    left_motor.brake();

    double curr0 = encoder0.getPulses();
    double curr1 = encoder1.getPulses();
    if ((curr0 - initial0) >= distance_to_go*0.6 && (curr1 - initial1) >= distance_to_go*0.6) {
        if (currDir % 4 == 0) {
            mouseY += 1;
        } else if (currDir % 4 == 1) {
            mouseX += 1;
        } else if (currDir % 4 == 2) {
            mouseY -= 1;
        } else if (currDir % 4 == 3) {
            mouseX -= 1;
        }

        // the mouse has moved forward, we need to update the wall map now
        receiverOneReading = IRP_1.getSamples(100);
        receiverTwoReading = IRP_2.getSamples(100);
        receiverThreeReading = IRP_3.getSamples(100);
        receiverFourReading = IRP_4.getSamples(100);

        // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
        // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);

        if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
            // serial.printf("Front wall is there\n");
            if (currDir % 4 == 0) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
            } else if (currDir % 4 == 1) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            } else if (currDir % 4 == 3) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
            }
        }
        if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
            // do nothing, the walls are not there
        } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there   RED
            // serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
            if (currDir % 4 == 0) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
            } else if (currDir % 4 == 1) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
            } else if (currDir % 4 == 3) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            }
        } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
            // serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
            if (currDir % 4 == 0) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
            } else if (currDir % 4 == 1) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
            } else if (currDir % 4 == 3) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
            }
        } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
            // serial.printf("Both walls \n");
            if (currDir %4 == 0) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
            } else if (currDir %4 == 1) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
            } else if (currDir %4 == 3) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
            }
        }
    }
}

void printDipFlag()
{
    if (DEBUGGING)
        serial.printf("Flag value is %d", dipFlags);
}

void enableButton1()
{
    dipFlags |= BUTTON1_FLAG;
    printDipFlag();
}
void enableButton2()
{
    dipFlags |= BUTTON2_FLAG;
    printDipFlag();
}
void enableButton3()
{
    dipFlags |= BUTTON3_FLAG;
    printDipFlag();
}
void enableButton4()
{
    dipFlags |= BUTTON4_FLAG;
    printDipFlag();
}
void disableButton1()
{
    dipFlags &= ~BUTTON1_FLAG;
    printDipFlag();
}
void disableButton2()
{
    dipFlags &= ~BUTTON2_FLAG;
    printDipFlag();
}
void disableButton3()
{
    dipFlags &= ~BUTTON3_FLAG;
    printDipFlag();
}
void disableButton4()
{
    dipFlags &= ~BUTTON4_FLAG;
    printDipFlag();
}

void pidOnEncoders()
{
    int count0;
    int count1;
    count0 = encoder0.getPulses();
    count1 = encoder1.getPulses();
    int diff = count0 - count1;
    double kp = 0.00016;
    double kd = 0.00016;
    int prev = 0;

    int counter = 0;
    while(1) {
        count0 = encoder0.getPulses();
        count1 = encoder1.getPulses();
        int x = count0 - count1;
        //double d = kp * x + kd * ( x - prev );
        double kppart = kp * x;
        double kdpart = kd * (x-prev);
        double d = kppart + kdpart;

        //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
        if( x < diff - 60 ) { // count1 is bigger, right wheel pushed forward
            left_motor.move( -d );
            right_motor.move( d );
        } else if( x > diff + 60 ) {
            left_motor.move( -d );
            right_motor.move( d );
        }
        // else
        // {
        //     left_motor.brake();
        //     right_motor.brake();
        // }
        prev = x;
        counter++;
        if (counter == 10)
            break;
    }
}

void nCellEncoderAndIR(double cellCount)
{
    double currentError = 0;
    double previousError = 0;
    double derivError = 0;
    double sumError = 0;

    double HIGH_PWM_VOLTAGE_R = WHEEL_SPEED;
    double HIGH_PWM_VOLTAGE_L = WHEEL_SPEED;

    double rightSpeed = WHEEL_SPEED;
    double leftSpeed = WHEEL_SPEED;

    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;

    left_motor.forward(WHEEL_SPEED);
    right_motor.forward(WHEEL_SPEED);

    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
    float ir3 = IRP_3.getSamples( SAMPLE_NUM );

    int state = 0;

    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
        // serial.printf("The desiredCount0 is: %d \t The desiredCount1 is: %d\t the 0encoderval is :%d\t the 1encoderval is : %d\t\n", desiredCount0, desiredCount1, encoder0.getPulses(), encoder1.getPulses());
        receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
        receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
        receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
        receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
        // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
        if(  receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {
            break;
        }

        if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
            // both sides gone
            double prev0 = encoder0.getPulses();
            double prev1 = encoder1.getPulses();
            double diff0 = desiredCount0 - prev0;
            double diff1 = desiredCount1 - prev1;
            double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
            redLed.write(1);
            greenLed.write(0);
            blueLed.write(0);
            // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
            moveForwardEncoder(valToPass);
            continue;
        } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
            state = 1;
            // double prev0 = encoder0.getPulses();
            // double prev1 = encoder1.getPulses();
            // double diff0 = desiredCount0 - prev0;
            // double diff1 = desiredCount1 - prev1;
            // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
            redLed.write(0);
            greenLed.write(1);
            blueLed.write(1);
            // moveForwardEncoder(valToPass);
            // break;
        } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
            // BLUE BLUE BLUE BLUE
            state = 2;
            // double prev0 = encoder0.getPulses();
            // double prev1 = encoder1.getPulses();
            // double diff0 = desiredCount0 - prev0;
            // double diff1 = desiredCount1 - prev1;
            // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
            redLed.write(1);
            greenLed.write(1);
            blueLed.write(0);
            // moveForwardEncoder(valToPass);
            // break;
        } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
            // both walls there
            state = 0;
            redLed.write(1);
            greenLed.write(0);
            blueLed.write(1);
        }

        //serial.printf("Entering switch\n");
        switch(state) {
            case(0): { // both walls there
                currentError = ( receiverTwoReading - IRP_2.sensorAvg)  - ( receiverThreeReading - IRP_3.sensorAvg);
                break;
            }
            case(1): { // RED RED RED RED RED
                currentError = (receiverTwoReading - IRP_2.sensorAvg) - (IRP_3.sensorAvg*0.001);
                break;
            }
            case(2): { // blue
                currentError =  (IRP_2.sensorAvg*0.001) - (receiverThreeReading - IRP_3.sensorAvg);
                break;
            }
            default: {
                currentError = ( receiverTwoReading - IRP_2.sensorAvg)  - ( receiverThreeReading - IRP_3.sensorAvg);
                break;
            }
        }
        //serial.printf("Exiting switch");

        sumError += currentError;
        derivError = currentError - previousError;
        double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;

        double prev0 = encoder0.getPulses();
        double prev1 = encoder1.getPulses();
        double diff0 = desiredCount0 - prev0;
        double diff1 = desiredCount1 - prev1;
        double kp = .1/1000;


        rightSpeed = HIGH_PWM_VOLTAGE_R;
        leftSpeed = HIGH_PWM_VOLTAGE_L;
        if (diff1 < 1000 || diff0 < 1000) {
            rightSpeed = kp * HIGH_PWM_VOLTAGE_R;
            leftSpeed = kp * HIGH_PWM_VOLTAGE_L;
        }

        if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
            rightSpeed = rightSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
            leftSpeed = leftSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
        } else { // r is faster than L. speed up l, slow down r
            rightSpeed = rightSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
            leftSpeed = leftSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
        }


        //serial.printf("%f, %f\n", leftSpeed, rightSpeed);
        if (leftSpeed > 0.30) leftSpeed = 0.30;
        if (leftSpeed < 0) leftSpeed = 0;
        if (rightSpeed > 0.30) rightSpeed = 0.30;
        if (rightSpeed < 0) rightSpeed = 0;

        right_motor.forward(rightSpeed);
        left_motor.forward(leftSpeed);
        pidOnEncoders();

        previousError = currentError;
    }

    // GO BACK A BIT BEFORE BRAKING??
    left_motor.backward(0.01);
    right_motor.backward(0.01);
    wait_us(150);
    // DELETE THIS IF IT DOES NOT WORK!!

    left_motor.brake();
    right_motor.brake();
    if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) {
        if (currDir % 4 == 0) {
            mouseY += 1;
        } else if (currDir % 4 == 1) {
            mouseX += 1;
        } else if (currDir % 4 == 2) {
            mouseY -= 1;
        } else if (currDir % 4 == 3) {
            mouseX -= 1;
        }

        // the mouse has moved forward, we need to update the wall map now
        receiverOneReading = IRP_1.getSamples(100);
        receiverTwoReading = IRP_2.getSamples(100);
        receiverThreeReading = IRP_3.getSamples(100);
        receiverFourReading = IRP_4.getSamples(100);

        // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
        // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);

        if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
            // serial.printf("Front wall is there\n");
            if (currDir % 4 == 0) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
            } else if (currDir % 4 == 1) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            } else if (currDir % 4 == 3) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
            }
        }
        if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
            // do nothing, the walls are not there
        } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there   RED
            // serial.printf("Left wall\n");
            if (currDir % 4 == 0) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
            } else if (currDir % 4 == 1) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
            } else if (currDir % 4 == 3) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            }
        } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
            // serial.printf("Right wall\n");
            if (currDir % 4 == 0) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
            } else if (currDir % 4 == 1) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
            } else if (currDir % 4 == 3) {
                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
            }
        } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
            // serial.printf("Both walls \n");
            if (currDir %4 == 0) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
            } else if (currDir %4 == 1) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
            } else if (currDir % 4 == 2) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
            } else if (currDir %4 == 3) {
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
                wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
            }
        }
    }
}

bool isWallInFront(int x, int y)
{
    return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
}
bool isWallInBack(int x, int y)
{
    return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
}
bool isWallOnRight(int x, int y)
{
    return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
}
bool isWallOnLeft(int x, int y)
{
    return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
}

int chooseNextMovement()
{
    int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
    if (goingToCenter && (currentDistance == 0)) {
        goingToCenter = false;
        changeManhattanDistance(goingToCenter);
    } else if (!goingToCenter && (currentDistance == 0)) {
        goingToCenter = true;
        changeManhattanDistance(goingToCenter);
    }

    visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
    int currX = 0;
    int currY = 0;
    int currDist = 0;
    int dirToGo = 0;
    if (!justTurned) {
        cellsToVisit.push(make_pair(mouseX, mouseY));
        while (!cellsToVisit.empty()) {
            pair<int, int> curr = cellsToVisit.top();
            cellsToVisit.pop();
            currX = curr.first;
            currY = curr.second;
            currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
            int minDist = INT_MAX;

            if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
                if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
                    }
                }
                if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
                    }
                }
                if (currY + 1 < MAZE_LEN && !isWallInFront( currX,  currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
                    }
                }
                if (currY - 1 >= 0 && !isWallInBack(currX,  currY)) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
                    }
                }
            } else {
                if (currX + 1 < MAZE_LEN) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
                    }
                }
                if (currX - 1 >= 0) {
                    if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
                    }
                }
                if (currY + 1 < MAZE_LEN) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
                    }
                }
                if (currY - 1 >= 0) {
                    if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
                        minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
                    }
                }
            }

            if (minDist == INT_MAX)
                continue;
            if (currDist > minDist)
                continue;
            if (currDist <= minDist) {
                manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
            }
            if (hasVisited(currX, currY)) {
                if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
                    cellsToVisit.push(make_pair(currX, currY + 1));
                }
                if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
                    cellsToVisit.push(make_pair(currX + 1, currY));
                }
                if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
                    cellsToVisit.push(make_pair(currX, currY - 1));
                }
                if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
                    cellsToVisit.push(make_pair(currX - 1, currY));
                }
            } else {
                if (currY + 1 < MAZE_LEN) {
                    cellsToVisit.push(make_pair(currX, currY + 1));
                }
                if (currX + 1 < MAZE_LEN) {
                    cellsToVisit.push(make_pair(currX + 1, currY));
                }
                if (currY - 1 >= 0) {
                    cellsToVisit.push(make_pair(currX, currY - 1));
                }
                if (currX - 1 >= 0) {
                    cellsToVisit.push(make_pair(currX - 1, currY));
                }
            }
        }

        int minDistance = INT_MAX;
        if (currDir % 4 == 0) {
            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
                    dirToGo = 1;
                }
            }
            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
                    dirToGo = 2;
                }
            }
            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 3;
                }
            }
            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 4;
                }
            }
        } else if (currDir % 4 == 2) {
            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
                    dirToGo = 1;
                }
            }
            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
                    dirToGo = 2;
                }
            }
            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 3;
                }
            }
            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 4;
                }
            }
        } else if (currDir % 4 == 1) {
            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 1;
                }
            }
            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    // serial.printf("Looks like the left wall was not there\n");
                    dirToGo = 2;
                }
            }
            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
                    dirToGo = 3;
                }
            }
            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
                    dirToGo = 4;
                }
            }
        } else if (currDir % 4 == 3) {
            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                    dirToGo = 1;
                }
            }
            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                    dirToGo = 2;
                }
            }
            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
                    dirToGo = 3;
                }
            }
            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
                    minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
                    dirToGo = 4;
                }
            }
        }
    } else {
        justTurned = false;
        dirToGo = 0;
    }

    return dirToGo;
}

bool hasVisited(int x, int y)
{
    return visitedCells[MAZE_LEN - 1 - y][x];
}

void changeManhattanDistance(bool headCenter)
{
    if (headCenter) {
        for (int i = 0; i < MAZE_LEN; i++) {
            for (int j = 0; j < MAZE_LEN; j++) {
                distanceToCenter[i][j] = manhattanDistances[i][j];
            }
        }
        for (int i = 0; i < MAZE_LEN; i++) {
            for (int j = 0; j < MAZE_LEN; j++) {
                manhattanDistances[i][j] = distanceToStart[i][j];
            }
        }
    } else {
        for (int i = 0; i < MAZE_LEN; i++) {
            for (int j = 0; j < MAZE_LEN; j++) {
                distanceToStart[i][j] = manhattanDistances[i][j];
            }
        }
        for (int i = 0; i < MAZE_LEN; i++) {
            for (int j = 0; j < MAZE_LEN; j++) {
                manhattanDistances[i][j] = distanceToCenter[i][j];
            }
        }
    }
}

void initializeDistances()
{
    for (int i = 0; i < MAZE_LEN / 2; i++) {
        for (int j = 0; j < MAZE_LEN / 2; j++) {
            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
        }
    }
    for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
        for (int j = 0; j < MAZE_LEN / 2; j++) {
            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
        }
    }
    for (int i = 0; i < MAZE_LEN / 2; i++) {
        for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
        }
    }
    for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
        for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
        }
    }
}

void waitButton4()
{
    //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
    int init_val = dipFlags & BUTTON4_FLAG;
    while( (dipFlags & BUTTON4_FLAG) == init_val ) {
        // do nothing until ready
    }
    //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
    wait( 3 );
}

int main()
{
    //Set highest bandwidth.
    //gyro.setLpBandwidth(LPFBW_42HZ);
    initializeDistances();
    serial.baud(9600);
    //serial.printf("The gyro's address is %s", gyro.getWhoAmI());

    wait (0.1);

    redLed.write(1);
    greenLed.write(0);
    blueLed.write(1);

    //left_motor.forward(0.1);
    //right_motor.forward(0.1);

    // PA_1 is A of right
    // PA_0 is B of right
    // PA_5 is A of left
    // PB_3 is B of left
    //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
    //    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );

    // TODO: Setting all the registers and what not for Quadrature Encoders
    /*    RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
        RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
        GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
        GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
        */

    // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
    // of alternate function specified
    // 4 modes sets AHB1ENR
    // Now TMR: enable clock with timer, APB1ENR
    // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
    //
    // Encoder mode: disable timer before changing timer to encoder
    // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
    // CCMR sets sample rate and set the channel to input
    // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
    // SMCR - encoder mode
    // CR1 reenabales
    // then read CNT reg of timer

    dipButton1.rise(&enableButton1);
    dipButton2.rise(&enableButton2);
    dipButton3.rise(&enableButton3);
    dipButton4.rise(&enableButton4);

    dipButton1.fall(&disableButton1);
    dipButton2.fall(&disableButton2);
    dipButton3.fall(&disableButton3);
    dipButton4.fall(&disableButton4);

    //waitButton4();

    // init the wall, and mouse loc arrays:
    wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
    visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;

    int prevEncoder0Count = 0;
    int prevEncoder1Count = 0;
    int currEncoder0Count = 0;
    int currEncoder1Count = 0;

    bool overrideFloodFill = false;
    right_motor.forward( WHEEL_SPEED );
    left_motor.forward( WHEEL_SPEED );
    //turn180();
    //wait_ms(1500);
    int nextMovement = 0;

    // moveForwardEncoder(1);

    while (1) {

        if (!overrideFloodFill) {
            nextMovement = chooseNextMovement();
            if (nextMovement == 0) {
                moveForwardEncoder(0.4);
                nCellEncoderAndIR(0.6);
            } else if (nextMovement == 1) {
                justTurned = true;
                turnRight();
            } else if (nextMovement == 2) {
                justTurned = true;
                turnLeft();
            } else if (nextMovement == 3) {
                nCellEncoderAndIR(1);
                //encoderAfterTurn(1);
            } else if (nextMovement == 4) {
                justTurned = true;
                turn180();
            }
        } else {
            receiverOneReading = IRP_1.getSamples(100);
            receiverTwoReading = IRP_2.getSamples(100);
            receiverThreeReading = IRP_3.getSamples(100);
            receiverFourReading = IRP_4.getSamples(100);

            if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {

                int randomNum = rand() %2;
                if(randomNum == 0) {
                    turnRight();
                } else {
                    turnLeft();
                }
            } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
                // both walls there
                nCellEncoderAndIR(1);
                redLed.write(1);
                greenLed.write(0);
                blueLed.write(1);
            } else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
                // both sides gone

                redLed.write(1);
                greenLed.write(0);
                blueLed.write(0);
                // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
                moveForwardEncoder(1);
                continue;
            } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
                turnRight();
                redLed.write(0);
                greenLed.write(1);
                blueLed.write(1);
                // moveForwardEncoder(valToPass);
                // break;
            } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
                turnLeft();
                redLed.write(1);
                greenLed.write(1);
                blueLed.write(0);
                // moveForwardEncoder(valToPass);
                // break;
            }
        }

        wait_ms(500);                          // reduce this once we fine tune this!


        //////////////////////// TESTING CODE GOES BELOW ///////////////////////////

        // encoderAfterTurn(1);
        // waitButton4();
        // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );
        // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
        // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
        //encoderAfterTurn(1);
        //waitButton4();
    }
}