/*
 * Line Follower Module Verison 0.1 SB-002
 * https://www.l33t.uk/arduino_projects/mipr/
 * Copyright David Bradshaw 2019
 * 
 * This is designed for tracks that use black lines on white background
 * 
 * The below code uses absolute values from the sensor board and for the code
 * to work you will need to use the resistor values from the tutorial for either
 * the Low Power or High Power board. If you use different valued resistors
 * you will need to convert the values; for the below code to work the baseLine
 * values will be between 250 and 450, if your baseLine values are 800 for instance 
 * you will have to divide all sensor values by at least half for them to work with this code
 * 
 *      Resistor values should be; Low Power: R1 220 Ohms, R2 - R6 4700 Ohms
 *                                 High Power: R1 20 Ohms, R2 - R6 470 Ohms (7805CV must be replaced with a BEC due to current draw)
 */

#include <PID_v1.h>
 
int L1 = A5; //Outer Left
int L2 = A4; //Inner Left
int M = A2; //Middle
int R2 = A1; //Inner Middle
int R1 = A0; //Outer Middle

float L1_bias = 1;
float L2_bias = 1;
float R1_bias = 1;
float R2_bias = 1;

int leftOVal;
int rightOVal;
int leftMVal;
int rightMVal;
int midVal;
    
int baseLineValue = 0;

boolean isCald = false;
boolean firstRun = true;

int lrErr = 0;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

double baseSpeed = 55; //base speed
int minLimit = -200;
int maxLimit = 200;

//values for Output trigger
double consKp=1.25, consKi=1.25, consKd=0.02;

PID leftPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

int refreshRate = 0;
long refreshStartTime = millis();

int prevDirection = 0; //0 == left, 1 == right
int directionCounter = 0;
#define direcitonCntr_MAX 20
int prevDirectionArray[direcitonCntr_MAX];

void line_FollowerSetup()
{
    pinMode(A0, INPUT);
    pinMode(A1, INPUT);
    pinMode(A2, INPUT);
    pinMode(A4, INPUT);
    pinMode(A5, INPUT);
    pinMode(A6, INPUT);
    pinMode(A7, INPUT);

    Setpoint = 0;
    //turn the PID on
    leftPID.SetOutputLimits(minLimit, maxLimit);
    leftPID.SetSampleTime(5);
    leftPID.SetMode(AUTOMATIC);
}

//Simple line follower using just 2 sensors L2 and R2. You need a fairly thick line for this to work properly.
//The robot will use absolute values for the speed of each motor not reacting to the environment just using simnple
//if then else statements to navigate the track
void simple_LF(int leftVal, int rightVal)
{ 
    int leftMax = leftVal + (leftVal * 0.2); //Tolerance of 20% to make the robot less jerky
    int leftMin = leftVal - (leftVal * 0.2);

    if (rightVal > leftMin && rightVal < leftMax) //If the left and right values are within 20%, go forward
    {
        Forwards(100, 100);
    }
    else if (leftVal > rightVal)
    {
        Right(50,50);
    }
    else if (rightVal > leftVal)
    {
        Left(50,50);
    }
}

