#include "robot.h"
#include <IRdecoder.h>
#include <limits.h>

void Robot::InitializeRobot(void)
{
    chassis.InititalizeChassis();

    /**
     * Initialize the IR decoder. Declared extern in IRdecoder.h; see robot-remote.cpp
     * for instantiation and setting the pin.
     */
    decoder.init();

    /**
     * Initialize the IMU and set the rate and scale to reasonable values.
     */
    imu.init();

    /**
     * TODO: Add code to set the data rate and scale of IMU (or edit LSM6::setDefaults())
     */

    // The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness
    lineSensor.Initialize();
}

void Robot::EnterIdleState(void)
{
    Serial.println("-> IDLE");
    chassis.Stop();
    keyString = "";
    robotState = ROBOT_IDLE;
}

/**
 * Functions related to the IMU (turning; ramp detection)
 */
void Robot::EnterTurn(float angleInDeg)
{
    Serial.println(" -> TURN");
    robotState = ROBOT_TURNING;
    Serial.println(" -> TURN");
    robotState = ROBOT_TURNING;
    targetDirection = (currDirection+2)%4;
    chassis.SetTwist(0.0,0.5);
}

bool Robot::CheckTurnComplete(void)
{
    bool retVal = false;

    if (targetDirection == currDirection) {return retVal;};

    // unsound
    if (lineSensor.LineDetected() && eulerAngles.z > targetDirection * 90) {
        retVal = true;
    }

    return retVal;
}

void Robot::HandleTurnComplete(void)
{
    currDirection = targetDirection;
    chassis.Stop();

    if (robotRampState == RAMP_ASCENDING) {
        robotRampState = RAMP_DESCENDING;
        Robot::EnterLineFollowing(20.);
    } else {
        Robot::EnterIdleState();
    }
}

/**
 * Here is a good example of handling information differently, depending on the state.
 * If the Romi is not moving, we can update the bias (but be careful when you first start up!).
 * When it's moving, then we update the heading.
 */
void Robot::HandleOrientationUpdate(void)
{
    prevEulerAngles = eulerAngles;
    if(robotState == ROBOT_IDLE)
    {
        // TODO: You'll need to add code to LSM6 to update the bias
        imu.updateGyroBias();
    }

    else // update orientation
    {
        float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
        z_rot /= 104.0;
        z_rot *= 245.0;
        eulerAngles.z += z_rot;

    }

    float g_lsb = 0.061 / 1000.;
    float acc_x = asin(imu.a.x * g_lsb) * 180 / PI;
    if (isnan(acc_x)) {
            acc_x = 90; // pointing straight down will never occur
    }
    float gyrodelt_x = (float)imu.g.x / (float)INT_MAX;
    gyrodelt_x *= 245.0 / 104.0;
    float kappa = 0.1;
    eulerAngles.x = (1-kappa) * (eulerAngles.x + gyrodelt_x) + kappa * acc_x;

    float acc_y = asin(imu.a.y * g_lsb) * 180 / PI;
    if (isnan(acc_y)) {
            acc_y = 90; // pointing straight down will never occur
    }
    float gyrodelt_y = (float)imu.g.y / (float)INT_MAX;
    gyrodelt_y *= 245.0 / 104.0;
    eulerAngles.y = (1-kappa) * (eulerAngles.y + gyrodelt_y) + kappa * acc_y;
#ifdef __IMU_DEBUG__
    Serial.print(imu.gyroBias.z);
    Serial.print('\t');
    Serial.print(eulerAngles.z);
    Serial.print('\t');
    //Serial.print(imu.g.x);
    //Serial.print('\t');
    //Serial.print(imu.g.y);
    //Serial.print('\t');
    Serial.println(imu.g.z);
#endif
}

/**
 * Functions related to line following and intersection detection.
 */
void Robot::EnterLineFollowing(float speed) 
{
    Serial.println(" -> LINING"); 
    baseSpeed = speed; 
    robotState = ROBOT_LINING;
}

int lineLostFrames = 24;

float lastError = 0;

int leveltimer = 0;

