#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>

#include <queue>

#include "logger/RusLog.h"
#include "Mutex/RusMutex.h"

#include "AdmittanceControl.h"

using namespace std;
using namespace AdmittanceAlgorithm;

static std::mutex m_mutex;

std::queue<WayPoint> _curWayPtList;
std::queue<FTData> _curForceList;
std::queue<WayPoint> _refWayPtList;

AdmittanceControl::AdmittanceControl()
{}

AdmittanceControl::~AdmittanceControl()
{}

void AdmittanceControl::SetAdmittanceControl(AdmittanceSpace admittanceSpace, AdmittanceType admittanceType)
{
    Init();
    switch (admittanceType)
    {
    case AdmittanceType::FreeDrag:
        switch (admittanceSpace)
        {
        case AdmittanceSpace::Joint:
            _pAdmittance = &_pJointFreeDrag;
            break;
        case AdmittanceSpace::Cartesian:
            _pAdmittance = &_pCartFreeDrag;
            break;
        default:
            break;
        }
        break;
    case AdmittanceType::ForceScan:
        switch (admittanceSpace)
        {
        case AdmittanceSpace::Joint:
            _pAdmittance = &_pJointForceScan;
            break;
        case AdmittanceSpace::Cartesian:
            _pAdmittance = &_pCartForceScan;
            break;
        default:
            break;
        }
        break;
    default:
        break;
    }

    _pAdmittance->Init();

    SetLogFileDirectory("F:/RUSLog");
    SetLogFileName("Admittance");
    SetLogLevel(RUS_LOG_INFO);
    EnableLogFile(true);
}

void AdmittanceControl::UpdateCurrentWayPoint(WayPoint wayPoint)
{
    try
    {
        if (wayPoint.jointPos.jointAngle != nullptr || wayPoint.cartPos.position != nullptr)
        {
            CRusLockGuard lock(m_mutex);
            _curWayPtList.push(wayPoint);

        }
    }
    catch (...)
    {
        LogError("Failed in UpdateCurrentWayPoint!\n")
    }
}


/**
 * \brief 获取六维力
 * \param force 六维力
 * \param lastTime 返回回调上次已运行的时间ms
 */
void AdmittanceControl::UpdateCurrentForce(FTData force)
{
    try
    {
        if (force.CartFT.GenForce != nullptr || force.JointT.Torque != nullptr)
        {
            CRusLockGuard lock(m_mutex);
            _curForceList.push(force);
        }
    }
    catch (...)
    {
        LogError("Failed in UpdateCurrentForce!\n")
    }
}


void AdmittanceControl::UpdateReferWayPoint(WayPoint wayPoint)
{
    try
    {
        if (wayPoint.jointPos.jointAngle != nullptr || wayPoint.cartPos.position != nullptr)
        {

            CRusLockGuard lock(m_mutex);
            _refWayPtList.push(wayPoint);

        }
    }
    catch (...)
    {
        LogError("Failed in UpdateReferWayPoint!\n")
    }
}

/// <summary>
/// 
/// </summary>
/// <param name="wayPoint"路点</param>
void AdmittanceControl::GetTargetWayPoint(WayPoint& wayPoint)
{
    try
    {
        //
        WayPoint curWayPt;
        while (true)
        {
            {
                CRusLockGuard lock(m_mutex);
                if (!_curWayPtList.empty())
                {
                    curWayPt = _curWayPtList.front();
                    _curWayPtList.pop();
                    break;
                }
            }
        }

        FTData curForce;
        while (true)
        {
            {
                CRusLockGuard lock(m_mutex);
                if (!_curForceList.empty())
                {
                    curForce = _curForceList.front();
                    _curForceList.pop();
                    break;
                }
            }
        }

        WayPoint refWayPt;
        while (true)
        {
            {
                CRusLockGuard lock(m_mutex);
                if (!_refWayPtList.empty())
                {
                    refWayPt = _refWayPtList.front();
                    _refWayPtList.pop();
                    break;
                }
            }
        }

        _pAdmittance->LoadData(curWayPt, curForce, refWayPt);
        _pAdmittance->GetActualTarget(wayPoint);
    }
    catch (...)
    {
        LogError("Failed in GetTargetWayPoint!\n")
    }
}

void AdmittanceControl::StopAdmittanceControl()
{
    try
    {
        Init();
        _pAdmittance->RefreshAdmittancePara();
    }
    catch (const std::exception&)
    {
        LogError("Failed in StopAdmittanceControl!\n")
    }


}

void AdmittanceControl::Init()
{
    while (!_curForceList.empty()) { _curForceList.pop(); }
    while (!_curWayPtList.empty()) { _curWayPtList.pop(); }
    while (!_refWayPtList.empty()) { _refWayPtList.pop(); }
}