#ifndef __control
#define __control

#include <stdint.h>
#include "./vl53l0x.h"
#include "./settings.h"


enum class ControlUpdateResult
{
  Idle,
  Stabilizing,
  Moving,
  TargetReached,
  SensorError,
  Overcurrent
};


enum class MoveDirection
{
  None,
  Up,
  Down
};



class ControlManager
{
  public:
    VL53L0XInitResult init();
    ControlUpdateResult update();


    void stabilizeStart();
    bool stabilized();

    void moveStart(uint16_t height);
    void moveStop();

    void snapToPreset();


    // Formats a height value as "0.00m" (always exactly 5 characters long).
    // Buffer must be at least 6 bytes long, a null character is added.
    // The value is the raw height sensor value, the offset is added by this function.
    void getDisplayHeight(char* buffer, uint16_t value);


    uint16_t getCurrentHeight() { return this->currentMeasurement + Settings.Height.Offset; }
    uint16_t getCurrentMeasurement() { return this->currentMeasurement; }

    uint16_t getLastMeasurement() { return this->lastMeasurement; }
    MoveDirection getMoveDirection() { return this->moveDirection; }
    uint16_t getMoveTargetHeight() { return this->moveTargetHeight; }
    bool getIsStabilizing() { return this->stabilizationStart != 0; }

  private:
    bool targetReached();

    VL53L0X heightSensor = VL53L0X();
    uint16_t currentMeasurement;
    uint16_t lastMeasurement = 0;
    uint32_t lastValidMeasurement = 0;

    uint32_t lastCurrentCheck = 0;

    MoveDirection moveDirection;
    uint16_t moveTargetHeight;

    uint32_t stabilizationStart = 0;
    uint8_t stabilizeCount = 0;
    uint16_t stabilizeTargetMeasurement = 0;
    uint32_t stabilizationLastCheck = 0;
};


extern ControlManager Control;


#endif