#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          SIMPLE SLEWING MODEL IMPLEMENTATION                            */


#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"

namespace mcc
{

enum class MccSimpleSlewingModelErrorCode : int {
    ERROR_OK,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_PCM_COMP,
    ERROR_GET_TELEMETRY,
    ERROR_DIST_TELEMETRY,
    ERROR_PZONE_CONTAINER_COMP,
    ERROR_TARGET_IN_PZONE,
    ERROR_NEAR_PZONE,
    ERROR_TIMEOUT,
    ERROR_ALREADY_SLEW,
    ERROR_ALREADY_STOPPED,
    ERROR_STOPPED
};

}  // namespace mcc


namespace std
{

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

}  // namespace std



namespace mcc
{

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

    const char* name() const noexcept
    {
        return "SIMPLE-SLEWING-MODEL";
    }

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

        switch (err) {
            case MccSimpleSlewingModelErrorCode::ERROR_OK:
                return "OK";
            case MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE:
                return "cannot get hardware state";
            case MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE:
                return "cannot set hardware state";
            case MccSimpleSlewingModelErrorCode::ERROR_PCM_COMP:
                return "PCM computation error";
            case MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY:
                return "cannot get telemetry";
            case MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY:
                return "cannot get target-to-mount-position distance";
            case MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP:
                return "pzone container computation error";
            case MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE:
                return "target is in prohibited zone";
            case MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE:
                return "near prohibited zone";
            case MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT:
                return "a timeout occured while slewing";
            case MccSimpleSlewingModelErrorCode::ERROR_ALREADY_SLEW:
                return "already slewing";
            case MccSimpleSlewingModelErrorCode::ERROR_ALREADY_STOPPED:
                return "slewing is already stopped";
            default:
                return "UNKNOWN";
        }
    }

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


inline std::error_code make_error_code(MccSimpleSlewingModelErrorCode ec)
{
    return std::error_code(static_cast<int>(ec), MccSimpleSlewingModelCategory::get());
}



/*
    The target celestial point must be set in telemetry->target
 */

class MccSimpleSlewingModel
{
    static constexpr auto DEG90INRADS = std::numbers::pi / 2.0;

public:
    typedef std::error_code error_t;

    typedef MccSimpleMovingModelParams slewing_params_t;

