Limelight.h

#pragma once
#include <LimelightHelpers.h>
 
#include <frc/geometry/Pose3d.h>
#include <frc/util/Color.h>
#include <units/angle.h>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include <units/time.h>
#include <wpi/struct/Struct.h>
 
#include "valkyrie/Loggable.h"
 
namespace valor {
 
namespace cameras {
 
class Limelight : public Loggable {
   public:
    enum class Model {
        One,
        Two,
        TwoPlus,
        Three,
        ThreeA,
        ThreeG,
        Four,
    };

    enum class IMUMode {
        ExternalOnly = 0,
        ExternalSeed = 1,
        InternalOnly = 2,
        InternalMT1Assist = 3,
        InternalExternalAssist = 4,
    };

    enum class LEDMode {
        PipelineControl = 0,
        ForceOff = 1,
        ForceBlink = 2,
        ForceOn = 3,
        ForceOnLeft = 4,
        ForceOnRight = 5,
        BounceHalves = 6,
        ForceBlinkLeft = 7,
        ForceBlinkRight = 8,
    };

    enum class StreamMode {
        Standard = 0,
        PiPMain = 1,
        PiPSecondary = 2,
    };

    struct TargetMetrics {
        bool targetValid;
        int targetCount;
        units::millisecond_t targetLatency;
        units::millisecond_t captureLatency;
        units::degree_t tx;
        units::degree_t ty;
        units::degree_t txnc;
        units::degree_t tync;
        double ta;
        int tid;
        int targetClassIndexDetector;
        int targetClassIndexClassifier;
        int targetLongSidePixels;
        int targetShortSidePixels;
        int targetHorizontalExtentPixels;
        int targetVerticalExtentPixels;
        units::degree_t targetSkew;
    };

    struct CornerCoordinates {
        double x0;
        double y0;
        double x1;
        double y1;
    };

    struct IMUData {
        units::degree_t robotYaw;
        units::degree_t roll;
        units::degree_t pitch;
        units::degree_t yaw;
        units::degrees_per_second_t gyroX;
        units::degrees_per_second_t gyroY;
        units::degrees_per_second_t gyroZ;
        units::degrees_per_second_squared_t accelX;
        units::degrees_per_second_squared_t accelY;
        units::degrees_per_second_squared_t accelZ;
 
        constexpr IMUData(LimelightHelpers::IMUData data)
              : robotYaw{data.robotYaw},
                roll{data.roll},
                pitch{data.pitch},
                yaw{data.yaw},
                gyroX{data.gyroX},
                gyroY{data.gyroY},
                gyroZ{data.gyroZ},
                accelX{data.accelX},
                accelY{data.accelY},
                accelZ{data.accelZ} {}
 
        constexpr IMUData() = default;
    };

    struct RawTarget : LimelightHelpers::RawTarget {
        units::degree_t txnc;
        units::degree_t tync;
 
        constexpr RawTarget(LimelightHelpers::RawTarget data) : LimelightHelpers::RawTarget{data}, txnc{data.txnc}, tync{data.tync} {}
 
        constexpr RawTarget() = default;
    };

    struct RawFiducial : LimelightHelpers::RawFiducial {
        units::degree_t txnc;
        units::degree_t tync;
        units::meter_t distToCamera;
        units::meter_t distToRobot;
 
        constexpr RawFiducial(LimelightHelpers::RawFiducial data)
              : LimelightHelpers::RawFiducial{data},
                txnc{data.txnc},
                tync{data.tync},
                distToCamera{data.distToCamera},
                distToRobot{data.distToRobot} {}
 
        constexpr RawFiducial() = default;
    };

    struct RawDetection : LimelightHelpers::RawDetection {
        units::degree_t txnc;
        units::degree_t tync;
 
        constexpr RawDetection(LimelightHelpers::RawDetection data)
              : LimelightHelpers::RawDetection{data}, txnc{data.txnc}, tync{data.tync} {}
 
        constexpr RawDetection() = default;
    };

