#pragma once

#include "chassis.h"

const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (cm)
const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)

class Robot
{
protected:
    /**
     * robotState is used to track the current task of the robot. You will add new states as 
     * the term progresses.
     */
    enum ROBOT_STATE 
    {
        ROBOT_IDLE,
        ROBOT_DRIVE_TO_POINT,
        ROBOT_CALIB_TURN,
        ROBOT_CALIB_DRIVE,
    };
    ROBOT_STATE robotState = ROBOT_IDLE;

    /* Define the chassis*/
    Chassis chassis;

    // For managing key presses
    String keyString;

    /**
     * For tracking current pose and the destination.
     */
    Pose currPose;
    Pose destPose;
    
public:
    Robot(void) {keyString.reserve(10);}
    void InitializeRobot(void);
    void RobotLoop(void);

protected:
    /* State changes */    
    void EnterIdleState(void);
    void EnterCalibTurn(void);
    void EnterCalibDrive(void);

    // /* Navigation methods.*/
    void UpdatePose(const Pose& u);
    void SetDestination(const Pose& destination);
    void DriveToPoint(void);
    void CalibTurn(void);
    void CalibDrive(void);
    bool CheckReachedDestination(void);
    void HandleDestination(void);

    // Square distance between current and target pose
    float SquareDistance(void);

    // minimum radians to transform current to setpoint 
    float RadError(float current, float setpoint);
};