void Robot::LineFollowingUpdate(void)
{
    if(robotState == ROBOT_LINING) 
    {
#ifdef DARK
        float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
#endif
#ifndef DARK
        float setpoint = 3.5;
#endif

        float lineError = setpoint - lineSensor.CalcError();


        float powErr = (lineError*lineError);
        powErr = (lineError < 0 ? -powErr : powErr);

        float errDiff = lastError - lineError; 
        if (lineSensor.LineDetected()) {
            lastError = lineError;
        } else {
            errDiff = 0;
            lineError = lastError > 0 ? 7:-7;
        }

        float turnEffort = powErr * lining_kP2 + lineError * lining_kP + errDiff * lining_kD;

#ifdef __LINING_DEBUG__
        Serial.print(lineSensor.CalcError());
        Serial.print(' ');
        Serial.print(lineError * lining_kP);
        Serial.print(' ');
        Serial.print(errDiff * lining_kD);
        Serial.print(' ');
        Serial.print(turnEffort);
        Serial.print(' ');
        Serial.println(lineSensor.AverageReflectance());
#endif

        rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);

        float speed = baseSpeed;

        speed *= 1 - (abs(rollingTurnRate) * KTurnRate);

        if (lineSensor.CheckIntersection()) {
            turnEffort = 0;
            speed *= 0.8;
        }

        if (!lineSensor.LineDetected()) { 
            lineLostFrames -= 1;
            if (lineLostFrames < 0) {
                rollingTurnRate = 0; 
                turnEffort = 0; 
                speed = 0;
                chassis.Stop();
                return;
            } else {
                speed *= 0.97;
            }
        }
        else { 
            lineLostFrames = 24;
        }

        chassis.SetTwist(speed, turnEffort);

        if (eulerAngles.x > 10.) {
            Serial.println("climbing");
        } else if (eulerAngles.x <= 10.) {
            Serial.println("level");
        }

        if (eulerAngles.x > 10.) { // activate timer when on ramp
            leveltimer = 30;
        }
        if (leveltimer > 0) {
            if (eulerAngles.x <= 5.) {
                leveltimer--; // count down when off ramp
                if (leveltimer == 1) {
                    leveltimer = 0;
                    if (robotRampState == RAMP_ASCENDING) {
                        Robot::EnterTurn(180);
                    } else {
                        EnterIdleState();
                    }
                }
            } else {
                leveltimer = 30;
            }
        }

    }
}

/**
 * As coded, HandleIntersection will make the robot drive out 3 intersections, turn around,
 * and stop back at the start. You will need to change the behaviour accordingly.
 */
void Robot::HandleIntersection(void)
{
    Serial.print("X: ");
    if(robotState == ROBOT_LINING) 
    {
        switch(nodeTo)
        {
            case NODE_START:
                if(nodeFrom == NODE_1)
                    EnterIdleState();
                break;
            case NODE_1:
                // By default, we'll continue on straight
                if(nodeFrom == NODE_START) 
                {
                    nodeTo = NODE_2;
                }
                else if(nodeFrom == NODE_2)
                {
                    nodeTo = NODE_START;
                }
                nodeFrom = NODE_1;
                break;
            case NODE_2:
                // By default, we'll continue on straight
                if(nodeFrom == NODE_1) 
                {
                    nodeTo = NODE_3;
                }
                else if(nodeFrom == NODE_3)
                {
                    nodeTo = NODE_1;
                }
                nodeFrom = NODE_2;
                break;
            case NODE_3:
                // By default, we'll bang a u-ey
                if(nodeFrom == NODE_2) 
                {
                    nodeTo = NODE_2;
                    nodeFrom = NODE_3;
                    EnterTurn(180);
                }
                break;
            default:
                break;
        }
        Serial.print(nodeFrom);
        Serial.print("->");
        Serial.print(nodeTo);
        Serial.print('\n');
    }
}

void Robot::RobotLoop(void) 
{
    /**
     * The main loop for your robot. Process both synchronous events (motor control),
     * and asynchronous events (IR presses, distance readings, etc.).
    */

    /**
     * Handle any IR remote keypresses.
     */
    int16_t keyCode = decoder.getKeyCode();
    if(keyCode != -1) HandleKeyCode(keyCode);

    /**
     * Check the Chassis timer, which is used for executing motor control
     */
    if(chassis.CheckChassisTimer())
    {
        // add synchronous, pre-motor-update actions here
        if(robotState == ROBOT_LINING)
        {
            LineFollowingUpdate();
        }

        chassis.UpdateMotors();

        // add synchronous, post-motor-update actions here

    }

    /**
     * Check for any intersections
     */
    if(lineSensor.CheckIntersection()) HandleIntersection();
    if(CheckTurnComplete()) HandleTurnComplete();

    /**
     * Check for an IMU update
     */
    if(imu.checkForNewData())
    {
        HandleOrientationUpdate();
    }
}