#pragma once


/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */

/*                          A REFERENCE "POINTING-CORRECTION-MODEL" CLASS IMPLEMENTATION                            */


#include <mutex>

#ifdef USE_BSPLINE_PCM
#include "fitpack/mcc_bsplines.h"
#endif

#include "mcc_concepts.h"
#include "mcc_coordinate.h"

namespace mcc::impl
{

enum class MccDefaultPCMErrorCode : int {
    ERROR_OK,
    ERROR_CCTE,
#ifdef USE_BSPLINE_PCM
    ERROR_INVALID_INPUTS_BISPLEV,
#endif
    ERROR_EXCEED_MAX_ITERS,
    ERROR_NULLPTR
};

/*  error category definition  */

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

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

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

        switch (err) {
            case MccDefaultPCMErrorCode::ERROR_OK:
                return "OK";
            case MccDefaultPCMErrorCode::ERROR_CCTE:
                return "CCTE calculation error";
#ifdef USE_BSPLINE_PCM
            case MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV:
                return "invalid input arguments for bispev";
#endif
            case MccDefaultPCMErrorCode::ERROR_EXCEED_MAX_ITERS:
                return "exceed maximum of iterations number";
            case MccDefaultPCMErrorCode::ERROR_NULLPTR:
                return "nullptr input argument";
            default:
                return "UNKNOWN";
        }
    }

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


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


namespace std
{

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

}  // namespace std



namespace mcc::impl
{


// type of PCM corrections (algorithm used):
//     PCM_TYPE_GEOMETRY - "classic" geometry-based correction coefficients
//     PCM_TYPE_GEOMETRY_BSPLINE - previous one and additional 2D B-spline corrections
//     PCM_TYPE_BSPLINE - pure 2D B-spline corrections
enum class MccDefaultPCMType { PCM_TYPE_GEOMETRY, PCM_TYPE_GEOMETRY_BSPLINE, PCM_TYPE_BSPLINE };

template <MccDefaultPCMType TYPE>
static constexpr std::string_view MccDefaultPCMTypeString =
    TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY ? "GEOMETRY"
#ifdef USE_BSPLINE_PCM
    : TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE ? "GEOMETRY-BSPLINE"
    : TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE ? "BSPLINE"
#endif
                                                           : "UNKNOWN";

template <MccMountType MOUNT_TYPE>
class MccDefaultPCM : public mcc_pcm_interface_t<std::error_code>
{
public:
    static constexpr MccMountType mountType = MOUNT_TYPE;

#ifdef USE_BSPLINE_PCM
    static constexpr std::string_view pcmName{"MCC-GEOMETRY-BSPLINES-PCM"};
#else
    static constexpr std::string_view pcmName{"MCC-GEOMETRY-ONLY-PCM"};
#endif

    typedef std::error_code error_t;
    typedef double coord_t;


    // "classic" geometric PCM coefficients
    struct pcm_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
    };

#ifdef USE_BSPLINE_PCM
    // B-splines related data structure (coefficients, knots ...)
    struct pcm_bspline_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{};
    };
