data.h

#pragma once
 
#include "../navtypes.h"
 
#include <bitset>
#include <chrono>
#include <optional>
#include <stdexcept>
#include <type_traits>
#include <vector>
 
#include <frozen/string.h>
#include <frozen/unordered_map.h>
#include <frozen/unordered_set.h>
 
// forward declare cv::Mat instead of importing OpenCV
// we do this to avoid unnecessarily including OpenCV in all build targets
namespace cv {
class Mat;
}

namespace robot::types {

using dataclock = std::chrono::steady_clock;
using datatime_t = std::chrono::time_point<dataclock>;
 
template <typename T>
class DataPoint;

using landmarks_t = std::vector<DataPoint<navtypes::point_t>>;

using CameraFrame = std::pair<cv::Mat, uint32_t>;
using CameraID = std::string;

enum class indication_t {
    off,
    autonomous,
    teleop,
    arrivedAtDest
};

enum class motorid_t {
    frontLeftWheel,
    frontLeftSwerve,
    frontRightWheel,
    frontRightSwerve,
    rearLeftWheel,
    rearLeftSwerve,
    rearRightWheel,
    rearRightSwerve,
    armBase,
    shoulder,
    elbow,
    forearm,
    wristDiffRight,
    wristDiffLeft,
    hand,
    activeSuspension,
    drillActuator,
    drillMotor
};

enum class mountedperipheral_t {
    none,
    arm,
    armServo,
    scienceStation,
    lidar
};
 
enum class jointid_t {
    frontLeftSwerve,
    frontRightSwerve,
    rearLeftSwerve,
    rearRightSwerve,
    armBase,
    shoulder,
    elbow,
    forearm,
    wristPitch,
    wristRoll,
    hand,
    activeSuspension,
    ikForward,
    ikUp
};
 
constexpr auto all_jointid_t = frozen::make_unordered_set<jointid_t>(
    {jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm,
     jointid_t::wristRoll, jointid_t::wristPitch, jointid_t::hand, jointid_t::activeSuspension,
     jointid_t::ikForward, jointid_t::ikUp});
 
constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
    {{"frontLeftSwerve", jointid_t::frontLeftSwerve},
     {"frontRightSwerve", jointid_t::frontRightSwerve},
     {"rearLeftSwerve", jointid_t::rearLeftSwerve},
     {"rearRightSwerve", jointid_t::rearRightSwerve},
     {"armBase", jointid_t::armBase},
     {"shoulder", jointid_t::shoulder},
     {"elbow", jointid_t::elbow},
     {"forearm", jointid_t::forearm},
     {"wristPitch", jointid_t::wristPitch},
     {"wristRoll", jointid_t::wristRoll},
     {"hand", jointid_t::hand},
     {"activeSuspension", jointid_t::activeSuspension},
     {"ikForward", jointid_t::ikForward},
     {"ikUp", jointid_t::ikUp}});
 
class bad_datapoint_access : public std::runtime_error {
public:
    bad_datapoint_access() : std::runtime_error("bad_datapoint_access") {}
};

template <typename T>
class DataPoint {
public:
    DataPoint() : datapoint() {}

    DataPoint(T data) : DataPoint(dataclock::now(), data) {}

    DataPoint(datatime_t time, T data) : datapoint({time, data}) {}

    operator bool() const {
        return isValid();
    }

    bool isFresh(std::chrono::milliseconds duration) const {
        return isValid() && dataclock::now() - duration <= getTime();
    }

    bool isValid() const {
        return datapoint.has_value();
    }

    datatime_t getTime() const {
        return datapoint.value().first;
    }

    T getData() const {
        if (isValid()) {
            return datapoint.value().second;
        } else {
            throw bad_datapoint_access();
        }
    }

    T getDataOrElse(T defaultData) {
        return isValid() ? getData() : defaultData;
    }

    template <typename F>
    DataPoint<std::invoke_result_t<F, T>> transform(const F& f) {
        if (isValid()) {
            return DataPoint<std::invoke_result_t<F, T>>(getTime(), f(getData()));
        } else {
            return {};
        }
    }
 
private:
    std::optional<std::pair<datatime_t, T>> datapoint;
};

constexpr size_t N_LIMIT_SWITCH = 8;
 
constexpr int LIMIT_SWITCH_LIM_MIN_IDX = 0;
constexpr int LIMIT_SWITCH_LIM_MAX_IDX = 1;

class LimitSwitchData {
public:
    LimitSwitchData(unsigned long long data);

    bool isOpen(size_t idx);

    bool isClosed(size_t idx);

    bool isAnyOpen();

    bool isAnyClosed();

    std::bitset<N_LIMIT_SWITCH> diff(const LimitSwitchData& other);
 
private:
    std::bitset<N_LIMIT_SWITCH> data;
};
 
} // namespace robot::types
 
namespace util {
std::string to_string(robot::types::jointid_t joint);
std::string to_string(const robot::types::CameraID& id);
std::string to_string(robot::types::mountedperipheral_t peripheral);
} // namespace util