#include "mbed.h"
#include "LSM9DS1.h"    // IMU
#include "ultrasonic.h" // Ultrasonic Sensor
#include "Motor.h"      // Motor Drivers
#include "PinDetect.h"  // Pin Detect (for switch)


// Global Variables:
bool tr = true;
bool turnflag = false;
int leftdistancestarting= 0;
int leftdistanceending= 0;
//---------------------|
// PIN INITIALIZATIONS |
//---------------------|
// Debug LED pin:
//DigitalOut led(p25);
DigitalIn sw(p20);
Serial pc(USBTX,USBRX);
// Setup Motor Driver pins:
Motor Lfront(p21, p6, p5);  // PWM to p21, forward to p5, reverse to p6...
Motor Rfront(p22, p8, p7);  // PWM to p22, forward to p7, reverse to p8...
Motor Lback(p23, p10, p9);  // PWM to p23, forward to p10, reverse to p9...
Motor Rback(p24, p12, p11);  // PWM to p24, forward to p12, reverse to p11...
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);

Timer t;

//-----------------------|
// CLASS INITIALIZATIONS |
//-----------------------|
class mctrl {
public:
    // Stop all motors:
    void stop(void) {
        Lfront.speed(0);
        Lback.speed(0);
        Rfront.speed(0);
        Rback.speed(0);
        wait(0.55);
    };
    
    // Go forward at constant speed:
    void fwd(void){
        //stop();
        
        Lfront.speed(0.85);
        Lback.speed(0.85);
        Rfront.speed(0.85);
        Rback.speed(0.85);
        wait(0.02);
    };
    
    // Reverse at constant speed:
    void rev(void){
        stop();
        Lfront.speed(-1);
        Lback.speed(-1);
        Rfront.speed(-1);
        Rback.speed(-1);
        wait(0.02);
    };
    
    // Turn left 90 degrees:
    void turnLeft(){
        stop();
        float degree = 0.0, angularV = 0.0;
        float currt = t.read();
        while(degree < 90) {
            while(imu.gyroAvailable());
            imu.readGyro();
            angularV = imu.gz;
            Lfront.speed(-1);
            Lback.speed(-1);
            Rfront.speed(1);
            Rback.speed(1);
            if(angularV > 50.0 || angularV <-50.0) {
                degree += (abs(angularV))/100.00;
            }
            wait(.45);
        }
        stop();
        wait(0.02);
    };
    
    // Turn right 90 degrees:
    void turnRight(){
        stop();
        float degree = 0.0, angularV = 0.0;
        float currt = t.read();
        while(degree < 90) {
            while(imu.gyroAvailable());
            imu.readGyro();
            angularV = imu.gz;
            Lfront.speed(1);
            Lback.speed(1);
            Rfront.speed(-1);
            Rback.speed(-1);
            if(angularV > 50.0 || angularV <-50.0) {
                degree += (abs(angularV))/100.00;
            }
            wait(.45);
        }
        stop();
        wait(0.02);
    };
    
    // Turn L or R, "mag" of turn prop. to dist. from wall
    // motor speed values are normalized to -1 to 1 (aka no 1.25)
    void turnLeftScaled(double scale) {
        // try commenting out this stop otherwise robot will stop before correcting
        stop();
        Lfront.speed(-1 * scale);
        Lback.speed(-1 * scale);
        Rfront.speed(1 * scale);
        Rback.speed(1 * scale);
        //stop();
        wait(0.02);
    };
        
    void turnRightScaled(double scale) {
        stop();
        Lfront.speed(1 * scale);
        Lback.speed(1 * scale);
        Rfront.speed(-1 * scale);
        Rback.speed(-1 * scale);
        //stop();
        wait(0.02);
    };
} mctrl;

//------------------|
// HELPER FUNCTIONS |
//------------------|
// PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
void dist(int distance)  // NOTE: by default "distance" is in mm...
{
    if (distance < 150) {
        
        mctrl.stop();

        // Turn 90 degrees
        float currt = t.read();
        if (tr) {
            mctrl.turnRight();
        } else {
            mctrl.turnLeft();
        }
        
        // Go forward 1.25 seconds... is it though???
        // change dstance???? globals?? to make sure it doesnt hit wall later on
        currt = t.read();
        while ((t.read() - currt) < 1.25) {
            mctrl.fwd();
        }
        
        // Turn 90 degrees
        currt = t.read();
        if (tr) {
            mctrl.turnRight();
        } else {
            mctrl.turnLeft();
        }

        tr = !tr;
    } else {
        // Continue forward until wall.
        mctrl.fwd();  
    }
}

