#pragma once


/*  MOUNT CONTROL COMPONENTS LIBRARY  */

/*  AN REFERENCE "PERIODIC-ERROR-CORRECTION" CLASS IMPLEMENTATION  */



#include "fitpack/fitpack.h"
#include "mcc_mount_coord.h"

namespace mcc
{


namespace traits
{

template <typename T, typename XT, typename YT>
concept mcc_mount_pec_c = requires(T t, XT x, YT y) {
    typename T::pec_result_t;

    { t.compute(std::declval<const XT&>(), std::declval<const YT&>()) } -> std::same_as<typename T::pec_result_t>;
};

}  // namespace traits

// type of PEC corrections (algorithm used):
//     PEC_TYPE_GEOMETRY - "classic" geometry-based correction coefficients
//     PEC_TYPE_GEOMETRY_BSPLINE - previous one and additional 2D B-spline corrections
//     PEC_TYPE_BSPLINE - pure 2D B-spline corrections
enum class MccMountPECType { PEC_TYPE_GEOMETRY, PEC_TYPE_GEOMETRY_BSPLINE, PEC_TYPE_BSPLINE };

template <MccMountPECType TYPE = MccMountPECType::PEC_TYPE_GEOMETRY>
class MccMountPEC
{
public:
    static constexpr MccMountPECType pecType = TYPE;

    struct pec_result_t {
        MccAngle dx, dy;
    };

    // "classic" geometric PEC coefficients
    struct pec_geom_coeffs_t {
        typedef double coeff_t;

        coeff_t zeroPointX;
        coeff_t zeroPointY;
        coeff_t collimationErr;  // tube collimation error
        coeff_t nonperpendErr;   // X-Y axes nonperpendicularity
        coeff_t misalignErr1;    // misalignment of hour-angle/azimuth axis: left-right for equatorial, East-West for
                                 // alt-azimuthal
        coeff_t misalignErr2;    // misalignment of hour-angle/azimuth axis: vertical for equatorial, North-South for
                                 // alt-azimuthal

        coeff_t tubeFlexure;
        coeff_t forkFlexure;
        coeff_t DECaxisFlexure;  // declination axis flexure
    };

    // B-splines coefficients
    struct pec_bspline_coeffs_t {
        typedef double knot_t;
        typedef double coeff_t;

        size_t bsplDegreeX = 3;
        size_t bsplDegreeY = 3;

        std::vector<knot_t> knotsX{};
        std::vector<knot_t> knotsY{};

        std::vector<coeff_t> coeffsX{};
        std::vector<coeff_t> coeffsY{};
    };


    // constructors

    template <std::derived_from<MccAngle> PhiT>
    MccMountPEC(const PhiT& phi, pec_geom_coeffs_t geom_coeffs, pec_bspline_coeffs_t bspline_coeffs)
        requires(TYPE == MccMountPECType::PEC_TYPE_GEOMETRY_BSPLINE)
        : _phi(phi), _geomCoeffs(std::move(geom_coeffs)), _bspleCoeffs(std::move(bspline_coeffs))
    {
    }

    template <std::derived_from<MccAngle> PhiT>
    MccMountPEC(const PhiT& phi, pec_geom_coeffs_t geom_coeffs)
        requires(TYPE == MccMountPECType::PEC_TYPE_GEOMETRY)
        : _phi(phi), _geomCoeffs(std::move(geom_coeffs)), _bspleCoeffs()
    {
    }


    MccMountPEC(pec_bspline_coeffs_t bspline_coeffs)
        requires(TYPE == MccMountPECType::PEC_TYPE_BSPLINE)
        : _geomCoeffs(), _bspleCoeffs(std::move(bspline_coeffs))
    {
    }


    // X and Y axis encoder coordinates
    template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
    pec_result_t compute(const XT& x, const YT& y)
    {
        static constexpr MccCoordPairKind coord_kind = traits::mcc_type_pair_hash<XT, YT>();

        pec_result_t res{0.0, 0.0};

        if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
            if constexpr (TYPE == MccMountPECType::PEC_TYPE_GEOMETRY) {
                const auto cosPhi = std::cos(_phi);
                const auto sinPhi = std::sin(_phi);
                const auto tanY = std::tan(y);
                const auto sinX = std::sin(x);
                const auto cosX = std::cos(x);
                const auto cosY = std::cos(y);


                res.dx = _geomCoeffs.zeroPointX + _geomCoeffs.collimationErr / cosY + _geomCoeffs.nonperpendErr * tanY -
                         _geomCoeffs.misalignErr1 * cosX * tanY + _geomCoeffs.misalignErr2 * sinX * tanY +
                         _geomCoeffs.tubeFlexure * cosPhi * sinX / cosY -
                         _geomCoeffs.DECaxisFlexure * (cosPhi * cosX + sinPhi * tanY);

                res.dy = _geomCoeffs.zeroPointY + _geomCoeffs.misalignErr1 * sinX + _geomCoeffs.misalignErr2 * cosX +
                         _geomCoeffs.tubeFlexure * (cosPhi * cosX * std::sin(y) - sinPhi * cosY) +
                         _geomCoeffs.forkFlexure / cosX;
            }

            if constexpr (TYPE == MccMountPECType::PEC_TYPE_BSPLINE ||
                          TYPE == MccMountPECType::PEC_TYPE_GEOMETRY_BSPLINE) {
                double spl_valX, spl_valY;

                int ret = fitpack::fitpack_eval_spl2d(_bspleCoeffs.knotsX, _bspleCoeffs.knotsY, _bspleCoeffs.coeffsX, x,
                                                      y, spl_valX, _bspleCoeffs.bsplDegreeX, _bspleCoeffs.bsplDegreeY);

                if (ret) {
                    res.dx = std::numeric_limits<double>::quiet_NaN();
                    res.dy = std::numeric_limits<double>::quiet_NaN();
                    return res;
                }


                ret = fitpack::fitpack_eval_spl2d(_bspleCoeffs.knotsX, _bspleCoeffs.knotsY, _bspleCoeffs.coeffsY, x, y,
                                                  spl_valY, _bspleCoeffs.bsplDegreeX, _bspleCoeffs.bsplDegreeY);

                if (ret) {
                    res.dx = std::numeric_limits<double>::quiet_NaN();
                    res.dy = std::numeric_limits<double>::quiet_NaN();
                    return res;
                }


                res.dx += spl_valX;
                res.dy += spl_valY;
            }
        } else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
        } else {
            static_assert(false, "UNSUPPORTED");
        }

        return res;
    }


    // X and Y apparent equatorial/altazimuthal coordinates (not corrected for refraction)
    template <std::derived_from<MccAngle> XT, std::derived_from<MccAngle> YT>
    pec_result_t computeInverse(const XT& x, const YT& y)
    {
        static constexpr MccCoordPairKind coord_kind = traits::mcc_type_pair_hash<XT, YT>();

        pec_result_t res{0.0, 0.0};

        if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
        } else if constexpr (coord_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
        } else {
            static_assert(false, "UNSUPPORTED");
        }

        return res;
    }

private:
    double _phi;
    pec_geom_coeffs_t _geomCoeffs;
    pec_bspline_coeffs_t _bspleCoeffs;
};

}  // namespace mcc