#endif

    struct pcm_data_t {
        MccDefaultPCMType type{MccDefaultPCMType::PCM_TYPE_GEOMETRY};
        double siteLatitude{0.0};  // in radians

        // from encoder to observed celestial
        pcm_geom_coeffs_t geomCoefficients{};
#ifdef USE_BSPLINE_PCM
        pcm_bspline_t bspline{};
#endif

        // from observed celestial to encoder
        pcm_geom_coeffs_t inverseGeomCoefficients{};
#ifdef USE_BSPLINE_PCM
        pcm_bspline_t inverseBspline{};
#endif
    };

    // constructors

    MccDefaultPCM() = default;
    MccDefaultPCM(pcm_data_t pdata) : MccDefaultPCM()
    {
        _pcmData = std::move(pdata);
    }

    MccDefaultPCM(MccDefaultPCM&& other) = default;
    MccDefaultPCM& operator=(MccDefaultPCM&& other) = default;

    MccDefaultPCM(const MccDefaultPCM&) = delete;
    MccDefaultPCM& operator=(const MccDefaultPCM&) = delete;

    virtual ~MccDefaultPCM() = default;

    // virtual ~MccDefaultPCM() {
    //     // _pcmData.geomCoefficients.DECaxisFlexure = 0.0;
    // };

    void setPCMData(pcm_data_t pdata)
    {
        std::lock_guard lock(*_pcmDataMutex);

        _pcmData = std::move(pdata);

        _cosPhi = std::cos(_pcmData.siteLatitude);
        _sinPhi = std::sin(_pcmData.siteLatitude);
    }


    pcm_data_t getPCMData() const
    {
        std::lock_guard lock(*_pcmDataMutex);

        return _pcmData;
    }

    void setPCMType(MccDefaultPCMType type)
    {
        std::lock_guard lock(*_pcmDataMutex);

#ifdef USE_BSPLINE_PCM
        _pcmData.type = type;
#else
        _pcmData.type = MccDefaultPCMType::PCM_TYPE_GEOMETRY;
#endif
    }

    MccDefaultPCMType getPCMType() const
    {
        std::lock_guard lock(*_pcmDataMutex);

        return _pcmData.type;
    }

    // The computed PCM quantities must be interpretated as:
    //     observed_X = encoder_X + res.pcmX
    //     observed_Y = encoder_Y + res.pcmY
    // so, input hw_coord is assumed to be coordinates pair of mount axis encoder readouts
    template <typename T = std::nullptr_t>
    error_t computePCM(mcc_coord_pair_c auto const& hw_coord, mcc_pcm_result_c auto* res, T* obs_skycoord = nullptr)
        requires(mcc_skypoint_c<T> || std::is_null_pointer_v<T>)
    {
        if (res == nullptr) {
            return MccDefaultPCMErrorCode::ERROR_NULLPTR;
        }

        std::lock_guard lock(*_pcmDataMutex);

        res->pcmX = 0.0;
        res->pcmY = 0.0;

        auto ret = _compResult(hw_coord.x(), hw_coord.y(), res, false);
        if (ret) {
            return ret;
        }

        if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {  // equatorial (HA and DEC axes)
            if constexpr (mcc_skypoint_c<T>) {
                obs_skycoord->from(MccSkyHADEC_OBS{double(hw_coord.x() + res->pcmX), double(hw_coord.y() + res->pcmY),
                                                   hw_coord.epoch()});
            }

        } else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
            if constexpr (mcc_skypoint_c<T>) {
                obs_skycoord->from(
                    MccSkyAZZD{double(hw_coord.x() + res->pcmX), double(hw_coord.y() + res->pcmY), hw_coord.epoch()});
            }
        } else {
            static_assert(false, "UNSUPPORTED MOUNT TYPE");
        }


        return MccDefaultPCMErrorCode::ERROR_OK;
    }

    //
    //  computed inverse PCM corrections must be interpretated as:
    //      X_encoder = X_observed + res->pcmX
    //      Y_encoder = Y_observed + res->pcmY
    //
    template <typename T>
    error_t computeInversePCM(mcc_skypoint_c auto const& obs_skycoord, mcc_pcm_result_c auto* res, T* hw_pt = nullptr)
        requires(mcc_coord_pair_c<T> || std::is_null_pointer_v<T>)
    {
        error_t ret = MccDefaultPCMErrorCode::ERROR_OK;

        if constexpr (mcc_coord_pair_c<T>) {
            static_assert(
                T::pairKind == MccCoordPairKind::COORDS_KIND_GENERIC || T::pairKind == MccCoordPairKind::COORDS_KIND_XY,
                "Invalid output coordinate pair type!");
        }

        // to be computed as observed celestial X and Y cordinate according to mount type (HA-DEC or AZ-ZD)
        double x, y;

        auto getXY = [&, this](auto& cp) {
            auto err = obs_skycoord.toAtSameEpoch(cp);
            if (err) {
                return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
            }

            x = cp.x();
            y = cp.y();

            return MccDefaultPCMErrorCode::ERROR_OK;
        };

        if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {
            MccSkyHADEC_OBS hadec;
            ret = getXY(hadec);
        } else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
            MccSkyAZZD azzd;
            ret = getXY(azzd);
        }

        std::lock_guard lock(*_pcmDataMutex);

        ret = _compResult(x, y, res, true);
        if (!ret) {
            if constexpr (mcc_coord_pair_c<T>) {
                *hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY};
            }
        }

        return ret;
    }

    /*
    template <typename T>
    error_t computeInversePCM(mcc_skypoint_c auto const& obs_skycoord,
                              mcc_pcm_result_c auto* result,
                              T* hw_pt = nullptr)
        requires(mcc_coord_pair_c<T> || std::is_null_pointer_v<T>)
    {
        error_t ret = MccDefaultPCMErrorCode::ERROR_OK;

        if constexpr (mcc_coord_pair_c<T>) {
            static_assert(
                T::pairKind == MccCoordPairKind::COORDS_KIND_GENERIC || T::pairKind == MccCoordPairKind::COORDS_KIND_XY,
                "Invalid output coordinate pair type!");
        }

        double x, y;

        auto comp_func = [&, this](auto& cp) {
            auto err = obs_skycoord.toAtSameEpoch(cp);
            if (err) {
                return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
            }

            x = cp.x();
            y = cp.y();

            return computePCM(cp, result);
        };


        // for small corrections only!!!
        if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {
            MccSkyHADEC_OBS hadec;
            ret = comp_func(hadec);
        } else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
            MccSkyAZZD azzd;
            ret = comp_func(azzd);
        }

        if (ret) {
            return ret;
        }

        if constexpr (mcc_coord_pair_c<T>) {
            *hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x - result->pcmX, y - result->pcmY};
        }

        return ret;
    }
    */