    struct PoseEstimate : LimelightHelpers::PoseEstimate {
        units::second_t timestamp;
        units::millisecond_t latency;
        units::meter_t tagSpan;
        units::meter_t avgTagDist;
        std::vector<RawFiducial> rawFiducials;
 
        constexpr PoseEstimate(LimelightHelpers::PoseEstimate data)
              : LimelightHelpers::PoseEstimate{data},
                timestamp{data.timestampSeconds},
                latency{data.latency},
                tagSpan{data.tagSpan},
                avgTagDist{data.avgTagDist},
                rawFiducials{data.rawFiducials.begin(), data.rawFiducials.end()} {}
 
        constexpr bool IsValid() const { return !rawFiducials.empty(); }
 
       private:
        using LimelightHelpers::PoseEstimate::timestampSeconds;
    };

    inline explicit Limelight(std::string_view name) : name{name} {}

    inline bool GetTV() const { return LimelightHelpers::getTV(name); }

    inline units::degree_t GetTX() const { return units::degree_t{LimelightHelpers::getTX(name)}; }

    inline units::degree_t GetTY() const { return units::degree_t{LimelightHelpers::getTY(name)}; }

    inline units::degree_t GetTXNC() const { return units::degree_t{LimelightHelpers::getTXNC(name)}; }

    inline units::degree_t GetTYNC() const { return units::degree_t{LimelightHelpers::getTYNC(name)}; }

    inline double GetTA() const { return LimelightHelpers::getTA(name); }

    TargetMetrics GetTargetMetrics() const;

    inline int GetTargetCount() const { return LimelightHelpers::getTargetCount(name); };

    inline int GetClassifierClassIndex() const { return LimelightHelpers::getClassifierClassIndex(name); }

    inline int GetDetectorClassIndex() const { return LimelightHelpers::getDetectorClassIndex(name); }

    inline std::string GetClassifierClass() const { return LimelightHelpers::getClassifierClass(name); }

    inline std::string GetDetectorClass() const { return LimelightHelpers::getDetectorClass(name); }

    inline units::millisecond_t GetLatencyPipeline() const { return units::millisecond_t{LimelightHelpers::getLatency_Pipeline(name)}; }

    inline units::millisecond_t GetLatencyCapture() const { return units::millisecond_t{LimelightHelpers::getLatency_Capture(name)}; }

    inline int GetCurrentPipelineIndex() const { return LimelightHelpers::getCurrentPipelineIndex(name); }

    inline std::string GetCurrentPipelineType() const { return LimelightHelpers::getCurrentPipelineType(name); }

    inline std::string GetJSONDump() const { return LimelightHelpers::getJSONDump(name); }

    inline frc::Pose3d GetBotPose() const { return LimelightHelpers::getBotPose3d(name); }

    inline frc::Pose3d GetBotPoseWpiRed() const { return LimelightHelpers::getBotPose3d_wpiRed(name); }

    inline frc::Pose3d GetBotPoseWpiBlue() const { return LimelightHelpers::getBotPose3d_wpiBlue(name); }

    inline frc::Pose3d GetBotPoseTargetSpace() const { return LimelightHelpers::getBotPose3d_TargetSpace(name); }

    inline frc::Pose3d GetCameraPoseTargetSpace() const { return LimelightHelpers::getCameraPose3d_TargetSpace(name); }

    inline frc::Pose3d GetCameraPoseRobotSpace() const { return LimelightHelpers::getCameraPose3d_RobotSpace(name); }

    inline frc::Pose3d GetTargetPoseCameraSpace() const { return LimelightHelpers::getTargetPose3d_CameraSpace(name); }

    inline frc::Pose3d GetTargetPoseRobotSpace() const { return LimelightHelpers::getTargetPose3d_RobotSpace(name); }

    frc::Color GetTargetColor() const;

    std::optional<int> GetFiducialID() const;

    inline std::string GetNeuralClassID() const { return LimelightHelpers::getNeuralClassID(name); }

    inline std::vector<std::string> GetRawBarcodeData() const { return LimelightHelpers::getRawBarcodeData(name); }

