#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 = 4;  // Number of samples for velocity smoothing (fewer = faster response)
constexpr int16_t MIN_POSITION_DELTA = 1; // Ignore position changes smaller than this (minimum for high gear ratio)
constexpr uint8_t CONSECUTIVE_REQUIRED = 3; // Require this many consecutive high readings to trigger

struct AidedMotorSettings {
  int16_t velocityThreshold = 80;      // Position units/sec to trigger assist (low for high gear ratio)
  int16_t resetThreshold = 30;         // Velocity must drop below this to re-trigger
  int16_t leadOffset = 200;            // How far ahead to command (position units)
  uint16_t assistSpeed = 1000;         // Speed to use when assisting
  unsigned long assistDuration = 500;  // ms to assist for
};

struct AidedMotor {
  uint8_t motorID;
  uint16_t lastPosition = 0;
  unsigned long lastUpdateTime = 0;
  
  // Per-motor settings
  AidedMotorSettings settings;
  
  // Velocity smoothing
  int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
  uint8_t sampleIndex = 0;
  uint8_t samplesCollected = 0;  // Track how many samples we've collected
  int16_t smoothedVelocity = 0;
  uint16_t stablePosition = 0;   // Position used for delta calculation (filters noise)
  
  // Assist state
  bool assisting = false;
  unsigned long assistStartTime = 0;
  uint16_t assistTargetPosition = 0;
  
  // Hysteresis - prevents re-triggering until velocity drops
  bool wasTriggered = false;
  
  // Warmup - don't trigger until we have enough samples
  bool warmedUp = false;
  
  // Consecutive high readings counter (filters noise spikes)
  uint8_t consecutiveHighCount = 0;
};

class MotorAid {
public:
  // Add a motor with default settings
  void addMotor(uint8_t motorID);
  
  // Add a motor with custom settings
  void addMotor(uint8_t motorID, const AidedMotorSettings& settings);
  
  // Remove a motor from aided list
  void removeMotor(uint8_t motorID);
  
  // Check if a motor is being aided
  bool isMotorAided(uint8_t motorID) const;
  
  // Get settings for a motor (to modify)
  AidedMotorSettings* getMotorSettings(uint8_t motorID);
  
  // Main update function - call from loop() when motorStream is active
  void update();
  
private:
  std::vector<AidedMotor> aidedMotors;
  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;