ControlPanel.cpp

/*
Copyright (C) 2002-2004 Jean-Francois Mercure Burroughs 
                        Etienne Lachance
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


Report problems and direct all questions to:

email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
*/

#include "ControlPanel.h"
#include "robot.h"

#include "CustomEvent.h"

#if defined __BCPLUSPLUS__ || defined __TURBOC__  || _MSC_VER // Borland, Turbo or MSVC
double rint (double num) {
   return floor(num + 0.5);
}
#endif

KinControlPanel::KinControlPanel(wxWindow* parent, wxWindowID id, //= -1 
                                 const wxPoint& pos,   // = wxDefaultPosition
                                 const wxSize& size,   // = wxDefaultSize
                                 long style,           // = wxTAB_TRAVERSAL
                                 const wxString& name, // = "panel"
                                 const bool angle_in_degree_)
    :wxPanel(parent, id, pos, size, style, name), angle_in_degree(angle_in_degree_)
{
    m_STbox_q = 0;
    m_model = CModel::Instance();
    InitDialog();
    m_model->Attach(this);

    m_lboxLog = new wxListBox(this,-1);
    m_lboxLog->SetSize(510,5,500,95);
    m_logTarget=new boxLogger(m_lboxLog,wxLog::GetActiveTarget());
}

KinControlPanel::~KinControlPanel()
{
    unsigned int i;

    for(i = 0; i < m_Txt_q.size(); i++)
        delete m_Txt_q[i];
    m_Txt_q.clear();
    for(i = 0; i < m_STxt_q.size(); i++)
        delete m_STxt_q[i];
    m_STxt_q.clear();
    for(i = 0; i < m_Btn_q.size(); i++)
        delete m_Btn_q[i];
    m_Btn_q.clear();
}

