#include <vector>
#include "WS2812Manager.hpp"

class HellMetServo{
private:
    int _origin = 0;
    int _movement = 0;
    int _pin = -1;
    int _start = 0;
    float lastAngle = 0;
    float _target = 0;
    volatile float _angle = 0;
    volatile int servoHandle = 0;
    bool wasClosing = false;
public:
    HellMetServo(int origin, int movement, int pin);
    void StartToggle();
    void StartOpen();
    void StartClose();
    bool Step();
    void Disable();
    void Enable();
    int getCurrentAngle(){return _angle;}
    int getCurrentTarget(){return _target;}
    int getPin(){return _pin;}
    int getHandle(){return servoHandle;}
};

class HellMetServoManager{
private:
    std::vector<HellMetServo*> topServos = std::vector<HellMetServo*>();
    std::vector<HellMetServo*> bottomServos = std::vector<HellMetServo*>();
    std::vector<HellMetServo*> allServos = std::vector<HellMetServo*>();
    int timingsArray[11] = {20,15,7,1,1,1,1,7,15,20,20};
    void ToggleAll();
public:
    volatile bool in_use = false;
    HellMetServoManager();
    static void ToggleAllWrapper(void *classInstance);
};