#! /usr/bin/env ruby

require 'orocos'
require 'orocos/async'
require 'rock/bundle'
require 'vizkit'
require 'optparse'

hostname = nil
number_contact_points = nil
options = OptionParser.new do |opt|
    opt.banner = <<-EOD
robot-odometer [options]  </path/to/model/file> <task context name of the odometry component>
    EOD
    opt.on '--host=HOSTNAME', String, 'the host we should contact to find RTT tasks' do |host|
        hostname = host
    end
    opt.on '--number_contact_points=#number', String, 'Number of the contact points in robot kinematics' do |number_cp|
        number_contact_points = number_cp
    end

    opt.on '--help', 'this help message' do
        puts opt
    end
end

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

if !model_file
    puts "missing model file argument"
    puts options
    exit 1
end


if !task_context_name
    puts "missing task context name"
    puts options
    exit 1
elsif
    if hostname
        Orocos::CORBA.name_service.ip = hostname
    end
end

if !number_contact_points
    puts "missing number of contact points"
    puts options
    exit 1
end

Bundles.initialize
Bundles.transformer.load_conf(Bundles.find_file('config', 'transforms_scripts.rb'))
puts "Getting task with name #{task_context_name}"
puts "Robot kinematics expected to have #{number_contact_points} contact points"
task = Orocos::Async.proxy task_context_name


view3d = Vizkit.vizkit3d_widget
view3d.show
vis_gui = Vizkit.default_loader.RobotVisualization
vis_gui.modelFile = model_file.dup
vis_gui.setPluginName("Robot")
#view3d.setPluginDataFrame("body_base", vis_gui )
#view3d.setPluginDataFrame("root_coyote3", vis_gui )
view3d.setPluginDataFrame("asguard_body", vis_gui )

main = Qt::Widget.new
layout = Qt::VBoxLayout.new(main)
layout.add_widget(view3d)

ctrl_gui = Vizkit.default_loader.ControlUi
override_vel_limits=0
only_positive=true
no_effort=true
no_velocity=false

#Contact points of the robot
array_contact_points = []
for i in 0..number_contact_points.to_i-1
    rb = Vizkit.default_loader.RigidBodyStateVisualization
    rb.displayCovariance(true)
    rb.setPluginName("contact_point_#{i}")
    rb.setColor(Eigen::Vector3.new(0, 0, 0))
    rb.resetModel(0.1)
    Vizkit.vizkit3d_widget.setPluginDataFrame("body", rb)
    array_contact_points.push(rb)
end

joint = Types::Base::Samples::Joints.new
imuSamples = Types::Base::Samples::IMUSensors.new
imuSamples.acc = Eigen::Vector3.new(0.00, 0.00, 9.81)
imuSamples.gyro = Eigen::Vector3.new(0.00, 0.00, 0.00)
orientation_rbs = Types::Base::Samples::RigidBodyState.new
orientation_rbs.position = Eigen::Vector3.new(0.00, 0.00, 0.00)
orientation_rbs.orientation = Eigen::Quaternion.Identity()

ctrl_gui.configureUi(override_vel_limits, only_positive, no_effort, no_velocity)
ctrl_gui.initFromURDF(model_file.dup)
layout.add_widget(ctrl_gui)
main.show
ctrl_gui.connect(SIGNAL('sendSignal()')) do
    joint = ctrl_gui.getJoints()
    vis_gui.updateData(joint)
    puts "Value of local variable is #{joint.names}"
    task.on_reachable do
        puts "Task #{task.name} has #{task.port_names}"
        port_joints = task.port('joints_samples')
        joint.time = Time.now;
        port_joints.write(joint) do |result|
            puts "Sent joints to task #{task.name}"
        end
        port_attitude = task.port('orientation_samples')
        orientation_rbs.time = Time.now;
        port_attitude.write(orientation_rbs) do |result|
            puts "Sent orientation to task #{task.name}"
        end
    end
end

task.on_reachable do
    #Access to the chains sub_ports
    vector_rbs = task.port('fkchains_rbs_out')
    vector_rbs.wait

    for i in 0..number_contact_points.to_i-1
        Vizkit.display vector_rbs.sub_port([:rbsChain, i]), :widget => array_contact_points[i]
    end
end

# disable the GUI until the task is reachable
task.on_reachable {main.setEnabled(true)}
task.on_unreachable {main.setEnabled(false)}

Vizkit.exec


