/**
 * @file DcMotor.hpp
 * @author Gustice
 * @brief Simple Driver implementation for DRVxxxx Derivatives from Texas instruments
 *  DRV8848
 *
 * @copyright Copyright (c) 2024
 */

#include <Arduino.h>

#include <array>

class DcMotor {
    static constexpr int FullScale = 256;

   public:
    struct Pins {
        std::array<int, 2> PwmA;  // PWM control
        int ErrorIn;              // Error Feedback from driver
        int Sleep;                // Sleep control (to enter double mode)
        int Ceil;                 // Maximum speed control
        int ErrOut;               // Maximum speed control
    };

    enum State {
        Startup = 0,
        Idle,
        Right,
        Left,
    };

    DcMotor(Pins pins) : _pins(pins) {
        pinMode(_pins.PwmA[0], OUTPUT);
        pinMode(_pins.PwmA[1], OUTPUT);
        pinMode(_pins.ErrorIn, INPUT);
        pinMode(_pins.Sleep, OUTPUT);
        pinMode(_pins.Ceil, INPUT);
        pinMode(_pins.ErrOut, OUTPUT);

        // sleep to enter double current mode
        delay(5);
        digitalWrite(_pins.Sleep, LOW);
        delay(5);  // @todo double after successful operation
        digitalWrite(_pins.Sleep, HIGH);

        // set to break
        analogWrite(_pins.PwmA[0], FullScale);
        analogWrite(_pins.PwmA[1], FullScale);
    }

    void stop() {
        analogWrite(_pins.PwmA[0], FullScale);
        analogWrite(_pins.PwmA[1], FullScale);

        _state = Idle;
    }

    void startRight() {
        int ceil = analogRead(_pins.Ceil) / 4;
        _state = Right;

        analogWrite(_pins.PwmA[0], FullScale);
        analogWrite(_pins.PwmA[1], FullScale - ceil);
    }

    void startLeft() {
        int ceil = analogRead(_pins.Ceil) / 4;
        _state = Left;

        analogWrite(_pins.PwmA[0], FullScale - ceil);
        analogWrite(_pins.PwmA[1], FullScale);
    }

    void tick() {
        int ceil = analogRead(_pins.Ceil) / 4;

        switch (_state) {
            case Right:
                analogWrite(_pins.PwmA[1], FullScale - ceil);
                break;

            case Left:
                analogWrite(_pins.PwmA[0], FullScale - ceil);
                break;

            default:
                break;
        }
        int nErr = digitalRead(_pins.ErrorIn);
        digitalWrite(_pins.ErrOut, nErr == 0);
    }

   private:
    Pins _pins;
    State _state = Startup;
};