#pragma once

#include <fstream>

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

namespace mcc
{

enum class MccSimpleMovingControlsErrorCode : 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::MccSimpleMovingControlsErrorCode> : public true_type
{
};

}  // namespace std



namespace mcc
{

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

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

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

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

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


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



class MccSimpleMovingControls
{
public:
    typedef std::error_code error_t;

    enum Mode { MOVING_MODE_SLEW, MOVING_MODE_TRACK, MOVING_MODE_ERROR };

    //     typedef std::CallbackFuncTion<void(Mode mode)> mode_switch_callback_t;

    // protected:
    //     constexpr static auto defaultModeSwitchCallback = [](Mode) {};

    // public:

    template <mcc_generic_mount_c MountT,
              std::invocable<typename MountT::mount_status_t> CallbackFuncT =
                  decltype([](typename MountT::mount_status_t) {})>
    MccSimpleMovingControls(
        MountT* mount,
        CallbackFuncT&& mode_switch_calback = [](typename MountT::mount_status_t) {})
        : _stopMoving(new std::atomic_bool), _currentParamsMutex(new std::mutex), _lastError(new std::atomic<error_t>)
    {
        auto send_to_hardware = [mount](typename MountT::hardware_state_t const& hw_state,
                                        MccTelemetryData const& tdata) {
            mount->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
                                        mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));

            auto start_point = tdata.time_point;  // needed for trajectory file

            if constexpr (mccIsEquatorialMount(MountT::mountType)) {
                mount->logDebug(std::format("    entered target: HA = {}, DEC = {}",
                                            mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
                mount->logDebug(std::format("    current mount:  HA = {}, DEC = {}",
                                            mcc::MccAngle{tdata.HA}.sexagesimal(true),
                                            mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));

            } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
                mount->logDebug(std::format("    entered target: AZ = {}, ZD = {}",
                                            mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
                mount->logDebug(std::format("    current mount:  AZ = {}, ZD = {}",
                                            mcc::MccAngle{tdata.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.ZD}.sexagesimal()));
            }

            auto hw_err = mount->hardwareSetState(hw_state);
            if (hw_err) {
                return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE);
            }

            mount->logDebug("    the 'hardwareSetState' method performed successfully!");

            return MccSimpleMovingControlsErrorCode::ERROR_OK;
        };


        *_stopMoving = true;
        *_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK;

        auto cb_sptr = std::make_shared(std::forward<CallbackFuncT>(mode_switch_calback));

        _slewingFunc = [mount, cb_sptr, this](bool slew_and_stop) {
            double braking_accelX, braking_accelY;
            double min_time_to_pzone_in_secs;

            bool store_path = false;
            std::ofstream fst;
            using path_tp_t = std::chrono::duration<double>;  // seconds represented as double

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

                min_time_to_pzone_in_secs =
                    std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();

                if (!_currentParams.slewingPathFilename.empty()) {  // open slewing trajectory file
                    fst.open(_currentParams.slewingPathFilename);
                    if (fst.is_open()) {
                        store_path = true;
                    } else {
                        mount->logError(std::format("Cannot open slewing path file: {}! Do not save it!",
                                                    _currentParams.slewingPathFilename));
                    }
                }
            }

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


            if (store_path) {
                fst << "# \n";
                fst << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
                fst << "# Config:\n";
                fst << "#     slewing tolerance radius: " << mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
                    << " arcsecs\n";
                fst << "#     slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
                fst << "# \n";
                fst << "# Format (time is in seconds, coordinates are in radians): \n";
                fst << "#     <time-since-start> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
                       "<dY_{target-mount}> <moving state>\n";
            }


            typename MountT::error_t t_err;
            MccTelemetryData tdata;

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

                if (t_err) {
                    return *_lastError =
                               mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
                }
            }

            auto last_hw_time = tdata.time_point;

            bool in_zone;
            std::vector<bool> in_zone_vec;

            MccCelestialPoint cpt;

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

            typename MountT::hardware_state_t hw_state;

            auto hw_err = mount->hardwareGetState(&hw_state);
            if (hw_err) {
                *_stopMoving = true;
                return *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::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;
            }
            hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING;

            if (*_stopMoving) {
                mount->logDebug("slewing was stopped!");
                return *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
            }
        };

        _trackingFunc = [mount, cb_sptr, this]() {

        };
    }

    error_t slewToTarget(bool slew_and_stop = false)
    {
        return *_lastError;
    }

    error_t trackTarget()
    {
        return *_lastError;
    }


    error_t stopMountMoving()
    {
        if (*_stopMoving) {
            *_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED;
        } else {
            *_stopMoving = true;
        }

        return *_lastError;
    }

    error_t setMovingParams(MccSimpleMovingModelParams params)
    {
        std::lock_guard lock{*_currentParamsMutex};

        _currentParams = std::move(params);

        return MccSimpleMovingControlsErrorCode::ERROR_OK;
    }

    MccSimpleMovingModelParams getMovingParams() const
    {
        std::lock_guard lock{*_currentParamsMutex};

        return _currentParams;
    }

    error_t mountMovingLastError() const
    {
        return *_lastError;
    }

protected:
    std::function<void(bool)> _slewingFunc{};
    std::function<void()> _trackingFunc{};

    std::unique_ptr<std::atomic_bool> _stopMoving;

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

    std::unique_ptr<std::atomic<error_t>> _lastError;
};

}  // namespace mcc