Limelight.h

#pragma once
 
#include <functional>
#include <iostream>
#include <memory>
#include <optional>
#include <string>
#include <unordered_set>
 
#include <Eigen/Core>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/estimator/PoseEstimator.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Twist3d.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/dimensionless.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <valkyrie/cameras/Limelight.h>
#include <valkyrie/sensors/PoseEstimateSensor.h>
#include <wpi/array.h>
 
namespace valor {
namespace sensors {
namespace apriltag {

class Limelight : public PoseEstimateSensor {
   public:
    enum class Solver {
        MT1,  
        MT2,  
    };

    struct Config {
        units::meter_t fieldBorderMargin = 0.5_m;

        units::meter_t translationalStdDevCoefficient = 0.01_m;
        units::radian_t rotationalStdDevCoefficient = 0.03_rad;
        double tagWeight = 1;

        units::second_t maxMeasurementAge = 2.0_s;
        double ambiguityThreshold = 0.4;
 
        Solver solver = Solver::MT1;
    };
 
    using VisionFilter = std::function<bool(const cameras::Limelight::PoseEstimate& estimate, const Config& config,
                                            const frc::AprilTagFieldLayout& fieldLayout)>;
    using DoubtCalculator = std::function<PoseEstimate::Doubts(const cameras::Limelight::PoseEstimate& estimate, const Config& config)>;

    static PoseEstimate::Doubts StandardDoubtCalculator(const cameras::Limelight::PoseEstimate& context, const Config& _config);

    static bool StandardVisionFilter(const cameras::Limelight::PoseEstimate& context, const Config& _config,
                                     const frc::AprilTagFieldLayout& fieldLayout);

    Limelight(std::shared_ptr<cameras::Limelight> camera, frc::AprilTagField field);

    void SetupSim(std::function<frc::Pose2d()> robotPoseGetter, std::unordered_set<int> tagIDs, units::degree_t horizontalFOV,
                  units::degree_t verticalFOV, units::meter_t maxDistance = units::meter_t{std::numeric_limits<double>::infinity()},
                  units::degree_t viewableAngle = 70_deg);

    inline const Config& GetConfig() const { return config; }
 
    inline void SetConfig(const Config& c) { config = c; }

    inline void SetVisionFilter(const VisionFilter _visionFilter) { visionFilter = _visionFilter; }

    inline void SetDoubtCalculator(const DoubtCalculator _doubtCalculator) { doubtCalculator = _doubtCalculator; }
 
    inline std::shared_ptr<cameras::Limelight> GetCamera() const { return cam; }
 
    void LoggablePeriodic() override;
 
   protected:
    struct SimState {
        std::function<frc::Pose2d()> robotPoseGetter;  
        std::unordered_set<int> tagIDs;                
        units::degree_t horizontalFOV;                 
        units::degree_t verticalFOV;                   
        units::meter_t maxDistance;                    
        units::degree_t viewableAngle;                 
    };
 
    std::unique_ptr<SimState> simState;  
 
   private:
    std::optional<PoseEstimate> GetPoseEstimate();
 
    void SimulationLoop();
 
    bool active = true;
 
    std::shared_ptr<cameras::Limelight> cam;
    Config config;
 
    frc::AprilTagFieldLayout fieldLayout;
 
    VisionFilter visionFilter = StandardVisionFilter;
    DoubtCalculator doubtCalculator = StandardDoubtCalculator;
};
 
}  // namespace apriltag
}  // namespace sensors
}  // namespace valor