#pragma once

#include <stdint.h>
#include <functional>
#include <map>
#include <mutex>
#include <string>
#include <vector>
#include <hermes/InterfaceDefs.h>

#include "aurora/device/Device.h"
#include "aurora/device/Motor.h"
#include "aurora/Types.h"

namespace lge::pri::aurora {

using AngleRange = Range<Angle>;
using MotorList  = std::vector<Motor*>;
using MoverAlarm = std::map<DeviceId, std::vector<std::string>>;


class Mover : public DeviceBase
{
    DECLARE_AS_DEVICE_BASE(Mover, DeviceType::MOVER)

    virtual const MotorList& getMotorList() = 0;

    virtual MoveRange getMoveRange(Axis axis) = 0;
    virtual AngleRange getAngleRange() = 0;
    virtual VelocityRange getVelocityRange(Axis axis) = 0;

    virtual VelocityLevel getVelocity(Axis axis) = 0;
    virtual int setVelocity(
        Axis axis,
        VelocityLevel level) = 0;

    virtual Position getCurrentPosition() = 0;
    virtual int setTargetPosition(
        const Position& targetPos,
        TargetPosType type = TargetPosType::ABS) = 0;
    virtual int setTargetPosition(
        const Point& point,
        TargetPosType type = TargetPosType::ABS) = 0;
    virtual int setTargetPosition(
        const Point& point,
        const Angle& angle,
        TargetPosType type = TargetPosType::ABS) = 0;
    virtual Position getTargetPosition() = 0;

    virtual int teachPosition(const Position& position) = 0;
    virtual Position getTeachOffset() = 0;

    virtual int setServoOn(
        bool on = true) = 0;
    virtual int setServoOn(
        Axis axis,
        bool on = true) = 0;
    virtual bool isServoOn() = 0;
    virtual bool isServoOn(Axis axis) = 0;

    virtual int home() = 0;
    virtual int move() = 0;
    virtual int jog(
        Axis axis,
        int direction,
        double velocity) = 0;
    virtual int jogIncrement(
        Axis axis,
        double increment,
        double velocity) = 0;

    virtual int stop() = 0;

    virtual bool isMoving() = 0;
    virtual bool isHomeCompleted() = 0;

    virtual bool isAlarm() = 0;
    virtual int clearAlarm() = 0;
    virtual MoverAlarm getAlarm() = 0;
};



/**
 * UTIL for variadic template
 */
template<typename T>
void __getArgs(std::vector<T>& list)
{}

template<typename T, typename... Args>
void __getArgs(std::vector<T>& list, T a1, Args... args)
{
    list.push_back(a1);
    __getArgs(list, args...);
}


class MoverBase : public Mover
{
protected:
    std::map<Axis, VelocityLevel> mVelocity;
    std::map<Axis, VelocityRange> mVelocityRange;
    std::map<Axis, MoveRange> mMoveRange;

    Position mTeachOffset;
    MotorList mMotors;


    MoverBase(
        const DeviceId& deviceId
    );

    template<typename... Args>
    void setMotors(Args... motors)
    {
        std::vector<Motor*> list;
        __getArgs(list, motors...);
        for (Motor* m : list) if (m) mMotors.push_back(m);
    }


protected:
    virtual int doActivate(const IData& config) = 0;
    virtual int doDeactivate() = 0;

    virtual int doSetVelocity(
        Axis axis,
        VelocityLevel level) = 0;
    virtual Position doGetCurrentPosition() = 0;
    virtual int doSetTargetPosition(
        const Position& targetPos) = 0; // absolute position
    virtual Position doGetTargetPosition() = 0;

    virtual int doHome();
    virtual int doMove();
    virtual int doJog(
        Axis axis,
        int direction,
        double velocity) = 0;
    virtual int doJogIncrement(
        Axis axis,
        double increment,
        double velocity) = 0;

public:
    virtual ~MoverBase();

    virtual int activate(const IData& config) override;
    virtual int deactivate() override;

    virtual const MotorList& getMotorList() override
    { return mMotors; }

    virtual MoveRange getMoveRange(Axis axis) override;
    virtual AngleRange getAngleRange() override;
    virtual VelocityRange getVelocityRange(Axis axis) override;

    virtual VelocityLevel getVelocity(Axis axis) override;
    virtual int setVelocity(Axis axis, VelocityLevel level) override;

    virtual Position getCurrentPosition() override;
    virtual int setTargetPosition(
        const Position& targetPos,
        TargetPosType type = TargetPosType::ABS) override;
    virtual int setTargetPosition(
        const Point& point,
        TargetPosType type = TargetPosType::ABS) override;
    virtual int setTargetPosition(
        const Point& point,
        const Angle& angle,
        TargetPosType type = TargetPosType::ABS) override;
    virtual Position getTargetPosition() override;

    virtual int teachPosition(const Position& position) override;
    virtual Position getTeachOffset() override;

    virtual int setServoOn(bool on = true) override;
    virtual int setServoOn(Axis axis, bool on = true) override;
    virtual bool isServoOn() override;
    virtual bool isServoOn(Axis axis) override;

    virtual int home() override;
    virtual int move() override;
    virtual int jog(
        Axis axis,
        int direction,
        double velocity) override;
    virtual int jogIncrement(
        Axis axis,
        double increment,
        double velocity) override;

    virtual int stop() override;

    virtual bool isMoving() override;
    virtual bool isHomeCompleted() override;

    virtual bool isAlarm() override;
    virtual int clearAlarm() override;
    virtual MoverAlarm getAlarm() override;
};

} // end of namespace