#pragma once


/*  MOUNT CONTROL COMPONENTS LIBRARY  */

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


#include <mutex>
#include "fitpack/fitpack.h"
#include "mcc_mount_concepts.h"
#include "mcc_mount_coord.h"

namespace mcc
{

enum class MccMountDefaultPECErrorCode : int { ERROR_OK, ERROR_INVALID_INPUTS_BISPLEV, ERROR_EXCEED_MAX_ITERS };

/*  error category definition  */

// error category
struct MccMountDefaultPECCategory : public std::error_category {
    MccMountDefaultPECCategory() : std::error_category() {}

    const char* name() const noexcept
    {
        return "ADC_GENERIC_DEVICE";
    }

    std::string message(int ec) const
    {
        MccMountDefaultPECErrorCode err = static_cast<MccMountDefaultPECErrorCode>(ec);

        switch (err) {
            case MccMountDefaultPECErrorCode::ERROR_OK:
                return "OK";
            case MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV:
                return "invalid input arguments for bispev";
            case MccMountDefaultPECErrorCode::ERROR_EXCEED_MAX_ITERS:
                return "exceed maximum of iterations number";
            default:
                return "UNKNOWN";
        }
    }

    static const MccMountDefaultPECCategory& get()
    {
        static const MccMountDefaultPECCategory constInst;
        return constInst;
    }
};


inline std::error_code make_error_code(MccMountDefaultPECErrorCode ec)
{
    return std::error_code(static_cast<int>(ec), MccMountDefaultPECCategory::get());
}
}  // namespace mcc


namespace std
{

template <>
class is_error_code_enum<mcc::MccMountDefaultPECErrorCode> : public true_type
{
};

}  // namespace std



namespace mcc
{


// 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 MccMountDefaultPECType { PEC_TYPE_GEOMETRY, PEC_TYPE_GEOMETRY_BSPLINE, PEC_TYPE_BSPLINE };

template <MccMountType MOUNT_TYPE>
class MccMountDefaultPEC final
{
public:
    static constexpr MccMountType mountType = MOUNT_TYPE;

    typedef std::error_code error_t;
    typedef MccAngle coord_t;

    struct pec_result_t {
        coord_t 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{};
    };

    struct pec_data_t {
        MccMountDefaultPECType type{MccMountDefaultPECType::PEC_TYPE_GEOMETRY};
        double siteLatitude{0.0};  // in radians
        pec_geom_coeffs_t geomCoefficients{};
        pec_bspline_coeffs_t bsplineCoefficients{};
    };

    // constructors

    MccMountDefaultPEC(pec_data_t pdata)
        : _pecData(std::move(pdata)),
          _phi(_pecData.siteLatitude),
          _geomCoeffs(_pecData.geomCoefficients),
          _bsplCoeffs(_pecData.bsplineCoefficients)
    {
    }


    void setData(pec_data_t pdata)
    {
        std::lock_guard lock(_pecDataMutex);

        _pecData = std::move(pdata);
        _phi = _pecData.siteLatitude;
        _geomCoeffs = _pecData.geomCoefficients;
        _bsplCoeffs = _pecData.bsplineCoefficients;
    }


    pec_data_t getData() const
    {
        std::lock_guard lock(_pecDataMutex);

        return _pecData;
    }

    void setType(MccMountDefaultPECType type)
    {
        std::lock_guard lock(_pecDataMutex);

        _pecData.type = type;
    }

    MccMountDefaultPECType getType() const
    {
        std::lock_guard lock(_pecDataMutex);

        return _pecData.type;
    }

    // The computed PEC quantities must be interpretated as:
    //     apparent_X = encoder_X + pec_result_t.dx
    //     apparent_Y = encoder_Y + pec_result_t.dy
    // so, input x and y are assumed to be mount axis encoder coordinates
    error_t compute(const coord_t& x, const coord_t& y, pec_result_t& res)
    {
        std::lock_guard lock(_pecDataMutex);

        if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {  // equatorial
            if (_pecData.type == MccMountDefaultPECType::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);


                if (utils::isEqual(cosY, 0.0)) {
                    res.dx = _geomCoeffs.zeroPointX;
                } else {
                    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);

                if constexpr (mountType == MccMountType::FORK_TYPE) {
                    if (!utils::isEqual(cosX, 0.0)) {
                        res.dy += _geomCoeffs.forkFlexure / cosX;
                    }
                }
            }

            if (_pecData.type == MccMountDefaultPECType::PEC_TYPE_BSPLINE ||
                _pecData.type == MccMountDefaultPECType::PEC_TYPE_GEOMETRY_BSPLINE) {
                double spl_valX, spl_valY;

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

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


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

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


                res.dx += spl_valX;
                res.dy += spl_valY;
            }
        } else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
        } else {
            static_assert(false, "UNSUPPORTED");
        }

        return MccMountDefaultPECErrorCode::ERROR_OK;
    }

    // from celestial to encoder (use of iterative scheme)
    error_t reverseCompute(const coord_t& x, const coord_t& y, pec_result_t& res, coord_t eps, size_t max_iter = 5)
    {
        coord_t e2 = eps * eps;

        coord_t xi = x, yi = y;
        coord_t xe, ye;
        size_t iter = 1;

        // the first iteration
        auto err = compute(x, y, res);

        if (!err) {
            // 'encoder' cocordinates
            xe = x - res.dx;
            ye = y - res.dy;

            err = compute(xe, ye, res);  // to celestial
            if (err) {
                return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
            }

            xi = xe + res.dx;  // celestial
            yi = ye + res.dy;

            auto rx = (x - xi);
            auto ry = (y - yi);
            auto d = rx * rx + ry * ry;

            bool ok = d <= e2;
            if (ok) {
                return MccMountDefaultPECErrorCode::ERROR_OK;
            }

            while (iter < max_iter) {
                err = compute(xi, yi, res);  // to encoder
                if (err) {
                    return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
                }
                xe = x - res.dx;
                ye = y - res.dy;

                err = compute(xe, ye, res);  // to celestial
                if (err) {
                    return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
                }

                xi = xe + res.dx;  // celestial
                yi = ye + res.dy;

                ok = ((x - xi) * (x - xi) + (y - yi) * (y - yi)) <= e2;
                if (ok) {
                    return MccMountDefaultPECErrorCode::ERROR_OK;
                }

                ++iter;
            }

            err = MccMountDefaultPECErrorCode::ERROR_EXCEED_MAX_ITERS;
        }

        return err;
    }


private:
    pec_data_t _pecData;
    double& _phi;
    pec_geom_coeffs_t& _geomCoeffs;
    pec_bspline_coeffs_t& _bsplCoeffs;

    mutable std::mutex _pecDataMutex;
};


typedef MccMountDefaultPEC<MccMountType::ALTAZ_TYPE> MccMountDefaultAltAzPec;
typedef MccMountDefaultPEC<MccMountType::FORK_TYPE> MccMountDefaultForkPec;

static_assert(traits::mcc_mount_pec_c<MccMountDefaultForkPec>, "");


}  // namespace mcc