MotorController.h

#pragma once
#include "RealSenseInterface/Headers/RealSenseOptions.h"
#include <NDEVR/Connection.h>
#include <NDEVR/Motor.h>
#include <NDEVR/Scene.h>
namespace NDEVR
{
    class RealSenseMotor : public Motor
    {
    public:
        RealSenseMotor(const Model& model, uint04 axis_number, Connection* connection)
            : Motor(model, connection->rawNetworkLog())
            , m_connection(connection)
            , m_axis_number(axis_number)
            , m_last_update_time(Time::SystemTime())
        {
            Model::set<NDPO::name>("Servo " + String::ToAlphaString(axis_number));
            m_current_angle = Angle<fltp08>(0);
        }

        [[nodiscard]] virtual bool supportsMoveTo() const override
        {
            return true;
        }

        [[nodiscard]] virtual bool supportsSetSpeed() const override
        {
            return true;
        }

        virtual void setLocation(const Angle<fltp08>& location) override
        {
            if (m_current_angle != location)
            {
                if (m_axis_number == ROLL)
                {
                    m_connection->tx("s" + String::ToAlphaString(m_axis_number).toLower()
                        + "," + String(location.as<DEGREES>()) + "\r\n");
                }
                else
                {
                    m_connection->tx("s" + String::ToAlphaString(m_axis_number).toLower()
                        + "," + String(location.as<DEGREES>() - 90.0) + "\r\n");
                }
            }
            m_connection->waitForReadyRead(TimeSpan(0.5));
            m_connection->rx();
            m_current_angle = location;
            emit onUpdated();
        }

        void getLocation()
        {
            m_connection->tx("s" + String::ToAlphaString(m_axis_number).toLower() + "?\r\n");
 
            m_connection->waitForReadyRead(TimeSpan(0.5));
            String x = m_connection->rx();
            if (x.size() > 0)
            {
                if (m_axis_number == ROLL)
                    m_current_angle = Angle<fltp08>(DEGREES, x.getAs<fltp08>());
                else
                    m_current_angle = Angle<fltp08>(DEGREES, 90.0 + x.getAs<fltp08>());
                m_is_valid = true;
                emit onUpdated();
            }
        }

        virtual void setSpeed(const Angle<fltp08>& speed) override
        {
            m_current_speed = speed;
            m_last_update_time = Time::SystemTime();
        }

        Bounds<2, Angle<fltp08>> speedRange() const override
        {
            Bounds<2, Angle<fltp08>> range;
            range[MIN] = Vector<2, Angle<fltp08>>(-Angle<fltp08>(DEGREES, 20.0), -Angle<fltp08>(DEGREES, 0.1));
            range[MAX] = Vector<2, Angle<fltp08>>(Angle<fltp08>(DEGREES, 0.1), Angle<fltp08>(DEGREES, 20.0));
            return range;
        }

        void update(Time time) override
        {
            try
            {
                if (!m_connection->isOpen())
                {
                    if (m_connection->open())
                    {
                        getLocation();
                    }
                    else
                    {
                        ThreadFunctions::RequestSleep(TimeSpan(5.0));
                        return;
                    }
                }
                if(!m_is_valid)
                    getLocation();
                if (m_current_speed != Angle<fltp08>(DEGREES, 0.0))
                {
                    TimeSpan span = time - m_last_update_time;
                    span = getMax(span, TimeSpan(0.1));
                    if (m_axis_number == ROLL)
                        setLocation(clip(m_current_angle + m_current_speed * span.elapsedSeconds(), Angle<fltp08>(DEGREES, -90.0), Angle<fltp08>(DEGREES, 90.0)));
                    else
                        setLocation(clip(m_current_angle + m_current_speed * span.elapsedSeconds(), Angle<fltp08>(DEGREES, -30.0), Angle<fltp08>(DEGREES, 180.0)));
                    m_last_update_time = time;
                }
                else if (time - m_last_update_time > TimeSpan(5.0))
                {
                    getLocation();
                    m_last_update_time = time;
                }
            }
            catch (...)
            {
 
            }
        }

        bool isValid() const
        {
            return m_is_valid;
        }
        Connection* m_connection;       
        uint01 m_axis_number = 0;       
        Time m_last_update_time;        
        bool m_is_valid = false;        
 
    };

    class RealSenseMotorManager : public Device
    {
    public:
        RealSenseMotorManager(const ConnectionInfo& connection)
            : Device(Scene(_t("Manager")), connection.log)
            , m_connection_info(connection)
        {
            init();
        }

        void init()
        {
            m_motors[0] = nullptr;
            m_motors[1] = nullptr;
            startAutoUpdate();
 
        }

        void update(Time time) override
        {
            if (m_motors[0] == nullptr)
            {
                Connection* connection = new Connection(m_connection_info, createChild(), m_connection_info.log);
                for (uint04 i = 0; i < 2; i++)
                    m_motors[i] = new RealSenseMotor(Scene(_t("Motor")), i, connection);
            }
            for (uint01 i = 0; i < 2; i++)
            {
                if (m_motors[i])
                {
                    m_motors[i]->update(time);
                }
            }
        }

        Vector<3, Angle<fltp08>> headAngle() const
        {
            Vector<3, Angle<fltp08>> head_angles(Angle<fltp08>(0));
            for (uint01 i = 0; i < 2; i++)
            {
                if(m_motors[i])
                    head_angles[2 - i] = m_motors[i]->location();
            }
            head_angles[0] = RealSenseOptions::realsense_roll.get();
            return head_angles;
 
        }

        [[nodiscard]] virtual TimeSpan updateInterval() const { return TimeSpan(1.0f / 30.0f); }
        RealSenseMotor* motor(uint01 axis) const
        {
            if(axis < 2)
                return m_motors[axis];
            return nullptr;
        }

        bool isValid() const
        {
            if (m_motors[0] == nullptr)
                return false;
            return m_motors[0]->isValid();
        }
        RealSenseMotor* m_motors[2];        
        ConnectionInfo m_connection_info;   
    };
}