void KinControlPanel::InitSpinButton_q(const int nbspin)
{
    int x,y;
    unsigned int i;

    wxTextCtrl *tmp1;
    wxSpinButton *tmp2;
    wxStaticText *tmp3;

    if(m_STbox_q)
    {
        delete m_STbox_q;
        m_STbox_q = 0;
    }
    for(i = 0; i < m_Btn_q.size(); i++)
    {
        delete m_Txt_q[i];
        delete m_Btn_q[i];
        delete m_STxt_q[i];
    }
    m_Txt_q.clear();
    m_Btn_q.clear();
    m_STxt_q.clear();
    
    switch(nbspin)
    {
        case 0:
            break;
        case 1:
            m_STbox_q = new wxStaticBox(this, -1, _("Kinematics"), 
                                        wxPoint(5,0), wxSize(90,50));
            x=30;y=20;
            tmp1 = new wxTextCtrl(this, Txt_q1,_T(""), wxPoint(x,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1);
            x=70;y=20;
            tmp2 = new wxSpinButton(this, Btn_q1, wxPoint(x,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            x=10;y=20;
            tmp3 = new wxStaticText(this, -1, _T("q1"), wxPoint(x,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            break;

        case 2:
            m_STbox_q = new wxStaticBox(this, -1, _("Kinematics"), 
                                        wxPoint(5,0), wxSize(90,70));
            x=30;y=20;
            tmp1 = new wxTextCtrl(this,Txt_q1,_T(""), wxPoint(x,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q2,_T(""), wxPoint(x,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            x=70;y=20;
            tmp2 = new wxSpinButton(this, Btn_q1, wxPoint(x,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q2, wxPoint(x,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            x=10;y=20;
            tmp3 = new wxStaticText(this, -1, _T("q1"), wxPoint(x,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q2"), wxPoint(x,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            break;

        case 3:
            m_STbox_q = new wxStaticBox(this, -1, _("Kinematics"), 
                                        wxPoint(5,0), wxSize(90,90));
            x=30;y=20;
            tmp1 = new wxTextCtrl(this, Txt_q1,_T(""), wxPoint(x,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q2,_T(""), wxPoint(x,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q3,_T(""), wxPoint(x,y+40), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            x=70;y=20;
            tmp2 = new wxSpinButton(this, Btn_q1, wxPoint(x,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q2, wxPoint(x,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this ,Btn_q3, wxPoint(x,y+40), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            x=10;y=20;
            tmp3 = new wxStaticText(this, -1, _T("q1"), wxPoint(x,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q2"), wxPoint(x,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q3"), wxPoint(x,y+40), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            break;

        case 4:
            m_STbox_q = new wxStaticBox(this, -1, _("Kinematics"), 
                                        wxPoint(5,0), wxSize(200,100));
            x=30;y=20;
            tmp1 = new wxTextCtrl(this, Txt_q1,_T(""), wxPoint(x,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q2,_T(""), wxPoint(x,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q3,_T(""), wxPoint(x,y+40), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q4,_T(""), wxPoint(x+100,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            x=70;y=20;
            tmp2 = new wxSpinButton(this, Btn_q1, wxPoint(x,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q2, wxPoint(x,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q3, wxPoint(x,y+40), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q4, wxPoint(x+100,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            x=10;y=20;
            tmp3 = new wxStaticText(this, -1, _T("q1"), wxPoint(x,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q2"), wxPoint(x,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q3"), wxPoint(x,y+40), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q4"), wxPoint(x+100,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            break;

        case 5:
            m_STbox_q = new wxStaticBox(this, -1, _("Kinematics"), 
                                        wxPoint(5,0), wxSize(200,100));
            x=30;y=20;
            tmp1 = new wxTextCtrl(this, Txt_q1,_T(""), wxPoint(x,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q2,_T(""), wxPoint(x,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q3,_T(""), wxPoint(x,y+40), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q4,_T(""), wxPoint(x+100,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q5,_T(""), wxPoint(x+100,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            x=70;y=20;
            tmp2 = new wxSpinButton(this, Btn_q1, wxPoint(x,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q2, wxPoint(x,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q3, wxPoint(x,y+40), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q4, wxPoint(x+100,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q5, wxPoint(x+100,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            x=10;y=20;
            tmp3 = new wxStaticText(this, -1, _T("q1"), wxPoint(x,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q2"), wxPoint(x,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q3"), wxPoint(x,y+40), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q4"), wxPoint(x+100,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q5"), wxPoint(x+100,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            break;

        case 6:
            m_STbox_q = new wxStaticBox(this, -1, _("Kinematics"), 
                                        wxPoint(5,0), wxSize(200,100));
            x=30;y=20;
            tmp1 = new wxTextCtrl(this, Txt_q1,_T(""), wxPoint(x,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q2,_T(""), wxPoint(x,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q3,_T(""), wxPoint(x,y+40), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q4,_T(""), wxPoint(x+100,y), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q5,_T(""), wxPoint(x+100,y+20), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            tmp1 = new wxTextCtrl(this, Txt_q6,_T(""), wxPoint(x+100,y+40), wxSize(45,20), 
                                  wxTE_PROCESS_ENTER);
            m_Txt_q.push_back(tmp1); 
            x=70;y=20;
            tmp2 = new wxSpinButton(this, Btn_q1, wxPoint(x,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q2, wxPoint(x,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q3, wxPoint(x,y+40), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q4, wxPoint(x+100,y), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q5, wxPoint(x+100,y+20), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            tmp2 = new wxSpinButton(this, Btn_q6, wxPoint(x+100,y+40), wxSize(20,20));
            m_Btn_q.push_back(tmp2);
            x=10;y=20;
            tmp3 = new wxStaticText(this, -1, _T("q1"), wxPoint(x,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q2"), wxPoint(x,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q3"), wxPoint(x,y+40), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q4"), wxPoint(x+100,y), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q5"), wxPoint(x+100,y+20), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            tmp3 = new wxStaticText(this, -1, _T("q6"), wxPoint(x+100,y+40), wxSize(15,20));
            m_STxt_q.push_back(tmp3);
            break;
        default:
            wxLogMessage(_T("Kinematics panel does not accept dof > 6")); 
    }

    int min=-10000,max=10000;
    for(i = 0; i < m_Btn_q.size(); i++)
    {
        m_Btn_q[i]->SetRange(min, max);
        m_Btn_q[i]->Enable(TRUE);
        m_Txt_q[i]->Enable(TRUE);
    }
}

void KinControlPanel::InitDialog()
{
    int x=250, y=20;
    m_STbox_inv = new wxStaticBox(this, -1, _("Inverse Kinematics"),
                                  wxPoint(x-15,0), wxSize(270,100));
    m_STxt_x = new wxStaticText(this, -1, _T("X:"), wxPoint(x,y), wxSize(15,15));
    m_Txt_x =new wxTextCtrl(this, Txt_x, _T("0.000"), wxPoint(x+15,y), wxSize(45,20), 
                            wxTE_PROCESS_ENTER);
    m_Btn_x = new wxSpinButton(this, Btn_x, wxPoint(x+60,y), wxSize(20,20));
    
    m_STxt_y = new wxStaticText(this, -1, _T("Y:"), wxPoint(x,y+25), wxSize(15,15));
    m_Txt_y =new wxTextCtrl(this,Txt_y,_T("0.000"), wxPoint(x+15,y+25), wxSize(45,20), 
                            wxTE_PROCESS_ENTER);
    m_Btn_y = new wxSpinButton(this,Btn_y, wxPoint(x+60,y+25), wxSize(20,20));
    
    m_STxt_z = new wxStaticText(this,-1,_T("Z:"), wxPoint(x,y+50), wxSize(15,15));
    m_Txt_z =new wxTextCtrl(this,Txt_z,_T("0.000"), wxPoint(x+15,y+50), wxSize(45,20), 
                            wxTE_PROCESS_ENTER);
    m_Btn_z = new wxSpinButton(this, Btn_z, wxPoint(x+60,y+50), wxSize(20,20));
    
    x=350;
    y=20;
    m_STxt_yaw = new wxStaticText(this, -1, _("Yaw:"), wxPoint(x-10,y), 
                                  wxSize(30,15));
    m_Txt_yaw = new wxTextCtrl(this, Txt_yaw,_T("0.000"), wxPoint(x+25,y), wxSize(45,20), 
                               wxTE_PROCESS_ENTER);
    m_Btn_yaw = new wxSpinButton(this, Btn_yaw, wxPoint(x+70,y), wxSize(20,20));
    
    m_STxt_pitch = new wxStaticText(this, -1, _("Pitch:"), wxPoint(x-10,y+25), 
                                    wxSize(30,15));
    m_Txt_pitch =new wxTextCtrl(this, Txt_pitch, _T("0.000"), wxPoint(x+25,y+25), wxSize(45,20), 
                                wxTE_PROCESS_ENTER);
    m_Btn_pitch = new wxSpinButton(this, Btn_pitch, wxPoint(x+70,y+25), wxSize(20,20));
    
    m_STxt_roll = new wxStaticText(this, -1, _("Roll:"), wxPoint(x-10,y+50), 
                                   wxSize(30,15));
    m_Txt_roll =new wxTextCtrl(this, Txt_roll, _T("0.000"), wxPoint(x+25,y+50), wxSize(45,20), 
                               wxTE_PROCESS_ENTER);
    m_Btn_roll = new wxSpinButton(this, Btn_roll, wxPoint(x+70,y+50), wxSize(20,20));
    
    m_Btn_x->SetValue(0);
    m_Btn_y->SetValue(0);
    m_Btn_z->SetValue(0);
    m_Btn_x->SetRange(-100000,100000);
    m_Btn_y->SetRange(-100000,100000);
    m_Btn_z->SetRange(-100000,100000);

    m_Btn_yaw->SetValue(0);
    m_Btn_pitch->SetValue(0);
    m_Btn_roll->SetValue(0);
    m_Btn_yaw->SetRange(-100000,100000);
    m_Btn_pitch->SetRange(-100000,100000);
    m_Btn_roll->SetRange(-100000,100000);

    m_Btn_InvKine = new wxButton(this,Btn_InvKine,_("Start"), 
                                 wxPoint(x+100,y), wxSize(40,70));
    m_Btn_InvKine->Enable(FALSE);

    UpdateData();
}

BEGIN_EVENT_TABLE(KinControlPanel, wxPanel)
        EVT_SPIN(Btn_q1, KinControlPanel::OnSpinBtn_q)
        EVT_SPIN(Btn_q2, KinControlPanel::OnSpinBtn_q) 
        EVT_SPIN(Btn_q3, KinControlPanel::OnSpinBtn_q) 
        EVT_SPIN(Btn_q4, KinControlPanel::OnSpinBtn_q) 
        EVT_SPIN(Btn_q5, KinControlPanel::OnSpinBtn_q) 
        EVT_SPIN(Btn_q6, KinControlPanel::OnSpinBtn_q)

        EVT_SPIN(Btn_x, KinControlPanel::OnSpinBtn_inv)
        EVT_SPIN(Btn_y, KinControlPanel::OnSpinBtn_inv)
        EVT_SPIN(Btn_z, KinControlPanel::OnSpinBtn_inv)
        EVT_SPIN(Btn_yaw, KinControlPanel::OnSpinBtn_inv)
        EVT_SPIN(Btn_pitch, KinControlPanel::OnSpinBtn_inv)
        EVT_SPIN(Btn_roll, KinControlPanel::OnSpinBtn_inv)

        EVT_BUTTON(Btn_InvKine, KinControlPanel::OnInvKine)

        EVT_TEXT_ENTER(Txt_q1, KinControlPanel::OnSpinBtn_q_t)
        EVT_TEXT_ENTER(Txt_q2, KinControlPanel::OnSpinBtn_q_t)
        EVT_TEXT_ENTER(Txt_q3, KinControlPanel::OnSpinBtn_q_t)
        EVT_TEXT_ENTER(Txt_q4, KinControlPanel::OnSpinBtn_q_t)
        EVT_TEXT_ENTER(Txt_q5, KinControlPanel::OnSpinBtn_q_t)
        EVT_TEXT_ENTER(Txt_q6, KinControlPanel::OnSpinBtn_q_t)

        EVT_TEXT_ENTER(Txt_x, KinControlPanel::OnSpinBtn_inv_t)
        EVT_TEXT_ENTER(Txt_y, KinControlPanel::OnSpinBtn_inv_t)
        EVT_TEXT_ENTER(Txt_z, KinControlPanel::OnSpinBtn_inv_t)
        EVT_TEXT_ENTER(Txt_yaw, KinControlPanel::OnSpinBtn_inv_t)
        EVT_TEXT_ENTER(Txt_pitch, KinControlPanel::OnSpinBtn_inv_t)
        EVT_TEXT_ENTER(Txt_roll, KinControlPanel::OnSpinBtn_inv_t)
END_EVENT_TABLE()


void KinControlPanel::OnSpinBtn_q(wxSpinEvent &event)
{
    int id;
    double value;

    id = event.GetId() - Btn_q1;
    if( (id >= 0) && (id < (int)m_Btn_q.size()) )
      {      
        if(angle_in_degree)
          value = (double) m_Btn_q[id]->GetValue()*SPIN_DEG_INCREMENT;
        else
          value = (double) m_Btn_q[id]->GetValue()*SPIN_INCREMENT;
        
        m_Txt_q[id]->SetValue(wxString::Format(_T("%0.3f"), value));
        if(angle_in_degree)
          value = deg2rad(value);
        m_model->Set_q(value,id+1);         
      }
    UpdateSpinBtn_inv();
    m_Btn_InvKine->Enable(FALSE);
    m_model->Notify(this);    
}

void KinControlPanel::OnSpinBtn_inv(wxSpinEvent &event)
{
    double x, y, z, yaw, pitch, roll;
    int id;

    id = event.GetId() - Btn_x;
    if( (id >=0) && (id <= 6) )
      {
        x     = m_Btn_x->GetValue()*SPIN_INCREMENT;
        y     = m_Btn_y->GetValue()*SPIN_INCREMENT;
        z     = m_Btn_z->GetValue()*SPIN_INCREMENT;
        m_Txt_x->SetValue(wxString::Format(_T("%0.3f"), x));
        m_Txt_y->SetValue(wxString::Format(_T("%0.3f"), y));
        m_Txt_z->SetValue(wxString::Format(_T("%0.3f"), z));
        
        if(angle_in_degree)
          {
            yaw   = m_Btn_yaw->GetValue()*SPIN_DEG_INCREMENT;
            pitch = m_Btn_pitch->GetValue()*SPIN_DEG_INCREMENT;
            roll  = m_Btn_roll->GetValue()*SPIN_DEG_INCREMENT;
          }
        else
          {
            yaw   = m_Btn_yaw->GetValue()*SPIN_INCREMENT;
            pitch = m_Btn_pitch->GetValue()*SPIN_INCREMENT;
            roll  = m_Btn_roll->GetValue()*SPIN_INCREMENT;
          }
        
        m_Txt_yaw->SetValue(wxString::Format(_T("%0.3f"), yaw));
        m_Txt_pitch->SetValue(wxString::Format(_T("%0.3f"), pitch));
        m_Txt_roll->SetValue(wxString::Format(_T("%0.3f"), roll));
      }
    m_Btn_InvKine->Enable(TRUE);        
    if(angle_in_degree)
    {
        yaw = deg2rad(yaw);
        pitch = deg2rad(pitch);
        roll = deg2rad(roll);
    }
    m_model->m_target->SetTargetPos(x, y, z, yaw, pitch, roll);
    m_model->Notify(this);
}

void KinControlPanel::OnSpinBtn_q_t(wxCommandEvent &event)
{
    int id;
    double value;

    id = event.GetId() - Txt_q1;
    if( (id >= 0) && (id < (int)m_Btn_q.size()) )
      {      
        (m_Txt_q[id]->GetValue()).ToDouble(&value);
        if(angle_in_degree)
          {
            m_Btn_q[id]->SetValue((int)(value/SPIN_DEG_INCREMENT));
            value = deg2rad(value);
          }
        else
          m_Btn_q[id]->SetValue((int)(value/SPIN_INCREMENT));
        m_model->Set_q(value,id+1);
      }
    UpdateSpinBtn_inv();
    m_Btn_InvKine->Enable(FALSE);
    m_model->Notify(this);    
}

void KinControlPanel::OnSpinBtn_inv_t(wxCommandEvent &event)
{
    double x, y, z, yaw, pitch, roll;
    int id;

    id = event.GetId() - Txt_x;
    if( (id >=0) && (id <= 6) )
      {
        (m_Txt_x->GetValue()).ToDouble(&x);
        m_Btn_x->SetValue((int)rint(x/SPIN_INCREMENT));     
        (m_Txt_y->GetValue()).ToDouble(&y);
        m_Btn_y->SetValue((int)rint(y/SPIN_INCREMENT));     
        (m_Txt_z->GetValue()).ToDouble(&z);
        m_Btn_z->SetValue((int)rint(z/SPIN_INCREMENT));
        
        (m_Txt_yaw->GetValue()).ToDouble(&yaw);
        (m_Txt_pitch->GetValue()).ToDouble(&pitch);
        (m_Txt_roll->GetValue()).ToDouble(&roll);
        if(angle_in_degree)
          {
            m_Btn_yaw->SetValue((int)rint(yaw/SPIN_DEG_INCREMENT));
            m_Btn_pitch->SetValue((int)rint(pitch/SPIN_DEG_INCREMENT)); 
            m_Btn_roll->SetValue((int)rint(roll/SPIN_DEG_INCREMENT));
          }
        else
          {
            m_Btn_yaw->SetValue((int)rint(yaw/SPIN_INCREMENT));
            m_Btn_pitch->SetValue((int)rint(pitch/SPIN_INCREMENT)); 
            m_Btn_roll->SetValue((int)rint(roll/SPIN_INCREMENT));
          }
      }
    m_Btn_InvKine->Enable(TRUE);        
    if(angle_in_degree)
    {
        yaw = deg2rad(yaw);
        pitch = deg2rad(pitch);
        roll = deg2rad(roll);
    }
    m_model->m_target->SetTargetPos(x, y, z, yaw, pitch, roll);
    m_model->Notify(this);
}

void KinControlPanel::UpdateSpinBtn_inv()
{
    Matrix Trans;
    ColumnVector rpy;

    Trans = m_model->kine();

// Update x,y,z inverse kinematics spin button with current 
// robot position
    m_Btn_x->SetValue((int)(Trans(1,4)/SPIN_INCREMENT));
    m_Txt_x->SetValue(wxString::Format(_T("%0.3f"), Trans(1,4)));
    m_Btn_y->SetValue((int)(Trans(2,4)/SPIN_INCREMENT));
    m_Txt_y->SetValue(wxString::Format(_T("%0.3f"), Trans(2,4)));
    m_Btn_z->SetValue((int)(Trans(3,4)/SPIN_INCREMENT));
    m_Txt_z->SetValue(wxString::Format(_T("%0.3f"), Trans(3,4)));

// Update roll, pitch, yaw inverse kinematics spin button with current 
// robot orientation
    rpy = irpy(Trans.SubMatrix(1,3,1,3));
    if(angle_in_degree)
    {
        rpy(1) = rad2deg(rpy(1));
        rpy(2) = rad2deg(rpy(2));
        rpy(3) = rad2deg(rpy(3));
    }

    m_Txt_roll->SetValue(wxString::Format(_T("%0.3f"), rpy(3)));
    m_Txt_pitch->SetValue(wxString::Format(_T("%0.3f"), rpy(2)));
    m_Txt_yaw->SetValue(wxString::Format(_T("%0.3f"), rpy(1)));

    if(angle_in_degree)
    {
        m_Btn_roll->SetValue((int)(rpy(3)/SPIN_DEG_INCREMENT));
        m_Btn_pitch->SetValue((int)(rpy(2)/SPIN_DEG_INCREMENT));
        m_Btn_yaw->SetValue((int)(rpy(1)/SPIN_DEG_INCREMENT));
    }
    else
    {
        m_Btn_roll->SetValue((int)(rpy(3)/SPIN_INCREMENT));
        m_Btn_pitch->SetValue((int)(rpy(2)/SPIN_INCREMENT));
        m_Btn_yaw->SetValue((int)(rpy(1)/SPIN_INCREMENT));
    }
}

void KinControlPanel::OnInvKine(wxCommandEvent& event)
{
    if(m_model->IsInitialise() == FALSE)
        return;
    bool converge;
    Matrix result;
    ColumnVector dest(3);

    if(angle_in_degree)
    {
        dest(3) = deg2rad((double) m_Btn_roll->GetValue()*SPIN_DEG_INCREMENT);
        dest(2) = deg2rad((double) m_Btn_pitch->GetValue()*SPIN_DEG_INCREMENT);
        dest(1) = deg2rad((double) m_Btn_yaw->GetValue()*SPIN_DEG_INCREMENT);
    }
    else
    {
        dest(3) = (double) m_Btn_roll->GetValue()*SPIN_INCREMENT;
        dest(2) = (double) m_Btn_pitch->GetValue()*SPIN_INCREMENT;
        dest(1) = (double) m_Btn_yaw->GetValue()*SPIN_INCREMENT;
    }

    result=rpy(dest);

    dest(1) = (double) m_Btn_x->GetValue()*SPIN_INCREMENT;
    dest(2) = (double) m_Btn_y->GetValue()*SPIN_INCREMENT;
    dest(3) = (double) m_Btn_z->GetValue()*SPIN_INCREMENT;
    
    result(1,4)=dest(1);
    result(2,4)=dest(2);
    result(3,4)=dest(3);
    
    m_model->inv_kin(result, 0, m_model->get_dof(), converge);
    if(converge==true)
    {   
        UpdateData();   
        wxLogMessage(_("Converged"));   
    }
    else
        wxLogMessage(_("Did not converged"));           

    m_Btn_InvKine->Enable(FALSE);

    m_model->Notify(this);    
}

void KinControlPanel::UpdateData()
{
    int dof = m_model->get_dof();
    double value;

    if(dof != (int)m_Btn_q.size())
        InitSpinButton_q(dof);

    for(unsigned int i = 0; i < m_Btn_q.size(); i++)
    {
        value = m_model->get_q(i+1);
        if(angle_in_degree)
        {
            value = rad2deg(value);
            m_Btn_q[i]->SetValue((int)(value/SPIN_DEG_INCREMENT));
        }
        else
            m_Btn_q[i]->SetValue((int)(value/SPIN_INCREMENT));
        m_Txt_q[i]->SetValue(wxString::Format(_T("%0.3f"), value));     
        UpdateSpinBtn_inv();
    }
}

void KinControlPanel::Update(Subject *TheChangedSubject)
{
    UpdateData();
}

void KinControlPanel::set_active_ListBox()
{
    wxLog::SetActiveTarget(m_logTarget);
}

// -------------------------------------------------------------------------------------------------------------

DynControlPanel::DynControlPanel(wxWindow* parent, wxWindowID id, //= -1 
                                 const wxPoint& pos,    // = wxDefaultPosition
                                 const wxSize& size,    // = wxDefaultSize
                                 long style,            // = wxTAB_TRAVERSAL
                                 const wxString & name) // = "panel"
  :wxPanel(parent, id, pos, size, style, name)
{
    InitDialog();

    m_lboxLog = new wxListBox(this,-1);
    m_lboxLog->SetSize(510,5,500,95);
    m_logTarget=new boxLogger(m_lboxLog,wxLog::GetActiveTarget());
}

DynControlPanel::~DynControlPanel()
{
}

void DynControlPanel::InitDialog()
{
  int x = 100, y = 20;

  x = 0;
  m_Btn_Select_Controller = new wxButton(this, Btn_DynSelectController, 
                                         _("Controller"), 
                                         wxPoint(x+100,y), wxSize(60,70));
  x = 75;
  m_Btn_Select_Trajectory = new wxButton(this, Btn_DynSelectTrajectory, 
                                         _("Trajectory"), 
                                         wxPoint(x+100,y), wxSize(60,70));
  x = 150;
  m_Btn_Start = new wxButton(this, Btn_DynStart, _("Start"), 
                             wxPoint(x+100,y), wxSize(40,70));
  x = 225;
  m_Btn_Stop = new wxButton(this, Btn_DynStop, _("Stop"), 
                            wxPoint(x+100,y), wxSize(40,70));

  m_Btn_Start->Enable(TRUE);
  m_Btn_Stop->Enable(FALSE);
  m_Btn_Select_Controller->Enable(TRUE);
  m_Btn_Select_Trajectory->Enable(FALSE);
}

BEGIN_EVENT_TABLE(DynControlPanel, wxPanel)
      EVT_BUTTON(Btn_DynSelectController, DynControlPanel::OnSelectController)
      EVT_BUTTON(Btn_DynSelectTrajectory, DynControlPanel::OnSelectTrajectory)
      EVT_BUTTON(Btn_DynStart, DynControlPanel::OnStart)
      EVT_BUTTON(Btn_DynStop, DynControlPanel::OnStop)
      EVT_PAINT_DYNAMICS(-1, DynControlPanel::UpdateData)
END_EVENT_TABLE()


void DynControlPanel::OnSelectController(wxCommandEvent& event)
{
  wxString path, file;
  wxFileDialog *dlg = new wxFileDialog(this,  _("Choose a file"), wxGetCwd()+_T("/controller/"), _T(""),  _T("*.*"),  
                                         wxOPEN, wxDefaultPosition);

    if(dlg->ShowModal() == wxID_OK)
    {
        file = dlg->GetPath();

        Control_Select newcontrol(string(file.mb_str()));
        Dynamics *dynamics;
        dynamics = Dynamics::Instance();
        if (dynamics)
        {
            dynamics->set_controller(newcontrol);

            wxString tempstr;
            switch (newcontrol.type)
            {
                case PD:
                    tempstr = _("Proportional Derivative");
                    break;
                case CTM:
                    tempstr = _("Computed Torque Method");
                    break;
                case RRA:
                    tempstr = _("Resolve Rate Acceleration");
                    break;
                default:
                    tempstr = _T("");
                    break;
            }

            LogTexte = wxString(_("Controller")) + _T(": ") + tempstr;
            wxLogMessage(LogTexte);
        }
        m_Btn_Select_Trajectory->Enable(TRUE);
    }
    dlg->Destroy();
}

void DynControlPanel::OnSelectTrajectory(wxCommandEvent& event)
{
    wxString path, file, trajectoryName, path_GLroboop;
    wxFileDialog *dlg = new wxFileDialog(this, _("Choose a file"), wxGetCwd()+_T("/trajectory/"), _T(""),  _T("*.*"),  
                                         wxOPEN, wxDefaultPosition);


    if(dlg->ShowModal() == wxID_OK)
    {
        file = dlg->GetPath();
        trajectoryName= dlg->GetFilename();
        trajectoryName.Replace(_T(".conf"), _T(""), true);

        Trajectory_Select newtrajectory(string(file.mb_str()));
        Dynamics *dynamics;
        dynamics = Dynamics::Instance();
        if (dynamics)
        {
            dynamics->set_trajectory(newtrajectory);
            dynamics->reset_time();

            wxString tempstr;
            switch (newtrajectory.type)
            {
                case JOINT_SPACE:
                    tempstr = _("Joint Space");
                    break;
                case CARTESIAN_SPACE:
                    tempstr = _("Cartesian Space");
                    break;
                default:
                    tempstr = _T("");
                    break;
            }
            LogTexte = wxString(_("Trajectory")) + _T(": ") + tempstr;
            wxLogMessage(LogTexte);

            // Erase the contant in the box log.
//          wxString choices[1] = "allo";
//          m_lboxLog->Set(1, choices, NULL);
        }

    }
    dlg->Destroy();
}

/*
void DynControlPanel::OnReset(wxCommandEvent& event)
{
  m_Btn_Start->Enable(TRUE);
  m_Btn_Stop->Enable(FALSE);
  m_Btn_Select_Controller->Enable(TRUE);
  m_Btn_Select_Trajectory->Enable(FALSE);

 Faire un reset du controleur et de la trajectoire
}
*/

void DynControlPanel::OnStart(wxCommandEvent& event)
{
  m_Btn_Start->Enable(FALSE);
  m_Btn_Stop->Enable(TRUE);
  m_Btn_Select_Controller->Enable(FALSE);
  m_Btn_Select_Trajectory->Enable(FALSE);

  Canvas3D *ptrCanvas3D = Canvas3D::Instance();
  if(ptrCanvas3D)
    ptrCanvas3D->Start();
}

void DynControlPanel::OnStop(wxCommandEvent& event)
{
  m_Btn_Start->Enable(TRUE);
  m_Btn_Stop->Enable(FALSE);
  m_Btn_Select_Controller->Enable(TRUE);
  m_Btn_Select_Trajectory->Enable(TRUE);

  Canvas3D *ptrCanvas3D = Canvas3D::Instance();
  if(ptrCanvas3D)
    ptrCanvas3D->Stop();
}

void DynControlPanel::UpdateData()
{
}

void DynControlPanel::Update(Subject *TheChangedSubject)
{
    UpdateData();
}

void DynControlPanel::set_active_ListBox()
{
    wxLog::SetActiveTarget(m_logTarget);
}

---