/**
 * @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 "config.h"
#include "debugTimes.h"
#include "component.h"

/**
 * @brief A struct to easy set the speed
 *
 */
struct DrivingSpeeds
{
    double x;
    double rot;
};

/**
 * @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 Component
{
public:
    /**
     * @brief Used to set driving status
     *
     * When set to stop all motors are set to halt
     *
     * When set to Raw the PIDs are not used. The target
     * values must be given withe setRawPowerLeft() and
     * setRawPowerRight().
     *
     * Drive is the normal working mode.
     *
     */
    enum Status
    {
        Stop,
        Drive,
        Raw
    };

    /**
     * @brief Construct a new Move Control object
     *
     * Initialize the motors, encoders and PIDs with given
     * values in config.h
     */
    MoveControl();

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

    /**
     * @brief Set the Status
     *
     * Control if this component should work normally or with
     * raw values for the motors.
     *
     * @see Status
     * @param status
     */
    void setDrivingStatus(Status status);
    Status getDrivingStatus() const { return this->driving_status; }

    /**
     * @brief Stops the engine immediately
     *
     */
    void emergencyStop();

    /**
     * @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);

    /**
     * @brief Set the speeds
     *
     * This function is a combination of the functions setSpeed() and
     * setRotationSpeed(). The struct DrivingSpeeds
     * is used for this.
     *
     *
     * @param drivingSpeeds
     */
    void setSpeeds(DrivingSpeeds drivingSpeeds);

    /**
     * @brief Set raw power left
     *
     * This value has only an effect if the status is raw.
     * @see setDrivingStatus
     *
     * @param power between -100 and 100
     */
    void setRawPowerLeft(int16_t power);

    /**
     * @brief Set the raw power right
     *
     * This value has only an effect if status is raw.
     *
     * @param power between -100 and 100
     */
    void setRawPowerRight(int16_t power);

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

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

    Speedometer *getSpeedometerLeft() const { return this->leftSpeedometer; }
    Speedometer *getSpeedometerRight() const { return this->rightSpeedometer; }

    uint16_t getDutycycleLeft() const { return this->leftMotor->getDutycycle(); }
    uint16_t getDutycycleRight() const { return this->rightMotor->getDutycycle(); }

private:
    void run() override;
    static void setSpeedometerDirection(Speedometer *speedometer, double value);
    void calcTargetWheelSpeed();
    void regulateMotors();
    void updateCurrentWheelSpeed();

    MotorControl *leftMotor;
    MotorControl *rightMotor;
    Speedometer *leftSpeedometer;
    Speedometer *rightSpeedometer;
    PID *leftPid;
    PID *rightPid;

    Status driving_status = Status::Stop;
    DrivingSpeeds drivingSpeeds = {0, 0};

    double wheelspeedLeftTarget = 0;
    double wheelspeedRightTarget = 0;
    double wheelspeedLeft = 0;
    double wheelspeedRight = 0;
    double leftPidOut = 0;
    double rightPidOut = 0;

    uint8_t overTimeCounter = 0;
    uint8_t overTimeMax = 100;
    int8_t rawPowerLeft = 0;
    int8_t rawPowerRight = 0;

    static constexpr int8_t maxPercentage = 100;
    static constexpr float minSpeed = 0.2;
    static constexpr uint8_t loopDelay = 20;
};
#endif // MOVE_CONTROL_H