/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "robotic_arm_kinematicsMain.h"

#include <wx/msgdlg.h>

//(*InternalHeaders(robotic_arm_kinematicsFrame)
#include <wx/artprov.h>
#include <wx/bitmap.h>
#include <wx/font.h>
#include <wx/icon.h>
#include <wx/image.h>
#include <wx/intl.h>
#include <wx/settings.h>
#include <wx/string.h>
//*)
#include <mrpt/gui/WxUtils.h>
#include <mrpt/gui/wx28-fixes.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CText.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/serialization/CArchive.h>

#include "../wx-common/mrpt_logo.xpm"
#include "CAboutBox.h"
#include "imgs/main_icon.xpm"

// A custom Art provider for customizing the icons:
class MyArtProvider : public wxArtProvider
{
 protected:
  wxBitmap CreateBitmap(const wxArtID& id, const wxArtClient& client, const wxSize& size) override;
};

// CreateBitmap function
wxBitmap MyArtProvider::CreateBitmap(
    const wxArtID& id, const wxArtClient& client, const wxSize& size)
{
  if (id == wxART_MAKE_ART_ID(MAIN_ICON)) return wxBitmap(main_icon_xpm);
  if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO)) return wxBitmap(mrpt_logo_xpm);

  // Any wxWidgets icons not implemented here
  // will be provided by the default art provider.
  return wxNullBitmap;
}

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::kinematics;
using namespace std;

//(*IdInit(robotic_arm_kinematicsFrame)
const long robotic_arm_kinematicsFrame::ID_RADIOBOX2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SIMPLEHTMLLISTBOX1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON5 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON6 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON7 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_RADIOBOX1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICLINE4 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SIMPLEHTMLLISTBOX2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_TEXTCTRL1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SLIDER1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICLINE1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SIMPLEHTMLLISTBOX3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_TEXTCTRL2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT5 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SLIDER2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICLINE2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SIMPLEHTMLLISTBOX4 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_TEXTCTRL3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT7 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SLIDER3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICLINE3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SIMPLEHTMLLISTBOX5 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_TEXTCTRL4 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT9 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_BUTTON4 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_SLIDER4 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_PANEL1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_XY_GLCANVAS = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT10 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_PANEL2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_STATICTEXT2 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_LISTBOX1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_TEXTCTRL5 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_PANEL3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_NOTEBOOK1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_MENUITEM3 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_MENUITEM1 = wxNewId();
const long robotic_arm_kinematicsFrame::ID_MENUITEM2 = wxNewId();
const long robotic_arm_kinematicsFrame::idMenuQuit = wxNewId();
const long robotic_arm_kinematicsFrame::idMenuAbout = wxNewId();
//*)

BEGIN_EVENT_TABLE(robotic_arm_kinematicsFrame, wxFrame)
//(*EventTable(robotic_arm_kinematicsFrame)
//*)
END_EVENT_TABLE()

robotic_arm_kinematicsFrame* the_win = nullptr;

