/**
 * robot-nav.cpp is where you should put navigation routines.
 */

#include "robot.h"

void Robot::UpdatePose(const Pose& delta)
{
    /**
     * TODO: Add your FK algorithm to update currPose here.
     */

    float transitHeading = currPose.theta + (delta.theta * 0.5);

    currPose.x += delta.x * cos(transitHeading);
    currPose.y += delta.x * sin(transitHeading);

    currPose.theta += delta.theta;

#ifdef __NAV_DEBUG__
    TeleplotPrint("x", currPose.x);
    TeleplotPrint("y", currPose.y);
    TeleplotPrint("theta", currPose.theta * 180.0 / PI);
#endif
}

/**
 * Sets a destination in the lab frame.
 */
void Robot::SetDestination(const Pose& dest)
{
    /**
     * TODO: Turn on LED, as well.
     */
    Serial.print("Setting dest to: ");
    Serial.print(dest.x);
    Serial.print(", ");
    Serial.print(dest.y);
    Serial.print('\n');

    destPose = dest;
    robotState = ROBOT_DRIVE_TO_POINT;
}

float Robot::SquareDistance(void) {
    return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
}

#define TAU (float) TWO_PI

float Robot::RadError(float current, float destination) {
    float diff = destination-current;
    diff += HALF_PI;
    diff = fmod(diff,TAU); // floating point modulo
    diff -= HALF_PI;

    if (diff > 180) {
        diff -= TAU;
    }
    return diff;
}

bool Robot::CheckReachedDestination(void)
{
    /**
     * TODO: Add code to check if you've reached destination here.
     */

    return SquareDistance() < TARGET_SQ_DIST 
        && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
}

void Robot::DriveToPoint(void)
{
    if(robotState == ROBOT_DRIVE_TO_POINT)
    {
        float target_heading;

        if (SquareDistance() < TARGET_SQ_DIST) {
            target_heading = destPose.theta;
        } else {
            target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
        }

        float heading_err = RadError(currPose.theta, target_heading);
        float forward_effort = cos(heading_err) * sqrt(SquareDistance());


        /**
         * TODO: Add your IK algorithm here. 
         */

#ifdef __NAV_DEBUG__
        // Print useful stuff here.
#endif

        /**
         * TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
         */
    }
}

void Robot::HandleDestination(void)
{
    /**
     * TODO: Stop and change state. Turn off LED.
     */
}