#! /usr/bin/env ruby

require 'vizkit'
require 'optparse'

use_test_gui = true
hostname = nil
do_read=true
do_write=true
only_positive=false
override_vel_limits=0.0
no_effort=false
no_velocity=false
command_noise_std_dev = 0
@command_port_name = nil
@state_port_name = nil
@command_task_name = nil
@state_task_name = nil
options = OptionParser.new do |opt|
    opt.banner = <<-EOD
rock-control_bot [options] /path/to/model/file
    EOD
    opt.on '--host=HOSTNAME', String, 'the host we should contact to find RTT tasks' do |host|
        hostname = host
    end
    opt.on '--help', 'this help message' do
        puts opt
        exit(0)
    end
    opt.on '--dont_read', "Don't read joint state from provided task context." do
        do_read=false
    end
    opt.on '--dont_write', "Don't write joint commands to provided task context." do
        do_write=false
    end
    opt.on '--only_positive_vel', "Use only positive velocities for sending" do
        only_positive=true
    end
    opt.on '--override_vel_limits=VALUE', "Oerrride velocity limits with the value given (positive and negative limits are changed to this value)" do |val|
        override_vel_limits=Float(val)
    end
    opt.on '--no_effort', "Don't generate UI elements for effort" do
        no_effort=true
    end
    opt.on '--add_noise=std_dev', "Add white noise with given std deviation to output signals" do |std_dev|
        command_noise_std_dev=Float(std_dev)
    end
    opt.on '--no_velocity', "Don't generate UI elements for velocity" do
        no_velocity=true
    end
    opt.on '--joint_command_port=TASK_CONTEXT_NAME:PORT_NAME', '-c=TASK_CONTEXT_NAME:PORT_NAME' , "Force joint command port to be PORT_NAME of task TASK_CONTEXT_NAME" do |val|
        splitted = val.split(':')
        if splitted.size < 2
            raise("Definition of command port must follow the pattern 'TASK_CONTEXT_NAME:PORT_NAME'. Example: --joint_command_port=my_task:the_port")
        end
        @command_task_name = splitted[0..-2].join(":")
        @command_port_name = splitted[-1]
    end
    opt.on '--joint_state_port=TASK_CONTEXT_NAME:PORT_NAME', '-s=TASK_CONTEXT_NAME:PORT_NAME' , "Force joint state port to be PORT_NAME of task TASK_CONTEXT_NAME" do |val|
        splitted = val.split(':')
        if splitted.size < 2
            raise("Definition of command port must follow the pattern 'TASK_CONTEXT_NAME:PORT_NAME'. Example: --joint_command_port=my_task:the_port")
        end
        @state_task_name = splitted[0..-2].join(":")
        @state_port_name = splitted[-1]
    end
end

args = options.parse(ARGV)
model_file = args.shift

if(!@command_task_name and do_write)
    raise("No command task given")
end
if(!@command_port_name and do_write)
    raise("No command port given")
end
if(!@state_task_name and do_read)
    puts "WARNING: No state task given"
    do_read = false
end
if(!@state_port_name and do_read)
    raise("No state port given")
end
if(!model_file)
    raise("No nodel file given")
end


if hostname
    Orocos::CORBA.name_service.ip = hostname
end

Orocos.initialize
Orocos.load_typekit('base')
require 'pry'
ctrl_gui = Vizkit.default_loader.ControlUi
ctrl_gui.configureUi(override_vel_limits, only_positive, no_effort, no_velocity, command_noise_std_dev)

YAML_FILE_EXT = [".yml", ".yaml"]
URDF_FILE_EXT = [".urdf", ".xml"]
SDF_FILE_EXT = [".sdf", ".world"]

model_file_ext = File.extname(model_file)

if YAML_FILE_EXT.include?(model_file_ext)
    ctrl_gui.initFromYaml(model_file.dup)
elsif URDF_FILE_EXT.include?(model_file_ext)
    ctrl_gui.initFromURDF(model_file.dup)
elsif SDF_FILE_EXT.include?(model_file_ext)
    ctrl_gui.initFromSDF(model_file.dup)
else
    puts "Invalid model file format. Got #{model_file_ext} but accepts only ", YAML_FILE_EXT, ", ", URDF_FILE_EXT, " or ", SDF_FILE_EXT
    exit 1
end

if do_write
    command_task = Orocos::Async.proxy @command_task_name
    command_port = command_task.port(@command_port_name)
    command_port.on_reachable do
       print "Connected output to task ", @command_task_name, ", port ", @command_port_name, "\n"
       ctrl_gui.enableSendCBs(true)

       ctrl_gui.connect(SIGNAL('sendSignal()')) do
          if command_port.reachable?
             command_port.write(ctrl_gui.getJoints()) do |result, error|
                if error or not result
                    puts "An Error occurred during sending: #{error}"
                    puts "Result value: #{result}"
                end
             end
          end
          sleep 0.01
       end
    end

    command_port.on_unreachable do
        print "Not connected to task ", @command_task_name, ", port ", @command_port_name, "\n"
        ctrl_gui.checkKeepSendingCB(false)
        ctrl_gui.enableSendCBs(false)

        ctrl_gui.disconnect(SIGNAL('sendSignal()')) do end
    end
end

if do_read
    state_task = Orocos::Async.proxy @state_task_name    

    state_port = state_task.port(@state_port_name)
    state_port.on_reachable do
       print "Connected input to task ", @state_task_name, ", port ", @state_port_name, "\n"
       ctrl_gui.enableUpdateCB(true)
    end

    state_port.on_data do |data|
        ctrl_gui.setJointState(data)
    end

    state_port.on_unreachable do
        print "Not connected to task ", @state_task_name, ", port ", @state_port_name, "\n"
        ctrl_gui.enableUpdateCB(false)
    end

    command_port.on_unreachable do
        ctrl_gui.checkUpdateCB(true)
    end
else
    ctrl_gui.checkUpdateCB(false)
    ctrl_gui.enableUpdateCB(false)
end

Vizkit.exec    
    
