#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-TRACKING-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
{
public:
    typedef std::error_code error_t;

    typedef MccSimpleMovingModelParams slewing_params_t;

    template <mcc_all_controls_c CONTROLS_T>
    MccSimpleSlewingModel(CONTROLS_T* controls)
        : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
    {
        *_stopSlewing = true;

        _slewingFunc = [controls, this](bool slew_and_stop) -> 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;
            auto pz_err = controls->inPZone(tdata.target, &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_TARGET_IN_PZONE;
            }

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

            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!");
            }

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

            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) {
                return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
            }

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

            // 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;

            while (true) {
                // 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;
                }

                // 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 + tdata.speedX * min_time_to_pzone_in_secs;
                    cpt.Y = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs;
                } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                    cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs;
                    cpt.Y = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs;
                }
                mcc_tp2tp(tdata.time_point, cpt.time_point);

                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) {
                        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);
                }

                t_err = controls->targetToMountDist(&dist);
                if (t_err) {
                    *_stopSlewing = true;
                    return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
                }

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

                {
                    std::lock_guard lock{*_currentParamsMutex};

                    // if (adjust_mode && !_currentParams.slewAndStop) {
                    if (adjust_mode && !slew_and_stop) {
                        // do not allow mount speed fall below sideral
                        if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                            // turn on sideral rate only if the current position point catches up with the target
                            if ((tdata.target.HA - tdata.HA) <= 0.0 && tdata.speedX < slewing_params_t::sideralRate) {
                                hw_state.X = (double)tdata.target.X;
                                hw_state.Y = (double)tdata.target.Y;

                                hw_state.speedX = slewing_params_t::sideralRate;

                                hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING;

                                hw_err = controls->hardwareSetState(hw_state);
                                if (hw_err) {
                                    *_stopSlewing = true;
                                    return mcc_deduce_error_code(hw_err,
                                                                 MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
                                }
                            }
                        } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                        } else {
                            static_assert(false, "UNKNOWN MOUNT TYPE!!");
                        }
                    }

                    if (dist <= _currentParams.slewToleranceRadius) {  // stop slewing and exit from cycle
                        if (hw_state.moving_state ==
                            CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) {  // mount was stopped
                            *_stopSlewing = true;
                            break;
                        }
                    }

                    if (dist <= _currentParams.adjustCoordDiff) {  // adjust mount pointing
                        auto now = std::chrono::steady_clock::now();
                        if ((now - last_adjust_tp) < _currentParams.adjustCycleInterval) {
                            continue;
                        }

                        hw_state.X = (double)tdata.target.X;
                        hw_state.Y = (double)tdata.target.Y;

                        hw_state.speedX = _currentParams.adjustRateX;
                        hw_state.speedY = _currentParams.adjustRateY;

                        hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_ADJUSTING;

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

                        last_adjust_tp = now;

                        adjust_mode = true;

                    } else {
                        adjust_mode = false;
                    }
                }

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

            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;

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


}  // namespace mcc