RemoteControl.cpp

#include "StdAfx.h"
#include "IMovableItemCallback.h"
#include "ISimGUICallback.h"
#include "IMotorizedJoint.h"
#include "AnimatBase.h"

#include "Node.h"
#include "Link.h"
#include "IPhysicsMovableItem.h"
#include "IPhysicsBody.h"
#include "BoundingBox.h"
#include "MovableItem.h"
#include "BodyPart.h"
#include "Joint.h"
#include "ReceptiveField.h"
#include "ContactSensor.h"
#include "RigidBody.h"
#include "Structure.h"
#include "NeuralModule.h"
#include "Adapter.h"
#include "NervousSystem.h"
#include "Organism.h"
#include "ActivatedItem.h"
#include "ActivatedItemMgr.h"
#include "DataChartMgr.h"
#include "ExternalStimuliMgr.h"
#include "KeyFrame.h"
#include "SimulationRecorder.h"
#include "OdorType.h"
#include "Odor.h"
#include "Light.h"
#include "LightManager.h"
#include "Simulator.h"

#include "RobotInterface.h"
#include "RobotIOControl.h"
#include "RemoteControlLinkage.h"
#include "RemoteControl.h"

namespace AnimatSim
{
        namespace Robotics
        {

RemoteControl::RemoteControl(void)
{
        m_iChangeSimStepCount = 5;
        m_bUseRemoteDataTypes = true;
}

RemoteControl::~RemoteControl(void)
{
try
{
        m_aryLinks.RemoveAll();
}
catch(...)
{Std_TraceMsg(0, "Caught Error in desctructor of RemoteControl\r\n", "", -1, false, true);}
}

CStdPtrArray<RemoteControlLinkage> *RemoteControl::Links() {return &m_aryLinks;}

CStdArray<RemoteControlLinkage *> *RemoteControl::InLinks() {return &m_aryInLinks;}

CStdArray<RemoteControlLinkage *> *RemoteControl::OutLinks() {return &m_aryOutLinks;}

CStdMap<std::string, int>* RemoteControl::DataIDMap() {return &m_aryDataIDMap;};

void RemoteControl::ChangeSimStepCount(int iRate)
{
        Std_IsAboveMin((int) 0, iRate, true, "ChangeSimStepCount");
        m_iChangeSimStepCount = iRate;

        //Reset all the button data.

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->m_Data.m_iChangeSimStepCount = m_iChangeSimStepCount;
}

int RemoteControl::ChangeSimStepCount() {return m_iChangeSimStepCount;}

bool RemoteControl::UseRemoteDataTypes() {return m_bUseRemoteDataTypes;}

#pragma region DataAccesMethods

bool RemoteControl::AddItem(const std::string &strItemType, const std::string &strXml, bool bThrowError, bool bDoNotInit)
{
        std::string strType = Std_CheckString(strItemType);

        if(strType == "REMOTECONTROLLINKAGE")
        {
                AddRemoteControlLinkage(strXml);
                return true;
        }

        return RobotIOControl::AddItem(strItemType, strXml, bThrowError, bDoNotInit);
}

bool RemoteControl::RemoveItem(const std::string &strItemType, const std::string &strID, bool bThrowError)
{
        std::string strType = Std_CheckString(strItemType);

        if(strType == "REMOTECONTROLLINKAGE")
        {
                RemoveRemoteControlLinkage(strID);
                return true;
        }

        return RobotIOControl::RemoveItem(strItemType, strID, bThrowError);
}
#pragma region DataAccesMethods

float *RemoteControl::GetDataPointer(const std::string &strDataType)
{
        std::string strType = Std_CheckString(strDataType);

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                if(strType == Std_CheckString(m_aryLinks[iIndex]->PropertyName()))
                {
                        int iLen = strType.length();
                        if(((iLen-5) >= 0) && strType.substr(strType.length() - 5, 5) == "START")
                                return &m_aryLinks[iIndex]->m_Data.m_fltStart;
                        else if(((iLen-4) >= 0) && strType.substr(strType.length() - 4, 4) == "STOP")
                                return &m_aryLinks[iIndex]->m_Data.m_fltStop;
                        else
                                return &m_aryLinks[iIndex]->m_Data.m_fltValue;
                }       

        return RobotIOControl::GetDataPointer(strDataType);
}

bool RemoteControl::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
{
        std::string strType = Std_CheckString(strDataType);
        
        if(RobotIOControl::SetData(strDataType, strValue, false))
                return true;

        if(strType == "CHANGESIMSTEPCOUNT")
        {
                ChangeSimStepCount((int) atoi(strValue.c_str()));
                return true;
        }

        //If it was not one of those above then we have a problem.
        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);

