WorldAlign.h

#pragma once
#include <utility>
 
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Transform2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/StructTopic.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/base.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
 
#include "valkyrie/Loggable.h"
#include "valkyrie/controllers/PIDF.h"
 
namespace valor {

class WorldAlign : public valor::Loggable {
   public:
    using TranslationalPIDF_t = valor::PIDF<units::meter, units::meters_per_second>;
    using RotationalPIDF_t = valor::PIDF<units::radian, units::radians_per_second>;

 
    WorldAlign(TranslationalPIDF_t xPIDF, TranslationalPIDF_t yPIDF, RotationalPIDF_t rotationalPIDF,
               frc::TrapezoidProfile<units::meter>::Constraints xConstraints, frc::TrapezoidProfile<units::meter>::Constraints yConstraints,
               frc::TrapezoidProfile<units::radian>::Constraints rotConstraints);

    static frc::Pose2d OffsetGetter(frc::Pose2d, frc::Transform2d);
 
    template <class Distance, class Velocity = units::compound_unit<Distance, units::inverse<units::seconds>>,
              class Acceleration = units::compound_unit<Velocity, units::inverse<units::seconds>>>
    struct StateComponent {
        units::unit_t<Distance> distance{0};
        units::unit_t<Velocity> velocity{0};
        units::unit_t<Acceleration> acceleration{0};
    };
 
    struct StateVector {
        StateComponent<units::meter> xState;
        StateComponent<units::meter> yState;
        StateComponent<units::radian> rotationState;
    };

    void SetPID(TranslationalPIDF_t x, TranslationalPIDF_t y, RotationalPIDF_t rot);

    void SetXPID(TranslationalPIDF_t);

    void SetYPID(TranslationalPIDF_t);

    void SetRotationalPID(RotationalPIDF_t);

    std::tuple<TranslationalPIDF_t, TranslationalPIDF_t, RotationalPIDF_t> GetPID();

    TranslationalPIDF_t GetXPID();

    TranslationalPIDF_t GetYPID();

    RotationalPIDF_t GetRotationalPID();

    void ResetControllers(frc::Pose2d, frc::ChassisSpeeds);

    void ResetXController(units::meter_t, units::meters_per_second_t);

    void ResetYController(units::meter_t, units::meters_per_second_t);

    void ResetRotationalController(frc::Rotation2d, units::radians_per_second_t);

    void SetConstraints(frc::TrapezoidProfile<units::meter>::Constraints x, frc::TrapezoidProfile<units::meter>::Constraints y,
                        frc::TrapezoidProfile<units::radian>::Constraints rot);

    void SetXConstraints(frc::TrapezoidProfile<units::meter>::Constraints);

    void SetYConstraints(frc::TrapezoidProfile<units::meter>::Constraints);

    void SetRotationalConstraints(frc::TrapezoidProfile<units::radian>::Constraints);

    void EnableContinuousRotation(bool enable = true, units::radian_t minimum = units::radian_t{-std::numbers::pi},
                                  units::radian_t maximum = units::radian_t{std::numbers::pi});

    frc::TrapezoidProfile<units::meter>::Constraints GetXConstraints();

    frc::TrapezoidProfile<units::meter>::Constraints GetYConstraints();

    frc::TrapezoidProfile<units::radian>::Constraints GetRotationalConstraints();

    void SetGoal(frc::Pose2d goal, StateVector state, bool rotate);

    std::pair<frc::Pose2d, StateVector> GetGoal();

    frc::ChassisSpeeds Calculate(frc::Pose2d pose, frc::ChassisSpeeds currentSpeeds);

    frc::ChassisSpeeds Calculate(frc::Pose2d pose, frc::ChassisSpeeds currentSpeeds, StateVector state);
 
    void LoggablePeriodic() override;
 
   private:
    frc::ProfiledPIDController<units::meter> xController, yController;
    frc::ProfiledPIDController<units::radian> rotationalController;
 
    TranslationalPIDF_t xPIDF, yPIDF;  // Store PIDF values to access kV
    RotationalPIDF_t rotationalPIDF;   // Store PIDF values to access kV
 
    frc::Pose2d goalPosition, currPosition;
    frc::ChassisSpeeds currSpeed;
    StateVector goalState, setpointState;
    double calculatedXVal, calculatedYVal;
};
 
}  // namespace valor