#ifndef USS_SRF02_H_
#define USS_SRF02_H_

#include "NetworkingLib/Timer.h"

class UltrasonicSensor
    : public std::enable_shared_from_this<UltrasonicSensor>
      , public networking::Busyable
{
private:
    struct PrivateTag
    {
    };

public:
    //gpio pins where the sonar is connected
    static constexpr int SRF02_SDA = 8; //i2c data pin ;
    static constexpr int SRF02_SCL = 9; //i2c clock pin ;

    // addresses of the sonars, the number is also as a sticker on the devices themselves
    static constexpr int DEVICE_ADDRESS1 = 0x74;
    static constexpr int DEVICE_ADDRESS2 = 0x76;
    static constexpr int DEVICE_ADDRESS3 = 0x77;

    //path to i2c file
    static constexpr char DEVICE[] = "/dev/i2c-1";

    static constexpr int COMMAND_REGISTER = 0x00;
    static constexpr int RESULT_HIGH_BYTE = 0x02;
    static constexpr int RESULT_LOW_BYTE = 0x03;
    static constexpr int RANGING_MODE_CM = 0x51;

    static constexpr int DELAY = 70; //70 ms for ranging to finish

    static constexpr int MAX_DISTANCE = 255;

    using Ptr = std::shared_ptr<UltrasonicSensor>;
    using OnDistanceUpatedCallback = std::function<void(int)>;

    static Ptr create(networking::Networking & net, int devId)
    { return std::make_shared<UltrasonicSensor>(PrivateTag{}, net, devId); }

    UltrasonicSensor(PrivateTag, networking::Networking & net, int devId);

    void start(const OnDistanceUpatedCallback & onDistanceUpatedCallback);

    void stop();

private:
    networking::Networking & net;
    networking::time::Timer::Ptr timer;
    int devId;
    int fd;

    struct AsyncState
    {
        using Ptr = std::shared_ptr<AsyncState>;

        AsyncState(UltrasonicSensor::Ptr self, const OnDistanceUpatedCallback & onDistanceUpdatedCallback)
            : lock(*self)
              , self(self)
              , onDistanceUpdatedCallback(onDistanceUpdatedCallback)
        {}

        networking::BusyLock lock;
        UltrasonicSensor::Ptr self;
        OnDistanceUpatedCallback onDistanceUpdatedCallback;
    };

    void updateSensor(const AsyncState::Ptr & state);

    void startRanging() const;

    int readDistance() const;
};


#endif /* USS_SRF02_H_ */