//A simple line follower using L2 and R2 sensors
//The speed is adjusted depending on the difference between left and right values
void simple_LF_M(int lftAv, int rgtAv, int lrErr)
{  
    baseSpeed = 96; //base speed
    int minSpeed = 0; //Minimum motor speed
    int maxSpeed = 255; //Maximum motor speed when using low power sensor board
    //int maxSpeed = 150; //Maximum motor speed when using high power sensor board
    
    int lftSpeed;
    int rgtSpeed;
    
    //Our goal is to get lrErr to 0
    //if it is negative then the robot needs to turn left (left side over the line too much)
    //if it is possitive then the robot needs to turn right (right side over the line too much)

    //lrErr = lrErr * 0.8;      //Only use for high power sensor board
    
    if(lrErr > 0) // turn right if the error is possitive
    {
        lftSpeed = baseSpeed - lrErr;
        rgtSpeed = baseSpeed + lrErr;
    }
    else // turn left if the error is negative or 0
    {
        lftSpeed = baseSpeed + abs(lrErr);
        rgtSpeed = baseSpeed - abs(lrErr);
    }

    //Make sure we are above the minimum speed and below the maximum speed
    if (lftSpeed < minSpeed){lftSpeed = minSpeed;}
    if (rgtSpeed < minSpeed){rgtSpeed = minSpeed;}
    if (lftSpeed > maxSpeed){lftSpeed = maxSpeed;}
    if (rgtSpeed > maxSpeed){rgtSpeed = maxSpeed;}

    float nFactor = 1.8; //If this value is set too low the robot will be very jerky
    //if the speed is nFactor% higher than the base speed then turn left or right, this will produce a sharper 
    //turn than the Forwards function as it will turn one motor forwards and the other backwards to produce the 
    //sharp turn.
    if(lftSpeed > float(float(baseSpeed) * nFactor))
    {
        Left(lftSpeed, rgtSpeed);
    }
     else if(rgtSpeed > float(float(baseSpeed) * nFactor))
    {
        Right(lftSpeed, rgtSpeed);
    }
        
    else
    {
        Forwards(lftSpeed, rgtSpeed);
    }
}


//Complex line follower using a PID algorithm, error tracking, line prediction and all left and right sensors 
void complex_LF(int leftOVal, int leftMVal, int midVal, int rightMVal, int rightOVal, int lrErr)
{
    //  P - Proportional an amount to multiply the error by 
    //  I - Integral the sum of previous errors
    //  D - Derivative the speed at which we react to errors

    /*
     * For this PID the setpoint will always be 0 i.e. no error between the left and right side
     */

    float scaleFactor = 1 / float(maxLimit);
    float ETV; //Error Tracking Value ratio
    
    int leftAv = leftOVal + leftMVal / 2;
    int rightAv = rightMVal + rightOVal / 2;
    int lrAv = leftAv-rightAv;

    Input = lrAv; 

    int lftSpeed;
    int rgtSpeed;
    int errThreshold = 30;
    int upperThreshold = 50;

    leftPID.Compute(); 

    if (abs(Output) > upperThreshold)
    {
        if (baseSpeed > 55)
        {
            baseSpeed = baseSpeed - 20;
        }
    }
    if (abs(Output) < errThreshold)
    {
        if (baseSpeed < 200)
        {
            baseSpeed = baseSpeed + 10;
        }
    }

    ETV = 1 - (scaleFactor * float(abs(Output)));

    //We're using the PID output to calculate the direction and motor speed
    //This can cause issues when direction change happens quickly due to the
    //lag built into the PID values. To combat this we will use a vector predict array
    //Please note that Output will be opposite to Input for instance if Input is
    //possitive Output will be negative
    if (Output < 0) // turn left if the error is negative
    {
        lftSpeed = (baseSpeed*ETV) + (ETV * abs(Output)); //Slower wheel
        rgtSpeed = baseSpeed + abs(Output); //Faster wheel
        prevDirectionArray[directionCounter] = 0;
    }
    else if (Output > 0) // turn right if the error is possitive
    {
        lftSpeed = baseSpeed + abs(Output); //Faster wheel
        rgtSpeed = (baseSpeed*ETV) + (ETV * abs(Output)); //Slower wheel
        prevDirectionArray[directionCounter] = 1;
    }
    else
    {
        lftSpeed = baseSpeed + abs(Output);
        rgtSpeed = baseSpeed + abs(Output);
    }
   
    int maxSpeed = 255; //Maximum motor speed

    if (lftSpeed > maxSpeed){lftSpeed = maxSpeed;}
    if (rgtSpeed > maxSpeed){rgtSpeed = maxSpeed;}
   
    Forwards(abs(lftSpeed), abs(rgtSpeed));

    prevDirection = 0;
    for (int i=0; i< direcitonCntr_MAX; i++)
    {
        prevDirection += prevDirectionArray[i];
    }

    directionCounter = directionCounter + 1;
    if (directionCounter == direcitonCntr_MAX)
    {
        directionCounter = 0;
    }
    
}

