#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          SIMPLE TRACKING MODEL IMPLEMENTATION                            */


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


namespace mcc
{

enum class MccSimpleTrackingModelErrorCode : int {
    ERROR_OK,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_TELEMETRY_TIMEOUT,
    ERROR_PZONE_CONTAINER_COMP,
    ERROR_IN_PZONE,
    ERROR_UNEXPECTED_AXIS_RATES
};

}  // namespace mcc


namespace std
{

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

}  // namespace std



namespace mcc
{

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

    typedef std::error_code error_t;

    struct tracking_params_t {
        static constexpr double sideralRate = 15.0410686_arcsecs;  // in radians per second

        double trackSpeedX{};
        double trackSpeedY{};

        std::chrono::seconds telemetryTimeout{3};
    };

    template <mcc_generic_mount_c MountT>
    MccSimpleTrackingModel(MountT* mount) : _stopTracking(new std::atomic_bool())
    {
        *_stopTracking = false;

        if constexpr (mccIsEquatorialMount(mountType)) {
            _currentTrackParams.trackSpeedX = tracking_params_t::sideralRate;  // move along HA-axis with sideral rate
            _currentTrackParams.trackSpeedY = 0.0;
        }

        _trackingFunc = [mount, this]() -> error_t {
            typename MountT::hardware_state_t hw_state;

            if constexpr (mccIsEquatorialMount(mountType)) {
                // just set sideral rate once
                auto err = mount->hardwareGetState(&hw_state);
                if (err) {
                    return mcc_deduce_error(err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
                }

                // hw_state.time_point;
                hw_state.speedX = _currentTrackParams.trackSpeedX;
                hw_state.speedY = _currentTrackParams.trackSpeedY;
                hw_state.moving_type = MountT::hardware_moving_state_t::HW_MOVE_TRACKING;

                err = mount->hardwareSetState(std::move(hw_state));
                if (err) {
                    return mcc_deduce_error(err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
                }

                // control prohibited zones
                MccTelemetryData tdata;
                std::vector<bool> pz_flags;
                bool inzone_flag;

                while (!*_stopTracking) {
                    auto t_err = mount->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
                    if (t_err) {
                        return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_TELEMETRY_TIMEOUT);
                    }

                    // check for current axis speed
                    if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) {
                        // unhandled stop state?!!!
                        return MccSimpleTrackingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES;
                    }

                    auto pz_err = mount->inPZone(tdata, &inzone_flag, &pz_flags);
                    if (pz_err) {
                        return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                    }

                    if (inzone_flag) {
                        // logging
                        return MccSimpleTrackingModelErrorCode::ERROR_IN_PZONE;
                    }
                }

                return MccSimpleTrackingModelErrorCode::ERROR_OK;
            } else if constexpr (mccIsAltAzMount(mountType)) {
                static_assert(false, "NOT IMPLEMENTED!");
            } else {
                static_assert(false, "UNKNOW MOUNT TYPE!");
            }
        };
    }

    MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
    MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;


    error_t trackMount()
    {
        return _trackingFunc();
    }


    error_t stopTracking()
    {
        *_stopTracking = true;

        return MccSimpleTrackingModelErrorCode::ERROR_OK;
    }


    error_t setTrackingParams(tracking_params_t params)
    {
        _currentTrackParams = std::move(params);
    }

    tracking_params_t getTrackingParams()
    {
        return _currentTrackParams;
    }

protected:
    std::function<error_t()> _trackingFunc{};
    std::unique_ptr<std::atomic_bool> _stopTracking{};

    tracking_params_t _currentTrackParams;
};

}  // namespace mcc