        return false;
}

void RemoteControl::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
{
        RobotIOControl::QueryProperties(aryProperties);

        aryProperties.Add(new TypeProperty("ChangeSimStepCount", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
        {
                aryProperties.Add(new TypeProperty(m_aryLinks[iIndex]->m_Data.m_strProperty, AnimatPropertyType::Float, AnimatPropertyDirection::Get));
                aryProperties.Add(new TypeProperty(m_aryLinks[iIndex]->m_Data.m_strProperty + "START", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
                aryProperties.Add(new TypeProperty(m_aryLinks[iIndex]->m_Data.m_strProperty + "STOP", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
        }
}

#pragma endregion

RemoteControlLinkage *RemoteControl::AddRemoteControlLinkage(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        oXml.FindChildElement("Link");

        RemoteControlLinkage *lpPart = LoadRemoteControlLinkage(oXml);

        lpPart->Initialize();

    return lpPart;
}

void RemoteControl::RemoveRemoteControlLinkage(std::string strID, bool bThrowError)
{
        int iPos = FindLinkageChildListPos(strID, bThrowError), iLinkPos = -1;

    RemoteControlLinkage *lpPart = m_aryLinks[iPos];

        if(lpPart->InLink())
                iLinkPos = FindLinkageChildListPos(m_aryInLinks, strID, bThrowError);
        else
                iLinkPos = FindLinkageChildListPos(m_aryOutLinks, strID, bThrowError);

        StartPause();
        if(lpPart->InLink())
                m_aryInLinks.RemoveAt(iLinkPos);
        else
                m_aryOutLinks.RemoveAt(iLinkPos);

        m_aryLinks.RemoveAt(iPos);
        ExitPause();
}


int RemoteControl::FindLinkageChildListPos(std::string strID, bool bThrowError)
{
        std::string sID = Std_ToUpper(Std_Trim(strID));

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                if(m_aryLinks[iIndex]->ID() == sID)
                        return iIndex;

        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lPartInterfaceIDNotFound, Al_Err_strPartInterfaceIDNotFound, "ID", strID);

        return -1;
}

int RemoteControl::FindLinkageChildListPos(CStdArray<RemoteControlLinkage *> &aryLinks, std::string strID, bool bThrowError)
{
        std::string sID = Std_ToUpper(Std_Trim(strID));

        int iCount = aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                if(aryLinks[iIndex]->ID() == sID)
                        return iIndex;

        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lPartInterfaceIDNotFound, Al_Err_strPartInterfaceIDNotFound, "ID", strID);

        return -1;
}

RemoteControlLinkage *RemoteControl::FindLinkageWithPropertyName(std::string strName, bool bThrowError)
{
        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                if(m_aryLinks[iIndex]->PropertyName() == strName)
                        return m_aryLinks[iIndex];

        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lPartInterfaceIDNotFound, Al_Err_strPartInterfaceIDNotFound, "PropertyName", strName);

        return NULL;
}

#pragma endregion

void RemoteControl::ResetData()
{
        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->m_Data.ClearData();
}

void RemoteControl::CheckStartedStopped()
{
        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->m_Data.CheckStartedStopped();

        //m_ButtonData[BUT_ID_RT].CheckStartedStopped();
}

void RemoteControl::ClearStartStops()
{

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->m_Data.ClearStartStops();

        //m_ButtonData[BUT_ID_LOOKH].ClearStartStops();
}

void RemoteControl::SetupIO()
{
        RobotIOControl::SetupIO();

        if(m_bEnabled)
        {
                int iCount = m_aryLinks.GetSize();
                for(int iIndex=0; iIndex<iCount; iIndex++)
                        if(m_aryLinks[iIndex]->Enabled())
                                m_aryLinks[iIndex]->SetupIO();
        }
}


void RemoteControl::StepIO()
{
        if(m_bEnabled)
        {
                //unsigned long long lStepStartTick = m_lpSim->GetTimerTick();
                RobotIOControl::StepIO();

                int iCount = m_aryLinks.GetSize();
                for(int iIndex=0; iIndex<iCount; iIndex++)
                        if(m_aryLinks[iIndex]->Enabled())
                                m_aryLinks[iIndex]->StepIO();

                //unsigned long long lEndStartTick = m_lpSim->GetTimerTick();
                //m_fltStepIODuration = m_lpSim->TimerDiff_m(lStepStartTick, lEndStartTick);
        }
}

void RemoteControl::ShutdownIO()
{
        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                if(m_aryLinks[iIndex]->Enabled())
                        m_aryLinks[iIndex]->ShutdownIO();

        //Put this last so everything gets its ShutdownIO called before we actually exit the thread.
        RobotIOControl::ShutdownIO();
}

void RemoteControl::CreateDataIDMap()
{
}

bool RemoteControl::FindDataToWrite(CStdArray<RemoteControlLinkage *> &aryWrites)
{
        int iCount = m_aryOutLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
        {
                if(fabs(m_aryOutLinks[iIndex]->m_Data.m_fltValue - m_aryOutLinks[iIndex]->m_Data.m_fltPrev) > 1e-10)
                {
                        m_aryOutLinks[iIndex]->m_Data.m_fltPrev = m_aryOutLinks[iIndex]->m_Data.m_fltValue;
                        aryWrites.Add(m_aryOutLinks[iIndex]);
                }
        }

        if(aryWrites.GetSize() > 0)
                return true;
        else
                return false;
}

void RemoteControl::SetDataValue(int iID, float fltVal)
{
        int iCount = m_aryInLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
        {
                if(m_aryInLinks[iIndex]->m_Data.m_iButtonID == iID && fabs(fltVal - m_aryInLinks[iIndex]->m_Data.m_fltValue) > 1e-10)
                {
                        m_aryInLinks[iIndex]->m_Data.m_fltPrev = m_aryInLinks[iIndex]->m_Data.m_fltValue;
                        m_aryInLinks[iIndex]->m_Data.m_fltValue = fltVal;
                }
        }
}

void RemoteControl::Initialize()
{
        RobotIOControl::Initialize();

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->Initialize();

        ResetData();
}

void RemoteControl::ResetSimulation()
{
        RobotIOControl::ResetSimulation();

        m_iCyclePartIdx = 0;
        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->ResetSimulation();

        ResetData();
}

void RemoteControl::AfterResetSimulation()
{
        RobotIOControl::AfterResetSimulation();

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                m_aryLinks[iIndex]->AfterResetSimulation();
}

void RemoteControl::StepSimulation()
{
    RobotIOControl::StepSimulation();

        int iCount = m_aryLinks.GetSize();
        for(int iIndex=0; iIndex<iCount; iIndex++)
                if(m_aryLinks[iIndex]->Enabled())
                        m_aryLinks[iIndex]->StepSimulation();

        ClearStartStops();
}

void RemoteControl::Load(CStdXml &oXml)
{
        RobotIOControl::Load(oXml);

        CreateDataIDMap();

        oXml.IntoElem();  //Into RigidBody Element

        ChangeSimStepCount(oXml.GetChildInt("ChangeSimStepCount", m_iChangeSimStepCount));

        if(oXml.FindChildElement("Links", false))
        {
                oXml.IntoElem();  //Into ChildBodies Element
                int iChildCount = oXml.NumberOfChildren();

                for(int iIndex=0; iIndex<iChildCount; iIndex++)
                {
                        oXml.FindChildByIndex(iIndex);
                        LoadRemoteControlLinkage(oXml);
                }
                oXml.OutOfElem(); //OutOf ChildBodies Element
        }

        oXml.OutOfElem(); //OutOf RigidBody Element
}

RemoteControlLinkage *RemoteControl::LoadRemoteControlLinkage(CStdXml &oXml)
{
        RemoteControlLinkage *lpChild = NULL;
        std::string strType;

try
{
        oXml.IntoElem(); //Into Child Element
        std::string strModule = oXml.GetChildString("ModuleName", "");
        strType = oXml.GetChildString("Type");
        oXml.OutOfElem(); //OutOf Child Element

        lpChild = dynamic_cast<RemoteControlLinkage *>(m_lpSim->CreateObject(strModule, "RemoteControlLinkage", strType));
        if(!lpChild)
                THROW_TEXT_ERROR(Al_Err_lConvertingClassToType, Al_Err_strConvertingClassToType, "RemoteControlLinkage");

        lpChild->ParentRemoteControl(this);
        lpChild->SetSystemPointers(m_lpSim, m_lpStructure, NULL, NULL, true);

        lpChild->Load(oXml);

        m_aryLinks.Add(lpChild);

        if(lpChild->InLink())
                m_aryInLinks.Add(lpChild);
        else
                m_aryOutLinks.Add(lpChild);

        return lpChild;
}
catch(CStdErrorInfo oError)
{
        if(lpChild) delete lpChild;
        RELAY_ERROR(oError);
        return NULL;
}
catch(...)
{
        if(lpChild) delete lpChild;
        THROW_ERROR(Std_Err_lUnspecifiedError, Std_Err_strUnspecifiedError);
        return NULL;
}
}


        }
}