function [ret]=c_MechaControl(command,arg1)
global defs MC
switch (command)
  case defs.name
    ret='mecha ctl';
  case defs.init
    if (nargin==2)
      MC.cnum = arg1;
      init;
    else
      disp('error: init ignored, no handle specified');
    end
  case defs.start
    start;
  case defs.iter
    iter;
  case defs.stop
    stop;
  case defs.destroy
    destroy;
  case defs.mecha_callback_iter
    real_iter;
  case defs.mecha_callback_reset
    if(arg1(1)) set(MC.s_dx,'Value',0); end
    if(arg1(2)) set(MC.s_da,'Value',0); end
    if(arg1(3)) set(MC.s_dy,'Value',0); end
    real_iter;
  case defs.mecha_callback_option
    if(MC.obj.isConnected)
      set(MC.i_XMIT,'Visible','on');
      MC.obj.sendCommand(int2str(arg1), 0);
      set(MC.i_XMIT,'Visible','off');
      MC.lasttime = now;
    end
    
  otherwise
    disp('warning: component called with invalid command');
end


%%
%% Initialization function
%%
function init
global defs MC
% Demand instant command update to robot
MC.lasttime = 0; % lasttime = the last time we sent a command to the robot
MC.locked = false; % don't do an iter if we're already doing an iter
% Create figure
MC.fig = figure('Visible','off','NumberTitle','off','MenuBar','none',...
                'DoubleBuffer','on','Name','Tekkotsu Mecha Command Console',...
		'CloseRequestFcn',['togglecomponents(' MC.cnum ',0);'],...
                'Position',[250 100 300 300],'Resize','off','Color',[0 0 0]);

%%
%% Panel startup
%%
function start
global MC conf defs
% set up java control object
MC.obj = MechaController(conf.ip, conf.port_mecha);
% prepare to draw!
set(MC.fig, 'Visible', 'on');
% MECHA CONTROL banner
uicontrol('Parent',MC.fig,'Style','text',...
	  'String','MECHA  CONTROL','FontSize',24,...
	  'HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Position',[0 270 300 30]);
% XMIT indicator
MC.i_XMIT = ...
uicontrol('Parent',MC.fig,'Style','text',...
	  'String','XMIT','HorizontalAlignment','center','FontWeight','bold',...
	  'ForegroundColor',[0 0.2 0],'BackgroundColor',[0 0.8 0],...
	  'Position',[10 250 45 15],'Visible','off');
% Parameter indicator text
uicontrol('Parent',MC.fig,'Style','text',...
	  'String',' Fwd','HorizontalAlignment','left','FontWeight','bold',...
	  'ForegroundColor',[1 0.6 0.4],'BackgroundColor',[0.3 0.18 0.12],...
	  'Position',[190 207 60 15]);
uicontrol('Parent',MC.fig,'Style','text',...
	  'String',' Roto','HorizontalAlignment','left','FontWeight','bold',...
	  'ForegroundColor',[0.4 1 0.4],'BackgroundColor',[0.12 0.3 0.12],...
	  'Position',[190 185 60 15]);
uicontrol('Parent',MC.fig,'Style','text',...
	  'String',' Side','HorizontalAlignment','left','FontWeight','bold',...
	  'ForegroundColor',[0.4 0.6 1],'BackgroundColor',[0.12 0.18 0.3],...
	  'Position',[190 163 60 15]);
% Parameter indicators
MC.i_dx = ...
uicontrol('Parent',MC.fig,'Style','text',...
	  'String','0','HorizontalAlignment','right','FontWeight','bold',...
	  'BackgroundColor',[1 0.6 0.4],'ForegroundColor',[0.3 0.18 0.12],...
	  'Position',[250 207 35 15]);
MC.i_da = ...
uicontrol('Parent',MC.fig,'Style','text',...
	  'String','0.0','HorizontalAlignment','right','FontWeight','bold',...
	  'BackgroundColor',[0.4 1 0.4],'ForegroundColor',[0.12 0.3 0.12],...
	  'Position',[250 185 35 15]);
MC.i_dy = ...
uicontrol('Parent',MC.fig,'Style','text',...
	  'String','0','HorizontalAlignment','right','FontWeight','bold',...
	  'BackgroundColor',[0.4 0.6 1],'ForegroundColor',[0.12 0.18 0.3],...
	  'Position',[250 163 35 15]);