robotic_arm_kinematicsFrame::robotic_arm_kinematicsFrame(wxWindow* parent, wxWindowID id)
{
  the_win = this;

  // Load my custom icons:
  wxArtProvider::Push(new MyArtProvider);

  //(*Initialize(robotic_arm_kinematicsFrame)
  wxMenuItem* MenuItem2;
  wxMenuItem* MenuItem1;
  wxFlexGridSizer* FlexGridSizer8;
  wxFlexGridSizer* FlexGridSizer1;
  wxFlexGridSizer* FlexGridSizer2;
  wxFlexGridSizer* FlexGridSizer15;
  wxMenu* Menu1;
  wxFlexGridSizer* FlexGridSizer17;
  wxFlexGridSizer* FlexGridSizer19;
  wxFlexGridSizer* FlexGridSizer11;
  wxFlexGridSizer* FlexGridSizer7;
  wxFlexGridSizer* FlexGridSizer4;
  wxFlexGridSizer* FlexGridSizer9;
  wxFlexGridSizer* FlexGridSizer14;
  wxFlexGridSizer* FlexGridSizer6;
  wxFlexGridSizer* FlexGridSizer3;
  wxFlexGridSizer* FlexGridSizer16;
  wxFlexGridSizer* FlexGridSizer10;
  wxFlexGridSizer* FlexGridSizer13;
  wxMenuBar* MenuBar1;
  wxFlexGridSizer* FlexGridSizer18;
  wxFlexGridSizer* FlexGridSizer12;
  wxMenu* Menu2;
  wxFlexGridSizer* FlexGridSizer5;
  wxStaticBoxSizer* boxProperties;

  Create(
      parent, wxID_ANY, _("Robotic Arm Kinematic GUI - Part of MRPT"), wxDefaultPosition,
      wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("wxID_ANY"));
  SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  {
    wxIcon FrameIcon;
    FrameIcon.CopyFromBitmap(
        wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("MAIN_ICON")), wxART_FRAME_ICON));
    SetIcon(FrameIcon);
  }
  FlexGridSizer1 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer1->AddGrowableCol(1);
  FlexGridSizer1->AddGrowableRow(0);
  FlexGridSizer2 = new wxFlexGridSizer(5, 1, 0, 0);
  FlexGridSizer2->AddGrowableCol(0);
  FlexGridSizer2->AddGrowableRow(2);
  wxString __wxRadioBoxChoices_1[3] = {_("Z"), _("Y"), _("X")};
  RadioBox1 = new wxRadioBox(
      this, ID_RADIOBOX2, _(" Orientation of first DOF "), wxDefaultPosition, wxDefaultSize, 3,
      __wxRadioBoxChoices_1, 1, wxRA_VERTICAL, wxDefaultValidator, _T("ID_RADIOBOX2"));
  FlexGridSizer2->Add(RadioBox1, 1, wxEXPAND, 2);
  StaticText1 = new wxStaticText(
      this, ID_STATICTEXT1, _("List of kinematic links:"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT1"));
  FlexGridSizer2->Add(
      StaticText1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  listLinks = new wxSimpleHtmlListBox(
      this, ID_SIMPLEHTMLLISTBOX1, wxDefaultPosition, wxDefaultSize, 0, nullptr,
      wxHLB_DEFAULT_STYLE, wxDefaultValidator, _T("ID_SIMPLEHTMLLISTBOX1"));
  listLinks->Append(_("aasas"));
  listLinks->Append(_("a<font color=\"red\">s</font>asas"));
  listLinks->SetMinSize(wxSize(360, 30));
  FlexGridSizer2->Add(listLinks, 1, wxEXPAND, 2);
  FlexGridSizer15 = new wxFlexGridSizer(1, 3, 0, 0);
  btnAddLink = new wxButton(
      this, ID_BUTTON5, _("Add new link"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_BUTTON5"));
  FlexGridSizer15->Add(
      btnAddLink, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnClear = new wxButton(
      this, ID_BUTTON6, _("Clear all"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_BUTTON6"));
  FlexGridSizer15->Add(btnClear, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnDelete = new wxButton(
      this, ID_BUTTON7, _("Delete selected"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON7"));
  FlexGridSizer15->Add(
      btnDelete, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer2->Add(FlexGridSizer15, 1, wxEXPAND, 0);
  panelProperties = new wxPanel(
      this, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1"));
  panelProperties->Disable();
  boxProperties = new wxStaticBoxSizer(wxHORIZONTAL, panelProperties, _(" Link properties: "));
  FlexGridSizer3 = new wxFlexGridSizer(9, 1, 0, 0);
  FlexGridSizer3->AddGrowableCol(0);
  wxString __wxRadioBoxChoices_2[2] = {_("Revolute"), _("Prismatic")};
  rbType = new wxRadioBox(
      panelProperties, ID_RADIOBOX1, _("Type"), wxDefaultPosition, wxDefaultSize, 2,
      __wxRadioBoxChoices_2, 2, wxRA_HORIZONTAL, wxDefaultValidator, _T("ID_RADIOBOX1"));
  FlexGridSizer3->Add(rbType, 1, wxEXPAND, 2);
  StaticLine4 = new wxStaticLine(
      panelProperties, ID_STATICLINE4, wxDefaultPosition, wxSize(10, -1), wxLI_HORIZONTAL,
      _T("ID_STATICLINE4"));
  FlexGridSizer3->Add(StaticLine4, 1, wxEXPAND, 1);
  FlexGridSizer5 = new wxFlexGridSizer(3, 1, 0, 0);
  FlexGridSizer5->AddGrowableCol(0);
  SimpleHtmlListBox2 = new wxSimpleHtmlListBox(
      panelProperties, ID_SIMPLEHTMLLISTBOX2, wxDefaultPosition, wxSize(-1, 25), 0, nullptr,
      wxNO_BORDER, wxDefaultValidator, _T("ID_SIMPLEHTMLLISTBOX2"));
  SimpleHtmlListBox2->Append(
      _("&theta;<sub>i</sub> (Angle between X<sub>i</sub> and  "
        "X<sub>i+1</sub>)"));
  SimpleHtmlListBox2->Disable();
  SimpleHtmlListBox2->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  FlexGridSizer5->Add(SimpleHtmlListBox2, 1, wxEXPAND, 2);
  FlexGridSizer6 = new wxFlexGridSizer(0, 3, 0, 0);
  FlexGridSizer6->AddGrowableCol(0);
  edTheta = new wxTextCtrl(
      panelProperties, ID_TEXTCTRL1, _("0"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_TEXTCTRL1"));
  FlexGridSizer6->Add(edTheta, 1, wxEXPAND, 5);
  StaticText3 = new wxStaticText(
      panelProperties, ID_STATICTEXT3, _("(deg)"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT3"));
  FlexGridSizer6->Add(
      StaticText3, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnTh = new wxButton(
      panelProperties, ID_BUTTON1, _("Save"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON1"));
  btnTh->SetDefault();
  FlexGridSizer6->Add(btnTh, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer5->Add(FlexGridSizer6, 1, wxEXPAND, 0);
  slTheta = new wxSlider(
      panelProperties, ID_SLIDER1, 0, -180, 180, wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_SLIDER1"));
  FlexGridSizer5->Add(slTheta, 1, wxEXPAND, 2);
  FlexGridSizer3->Add(FlexGridSizer5, 1, wxEXPAND, 1);
  StaticLine1 = new wxStaticLine(
      panelProperties, ID_STATICLINE1, wxDefaultPosition, wxSize(10, -1), wxLI_HORIZONTAL,
      _T("ID_STATICLINE1"));
  FlexGridSizer3->Add(StaticLine1, 1, wxEXPAND, 1);
  FlexGridSizer8 = new wxFlexGridSizer(3, 1, 0, 0);
  FlexGridSizer8->AddGrowableCol(0);
  SimpleHtmlListBox3 = new wxSimpleHtmlListBox(
      panelProperties, ID_SIMPLEHTMLLISTBOX3, wxDefaultPosition, wxSize(-1, 25), 0, nullptr,
      wxNO_BORDER, wxDefaultValidator, _T("ID_SIMPLEHTMLLISTBOX3"));
  SimpleHtmlListBox3->Append(_("d<sub>i</sub> (Distance to common normal)"));
  SimpleHtmlListBox3->Disable();
  SimpleHtmlListBox3->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  FlexGridSizer8->Add(SimpleHtmlListBox3, 1, wxEXPAND, 2);
  FlexGridSizer9 = new wxFlexGridSizer(0, 3, 0, 0);
  FlexGridSizer9->AddGrowableCol(0);
  edD = new wxTextCtrl(
      panelProperties, ID_TEXTCTRL2, _("0"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_TEXTCTRL2"));
  FlexGridSizer9->Add(edD, 1, wxEXPAND, 5);
  StaticText5 = new wxStaticText(
      panelProperties, ID_STATICTEXT5, _("(mm)"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT5"));
  FlexGridSizer9->Add(
      StaticText5, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnD = new wxButton(
      panelProperties, ID_BUTTON2, _("Save"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON2"));
  btnD->SetDefault();
  FlexGridSizer9->Add(btnD, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer8->Add(FlexGridSizer9, 1, wxEXPAND, 0);
  slD = new wxSlider(
      panelProperties, ID_SLIDER2, 0, -2000, 2000, wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_SLIDER2"));
  FlexGridSizer8->Add(slD, 1, wxEXPAND, 2);
  FlexGridSizer3->Add(FlexGridSizer8, 1, wxEXPAND, 0);
  StaticLine2 = new wxStaticLine(
      panelProperties, ID_STATICLINE2, wxDefaultPosition, wxSize(10, -1), wxLI_HORIZONTAL,
      _T("ID_STATICLINE2"));
  FlexGridSizer3->Add(StaticLine2, 1, wxEXPAND, 1);
  FlexGridSizer10 = new wxFlexGridSizer(3, 1, 0, 0);
  FlexGridSizer10->AddGrowableCol(0);
  SimpleHtmlListBox4 = new wxSimpleHtmlListBox(
      panelProperties, ID_SIMPLEHTMLLISTBOX4, wxDefaultPosition, wxSize(-1, 25), 0, nullptr,
      wxNO_BORDER, wxDefaultValidator, _T("ID_SIMPLEHTMLLISTBOX4"));
  SimpleHtmlListBox4->Append(_("a<sub>i</sub> (Common normal length)"));
  SimpleHtmlListBox4->Disable();
  SimpleHtmlListBox4->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  FlexGridSizer10->Add(SimpleHtmlListBox4, 1, wxEXPAND, 2);
  FlexGridSizer11 = new wxFlexGridSizer(0, 3, 0, 0);
  FlexGridSizer11->AddGrowableCol(0);
  edA = new wxTextCtrl(
      panelProperties, ID_TEXTCTRL3, _("0"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_TEXTCTRL3"));
  FlexGridSizer11->Add(edA, 1, wxEXPAND, 5);
  StaticText7 = new wxStaticText(
      panelProperties, ID_STATICTEXT7, _("(mm)"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT7"));
  FlexGridSizer11->Add(
      StaticText7, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnA = new wxButton(
      panelProperties, ID_BUTTON3, _("Save"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON3"));
  btnA->SetDefault();
  FlexGridSizer11->Add(btnA, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer10->Add(FlexGridSizer11, 1, wxEXPAND, 0);
  slA = new wxSlider(
      panelProperties, ID_SLIDER3, 0, -2000, 2000, wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_SLIDER3"));
  FlexGridSizer10->Add(slA, 1, wxEXPAND, 2);
  FlexGridSizer3->Add(FlexGridSizer10, 1, wxEXPAND, 0);
  StaticLine3 = new wxStaticLine(
      panelProperties, ID_STATICLINE3, wxDefaultPosition, wxSize(10, -1), wxLI_HORIZONTAL,
      _T("ID_STATICLINE3"));
  FlexGridSizer3->Add(StaticLine3, 1, wxEXPAND, 1);
  FlexGridSizer12 = new wxFlexGridSizer(3, 1, 0, 0);
  FlexGridSizer12->AddGrowableCol(0);
  SimpleHtmlListBox5 = new wxSimpleHtmlListBox(
      panelProperties, ID_SIMPLEHTMLLISTBOX5, wxDefaultPosition, wxSize(-1, 25), 0, nullptr,
      wxNO_BORDER, wxDefaultValidator, _T("ID_SIMPLEHTMLLISTBOX5"));
  SimpleHtmlListBox5->Append(_("&alpha;<sub>i</sub> (Angle Z<sub>i</sub> to Z<sub>i+1</sub>)"));
  SimpleHtmlListBox5->Disable();
  SimpleHtmlListBox5->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  FlexGridSizer12->Add(SimpleHtmlListBox5, 1, wxEXPAND, 2);
  FlexGridSizer13 = new wxFlexGridSizer(0, 3, 0, 0);
  FlexGridSizer13->AddGrowableCol(0);
  edAlpha = new wxTextCtrl(
      panelProperties, ID_TEXTCTRL4, _("0"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_TEXTCTRL4"));
  FlexGridSizer13->Add(edAlpha, 1, wxEXPAND, 5);
  StaticText9 = new wxStaticText(
      panelProperties, ID_STATICTEXT9, _("(deg)"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT9"));
  FlexGridSizer13->Add(
      StaticText9, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnAlpha = new wxButton(
      panelProperties, ID_BUTTON4, _("Save"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON4"));
  btnAlpha->SetDefault();
  FlexGridSizer13->Add(btnAlpha, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer12->Add(FlexGridSizer13, 1, wxEXPAND, 0);
  slAlpha = new wxSlider(
      panelProperties, ID_SLIDER4, 0, -180, 180, wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_SLIDER4"));
  FlexGridSizer12->Add(slAlpha, 1, wxEXPAND, 2);
  FlexGridSizer3->Add(FlexGridSizer12, 1, wxEXPAND, 0);
  boxProperties->Add(FlexGridSizer3, 1, wxEXPAND, 0);
  panelProperties->SetSizer(boxProperties);
  boxProperties->Fit(panelProperties);
  boxProperties->SetSizeHints(panelProperties);
  FlexGridSizer2->Add(panelProperties, 1, wxEXPAND, 0);
  FlexGridSizer1->Add(FlexGridSizer2, 1, wxEXPAND, 0);
  FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer4->AddGrowableCol(0);
  FlexGridSizer4->AddGrowableRow(0);
  m_plot3D = new CMyGLCanvas(
      this, ID_XY_GLCANVAS, wxDefaultPosition, wxSize(450, 350), wxTAB_TRAVERSAL,
      _T("ID_XY_GLCANVAS"));
  FlexGridSizer4->Add(m_plot3D, 1, wxEXPAND, 0);
  FlexGridSizer16 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer16->AddGrowableCol(0);
  FlexGridSizer16->AddGrowableRow(0);
  FlexGridSizer7 = new wxFlexGridSizer(2, 2, 0, 0);
  FlexGridSizer7->AddGrowableCol(1);
  FlexGridSizer7->AddGrowableRow(1);
  FlexGridSizer7->Add(1, 15, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 0);
  FlexGridSizer14 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer14->AddGrowableCol(0);
  StaticText10 = new wxStaticText(
      this, ID_STATICTEXT10, _("Degrees of Freedom"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT10"));
  wxFont StaticText10Font = wxSystemSettings::GetFont(wxSYS_OEM_FIXED_FONT);
  if (!StaticText10Font.Ok()) StaticText10Font = wxSystemSettings::GetFont(wxSYS_DEFAULT_GUI_FONT);
  StaticText10Font.SetPointSize(8);
  StaticText10Font.SetWeight(wxFONTWEIGHT_BOLD);
  StaticText10->SetFont(StaticText10Font);
  FlexGridSizer14->Add(
      StaticText10, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer7->Add(FlexGridSizer14, 1, wxEXPAND, 0);
  FlexGridSizer7->Add(3, 55, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 0);
  pnDOFs = new wxPanel(
      this, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL2"));
  boxSizerDOFs = new wxBoxSizer(wxHORIZONTAL);
  pnDOFs->SetSizer(boxSizerDOFs);
  boxSizerDOFs->Fit(pnDOFs);
  boxSizerDOFs->SetSizeHints(pnDOFs);
  FlexGridSizer7->Add(pnDOFs, 1, wxEXPAND, 0);
  FlexGridSizer16->Add(FlexGridSizer7, 1, wxEXPAND, 0);
  FlexGridSizer17 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer17->AddGrowableCol(0);
  FlexGridSizer17->AddGrowableRow(1);
  FlexGridSizer18 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer18->AddGrowableCol(0);
  StaticText2 = new wxStaticText(
      this, ID_STATICTEXT2, _("State details"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT2"));
  wxFont StaticText2Font = wxSystemSettings::GetFont(wxSYS_OEM_FIXED_FONT);
  if (!StaticText2Font.Ok()) StaticText2Font = wxSystemSettings::GetFont(wxSYS_DEFAULT_GUI_FONT);
  StaticText2Font.SetPointSize(8);
  StaticText2Font.SetWeight(wxFONTWEIGHT_BOLD);
  StaticText2->SetFont(StaticText2Font);
  FlexGridSizer18->Add(
      StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer17->Add(FlexGridSizer18, 1, wxEXPAND, 0);
  Notebook1 = new wxNotebook(
      this, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, wxNB_BOTTOM, _T("ID_NOTEBOOK1"));
  Panel1 = new wxPanel(
      Notebook1, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL3"));
  FlexGridSizer19 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer19->AddGrowableCol(1);
  FlexGridSizer19->AddGrowableRow(0);
  lbXYZs = new wxListBox(
      Panel1, ID_LISTBOX1, wxDefaultPosition, wxSize(45, -1), 0, nullptr, 0, wxDefaultValidator,
      _T("ID_LISTBOX1"));
  wxFont lbXYZsFont(7, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL);
  lbXYZs->SetFont(lbXYZsFont);
  FlexGridSizer19->Add(lbXYZs, 1, wxEXPAND, 0);
  edMatrix = new wxTextCtrl(
      Panel1, ID_TEXTCTRL5, wxEmptyString, wxDefaultPosition, wxDefaultSize,
      wxTE_MULTILINE | wxTE_READONLY | wxTE_DONTWRAP | wxALWAYS_SHOW_SB, wxDefaultValidator,
      _T("ID_TEXTCTRL5"));
  edMatrix->SetMinSize(wxSize(300, -1));
  wxFont edMatrixFont(
      7, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Monospace"),
      wxFONTENCODING_DEFAULT);
  edMatrix->SetFont(edMatrixFont);
  FlexGridSizer19->Add(edMatrix, 1, wxEXPAND, 0);
  Panel1->SetSizer(FlexGridSizer19);
  FlexGridSizer19->Fit(Panel1);
  FlexGridSizer19->SetSizeHints(Panel1);
  Notebook1->AddPage(Panel1, _("Transf. matrices"), false);
  FlexGridSizer17->Add(Notebook1, 1, wxEXPAND, 0);
  FlexGridSizer16->Add(FlexGridSizer17, 1, wxEXPAND, 0);
  FlexGridSizer4->Add(FlexGridSizer16, 1, wxEXPAND, 0);
  FlexGridSizer1->Add(FlexGridSizer4, 1, wxEXPAND, 0);
  SetSizer(FlexGridSizer1);
  MenuBar1 = new wxMenuBar();
  Menu1 = new wxMenu();
  MenuItem5 = new wxMenuItem(Menu1, ID_MENUITEM3, _("New"), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(MenuItem5);
  Menu1->AppendSeparator();
  MenuItem3 = new wxMenuItem(
      Menu1, ID_MENUITEM1, _("Load (binary format)..."), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(MenuItem3);
  MenuItem4 = new wxMenuItem(
      Menu1, ID_MENUITEM2, _("Save (binary format)..."), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(MenuItem4);
  Menu1->AppendSeparator();
  MenuItem1 = new wxMenuItem(
      Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"), wxITEM_NORMAL);
  Menu1->Append(MenuItem1);
  MenuBar1->Append(Menu1, _("&File"));
  Menu2 = new wxMenu();
  MenuItem2 = new wxMenuItem(
      Menu2, idMenuAbout, _("About\tF1"), _("Show info about this application"), wxITEM_NORMAL);
  Menu2->Append(MenuItem2);
  MenuBar1->Append(Menu2, _("Help"));
  SetMenuBar(MenuBar1);
  FlexGridSizer1->Fit(this);
  FlexGridSizer1->SetSizeHints(this);
  Center();

  using rakf = robotic_arm_kinematicsFrame;

  Bind(wxEVT_RADIOBOX, &rakf::On1stXYZSelect, this, ID_RADIOBOX2);
  Bind(wxEVT_LISTBOX, &rakf::OnlistLinksSelect, this, ID_SIMPLEHTMLLISTBOX1);
  Bind(wxEVT_BUTTON, &rakf::OnbtnAddLinkClick, this, ID_BUTTON5);
  Bind(wxEVT_BUTTON, &rakf::OnbtnClearClick, this, ID_BUTTON6);
  Bind(wxEVT_BUTTON, &rakf::OnbtnDeleteClick, this, ID_BUTTON7);
  Bind(wxEVT_RADIOBOX, &rakf::OnrbTypeSelect, this, ID_RADIOBOX1);
  Bind(wxEVT_BUTTON, &rakf::OnButtonSaveFromEdit, this, ID_BUTTON1);
  Bind(wxEVT_SLIDER, &rakf::OnSliderScroll, this, ID_SLIDER1);
  Bind(wxEVT_BUTTON, &rakf::OnButtonSaveFromEdit, this, ID_BUTTON2);
  Bind(wxEVT_SCROLL_TOP, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_BOTTOM, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_LINEUP, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_LINEDOWN, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_PAGEUP, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_PAGEDOWN, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_THUMBTRACK, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_THUMBRELEASE, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_CHANGED, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_SLIDER, &rakf::OnSliderScroll, this, ID_SLIDER2);
  Bind(wxEVT_BUTTON, &rakf::OnButtonSaveFromEdit, this, ID_BUTTON3);
  Bind(wxEVT_SLIDER, &rakf::OnSliderScroll, this, ID_SLIDER3);
  Bind(wxEVT_SLIDER, &rakf::OnSliderScroll, this, ID_SLIDER3);
  Bind(wxEVT_BUTTON, &rakf::OnButtonSaveFromEdit, this, ID_BUTTON4);
  Bind(wxEVT_SLIDER, &rakf::OnSliderScroll, this, ID_SLIDER4);
  Bind(wxEVT_SLIDER, &rakf::OnSliderScroll, this, ID_SLIDER4);
  Bind(wxEVT_LISTBOX, &rakf::OnlbXYZsSelect, this, ID_LISTBOX1);
  Bind(wxEVT_MENU, &rakf::OnbtnClearClick, this, ID_MENUITEM3);
  Bind(wxEVT_MENU, &rakf::OnLoadBinary, this, ID_MENUITEM1);
  Bind(wxEVT_MENU, &rakf::OnSaveBinary, this, ID_MENUITEM2);
  Bind(wxEVT_MENU, &rakf::OnQuit, this, idMenuQuit);
  Bind(wxEVT_MENU, &rakf::OnAbout, this, idMenuAbout);
  //*)

  // maximize:
  this->Maximize();

  // Load default robot:
  m_robot.addLink(0, 0.4, 0, 90.0_deg, false);
  m_robot.addLink(0, 0.3, 0, 90.0_deg, false);
  m_robot.addLink(0, 0.2, 0, 0, false);

  auto openGLSceneRef = m_plot3D->getOpenGLSceneRef();

  // Initialize 3D scene:
  {
    mrpt::opengl::CGridPlaneXY::Ptr grid_10cm =
        mrpt::opengl::CGridPlaneXY::Create(-5, 5, -5, 5, 0, 0.1f);
    mrpt::opengl::CGridPlaneXY::Ptr grid_1m =
        mrpt::opengl::CGridPlaneXY::Create(-5, 5, -5, 5, 0.001f, 1);

    grid_10cm->setColor_u8(mrpt::img::TColor(0xC0, 0xC0, 0xC0, 0xA0));
    grid_1m->setColor_u8(mrpt::img::TColor(0xFF, 0xFF, 0xFF));

    openGLSceneRef->insert(grid_10cm);
    openGLSceneRef->insert(grid_1m);
  }

  m_gl_robot = mrpt::opengl::CSetOfObjects::Create();
  openGLSceneRef->insert(m_gl_robot);

  this->Regenerate3DView();

  {
    auto gl_view = openGLSceneRef->createViewport("small-view");

    gl_view->setViewportPosition(0, 0, 0.2, 0.3);
    gl_view->setTransparent(true);
    {
      mrpt::opengl::CText::Ptr obj = mrpt::opengl::CText::Create("X");
      obj->setLocation(1.1, 0, 0);
      gl_view->insert(obj);
    }
    {
      mrpt::opengl::CText::Ptr obj = mrpt::opengl::CText::Create("Y");
      obj->setLocation(0, 1.1, 0);
      gl_view->insert(obj);
    }
    {
      mrpt::opengl::CText::Ptr obj = mrpt::opengl::CText::Create("Z");
      obj->setLocation(0, 0, 1.1);
      gl_view->insert(obj);
    }
    gl_view->insert(mrpt::opengl::stock_objects::CornerXYZ());
  }

  m_plot3D->setZoomDistance(3.0f);

  m_plot3D->Refresh();

  // Update controls:
  this->RegenerateDOFPanels();
  this->UpdateListLinks();

  this->OnListSelectionChange();
}

robotic_arm_kinematicsFrame::~robotic_arm_kinematicsFrame()
{
  //(*Destroy(robotic_arm_kinematicsFrame)
  //*)
}

void robotic_arm_kinematicsFrame::Regenerate3DView()
{
  m_gl_robot->clear();
  m_robot.getAs3DObject(m_gl_robot, &m_all_poses);
}

void robotic_arm_kinematicsFrame::OnQuit(wxCommandEvent& event) { Close(); }
void robotic_arm_kinematicsFrame::OnAbout(wxCommandEvent& event)
{
  CAboutBox box(this);
  box.ShowModal();
}

/** Regenerate the left list from m_robot */
void robotic_arm_kinematicsFrame::UpdateListLinks()
{
  // Show orientation of 1st DOF:
  {
    const CPose3D& pose0 = m_robot.getOriginPose();
    const CMatrixDouble33& R = pose0.getRotationMatrix();

    if (std::abs(R(0, 2)) > std::abs(R(1, 2)) && std::abs(R(0, 2)) > std::abs(R(2, 2)))
      RadioBox1->SetSelection(2);  // X
    else if (std::abs(R(1, 2)) > std::abs(R(0, 2)) && std::abs(R(1, 2)) > std::abs(R(2, 2)))
      RadioBox1->SetSelection(1);  // Y
    else if (std::abs(R(2, 2)) > std::abs(R(0, 2)) && std::abs(R(2, 2)) > std::abs(R(1, 2)))
      RadioBox1->SetSelection(0);  // Z
  }

  listLinks->Freeze();

  const int oldSelect = listLinks->GetSelection();
  const int oldSelectXYZ = lbXYZs->GetSelection();
  listLinks->Clear();

  lbXYZs->Freeze();
  lbXYZs->Clear();

  for (unsigned int i = 0; i < m_robot.size(); i++)
  {
    const TKinematicLink& l = m_robot.getLink(i);

    std::stringstream ss;
    ss << "<b>" << (i + 1) << "</b>: ";
    ss << "Type: ";
    if (l.is_prismatic)
      ss << "<font color='blue'>P</font>";
    else
      ss << "<font color='red'>R</font>";
    ss << mrpt::format(
        " &theta;=%.02f&deg; d=%.02fmm a=%.02fmm &alpha;=%.02f&deg;", RAD2DEG(l.theta), l.d * 1e3,
        l.a * 1e3, RAD2DEG(l.alpha));

    listLinks->Append(ss.str().c_str());
  }

  for (unsigned int i = 0; i <= m_robot.size(); i++)
  {
    lbXYZs->Append((mrpt::format("XYZ%u", static_cast<unsigned int>(i)).c_str()));
  }

  if (oldSelect >= 0) listLinks->SetSelection(oldSelect);
  if (oldSelectXYZ >= 0) lbXYZs->SetSelection(oldSelectXYZ);

  listLinks->Thaw();
  lbXYZs->Thaw();

  // Update DOF panels:
  if (m_dof_panels.size() == m_robot.size())
  {
    pnDOFs->Freeze();

    for (size_t i = 0; i < m_dof_panels.size(); i++)
    {
      const TKinematicLink& l = m_robot.getLink(i);
      m_dof_panels[i]->Label1->Clear();

      {
        std::stringstream ss;
        if (l.is_prismatic)
          ss << "d<sub>" << (i + 1) << "</sub>";
        else
          ss << "&theta;<sub>" << (i + 1) << "</sub>";

        m_dof_panels[i]->Label1->Append(ss.str().c_str());
      }

      if (l.is_prismatic)
      {
        m_dof_panels[i]->Slider1->SetMin(-2000);
        m_dof_panels[i]->Slider1->SetMax(2000);

        m_dof_panels[i]->Slider1->SetValue(l.d * 1e3);

        std::string s = mrpt::format("%.02f", l.d * 1e3);
        m_dof_panels[i]->TextCtrl1->SetValue(s.c_str());
      }
      else
      {
        m_dof_panels[i]->Slider1->SetMin(-180);
        m_dof_panels[i]->Slider1->SetMax(180);
        m_dof_panels[i]->Slider1->SetValue(RAD2DEG(l.theta));

        std::string s = mrpt::format("%.02f", RAD2DEG(l.theta));
        m_dof_panels[i]->TextCtrl1->SetValue(s.c_str());
      }
    }

    pnDOFs->Thaw();
  }
}

/** Regenerate the bottom controls from m_robot */
void robotic_arm_kinematicsFrame::RegenerateDOFPanels()
{
  // Clear:
  m_dof_panels.clear();

  // Create:
  const size_t N = m_robot.size();
  m_dof_panels.resize(N);
  for (unsigned int i = 0; i < N; i++)
  {
    m_dof_panels[i] = std::make_unique<PanelDOF>(pnDOFs);
    boxSizerDOFs->Add(m_dof_panels[i].get());
  }

  boxSizerDOFs->Fit(pnDOFs);
  boxSizerDOFs->SetSizeHints(pnDOFs);
}

/** Just update the DOF panel status from m_robot */
void robotic_arm_kinematicsFrame::UpdateDOFPanels() {}
void robotic_arm_kinematicsFrame::OnListSelectionChange()
{
  UpdateMatrixView();

  const int sel = listLinks->GetSelection();
  if (sel < 0 || size_t(sel) >= m_robot.size())
  {
    panelProperties->Enable(false);
    // panelProperties->Show(false);
    return;
  }

  panelProperties->Enable(true);
  panelProperties->Show(true);

  const TKinematicLink& l = m_robot.getLink(sel);
  edTheta->SetValue(wxString::Format(_("%.04f"), RAD2DEG(l.theta)));
  edD->SetValue(wxString::Format(_("%.04f"), 1e3 * l.d));
  edA->SetValue(wxString::Format(_("%.04f"), 1e3 * l.a));
  edAlpha->SetValue(wxString::Format(_("%.04f"), RAD2DEG(l.alpha)));

  rbType->SetSelection(l.is_prismatic ? 1 : 0);

  slTheta->SetValue(RAD2DEG(l.theta));
  slAlpha->SetValue(RAD2DEG(l.alpha));
  slD->SetValue(1e3 * l.d);
  slA->SetValue(1e3 * l.a);
}

void robotic_arm_kinematicsFrame::OnlistLinksSelect(wxCommandEvent& event)
{
  OnListSelectionChange();
}

// Sliders of the DOFs bottom panels:
void robotic_arm_kinematicsFrame::OnSliderDOFScroll(wxScrollEvent&)
{
  for (size_t i = 0; i < m_dof_panels.size(); i++)
  {
    TKinematicLink& l = m_robot.getLinkRef(i);
    if (l.is_prismatic)
      l.d = m_dof_panels[i]->Slider1->GetValue() * 1e-3;
    else
      l.theta = DEG2RAD(m_dof_panels[i]->Slider1->GetValue());
  }

  UpdateListLinks();

  m_robot.update3DObject(&m_all_poses);
  m_plot3D->Refresh();

  UpdateMatrixView();
}

// Sliders of the left panel:
void robotic_arm_kinematicsFrame::OnSliderScroll(wxCommandEvent&)
{
  UpdateMatrixView();

  const int sel = listLinks->GetSelection();
  if (sel < 0 || size_t(sel) >= m_robot.size())
  {
    panelProperties->Enable(false);
    // panelProperties->Show(false);
    return;
  }

  TKinematicLink& l = m_robot.getLinkRef(sel);

  const double th = DEG2RAD(slTheta->GetValue() * 1.0);
  const double alpha = DEG2RAD(slAlpha->GetValue() * 1.0);
  const double d = slD->GetValue() * 1e-3;
  const double a = slA->GetValue() * 1e-3;

  l.theta = th;
  l.a = a;
  l.alpha = alpha;
  l.d = d;

  edTheta->SetValue(wxString::Format(_("%.04f"), RAD2DEG(l.theta)));
  edD->SetValue(wxString::Format(_("%.04f"), 1e3 * l.d));
  edA->SetValue(wxString::Format(_("%.04f"), 1e3 * l.a));
  edAlpha->SetValue(wxString::Format(_("%.04f"), RAD2DEG(l.alpha)));

  UpdateListLinks();

  m_robot.update3DObject(&m_all_poses);
  m_plot3D->Refresh();
}

void robotic_arm_kinematicsFrame::OnButtonSaveFromEdit(wxCommandEvent& event)
{
  const int sel = listLinks->GetSelection();
  if (sel >= 0 && size_t(sel) < m_robot.size())
  {
    // Get ref to selected link and apply:
    TKinematicLink& l = m_robot.getLinkRef(sel);

    double d;
    edTheta->GetValue().ToCDouble(&d);
    l.theta = DEG2RAD(d);

    edD->GetValue().ToCDouble(&d);
    l.d = 1e-3 * d;

    edA->GetValue().ToCDouble(&d);
    l.a = 1e-3 * d;

    edAlpha->GetValue().ToCDouble(&d);
    l.alpha = DEG2RAD(d);

    l.is_prismatic = (rbType->GetSelection() == 1);

    OnListSelectionChange();
    UpdateListLinks();
    UpdateMatrixView();
    m_robot.update3DObject(&m_all_poses);
    m_plot3D->Refresh();
  }
  else
  {
    panelProperties->Enable(false);
    return;
  }
}

void robotic_arm_kinematicsFrame::OnbtnClearClick(wxCommandEvent& event)
{
  m_robot.clear();

  this->RegenerateDOFPanels();
  this->UpdateListLinks();

  this->OnListSelectionChange();

  this->Regenerate3DView();
  m_plot3D->Refresh();
}

void robotic_arm_kinematicsFrame::OnbtnAddLinkClick(wxCommandEvent& event)
{
  m_robot.addLink(0, 0, 0, 0, false);

  this->RegenerateDOFPanels();
  this->UpdateListLinks();

  listLinks->SetSelection(m_robot.size() - 1);

  this->OnListSelectionChange();
  this->Regenerate3DView();
  m_plot3D->Refresh();
}

void robotic_arm_kinematicsFrame::OnLoadBinary(wxCommandEvent& event)
{
  WX_START_TRY

  wxFileDialog dlg(
      this, _("Select kinematic chain to load"), _("."), _("*.kinbin"),
      wxT("Kinematic chains binary files (*.kinbin)|*.kinbin|All files "
          "(*.*)|*.*"),
      wxFD_OPEN | wxFD_FILE_MUST_EXIST);

  if (dlg.ShowModal() != wxID_OK) return;

  const wxString sFil = dlg.GetPath();
  const std::string fil = std::string(sFil.mb_str());

  mrpt::io::CFileGZInputStream f(fil);
  mrpt::serialization::archiveFrom(f) >> m_robot;

  this->RegenerateDOFPanels();
  this->UpdateListLinks();

  this->OnListSelectionChange();

  this->Regenerate3DView();
  m_plot3D->Refresh();

  WX_END_TRY
}

void robotic_arm_kinematicsFrame::OnSaveBinary(wxCommandEvent& event)
{
  WX_START_TRY

  wxFileDialog dlg(
      this, _("Save kinematic chain"), _("."), _("*.kinbin"),
      wxT("Kinematic chains binary files (*.kinbin)|*.kinbin|All files "
          "(*.*)|*.*"),
      wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

  if (dlg.ShowModal() != wxID_OK) return;

  const wxString sFil = dlg.GetPath();
  const std::string fil = std::string(sFil.mb_str());

  mrpt::io::CFileOutputStream f(fil);
  mrpt::serialization::archiveFrom(f) << m_robot;

  WX_END_TRY
}

void robotic_arm_kinematicsFrame::OnrbTypeSelect(wxCommandEvent& event)
{
  OnButtonSaveFromEdit(event);
}

void robotic_arm_kinematicsFrame::OnbtnDeleteClick(wxCommandEvent& event)
{
  const int sel = listLinks->GetSelection();
  if (sel < 0 || size_t(sel) >= m_robot.size())
  {
    panelProperties->Enable(false);
    return;
  }

  m_robot.removeLink(sel);

  this->RegenerateDOFPanels();
  this->UpdateListLinks();

  listLinks->SetSelection(m_robot.size() - 1);

  this->OnListSelectionChange();
  this->Regenerate3DView();
  m_plot3D->Refresh();
}

void robotic_arm_kinematicsFrame::OnlbXYZsSelect(wxCommandEvent& event) { UpdateMatrixView(); }

void robotic_arm_kinematicsFrame::UpdateMatrixView()
{
  const int sel = lbXYZs->GetSelection();
  if (sel < 0 || size_t(sel) > m_all_poses.size())
  {
    return;
  }

  edMatrix->Freeze();

  const auto M = m_all_poses[sel].getHomogeneousMatrixVal<CMatrixDouble44>();

  const std::string s = format(
      "%6.03f %6.03f %6.03f %6.03f\n"
      "%6.03f %6.03f %6.03f %6.03f\n"
      "%6.03f %6.03f %6.03f %6.03f\n"
      "%6.03f %6.03f %6.03f %6.03f\n",
      M(0, 0), M(0, 1), M(0, 2), M(0, 3), M(1, 0), M(1, 1), M(1, 2), M(1, 3), M(2, 0), M(2, 1),
      M(2, 2), M(2, 3), M(3, 0), M(3, 1), M(3, 2), M(3, 3));
  edMatrix->SetValue(s.c_str());

  edMatrix->Thaw();
}

void robotic_arm_kinematicsFrame::On1stXYZSelect(wxCommandEvent& event)
{
  // Save orientation of 1st DOF:
  CPose3D pose0;

  switch (RadioBox1->GetSelection())
  {
    // Z:
    default:
    case 0:
      pose0 = CPose3D(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
      break;
    // Y:
    case 1:
      pose0 = CPose3D(0, 0, 0, 0.0_deg, 0.0_deg, -90.0_deg);
      break;
    // X:
    case 2:
      pose0 = CPose3D(0, 0, 0, 0.0_deg, 90.0_deg, 0.0_deg);
      break;
  };

  m_robot.setOriginPose(pose0);
  UpdateListLinks();
  UpdateMatrixView();
  m_robot.update3DObject(&m_all_poses);
  m_plot3D->Refresh();
}
