#! /usr/bin/env ruby

require 'vizkit'
require 'optparse'

hostname = "localhost"
only_positive=false
override_vel_limits=0.0
no_effort=false
no_velocity=false
driver_task_name = nil

OptionParser.new do |opt|
    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 '--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 '--no_velocity', "Don't generate UI elements for velocity" do
        no_velocity=true
    end
    opt.on '--driver=DRIVER_TASK_NAME', String, "BLDC driver task name" do |name|
       driver_task_name = name 
    end
end.parse!

if driver_task_name.nil?
    raise("No driver task name given")
end

Orocos::CORBA.name_service.ip = hostname
Orocos.initialize
Orocos.load_typekit('base')

ctrl_gui = Vizkit.default_loader.ControlUi
ctrl_gui.configureUi( override_vel_limits, only_positive, no_effort, no_velocity )
configured = false

def getReg( regs_comm, regs, dev_name, reg_name, def_val )
    val = def_val
    regs_comm.each do | devs |
        if devs.deviceclass == dev_name
            devs.registers.each do | reg |
                val = reg.value if reg.name == reg_name
            end
        end
    end
    regs.each do | devs |
        if devs.deviceclass == dev_name
            devs.registers.each do | reg |
                val = reg.value if reg.name == reg_name
            end
        end
    end
    return val
end

ctrl_gui.connect_to_task driver_task_name do |task|
    task.on_reachable do
        driver_task = Orocos::Async.name_service.get driver_task_name
        unless configured
            regs_comm = driver_task.property("joint_config_general").read.configureRegisters
            joints_config = driver_task.property("joints_config").read
            jointLimits = Types::Base::JointLimits.new
            joints_config.each do |joint_config|
                regs =  joint_config.configureRegisters
                jointLimits.names.push( joint_config.jointName )
                jointLimitRange = Types::Base::JointLimitRange.new
                jointLimitRange.min.position =  getReg( regs_comm, regs, "CascadedController", "POS_MIN", -3.1 )
                jointLimitRange.max.position =  getReg( regs_comm, regs, "CascadedController", "POS_MAX", +3.1 )
                jointLimitRange.min.speed    = -getReg( regs_comm, regs, "CascadedController", "VEL_MAX", +2.0 )
                jointLimitRange.max.speed    =  getReg( regs_comm, regs, "CascadedController", "VEL_MAX", +2.0 )
                jointLimitRange.min.effort   = -getReg( regs_comm, regs, "CascadedController", "CUR_MAX", +1.0 )
                jointLimitRange.max.effort   =  getReg( regs_comm, regs, "CascadedController", "CUR_MAX", +1.0 )
                jointLimits.elements.push( jointLimitRange )
            end
            ctrl_gui.initModel( jointLimits )
            configured = true
        end
        ctrl_gui.setEnabled( true )
        driver_task.port("joints_status").on_data do |data| 
            ctrl_gui.setJointState(data)
        end
        writer = driver_task.port("joints_command").writer
        ctrl_gui.connect(SIGNAL('sendSignal()')) do 
            sample = ctrl_gui.getJoints()
            writer.write(sample)
        end
    end
    task.on_unreachable do
        ctrl_gui.setEnabled( false )
    end
end

ctrl_gui.show
Vizkit.exec    