private:
    pcm_data_t _pcmData{};
    double _cosPhi{}, _sinPhi{};

    std::unique_ptr<std::mutex> _pcmDataMutex{new std::mutex};

    error_t _compResult(double x, double y, mcc_pcm_result_c auto* res, bool inverse)
    {
        pcm_geom_coeffs_t* geom_coeffs;
#ifdef USE_BSPLINE_PCM
        pcm_bspline_t* bspline;
#endif

        geom_coeffs = inverse ? &_pcmData.inverseGeomCoefficients : &_pcmData.geomCoefficients;
#ifdef USE_BSPLINE_PCM
        bspline = inverse ? &_pcmData.inverseBspline : &_pcmData.bspline;
#endif

        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY
#ifdef USE_BSPLINE_PCM
            || _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE
#endif
        ) {
            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->pcmX = _pcmData.geomCoefficients.zeroPointX;
            } else {
                res->pcmX = geom_coeffs->zeroPointX + geom_coeffs->collimationErr / cosY +
                            geom_coeffs->nonperpendErr * tanY - geom_coeffs->misalignErr1 * cosX * tanY +
                            geom_coeffs->misalignErr2 * sinX * tanY + geom_coeffs->tubeFlexure * _cosPhi * sinX / cosY -
                            geom_coeffs->DECaxisFlexure * (_cosPhi * cosX + _sinPhi * tanY);
            }

            res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX +
                        geom_coeffs->tubeFlexure * (_cosPhi * cosX * std::sin(y) - _sinPhi * cosY);

            if constexpr (mountType == MccMountType::FORK_TYPE) {
                if (!utils::isEqual(cosX, 0.0)) {
                    res->pcmY += geom_coeffs->forkFlexure / cosX;
                }
            }
        }


#ifdef USE_BSPLINE_PCM
        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE ||
            _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
            double spl_valX, spl_valY;

            int ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, spl_valX,
                                                   bspline->bsplDegreeX, bspline->bsplDegreeY);

            if (ret) {
                res->pcmX = std::numeric_limits<double>::quiet_NaN();
                res->pcmY = std::numeric_limits<double>::quiet_NaN();
                return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
            }


            ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, spl_valY,
                                               bspline->bsplDegreeX, bspline->bsplDegreeY);

            if (ret) {
                res->pcmX = std::numeric_limits<double>::quiet_NaN();
                res->pcmY = std::numeric_limits<double>::quiet_NaN();
                return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
            }


            res->pcmX += spl_valX;
            res->pcmY += spl_valY;
        }
#endif

        return MccDefaultPCMErrorCode::ERROR_OK;
    }
};


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

static_assert(mcc_pcm_c<MccMountDefaultForkPec>, "");
static_assert(std::movable<MccMountDefaultForkPec>);


}  // namespace mcc::impl