void LFHandler(int mode, boolean fullTele)
{
     //Check to see if this is the first run, if so calibrate the sensor board
     if (firstRun == true)
     {
         sensor_Cal();
         firstRun = false;
     }

    //get the sensor values
    int RefreshTimer = millis() - refreshStartTime;
    if (RefreshTimer > refreshRate)
    {
        leftOVal = getCalSensorVal(L1);
        rightOVal = getCalSensorVal(R1);
        leftMVal = getCalSensorVal(L2);
        rightMVal = getCalSensorVal(R2);
        midVal =  getCalSensorVal(M);
        RefreshTimer = 0;
        refreshStartTime = millis();
    }


    //average the value for the outer sensors
    int lftAv = (leftOVal + leftMVal) / 2;
    int rgtAv = (rightOVal + rightMVal) / 2;

    int outtertAv = (leftOVal + rightOVal) / 2;
    int innerAv =   (leftMVal + rightMVal) / 2;

    lrErr = lftAv - rgtAv; //Error for the intter left and inner right sensors
    
    int absAv = (leftOVal + rightOVal + leftMVal + rightMVal + midVal) / 5;

    float lftSpeed = 0;
    float rgtSpeed = 0;

    //---------------------------------------------------------------------------------------------------------------------------------------------------------------
    //THE BELOW CODE IS ALL ABOUT DETECTING A LINE. IF THE LINE IS THERE THEN THE ALGORITHM WILL BE EXECUTED OTHER WISE THE ROBOT WILL STOP AND TURN ON THE SPEAKER
    //---------------------------------------------------------------------------------------------------------------------------------------------------------------
    
    boolean detectLine = false; //can we see a line
    
    //Compare readings from all sensors to see if a line is present we should have a decrease of at least 10% if we can see a line
    float lineThresh = 0.90; //This value might need tuning depending on the surface
    
    if (lftAv < 50 && rgtAv < 50) //No IR detected so we are either in a black surface, upside down or theres a thick cross road
    {
        //no floor can't be detected
        detectLine = false;
        speaker_on();
    }
    else if(lftAv >  rgtAv)  //Line could be on the right if the robot
    {
        if (float(float(rgtAv) / float(lftAv)) > lineThresh )
        {
            //No line detected check middle sensor
            if (checkMid(midVal, lftAv, rgtAv) == true) //No line detected in the right side so check the middle sensor
            {
                //line detected
                detectLine = true;
            }
            else
            {
                detectLine = false;
            }
        }
        else //RIGHT SENSORS OVER THE LINE
        {
            //line detected
            detectLine = true;
        }
    }
    else if(lftAv <  rgtAv) //Line coule be on the left of the robot
    {
        if (float(float(lftAv) / float(rgtAv)) > lineThresh )
        {
            //No line detected check middle sensor
            if (checkMid(midVal, lftAv, rgtAv) == true) //No line detected in the left side so check the middle sensor
            {
                //line detected
                detectLine = true;
            }
            else
            {
                detectLine = false;
            }
        }
        else  //LEFT SENSORS OVER THE LINE
        {
            //line detected
            detectLine = true;
        }
    }

    //---------------------------------------------------------------------------------------------------------------------------------------------------------------
    //------------------------------------------- END OF LINE DETECTION STUFF ---------------------------------------------------------------------------------------
    //---------------------------------------------------------------------------------------------------------------------------------------------------------------

    if (detectLine == false) //No line detected reverse the robot and sound the speaker
    {
       if (opMode == '8')
       {
            baseSpeed  = 45;
            if (prevDirection > (direcitonCntr_MAX /2 ))
            {
                Left(50,50);
            }
            else if (prevDirection < (direcitonCntr_MAX /2 ))
            {
                Right(50,50);
            }
            else if (prevDirection == (direcitonCntr_MAX /2 ))
            {
                softStop();
                Backwards(42,42);
            }
        }
        else
        {
            softStop();
            Backwards(42,42);
        }
        speaker_on();
    }
    else //We have a line so execute the correct algorithm
    {
        if(mode == 1) //Simple LF
        {
            speaker_off();
            simple_LF(leftMVal, rightMVal);
        }
        else if (mode == 2) //Intemediate LF
        {
            speaker_off();
            simple_LF_M(lftAv, rgtAv, lrErr);
        }
        else if (mode == 3) //PID
        {
            speaker_off();
             complex_LF(leftOVal, leftMVal, midVal, rightMVal, rightOVal, lrErr);

        }
    }
}