    template <mcc_all_controls_c CONTROLS_T, mcc_logger_c LoggerT = MccNullLogger>
    MccSimpleSlewingModel(CONTROLS_T* controls, LoggerT logger)
        : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
    {
        std::ostringstream os;
        os << std::this_thread::get_id();

        logger.logDebug(std::format("Create MccSimpleSlewingModel class instance (thread: {})", os.str()));

        *_stopSlewing = true;

        _checkTargetFunc = [controls, logger, this]() mutable -> error_t {
            typename CONTROLS_T::error_t t_err;

            MccTelemetryData tdata;
            bool in_zone;

            t_err = controls->telemetryData(&tdata);

            if (t_err) {
                return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
            }
            auto pz_err = controls->inPZone(tdata.target, &in_zone);
            if (pz_err) {
                return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            if (in_zone) {
                logger.logError("target point is in prohibited zone! Entered target coordinates:");
                logger.logError(std::format("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
                                            mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true),
                                            mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.LST}.sexagesimal(true)));
                logger.logError(std::format("    AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ZD}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ALT}.sexagesimal()));

                return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
            }

            return MccSimpleSlewingModelErrorCode::ERROR_OK;
        };

        _slewingFunc = [controls, logger = std::move(logger), this](bool slew_and_stop) mutable -> error_t {
            // first, check target coordinates
            typename CONTROLS_T::error_t t_err;
            MccTelemetryData tdata;

            {
                std::lock_guard lock{*_currentParamsMutex};
                t_err = controls->telemetryData(&tdata);

                if (t_err) {
                    return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
                }
            }

            bool in_zone;
            std::vector<bool> in_zone_vec;
            auto pz_err = controls->inPZone(tdata.target, &in_zone, &in_zone_vec);
            if (pz_err) {
                *_stopSlewing = true;
                return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            if (in_zone) {
                *_stopSlewing = true;

                size_t i = 0;
                for (; i < in_zone_vec.size(); ++i) {
                    if (in_zone_vec[i]) {
                        break;
                    }
                }

                logger.logError("target point is in prohibited zone (zone index: {})! Entered target coordinates:", i);
                logger.logError(std::format("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
                                            mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true),
                                            mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.LST}.sexagesimal(true)));
                logger.logError(std::format("    AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ZD}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ALT}.sexagesimal()));

                logger.logError(std::format("    hardware X, Y: {}, {}", mcc::MccAngle{tdata.target.X}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.Y}.sexagesimal()));

                return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
            }

            if (*_stopSlewing) {
                return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
            }

            double braking_accelX, braking_accelY;

            {
                // std::lock_guard lock{*_currentParamsMutex};
                if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
                    braking_accelX = std::numeric_limits<double>::min();
                } else {
                    braking_accelX = std::abs(_currentParams.brakingAccelX);
                }

                if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
                    braking_accelY = std::numeric_limits<double>::min();
                } else {
                    braking_accelY = std::abs(_currentParams.brakingAccelY);
                }
            }

            logger.logInfo(
                std::format("Start slewing in mode '{}'", (slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK")));
            logger.logInfo(std::format("    slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
            if (!slew_and_stop) {
                logger.logInfo(std::format("    slewing tolerance radius: {} arcsecs",
                                           mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
            }
            logger.logInfo(std::format("    braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
                                       mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
            logger.logInfo(std::format("    braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
                                       mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));

            MccCelestialPoint cpt;
            double min_time_to_pzone_in_secs;

            if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
            } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
            } else {
                static_assert(false, "UNKNOWN MOUNT TYPE!");
            }

            typename CONTROLS_T::hardware_state_t hw_state;

            auto hw_err = controls->hardwareGetState(&hw_state);
            if (hw_err) {
                *_stopSlewing = true;
                return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
            }

            hw_state.X = (double)tdata.target.X;
            hw_state.Y = (double)tdata.target.Y;
            {
                std::lock_guard lock{*_currentParamsMutex};

                hw_state.speedX = _currentParams.slewRateX;
                hw_state.speedY = _currentParams.slewRateY;

                min_time_to_pzone_in_secs =
                    std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
            }
            hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_SLEWING;

            if (*_stopSlewing) {
                logger.logDebug("slewing was stopped!");
                return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
            }

            // start slewing
            logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
                                        mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
            if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                logger.logDebug(std::format("    entered target: HA = {}, DEC = {}",
                                            mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
                logger.logDebug(std::format("    current mount:  HA = {}, DEC = {}",
                                            mcc::MccAngle{tdata.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
            } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                logger.logDebug(std::format("    entered target: AZ = {}, ZD = {}",
                                            mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
                logger.logDebug(std::format("    current mount:  AZ = {}, ZD = {}",
                                            mcc::MccAngle{tdata.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.ZD}.sexagesimal()));
            }

            hw_err = controls->hardwareSetState(hw_state);
            if (hw_err) {
                *_stopSlewing = true;
                return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
            }

            logger.logDebug("    the 'hardwareSetState' method performed successfully!");

            // std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
            // mcc_tp2tp(hw_state.time_point, start_slewing_tp); // not compiled!!

            // double dist, dx, dy, sinY, rate2, xrate;
            // std::chrono::duration<double> dtx, dty;  // seconds in double

            double dist;

            // bool adjust_mode = false;
            // static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate;

            std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
            start_slewing_tp = std::chrono::steady_clock::now();
            last_adjust_tp = start_slewing_tp;

            std::pair<double, double> distXY;


            if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                if (tdata.target.pair_kind != MccCoordPairKind::COORDS_KIND_HADEC_APP) {
                    // here, HA and DEC are changed during slewing process!!
                    slew_and_stop = false;
                }
            } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                if (!(tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT &&
                      tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD)) {
                    slew_and_stop = false;
                }
            }


            // main loop (simply monitors the current position taking into account the prohibited zones, as well as the
            // timeout of the entire process)
            while (!*_stopSlewing) {
                // wait for updated telemetry data
                {
                    std::lock_guard lock{*_currentParamsMutex};

                    t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);

                    if (t_err) {
                        *_stopSlewing = true;
                        return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
                    }
                }

                if (*_stopSlewing) {
                    return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
                }

                distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);

                logger.logTrace(
                    std::format("    the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
                                min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first),
                                mcc::MccAngleFancyString(distXY.second)));

                // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
                // and check them for getting into the prohibited zones
                if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                    cpt.X = tdata.HA + distXY.first;
                    cpt.Y = tdata.DEC_APP + distXY.second;

                    // cpt.X = tdata.HA + tdata.speedX * min_time_to_pzone_in_secs;
                    // cpt.Y = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs;
                    if (cpt.Y > DEG90INRADS) {
                        cpt.Y = DEG90INRADS;
                    }
                    if (cpt.Y < -DEG90INRADS) {
                        cpt.Y = -DEG90INRADS;
                    }

                    logger.logTrace(std::format("    current target: HA = {}, DEC = {}",
                                                mcc::MccAngle(tdata.target.HA).sexagesimal(true),
                                                mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
                    logger.logTrace(std::format("    current mount:  HA = {}, DEC = {}",
                                                mcc::MccAngle(tdata.HA).sexagesimal(true),
                                                mcc::MccAngle(tdata.DEC_APP).sexagesimal()));
                } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                    cpt.X = tdata.AZ + distXY.first;
                    cpt.Y = tdata.ZD + distXY.second;
                    // cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs;
                    // cpt.Y = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs;
                    if (cpt.Y < 0.0) {
                        cpt.Y = 0.0;
                    }
                    if (cpt.Y > std::numbers::pi) {
                        cpt.Y = std::numbers::pi;
                    }

                    logger.logTrace(std::format("    target: AZ = {}, ZD = {}",
                                                mcc::MccAngle(tdata.target.AZ).sexagesimal(),
                                                mcc::MccAngle(tdata.target.ZD).sexagesimal()));
                    logger.logTrace(std::format("    mount:  AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(),
                                                mcc::MccAngle(tdata.ZD).sexagesimal()));
                }
                mcc_tp2tp(tdata.time_point, cpt.time_point);

                logger.logTrace(std::format("    mount:  speedX = {}/s, speedY = {}/s",
                                            mcc::MccAngleFancyString(tdata.speedX),
                                            mcc::MccAngleFancyString(tdata.speedY)));

                pz_err = controls->inPZone(cpt, &in_zone);
                if (pz_err) {
                    *_stopSlewing = true;
                    return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                }

                if (in_zone) {
                    *_stopSlewing = true;
                    return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
                }



                {
                    std::lock_guard lock{*_currentParamsMutex};

                    if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) {
                        logger.logError("slewing process timeout!");
                        return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT;
                    }
                }

                hw_err = controls->hardwareGetState(&hw_state);
                if (hw_err) {
                    *_stopSlewing = true;
                    return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
                }

                if (slew_and_stop) {  // just wait for mount to be stopped
                    if (hw_state.moving_state == CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) {
                        logger.logInfo("mount moving state is STOPPED - exit!");
                        break;
                    }
                } else {
                    t_err = controls->targetToMountDist(&dist);
                    if (t_err) {
                        *_stopSlewing = true;
                        return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
                    }

                    logger.logTrace(std::format("    target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));

                    if (dist <= _currentParams.slewToleranceRadius) {  // stop slewing and exit from cycle
                        logger.logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!");
                        break;
                    }


                    // resend new position since target coordinates are changed in time
                    hw_state.X = (double)tdata.target.X;
                    hw_state.Y = (double)tdata.target.Y;

                    logger.logTrace(std::format("Send to hardware: X = {} degs, Y = {} degs",
                                                mcc::MccAngle{hw_state.X}.degrees(),
                                                mcc::MccAngle{hw_state.Y}.degrees()));
                    hw_err = controls->hardwareSetState(hw_state);
                    if (hw_err) {
                        *_stopSlewing = true;
                        return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
                    }


                    {
                        std::lock_guard lock{*_currentParamsMutex};
                        logger.logDebug("    the 'hardwareSetState' method performed successfully!");
                    }
                }

                // sleep here
                std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
            }

            *_stopSlewing = true;

            logger.logInfo("Slewing finished");

            // wait for updated telemetry data
            {
                std::lock_guard lock{*_currentParamsMutex};

                t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);

                if (t_err) {
                    *_stopSlewing = true;
                    return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
                }
            }

            if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                logger.logDebug(std::format("    entered target: HA = {}, DEC = {}",
                                            mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
                logger.logDebug(std::format("    current mount:  HA = {}, DEC = {}",
                                            mcc::MccAngle{tdata.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
            } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                logger.logDebug(std::format("    entered target: AZ = {}, ZD = {}",
                                            mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
                logger.logDebug(std::format("    current mount:  AZ = {}, ZD = {}",
                                            mcc::MccAngle{tdata.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.ZD}.sexagesimal()));
            }

            return MccSimpleSlewingModelErrorCode::ERROR_OK;
        };
    }


    MccSimpleSlewingModel(MccSimpleSlewingModel&&) = default;
    MccSimpleSlewingModel& operator=(MccSimpleSlewingModel&&) = default;

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


    virtual ~MccSimpleSlewingModel() = default;

    error_t slewToTarget(bool slew_and_stop = false)
    {
        if (!(*_stopSlewing)) {
            return MccSimpleSlewingModelErrorCode::ERROR_ALREADY_SLEW;
        }

        *_stopSlewing = false;

        return _slewingFunc(slew_and_stop);
    }


    error_t stopSlewing()
    {
        if (*_stopSlewing) {
            return MccSimpleSlewingModelErrorCode::ERROR_ALREADY_STOPPED;
        }

        *_stopSlewing = true;

        return MccSimpleSlewingModelErrorCode::ERROR_OK;
    }


    error_t setSlewingParams(slewing_params_t pars)
    {
        std::lock_guard lock{*_currentParamsMutex};

        _currentParams = std::move(pars);

        return MccSimpleSlewingModelErrorCode::ERROR_OK;
    }


    slewing_params_t getSlewingParams() const
    {
        std::lock_guard lock{*_currentParamsMutex};

        return _currentParams;
    }

protected:
    std::function<error_t(bool)> _slewingFunc{};
    std::unique_ptr<std::atomic_bool> _stopSlewing;
    std::function<error_t()> _checkTargetFunc{};

    slewing_params_t _currentParams{};
    std::unique_ptr<std::mutex> _currentParamsMutex{};
};


}  // namespace mcc