#pragma once
#include <cstdint>
#include <cmath>

namespace cody_waite {

static constexpr double PI_HI = 3.141592653589793116e+00;
static constexpr double PI_LO = 1.224646799147353207e-16;
static constexpr double ONE_OVER_PI = 3.183098861837906912e-01;
static constexpr double MAGIC = 6755399441055744.0;

inline void reduce_angle(double x, double& k, double& r) {
    k = std::rint(x * ONE_OVER_PI);
    r = x - k * PI_HI - k * PI_LO;
}

inline double sin_poly(double y) {
    double y2 = y * y;
    return y * (1.0 + y2 * (-1.666666666666666574e-01 +
                            y2 * (8.333333333333333217e-03 +
                                  y2 * (-1.984126984126984063e-04 +
                                        y2 * (2.755731922398588873e-06 +
                                              y2 * (-2.505210838544171878e-08 +
                                                    y2 * (1.589623016257666155e-10 +
                                                          y2 * (-6.613756800334100348e-13 +
                                                                y2 * (1.801160902500000203e-15)))))))));
}

inline double cos_poly(double y) {
    double y2 = y * y;
    return 1.0 + y2 * (-5.000000000000000000e-01 +
                       y2 * (4.166666666666666667e-02 +
                             y2 * (-1.388888888888888889e-03 +
                                   y2 * (2.480158730158730159e-05 +
                                         y2 * (-2.755731922398588824e-07 +
                                               y2 * (2.087675698786809708e-09 +
                                                     y2 * (-1.135230397901676876e-11 +
                                                           y2 * (4.673742409611093972e-14))))))));
}

inline void sincos(double x, double& s, double& c) {
    double k, r;
    reduce_angle(x, k, r);
    int quadrant = static_cast<int>(k) & 3;
    double s_abs = sin_poly(r);
    double c_abs = cos_poly(r);
    switch (quadrant) {
        case 0: s = s_abs; c = c_abs; break;
        case 1: s = c_abs; c = -s_abs; break;
        case 2: s = -s_abs; c = -c_abs; break;
        case 3: s = -c_abs; c = s_abs; break;
    }
}

}