    inline int GetHeartbeat() const { return LimelightHelpers::getHeartbeat(name); }

    std::vector<CornerCoordinates> GetCornerCoordinates() const;

    inline void SetPipelineIndex(int index) { LimelightHelpers::setPipelineIndex(name, index); }

    inline void SetPriorityTagID(int ID) { LimelightHelpers::setPriorityTagID(name, ID); }

    void SetLEDMode(LEDMode mode);

    void SetStreamMode(StreamMode mode);

    void SetCropWindow(double xMin, double xMax, double yMin, double yMax);

    void SetRobotOrientation(units::degree_t yaw, units::degrees_per_second_t yawRate, units::degree_t pitch,
                             units::degrees_per_second_t pitchRate, units::degree_t roll, units::degrees_per_second_t rollRate,
                             bool flush = true);

    inline void SetKeystone(double horizontal, double vertical) { LimelightHelpers::setKeystone(name, horizontal, vertical); }

    inline void SetIMUMode(IMUMode mode) { LimelightHelpers::SetIMUMode(name, static_cast<int>(mode)); }

    inline void SetIMUAssistAlpha(double alpha) { LimelightHelpers::SetIMUAssistAlpha(name, alpha); }

    inline void SetThrottle(int throttle) { LimelightHelpers::SetThrottle(name, throttle); }

    void SetFiducial3DOffset(const frc::Translation3d& offset);

    inline void SetFiducialIDFiltersOverride(std::vector<int> validIDs) { LimelightHelpers::SetFiducialIDFiltersOverride(name, validIDs); }

    inline void SetFiducialDownscalingOverride(float downscale) { LimelightHelpers::SetFiducialDownscalingOverride(name, downscale); }

    void SetCameraPoseRobotSpace(const frc::Pose3d& pose);

    inline void SetPythonScriptData(const std::vector<double>& outgoingData) { LimelightHelpers::setPythonScriptData(name, outgoingData); }

    inline std::vector<double> GetPythonScriptData() const { return LimelightHelpers::getPythonScriptData(name); }

    inline void TriggerSnapshot() { LimelightHelpers::triggerSnapshot(name); }

    inline void SetRewindEnabled(bool enabled) { LimelightHelpers::setRewindEnabled(name, enabled); }

    inline void TriggerRewindCapture(units::second_t duration) { LimelightHelpers::triggerRewindCapture(name, duration.value()); }

    PoseEstimate GetBotPoseEstimateWpiBlue(bool mt2) const;

    PoseEstimate GetBotPoseEstimateWpiRed(bool mt2) const;

    std::vector<RawFiducial> GetRawFiducials() const;

    std::vector<RawDetection> GetRawDetections() const;

    std::vector<RawTarget> GetRawTargets() const;

    inline IMUData GetIMUData() const { return LimelightHelpers::getIMUData(name); }

    inline void SetupPortForwarding() { LimelightHelpers::SetupPortForwarding(name); }

    LimelightHelpers::LimelightResultsClass GetLatestResults(bool profile = false) const;
 
    void LoggablePeriodic() override;

    static inline void SetupPortForwardingUSB(int usbIndex) { LimelightHelpers::SetupPortForwardingUSB(usbIndex); }

    static constexpr std::pair<units::degree_t, units::degree_t> GetFOV(Model model) {
        switch (model) {
            case Model::One:
                return {54_deg, 41_deg};
            case Model::Two:
            case Model::TwoPlus:
                return {62.5_deg, 48.9_deg};
            case Model::Three:
                return {62.5_deg, 48.9_deg};
            case Model::ThreeA:
                return {54.5_deg, 42_deg};
            case Model::ThreeG:
                return {82_deg, 56.2_deg};
            default:
                return {82_deg, 56.2_deg};
        }
    }
 
   private:
    std::string name;
};
 
}  // namespace cameras
 
}  // namespace valor
 
template <>
struct wpi::Struct<valor::cameras::Limelight::TargetMetrics> {
    static constexpr std::string_view GetTypeName() { return "LimelightTargetMetrics"; }
 