% Zeroing pushbuttons
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','ZERO','HorizontalAlignment','center','FontWeight','bold',...
	  'BackgroundColor',[1 0.6 0.4],'ForegroundColor',[0.3 0.18 0.12],...
	  'Callback','c_MechaControl(defs.mecha_callback_reset,[1 0 0]);',...
	  'Position',[184 127 50 20]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','ZERO','HorizontalAlignment','center','FontWeight','bold',...
	  'BackgroundColor',[0.4 1 0.4],'ForegroundColor',[0.12 0.3 0.12],...
	  'Callback','c_MechaControl(defs.mecha_callback_reset,[0 1 0]);',...
	  'Position',[184 105 50 20]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','ZERO','HorizontalAlignment','center','FontWeight','bold',...
	  'BackgroundColor',[0.4 0.6 1],'ForegroundColor',[0.12 0.18 0.3],...
	  'Callback','c_MechaControl(defs.mecha_callback_reset,[0 0 1]);',...
	  'Position',[184 83 50 20]);
% FULL STOP pushbutton
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','FULL STOP','HorizontalAlignment','center',...
	  'FontWeight','bold','FontAngle','italic',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_reset,[1 1 1]);',...
	  'Position',[20 150 90 80]);
% Optional bindings pushbuttons
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','0','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,0);',...
	  'Position',[18 106 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','1','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,1);',...
	  'Position',[38 106 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','2','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,2);',...
	  'Position',[58 106 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','3','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,3);',...
	  'Position',[78 106 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','4','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,4);',...
	  'Position',[98 106 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','5','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,5);',...
	  'Position',[18 86 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','6','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,6);',...
	  'Position',[38 86 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','7','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,7);',...
	  'Position',[58 86 15 15]);
uicontrol('Parent',MC.fig,'Style','pushbutton',...
	  'String','8','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,8);',...
	  'Position',[78 86 15 15]);
uicontrol('Parent',MC.fig','Style','pushbutton',...
	  'String','9','HorizontalAlignment','center',...
	  'ForegroundColor',[0 1 0],'BackgroundColor',[0 0.2 0],...
	  'Callback','c_MechaControl(defs.mecha_callback_option,9);',...
	  'Position',[98 86 15 15]);
% Control sliders
MC.s_dx = ...
uicontrol('Parent',MC.fig,'Style','slider',...
	  'Max',180,'Min',-180,'Value',0,...
	  'BackgroundColor',[1 0.6 0.4],...
	  'Callback','c_MechaControl(defs.mecha_callback_iter);',...
	  'Position',[125 80 50 180]);
MC.s_da = ...
uicontrol('Parent',MC.fig,'Style','slider',...
	  'Max',1.8,'Min',-1.8,'Value',0,...
	  'BackgroundColor',[0.4 1 0.4],...
	  'Callback','c_MechaControl(defs.mecha_callback_iter);',...
	  'Position',[15 40 270 35]);
MC.s_dy = ...
uicontrol('Parent',MC.fig,'Style','slider',...
	  'Max',140,'Min',-140,'Value',0,...
	  'BackgroundColor',[0.4 0.6 1],...
	  'Callback','c_MechaControl(defs.mecha_callback_iter);',...
	  'Position',[40 10 220 23]);

%%
%% Iterator wrapper for Tekkotsu mon code
%%
function iter
global MC conf
if(MC.locked == false)
  if(now - MC.lasttime > conf.mecha_autosend_interval)
%I moved this to the java layer - ethan
%    real_iter;
     MC.lasttime = now;
  end
end

%%
%% True iterator code
%%
function real_iter
global MC
% We're updating--lock out updates
MC.locked = true;
% Retrieve and show new values
MC.dx = get(MC.s_dx,'Value');
MC.da = -get(MC.s_da,'Value'); % Negative to reflect AIBO motion axes
MC.dy = -get(MC.s_dy,'Value'); % Negative to reflect AIBO motion axes
set(MC.i_dx,'String',num2str(MC.dx,'%.0f'));
set(MC.i_da,'String',num2str(MC.da,'%.1f'));
set(MC.i_dy,'String',num2str(MC.dy,'%.0f'));
% Transmit new values.
if(MC.obj.isConnected)
  set(MC.i_XMIT,'Visible','on');
  MC.obj.sendCommand('f', MC.dx);
  MC.obj.sendCommand('r', MC.da);
  MC.obj.sendCommand('s', MC.dy);
  set(MC.i_XMIT,'Visible','off');
end
MC.lasttime = now;
% Release lock
MC.locked = false;

%%
%% Stop session
%%
function stop
global MC
MC.obj.close;
set(MC.fig, 'Visible', 'off');

%%
%% End it all
%%
function destroy
global MC
delete(MC.fig);
