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 = uint32_t;

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

enum class motorid_t {
    leftTread,
    rightTread,
    armBase,
    shoulder,
    elbow,
    forearm,
    wristDiffRight,
    wristDiffLeft,
    hand,
    drillActuator,
    drillMotor,
    fourbar1,
    fourbar2,
  // Hack, see real_world_interface.cpp for details
  scienceServoBoard,
  scienceStepperBoard,
};

enum class mountedperipheral_t {
    none,
    arm,
    science,
};
 
enum class jointid_t {
    armBase,
    shoulder,
    elbow,
    forearm,
    wristPitch,
    wristRoll,
    hand,
  handActuator,
    ikForward,
    ikUp,
    fourBarLinkage,
    drillActuator,
    drillMotor
};
 
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::handActuator, jointid_t::ikForward,
   jointid_t::ikUp, jointid_t::fourBarLinkage, jointid_t::drillActuator, jointid_t::drillMotor});
 
constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
    {{"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},
   {"handActuator", jointid_t::handActuator},
     {"ikForward", jointid_t::ikForward},
     {"ikUp", jointid_t::ikUp},
     {"fourBarLinkage", jointid_t::fourBarLinkage},
     {"drillActuator", jointid_t::drillActuator},
     {"drillMotor", jointid_t::drillMotor}});
 
enum class servoid_t {
  microscope,
  syringe,
  cuvette,
  filter,
  soilBox,
};
 
constexpr auto all_servoid_t = frozen::make_unordered_set<servoid_t>(
  {servoid_t::microscope, servoid_t::syringe, servoid_t::soilBox,
   servoid_t::cuvette, servoid_t::filter});
 
constexpr auto name_to_servoid = frozen::make_unordered_map<frozen::string, servoid_t>(
  {{"microscope", servoid_t::microscope},
   {"syringe", servoid_t::syringe},
   {"cuvette", servoid_t::cuvette},
   {"filter", servoid_t::filter},
   {"soilBox", servoid_t::soilBox}});
 
constexpr auto servoid_to_servo_num = frozen::make_unordered_map<servoid_t, int>(
  {{servoid_t::microscope, 7},
   {servoid_t::syringe, 9},
   {servoid_t::cuvette, 5},
   {servoid_t::filter, 8},
   {servoid_t::soilBox, 6}});
 
enum class stepperid_t {
  plunger,
  judges,
  mast,
  lock,
  lazySusan,
};
 
constexpr auto all_stepperid_t = frozen::make_unordered_set<stepperid_t>(
  {stepperid_t::plunger, stepperid_t::judges, stepperid_t::mast,
   stepperid_t::lock, stepperid_t::lazySusan});
 
constexpr auto name_to_stepperid = frozen::make_unordered_map<frozen::string, stepperid_t>(
  {{"plunger", stepperid_t::plunger},
   {"judges", stepperid_t::judges},
   {"mast", stepperid_t::mast},
   {"lock", stepperid_t::lock},
   {"lazySusan", stepperid_t::lazySusan}});
 
constexpr auto stepperid_to_stepper_num = frozen::make_unordered_map<stepperid_t, uint8_t>(
  {{stepperid_t::plunger, 4},
   {stepperid_t::judges, 5},
   {stepperid_t::mast, 2},
   {stepperid_t::lock, 1},
   {stepperid_t::lazySusan, 6}});
 
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);
std::string to_string(robot::types::servoid_t servo);
} // namespace util