#pragma once
#include <Arduino.h>
#include <vector>

// Motor-aided movement: detects external manipulation and assists to reduce gear stress
// Works by monitoring position velocity - when someone moves the motor manually,
// it briefly enables torque and moves to the current position to assist.

constexpr uint8_t VELOCITY_SAMPLES = 5;  // Number of samples for velocity smoothing

struct AidedMotor {
  uint8_t motorID;
  uint16_t lastPosition = 0;
  unsigned long lastUpdateTime = 0;
  
  // Velocity smoothing
  int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
  uint8_t sampleIndex = 0;
  int16_t smoothedVelocity = 0;
  
  // Assist state
  bool assisting = false;
  unsigned long assistStartTime = 0;
  uint16_t assistTargetPosition = 0;
  
  // Hysteresis - prevents re-triggering until velocity drops
  bool wasTriggered = false;
};

class MotorAid {
public:
  // Add a motor to be aided
  void addMotor(uint8_t motorID);
  
  // Remove a motor from aided list
  void removeMotor(uint8_t motorID);
  
  // Check if a motor is being aided
  bool isMotorAided(uint8_t motorID) const;
  
  // Main update function - call from loop() when motorStream is active
  void update();
  
  // Configuration
  void setVelocityThreshold(int16_t threshold) { velocityThreshold = threshold; }
  void setResetThreshold(int16_t threshold) { resetThreshold = threshold; }
  void setAssistDuration(unsigned long duration) { assistDuration = duration; }
  void setAssistSpeed(uint16_t speed) { assistSpeed = speed; }
  
private:
  std::vector<AidedMotor> aidedMotors;
  
  // Thresholds and settings
  int16_t velocityThreshold = 200;     // Position units/sec to trigger assist
  int16_t resetThreshold = 60;         // Velocity must drop below this to re-trigger (hysteresis)
  unsigned long assistDuration = 250;  // ms to assist for
  uint16_t assistSpeed = 600;          // Speed to use when assisting
  unsigned long updateInterval = 30;   // ms between position checks
  
  AidedMotor* findMotor(uint8_t motorID);
  const AidedMotor* findMotor(uint8_t motorID) const;
  
  // Calculate smoothed velocity from samples
  int16_t calculateSmoothedVelocity(AidedMotor& motor);
};

extern MotorAid motorAid;