// nonsense so far
void dist2(int distance)   // left sensor interrupt
{
    
     leftdistanceending = distance; 
    //pc.printf(" Distance %d mm\r\n", distance);


}

// nonsense so far
void dist3(int distance)  // right sensor interrupt
{
    /*if (distance < 150) {
     pc.printf("Right Sensor trigg"); 
    }
    else {
        pc.printf("Right Sensor echo");
    }*/
    //rightdistance = distance;
}

//------------------------------|
// PIN INITIALIZATIONS (cont'd) |
//------------------------------|
// Setup Ultrasonic Sensor pins:
ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
                                             // update every .07 secs w/ timeout after 1 sec...
                                              // call "dist" when the distance changes...
ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger


//---------------|
// MAIN FUNCTION |
//---------------|
int main() {
    // Use internal pullup for the switch:
    sw.mode(PullUp);
    
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    usensor2.startUpdates();
    usensor3.startUpdates(); 
    wait(0.5);
    
    // obtain and print starting distances
    int forwardDist = usensor.getCurrentDistance();
    int leftDist = usensor2.getCurrentDistance();
    int rightDist = usensor3.getCurrentDistance();
    wait(0.5);
    
    pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist);
    pc.printf("Current Left Starting Distance %d mm\r\n", leftDist);
    pc.printf("Current Right Starting Distance %d mm\r\n", rightDist);

    // Initialize the Timer:
    t.start();
    
   //while(1) {
        while (!sw) {
            // if front sonar detects a change in the distace measured, dist is called
            usensor.checkDistance();
            
            // obtain the distance measurements for the left and right sonars
            int currLeftDist = usensor2.getCurrentDistance();
            int currRightDist = usensor3.getCurrentDistance();
            
            // if the left and right sonars detect distances greater than 100mm
            // (10 cm or 3.93 inches), just go forward aka ignore path corr.
            if (currLeftDist > 100 && currRightDist > 100) {
                mctrl.fwd();
            } else {
                // if left side of robot is nearer to the wall...
                if (currLeftDist < currRightDist) {
                    //if robot has drifted toward the wall by a lot, scale R turn proportionally
                    if ((leftDist - currLeftDist) > 40) {
                        // take a right turn with a greater speed
                        // (trying to emulate a "big" turn, may need to be changed)
                        mctrl.turnRightScaled(1);
                        // this is an attempt to go forward (may need to change)
                        mctrl.fwd();
                    //if drifted toward wall a little (less than 40 but greater than 5 mm), scale accordingly
                    } else if ((leftDist - currLeftDist) > 5) {
                        mctrl.turnRightScaled(0.75);
                        mctrl.fwd();
                    //if drift away from wall a lot, scale L turn accordingly
                    } else if ((currLeftDist - leftDist) > 40) {
                        mctrl.turnLeftScaled(1);
                        mctrl.fwd();
                    } else if ((currLeftDist - leftDist) > 5) {
                        mctrl.turnLeftScaled(0.75);
                        mctrl.fwd();
                    // if drift if less than 5 mm in either direction (away or toward wall), go forward normally...
                    // last meeting i had this commented out, see how the robot behaves this time!!
                    } else {
                        mctrl.fwd();
                    }
                } else if (currLeftDist > currRightDist) {
                    //if robot has drifted toward the wall by a lot, scale L turn by a lot
                    if ((rightDist - currRightDist) > 40) {
                        mctrl.turnLeftScaled(1);
                        mctrl.fwd();
                    //if drifted toward wall a little, scale accordingly
                    } else if ((rightDist - currRightDist) > 5) {
                        mctrl.turnLeftScaled(0.75);
                        mctrl.fwd();
                    //if drift away a lot, scale R turn accordingly
                    } else if ((currRightDist - rightDist) > 40) {
                        mctrl.turnRightScaled(1);
                        mctrl.fwd();
                    } else if ((currRightDist - rightDist) > 5) {
                        mctrl.turnRightScaled(0.75);
                        mctrl.fwd();
                    } else {
                        mctrl.fwd();
                    }
                }
                
                // update the left and right distances that will the next values will be compared to. may be weird so maybe wait/etc.
                leftDist = currLeftDist;
                rightDist = currRightDist;
            }       
        }
        // if switch flipped, stop robot
        mctrl.stop();
}