AllianceFlipperUtil.h

// Copyright (c) Choreo contributors
 
#pragma once
 
#include <array>
#include <numbers>
#include <utility>
 
#include <units/angle.h>
#include <units/length.h>
 
#include "choreo/util/FieldDimensions.h"
#include "choreo/util/Map.h"
 
namespace choreo::util {
 
enum class FlipperType { Mirrored, RotateAround };
 
struct MirroredFlipper {
  static constexpr bool isMirrored = true;
 
  static constexpr units::meter_t FlipX(units::meter_t x) {
    return fieldLength - x;
  }
 
  static constexpr units::meter_t FlipY(units::meter_t y) { return y; }
 
  static constexpr units::radian_t FlipHeading(units::radian_t heading) {
    return units::radian_t{std::numbers::pi} - heading;
  }
};
 
struct MirroredYFlipper {
  static constexpr bool isMirrored = true;
 
  static constexpr units::meter_t FlipX(units::meter_t x) { return x; }
 
  static constexpr units::meter_t FlipY(units::meter_t y) {
    return fieldWidth - y;
  }
 
  static constexpr units::radian_t FlipHeading(units::radian_t heading) {
    return -heading;
  }
};
 
struct RotateAroundFlipper {
  static constexpr bool isMirrored = false;
 
  static constexpr units::meter_t FlipX(units::meter_t x) {
    return fieldLength - x;
  }
 
  static constexpr units::meter_t FlipY(units::meter_t y) {
    return fieldWidth - y;
  }
 
  static constexpr units::radian_t FlipHeading(units::radian_t heading) {
    return units::radian_t{std::numbers::pi} + heading;
  }
};
 
inline constexpr Map flipperMap{
    std::array{std::pair{2022, FlipperType::RotateAround},
               std::pair{2023, FlipperType::Mirrored},
               std::pair{2024, FlipperType::Mirrored},
               std::pair{2025, FlipperType::RotateAround},
               std::pair{2026, FlipperType::RotateAround}}};
 
inline constexpr int kDefaultYear = 2026;
 
template <int Year = kDefaultYear>
constexpr auto GetFlipperForYear() {
  constexpr bool yearInMap = [] {
    try {
      [[maybe_unused]]
      auto checked = flipperMap.at(Year);
      return true;
    } catch (...) {
      return false;
    }
  }();
 
  if constexpr (!yearInMap) {
    static_assert(yearInMap, "Year not found in flipperMap");
  } else {
    constexpr auto flipperType = flipperMap.at(Year);
    if constexpr (flipperType == FlipperType::RotateAround) {
      return RotateAroundFlipper{};
    } else if constexpr (flipperType == FlipperType::Mirrored) {
      return MirroredFlipper{};
    } else {
      static_assert(flipperType == FlipperType::RotateAround ||
                        flipperType == FlipperType::Mirrored,
                    "Invalid FlipperType in flipperMap");
    }
  }
}
 
template <FlipperType flipperType>
constexpr auto GetFlipper() {
  if constexpr (flipperType == FlipperType::RotateAround) {
    return RotateAroundFlipper{};
  } else if constexpr (flipperType == FlipperType::Mirrored) {
    return MirroredFlipper{};
  } else {
    return MirroredYFlipper{};
  }
}
 
}  // namespace choreo::util