/**
 * @file MotorDriver.h
 * @brief TB67H450FNG Motor Driver Controller
 * 
 * Controls the TB67H450FNG H-bridge motor driver with PWM speed control,
 * direction control, and optional load compensation/BEMF feedback.
 */

#ifndef MOTOR_DRIVER_H
#define MOTOR_DRIVER_H

#include <Arduino.h>

// TB67H450FNG control pins
// IN1 and IN2 control direction and brake
// PWM controls speed

class MotorDriver {
public:
    MotorDriver();
    
    /**
     * @brief Initialize motor driver
     * @param in1Pin GPIO for IN1 (Motor phase A)
     * @param in2Pin GPIO for IN2 (Motor phase B)
     * @param pwmPin GPIO for PWM speed control
     * @param currentSensePin ADC pin for current sensing (optional, 255 = disabled)
     * @return true if successful
     */
    bool begin(uint8_t in1Pin, uint8_t in2Pin, uint8_t pwmPin, uint8_t currentSensePin = 255);
    
    /**
     * @brief Set motor speed and direction
     * @param speed Speed value (0-126, DCC format: 0=stop, 1=emergency stop, 2-127=speed)
     * @param forward Direction (true=forward, false=reverse)
     */
    void setSpeed(uint8_t speed, bool forward);
    
    /**
     * @brief Emergency stop
     */
    void emergencyStop();
    
    /**
     * @brief Update motor control (call regularly for load compensation)
     */
    void update();
    
    /**
     * @brief Enable/disable load compensation
     * @param enable true to enable
     */
    void setLoadCompensation(bool enable);
    
    /**
     * @brief Get motor current (if current sensing enabled)
     * @return Current in mA
     */
    uint16_t getMotorCurrent();
    
    /**
     * @brief Set PID parameters for load compensation
     * @param kp Proportional gain
     * @param ki Integral gain
     * @param kd Derivative gain
     */
    void setPIDParameters(float kp, float ki, float kd);
    
    /**
     * @brief Set acceleration rate
     * @param rate Rate value (0-255, higher = faster)
     */
    void setAccelRate(uint8_t rate);
    
    /**
     * @brief Set deceleration rate
     * @param rate Rate value (0-255, higher = faster)
     */
    void setDecelRate(uint8_t rate);

private:
    uint8_t pinIN1;
    uint8_t pinIN2;
    uint8_t pinPWM;
    uint8_t pinCurrentSense;
    
    uint8_t targetSpeed;
    uint8_t currentSpeed;
    bool targetDirection;
    bool loadCompensationEnabled;
    
    // Acceleration/deceleration
    uint8_t accelRate;
    uint8_t decelRate;
    unsigned long lastSpeedUpdate;
    
    // Load compensation (PID)
    float Kp, Ki, Kd;
    float integral;
    float lastError;
    uint16_t targetCurrent;
    
    // PWM settings
    const uint8_t pwmChannel = 0;
    const uint32_t pwmFrequency = 20000; // 20 kHz
    const uint8_t pwmResolution = 8;      // 8-bit (0-255)
    
    void applyMotorControl();
    void updateAcceleration();
    void updateLoadCompensation();
    uint16_t readCurrent();
};

#endif // MOTOR_DRIVER_H