CANdleSensor.h

#pragma once
 
#include <bitset>
#include <iostream>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/core/CoreCANdle.hpp>
#include <ctre/phoenix6/core/CoreCANrange.hpp>
#include <frc/TimedRobot.h>
#include <valkyrie/sensors/BaseSensor.h>
 
namespace valor {
namespace sensors {

class CANdleSensor : public BaseSensor<int>, public ctre::phoenix6::hardware::core::CoreCANdle {
   public:
    static constexpr frc::Color8Bit VALOR_GOLD{0xEE, 0xA8, 0x00};
    static constexpr frc::Color8Bit VALOR_PURPLE{0xFF, 0x00, 0xFF};
    static constexpr frc::Color8Bit RED{0xFF, 0x00, 0x00};
    static constexpr frc::Color8Bit ORANGE{0xFF, 0x30, 0x00};
    static constexpr frc::Color8Bit GREEN{0x00, 0xFF, 0x00};
    static constexpr frc::Color8Bit BLUE{0x00, 0x00, 0xFF};
    static constexpr frc::Color8Bit LIGHT_BLUE{0xFF, 0x00, 0xF9};
    static constexpr frc::Color8Bit WHITE{0xFF, 0xFF, 0xFF};
    static constexpr frc::Color8Bit OFF{0x00, 0x00, 0x00};
 
    enum class Priority { LOW, MEDIUM, HIGH };

    static frc::Color8Bit CANcoderMagnetHealthGetter(ctre::phoenix6::hardware::CANcoder* cancoder);
 
    static frc::Color8Bit CANrangeHealthGetter(ctre::phoenix6::hardware::core::CoreCANrange& canrange);

    CANdleSensor(int ledCount, int numSegments, int canID, std::string canbus = "baseCAN");
    CANdleSensor(std::vector<int> segmentSizes, int canID, std::string canbus = "baseCAN");
 
    CANdleSensor(int ledCount, int numSegments);
    explicit CANdleSensor(std::vector<int> segmentSizes);
 
    void SetBrightness(double brightness, bool saveImmediately = true);
    void SetStripType(ctre::phoenix6::signals::StripTypeValue stripType, bool saveImmediately = true);
 
    inline void ApplyConfig() { GetConfigurator().Apply(config); }

    void SetLED(int led, frc::Color8Bit color);

    void SetColor(int segment, frc::Color8Bit color, Priority priority = Priority::LOW);

    void SetColor(frc::Color8Bit color, Priority priority = Priority::LOW);

    template <typename T>
    void SetAnimation(int segment, frc::Color8Bit color, Priority priority = Priority::LOW) {
        if (priority < currentPriority)
            return;
        currentPriority = priority;
        if (!segments[segment].animationSlot) {
            for (size_t i = 0; i < slots.size(); i++) {
                if (!slots.test(i)) {
                    segments[segment].animationSlot = i;
                    slots.set(i);
                    break;
                }
            }
        }
        auto controlRequest = std::make_unique<T>(segments[segment].startLed, segments[segment].endLed);
        if constexpr (requires(T& t) { t.Color; })
            controlRequest->Color = color;
        controlRequest->Slot = segments[segment].animationSlot.value();
        segments[segment].controlRequest = std::move(controlRequest);
    }

    template <typename T>
    void SetAnimation(frc::Color8Bit color, Priority priority = Priority::LOW) {
        if (priority < currentPriority)
            return;
        currentPriority = priority;
        slots.reset();
        slots.set(0);
        segments[0].animationSlot = 0;
        for (size_t i = 1; i < segments.size(); i++) {
            segments[i].animationSlot.reset();
            segments[i].controlRequest.reset();
        }
        auto controlRequest = std::make_unique<T>(segments[1].startLed, segments.back().endLed);
        controlRequest->Slot = 0;
        if constexpr (requires(T& t) { t.Color; })
            controlRequest->Color = color;
        segments[0].controlRequest = std::move(controlRequest);
    }

    void ClearAnimation(int segment);

    void Reset() override;
 
   private:
    struct Segment {
        std::unique_ptr<ctre::phoenix6::controls::ControlRequest> controlRequest; 
        std::optional<int> animationSlot;                                         
        int startLed;                                                             
        int endLed;                                                               
    };
 
    std::vector<int> GenerateSegmentSizes(int ledCount, int numSegments);
 
    Priority currentPriority = Priority::LOW; 
    std::vector<Segment> segments;            

    void Calculate() override;
 
    std::bitset<8> slots; 
 
    ctre::phoenix6::configs::CANdleConfiguration config;
};
 
}  // namespace sensors
}  // namespace valor