#! /usr/bin/env ruby
#
require 'orocos'
require 'vizkit'
require 'optparse'
Orocos.initialize

gui = false
optparse = OptionParser.new do |opt|
    opt.banner = "run [--gui] DEVICE"
    opt.on '--gui', "starts the Rock task inspection widget on the running task" do
        gui = true
    end
end
dev_id = *optparse.parse(ARGV)
if !dev_id
    puts optparse
    exit 1
end

if gui
    require 'vizkit'
end

Orocos.run 'act_schilling_test' do
    
    Orocos.logger.level = Logger::DEBUG
    Orocos.log_all
    puts "deployed the act_schilling::Task task"

    act = Orocos::TaskContext.get 'act_schilling'
    act.io_read_timeout = Time.at(2);
    Orocos.apply_conf_file(act, 'act.yml', dev_id)
    #act.io_port = "tcp://10.112.102.20:4001"
    #act.config.ctrl_mode = 'MODE_VEL'
    #act.config.home_pos = 45
    #act.config.velocity = 1250
    act.configure
    act.start
    
    if gui
	task_inspector = Vizkit.default_loader.TaskInspector
	task_inspector.add_task(act)
	task_inspector.show
    
	guiviz = Vizkit.load 'scripts/act.ui'
	
	writer = act.pos.writer
	  guiviz.slPos.connect(SIGNAL('valueChanged(int)')) do 
	  sample = writer.new_sample
	  sample = guiviz.slPos.value
	  writer.write sample
	end

	writer1 = act.vel.writer
	  guiviz.slVel.connect(SIGNAL('valueChanged(int)')) do 
	  sample = writer1.new_sample
	  sample = guiviz.slVel.value
	  writer1.write sample
	end
	
	act.act_samples.connect_to do |data,_|
           guiviz.edPos.setText data.shaft_ang.to_s
        end
	
	act.act_samples.connect_to do |data,_|
           guiviz.edVel.setText data.shaft_vel.to_s
        end

	guiviz.pbCalibrate.connect(SIGNAL('clicked()')) do 
            act.calibrate();
        end
	guiviz.pbStop.connect(SIGNAL('clicked()')) do 
            guiviz.slVel.value = 0;
        end
	guiviz.rbPos.connect(SIGNAL('clicked()')) do 
	    act.setControlMode('MODE_POS');
	    #act.config do |p|
	     # p.ctrlMode = 'MODE_POS';
	    #end
            #act.config.ctrlMode = 'MODE_POS'
        end
	
	guiviz.rbVel.connect(SIGNAL('clicked()')) do 
            act.setControlMode('MODE_VEL');
        end
	
	guiviz.rbNone.connect(SIGNAL('clicked()')) do 
            act.setControlMode('MODE_NONE');
        end
	
	guiviz.show
        Vizkit.exec
    else
        act.calibrate
	reader = act.act_samples.reader
        Orocos.watch(act) do
            if sample = reader.read
                #pp sample
            end
        end
    end
end

