/**
 * @file moveControl.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the MoveControl class
 * @version 0.1
 * @date 2021-12-14
 * 
 * @copyright Copyright (c) 2021
 * 
 */
#ifndef MOVE_CONTROL_H
#define MOVE_CONTROL_H

#include <cstdint>
#include <iostream>
#include <PID_v1.h>
#include <Arduino.h>

#include "motorControl.h"
#include "speedometer.h"
#include "moveControlConfig.h"
#include "debugTimes.h"

/**
 * @brief used to set driving status
 * 
 * When set to stop all motors are set to halt
 * 
 */
enum DrivingStatus {stop,
                    drive,
                    raw};

/**
 * @brief This class manages the motors and the encoders
 * 
 * The class uses two PIDs to control the motors. The PIDs
 * use the Speedometer class to get the current speed and the
 * given target speed to calculate a new duty cycle for the motors.
 * 
 */
class MoveControl {
    public:
        /**
         * @brief Construct a new Move Control object
         * 
         * Initialize the motors, encoders and PIDs with given
         * values in moveControlConfig.h
         */
        MoveControl();

        /**
         * @brief Destroy the Move Control object
         * 
         * Stops the motors with emergencyStop()
         */
        ~MoveControl();

        /**
         * @brief Calls runMoveControl() to update all Values.
         * 
         * Besides that this function calls the loop() functions for the motors and encodes.
         * This function should be called every mainloop. If the delay is not reached, than the
         * functions returns immediately.
         * @see runMoveControl()
         * @see setDelay()
         */
        void loop();

        /**
         * @brief Normally called repeatedly by loop() to calculate new values.
         */
        void runMoveControl();

        /**
         * @brief Set the DrivingStatus
         * 
         * @see DrivingStatus
         * @param status 
         */
        void setDrivingStatus(DrivingStatus status);

        /**
         * @brief Set the target speed
         * 
         * If the given speed is close to zero, then the
         * target speed is set to zero.
         * @param speed in m/s
         */
        void setSpeed(double speed);

        /**
         * @brief Set the rotationspeed
         * 
         * If the given rotationspeed is close to zero, then the
         * target rotationspeed is set to zero.
         * @param speed rad/s
         */
        void setRotationSpeed(double speed);

        void setRawPowerLeft(int16_t power);
        void setRawPowerRight(int16_t power);

        /**
         * @brief Set the pid tunings
         * 
         * @param side 0 -> left, 1 -> right
         * @param p 
         * @param i 
         * @param d 
         */
        void setPidTunings(uint8_t side, double p, double i, double d);

        /**
         * @brief Returns the PID object of the choosen side.
         * 
         * @param side 0 -> left, 1 -> right
         * @return PID* 
         */
        PID* getPID(uint8_t side);

        /**
         * @brief Set the min delay between each loop
         * 
         * @param delay_ time in Milliseconds
         */
        void setDelay(uint8_t delay) { this->delay = delay; }

    private:
        /**
         * @brief Converts values into wheel speeds
         * 
         * Converts target speed and target rotationspeed into
         * wheel speeds
         */
        void calcTargetWheelSpeed();

        /**
         * @brief Set the target power to motors
         * 
         * Checks if the driving_status is set to drive.
         * If yes, then the motors get the pid_out values as targetpower.
         * If no, then the motors target power is set to zero.
         */
        void regulateMotors();

        /**
         * @brief Updates the wheel speeds with speedometer
         */
        void updateCurrentWheelSpeed();

        MotorControl *left_motor;
        MotorControl *right_motor;
        Speedometer *left_speedometer;
        Speedometer *right_speedometer;
        PID *left_pid;
        PID *right_pid;

        DrivingStatus driving_status = DrivingStatus::stop;

        double x_speed = 0;
        double rotation_speed = 0;

        double wheelspeed_left_target = 0;
        double wheelspeed_right_target = 0;
        double wheelspeed_left = 0;
        double wheelspeed_right = 0;
        double left_pid_out;
        double right_pid_out;

        uint8_t delay = 20;
        uint8_t overTimeCounter = 0;
        uint8_t overTimeMax = 100;
        int8_t rawPowerLeft = 0;
        int8_t rawPowerRight = 0;
        uint32_t lastMillis = 0;
};
#endif // MOVE_CONTROL_H