boolean checkMid(int midVal, int lftAv, int rgtAv)
{
    float lineThresh = 0.95; //This value might need tuning
    //Average left and right values here for simplicity
    if (float(float(midVal) / float(((lftAv + rgtAv) / 2))) > lineThresh)
    {
        //No line detected
        return false;
    }
    else  //MID SENSOR OVER THE LINE
    {
        //line detected
        return true;
    }
}

//Returns raw value from a sensor
int getRawSensorVal(int sensor)
{
    return analogRead(sensor);
}

//Returns calibrated value from a sensor
int getCalSensorVal(int sensor)
{
    float sensorVal;

    if(sensor == M){sensorVal = (float)analogRead(sensor);}
    else if(sensor == L1){sensorVal = L1_bias * (float)analogRead(sensor);}
    else if(sensor == L2){sensorVal = L2_bias * (float)analogRead(sensor);}
    else if(sensor == R1){sensorVal = R1_bias * (float)analogRead(sensor);}
    else if(sensor == R2){sensorVal = R2_bias * (float)analogRead(sensor);}

    return (int)sensorVal;
}

//Calibration routine to calibrate the sensors accounting for impedance mismatches in the circuit
//and for IR reflectance properties of different materials.
void sensor_Cal()
{
    //One beep indicates that Cal is about to start
    speaker_on();
    delay(450);
    speaker_off();
    
    Serial.println("Put all sensors over a white background and wait for a beep");
    delay(500);

    int bechmarkM = 0; //Benchmark value all other sensors will align themselves with this value

    int itterations = 250;
    long averageVal = 0;

    //get middle average value
    for (int ii = 0; ii < itterations; ii++)
    {
        delay(1);
        averageVal = averageVal + (long)getRawSensorVal(M);
    }
    bechmarkM = averageVal / itterations;

     long averageValL1 = 0; long averageValL2 = 0; long averageValR1 = 0; long averageValR2 = 0;
     
    //Bias values that will be used to align the senor values
    for (int ii = 0; ii < itterations; ii++)
    {
        delay(1);
        averageValL1 = averageValL1 + (long)getRawSensorVal(L1);
        averageValL2 = averageValL2 + (long)getRawSensorVal(L2);
        averageValR1 = averageValR1 + (long)getRawSensorVal(R1);
        averageValR2 = averageValR2 + (long)getRawSensorVal(R2);
    }

    averageValL1 = averageValL1 / itterations;
    averageValL2 = averageValL2 / itterations; 
    averageValR1 = averageValR1 / itterations;
    averageValR2 = averageValR2 / itterations;

    //Calculate the bias as a percentage
    L1_bias = (float)bechmarkM / (float)averageValL1;
    L2_bias = (float)bechmarkM / (float)averageValL2;
    R1_bias = (float)bechmarkM / (float)averageValR1;
    R2_bias = (float)bechmarkM / (float)averageValR2;

    baseLineValue = (getCalSensorVal(L1) + getCalSensorVal(L2) + getCalSensorVal(R1) + getCalSensorVal(R2)) / 4; //Baseline value of left and right sensors

    if (baseLineValue == 1023)
    {
        Serial.println("The sensors are saturated. If you are in an area of high IR activity either shield the robot or use different value resistors on SB-002");
        speaker_on();
        delay(5000);
        speaker_off();
        executeBTcommand("O");
    }

    // 2 beeps to indicate that cal has finished
    speaker_on();
    delay(450);
    speaker_off();
    delay(200);
    speaker_on();
    delay(450);
    speaker_off();
    Serial.print("Sensors calibrated, baseline value:  ");
    Serial.println(baseLineValue);

    isCald = true;
    // Delay to allow user to put robot over the line
    delay(500);
}