rotation2d.h

#pragma once
 
#include "cevalm.hpp"
#include "core/math/eigen_interface.h"
#include "core/math/units.h"

class Rotation2d {
   private:
    double cos_ = 1;
    double sin_ = 0;
 
   public:
    constexpr Rotation2d() = default;

    constexpr Rotation2d(units::Angle value) : cos_(units::cos(value)), sin_(units::sin(value)) {}

    constexpr Rotation2d(double value) : cos_(cevalm::cos(value)), sin_(cevalm::sin(value)) {}

    constexpr Rotation2d(double x, double y) {
        double mag = cevalm::hypot(x, y);
        if (x != 0 || y != 0) {
            cos_ = x / mag;
            sin_ = y / mag;
        } else {
            cos_ = 1;
            sin_ = 0;
        }
    }

    constexpr Rotation2d(units::Length x, units::Length y)
        : Rotation2d(x.to(units::inches), y.to(units::inches)) {}

    constexpr units::Angle angle() const {
        return units::Angle(cevalm::atan2(sin_, cos_), units::radians);
    }

    constexpr double angle(units::Angle unit) const { return angle().to(unit); }

    constexpr double radians() const { return angle().to(units::radians); }

    constexpr double degrees() const { return angle().to(units::degrees); }

    constexpr double revolutions() const { return angle().to(units::revolutions); }

    constexpr double gradians() const { return angle().to(units::gradians); }

    constexpr double f_cos() const { return cos_; }

    constexpr double f_sin() const { return sin_; }

    constexpr double f_tan() const { return sin_ / cos_; }

    constexpr Rotation2d inverse() const { return Rotation2d(cos_, -sin_); }

    constexpr Rotation2d opposite() const { return Rotation2d(-cos_, -sin_); }

    constexpr EMat<2, 2> rotation_matrix() const { return EMat<2, 2>{{cos_, -sin_}, {sin_, cos_}}; }

    constexpr double wrapped_radians_180() const { return wrap_radians_180(radians()); }

    constexpr double wrapped_degrees_180() const { return wrap_degrees_180(degrees()); }

    constexpr double wrapped_revolutions_180() const { return wrap_revolutions_180(revolutions()); }

    constexpr double wrapped_gradians_180() const { return wrap_gradians_180(gradians()); }

    constexpr double wrapped_radians_360() const { return wrap_radians_360(radians()); }

    constexpr double wrapped_degrees_360() const { return wrap_degrees_360(degrees()); }

    constexpr double wrapped_revolutions_360() const { return wrap_revolutions_360(revolutions()); }

    constexpr double wrapped_gradians_360() const { return wrap_gradians_360(gradians()); }

    constexpr Rotation2d operator+(Rotation2d other) const {
        return Rotation2d(
                cos_ * other.cos_ - sin_ * other.sin_, cos_ * other.sin_ + sin_ * other.cos_
        );
    }

    constexpr Rotation2d& operator+=(Rotation2d other) { return *this = *this + other; }

    constexpr Rotation2d operator-(Rotation2d other) const { return *this + -other; }

    constexpr Rotation2d& operator-=(Rotation2d other) { return *this = *this - other; }

    constexpr Rotation2d operator-() const { return inverse(); }

    constexpr Rotation2d operator*(double scalar) const { return Rotation2d(radians() * scalar); }

    friend constexpr Rotation2d operator*(double scalar, Rotation2d rotation) {
        return rotation * scalar;
    }

    constexpr Rotation2d& operator*=(double scalar) { return *this = *this * scalar; }

    constexpr Rotation2d operator/(double scalar) const { return *this * (1.0 / scalar); }

    constexpr Rotation2d& operator/=(double scalar) { return *this = *this / scalar; }

    constexpr bool operator==(Rotation2d other) const {
        return cos_ == other.cos_ && sin_ == other.sin_;
    }

    constexpr bool is_near(Rotation2d other, units::Angle tolerance = units::Angle(1e-6)) const {
        return units::abs((*this - other).angle()) <= tolerance;
    }

    static constexpr double deg2rad(double deg) { return deg * (std::numbers::pi / 180.0); }

    static constexpr double rad2deg(double rad) { return rad * (180.0 / std::numbers::pi); }

    static constexpr double wrap_radians_180(double angle) {
        if (angle >= -std::numbers::pi && angle <= std::numbers::pi) {
            return angle;
        }
        double x = cevalm::fmod(angle, 2 * std::numbers::pi);
        if (x > std::numbers::pi) {
            x -= 2 * std::numbers::pi;
        } else if (x < -std::numbers::pi) {
            x += 2 * std::numbers::pi;
        }
        return x;
    }

    static constexpr double wrap_degrees_180(double angle) {
        if (angle >= -180.0 && angle <= 180.0) {
            return angle;
        }
        double x = cevalm::fmod(angle, 360.0);
        if (x > 180.0) {
            x -= 360.0;
        } else if (x < -180.0) {
            x += 360.0;
        }
        return x;
    }

    static constexpr double wrap_revolutions_180(double angle) {
        if (angle >= -0.5 && angle <= 0.5) {
            return angle;
        }
        double x = cevalm::fmod(angle, 1.0);
        if (x > 0.5) {
            x -= 1.0;
        } else if (x < -0.5) {
            x += 1.0;
        }
        return x;
    }

    static constexpr double wrap_gradians_180(double angle) {
        if (angle >= -200.0 && angle <= 200.0) {
            return angle;
        }
        double x = cevalm::fmod(angle, 400.0);
        if (x > 200.0) {
            x -= 400.0;
        } else if (x < -200.0) {
            x += 400.0;
        }
        return x;
    }

    static constexpr double wrap_radians_360(double angle) {
        if (angle >= 0.0 && angle < 2 * std::numbers::pi) {
            return angle;
        }
        double x = cevalm::fmod(angle, 2 * std::numbers::pi);
        if (x < 0.0) {
            x += 2 * std::numbers::pi;
        }
        return (x >= 2 * std::numbers::pi) ? 0.0 : x;
    }

    static constexpr double wrap_degrees_360(double angle) {
        if (angle >= 0.0 && angle < 360.0) {
            return angle;
        }
        double x = cevalm::fmod(angle, 360.0);
        if (x < 0.0) {
            x += 360.0;
        }
        return (x >= 360.0) ? 0.0 : x;
    }

    static constexpr double wrap_revolutions_360(double angle) {
        if (angle >= 0.0 && angle < 1.0) {
            return angle;
        }
        double x = cevalm::fmod(angle, 1.0);
        if (x < 0.0) {
            x += 1.0;
        }
        return (x >= 1.0) ? 0.0 : x;
    }

    static constexpr double wrap_gradians_360(double angle) {
        if (angle >= 0.0 && angle < 400.0) {
            return angle;
        }
        double x = cevalm::fmod(angle, 400.0);
        if (x < 0.0) {
            x += 400.0;
        }
        return (x >= 400.0) ? 0.0 : x;
    }
};