#pragma once

#include <map>
#include <vector>

class Leg {
public:
    enum Joint {
        Hipp,
        Knee,
    };

    Leg(uint8_t hippIndex, uint8_t kneeIndex);

    //! @brief positive trim in rad to center this joint
    void setTrim(Joint, float angle);

    //! @brief position in rad
    void setPos(Joint, float angle);

private:
    struct JointInfo {
        uint8_t index;
        float trim;
        float center;
        float pos;
    };

    std::map<Joint, JointInfo> _joints;
};

typedef std::vector<Leg> Legs;

class Body {
public:
    static Body &instance() {
        static Body instance;
        return instance;
    }

    //! @brief Nescessary to configure the driver
    void init();

    //! @brief Check if driver has frozen or lost contact/sync
    //!        Resets the driver if nescesarry
    void healthCheck();

    Legs legs;

private:
    Body();
};