    static constexpr size_t GetSize() { return 97; }
 
    static constexpr std::string_view GetSchema() {
        return "bool targetValid; int32 targetCount; double targetLatency; double captureLatency; double tx; double ty; double txnc; "
               "double tync; double ta; int32 tid; int32 targetClassIndexDetector; int32 targetClassIndexClassifier; int32 "
               "targetLongSidePixels; int32 targetShortSidePixels; int32 targetHorizontalExtentPixels; int32 targetVerticalExtentPixels; "
               "double targetSkew";
    }
 
    static void Pack(std::span<uint8_t> data, const valor::cameras::Limelight::TargetMetrics& value);
    static valor::cameras::Limelight::TargetMetrics Unpack(std::span<const uint8_t> data);
};
 
template <>
struct wpi::Struct<valor::cameras::Limelight::CornerCoordinates> {
    static constexpr std::string_view GetTypeName() { return "LimelightCornerCoordinates"; }
 
    static constexpr size_t GetSize() { return sizeof(valor::cameras::Limelight::CornerCoordinates); }
 
    static constexpr std::string_view GetSchema() { return "double x0; double y0; double x1; double y1"; }
 
    static void Pack(std::span<uint8_t> data, const valor::cameras::Limelight::CornerCoordinates& value);
    static valor::cameras::Limelight::CornerCoordinates Unpack(std::span<const uint8_t> data);
};
 
template <>
struct wpi::Struct<valor::cameras::Limelight::IMUData> {
    static constexpr std::string_view GetTypeName() { return "LimelightIMUData"; }
 
    static constexpr size_t GetSize() { return sizeof(double) * 10; }
 
    static constexpr std::string_view GetSchema() {
        return "double robotYaw; double roll; double pitch; double yaw; double gyroX; double gyroY; double gyroZ; double accelX; double "
               "accelY; double accelZ";
    }
 
    static void Pack(std::span<uint8_t> data, const valor::cameras::Limelight::IMUData& value);
    static valor::cameras::Limelight::IMUData Unpack(std::span<const uint8_t> data);
};
 
template <>
struct wpi::Struct<valor::cameras::Limelight::RawTarget> {
    static constexpr std::string_view GetTypeName() { return "LimelightRawTarget"; }
 
    static constexpr size_t GetSize() { return sizeof(double) * 3; }
 
    static constexpr std::string_view GetSchema() { return "double txnc; double tync; double ta"; }
 
    static void Pack(std::span<uint8_t> data, const valor::cameras::Limelight::RawTarget& value);
    static valor::cameras::Limelight::RawTarget Unpack(std::span<const uint8_t> data);
};
 
template <>
struct wpi::Struct<valor::cameras::Limelight::RawFiducial> {
    static constexpr std::string_view GetTypeName() { return "LimelightRawFiducial"; }
 
    static constexpr size_t GetSize() { return sizeof(int) + sizeof(double) * 6; }
 
    static constexpr std::string_view GetSchema() {
        return "int32 id; double txnc; double tync; double ta; double distToCamera; double distToRobot; double ambiguity";
    }
 
    static void Pack(std::span<uint8_t> data, const valor::cameras::Limelight::RawFiducial& value);
    static valor::cameras::Limelight::RawFiducial Unpack(std::span<const uint8_t> data);
};
 
template <>
struct wpi::Struct<valor::cameras::Limelight::RawDetection> {
    static constexpr std::string_view GetTypeName() { return "LimelightRawDetection"; }
 
    static constexpr size_t GetSize() { return sizeof(int) + sizeof(double) * 11; }
 
    static constexpr std::string_view GetSchema() {
        return "int32 classId; double txnc; double tync; double ta; double corner0_X; double corner0_Y; double corner1_X; double "
               "corner1_Y; double corner2_X; double corner2_Y; double corner3_X; double corner3_Y";
    }
 
    static void Pack(std::span<uint8_t> data, const valor::cameras::Limelight::RawDetection& value);
    static valor::cameras::Limelight::RawDetection Unpack(std::span<const uint8_t> data);
};