#!/usr/bin/env ruby

require 'optparse'
require 'orocos'

policy = Hash[type: :buffer, size: 1000, pull: false]
one_s_progress = [nil]
ten_s_progress = [nil]
options = Hash.new
optparse = OptionParser.new do |opts|
    opts.banner = "usage: rock-hz -t <task-name> -p <port-name>"

    opts.on('--host=HOST', String) do |host|
        Orocos::CORBA.name_service.ip = host
    end

    opts.on('-c COUNT', '--count=COUNT', Integer) do |count|
        one_s_progress = [nil] * count
        ten_s_progress = [nil] * count
    end

    opts.on("-t","--task TASK","task name") do |t|
        options[:task_name] = t
    end

    opts.on("-p","--port PORT","port name") do |p|
        options[:port_name] = p
    end

    opts.on("-b","--buffersize BUFFERSIZE",Integer,"buffersize") do |size|
        policy[:type] = :buffer
        policy[:size] = Integer(size)
    end
    opts.on '--pull', 'use a pull connection' do
        policy[:pull] = true
    end

    opts.on_tail("-h","--help", "Show this message") do
        puts opts
        exit
    end
end
task_name, port_name = optparse.parse(ARGV)

require 'rock/cli'
require 'tty-cursor'
require 'tty-table'
Orocos.initialize
port = Rock::CLI.choose_orocos_task_and_port(
    task_name: options[:task_name] || task_name, port_name: options[:port_name] || port_name)
if !port
    exit 1
end

port_reader = port.reader(**policy)

min_reporting_period = 1
def compute_frequency(now, queue, window_duration)
    return 0 if queue.empty?

    deadline = now - window_duration
    while !queue.empty? && (queue.last < deadline)
        queue.pop
    end
    if !queue.empty?
        Float(queue.size) / (now - queue.last)
    else
        0
    end
end

msg_header = ["Message Frequencies"]
if one_s_progress.size != 1
    msg_header.concat(one_s_progress.size.times.map { |i| "t-#{i}" })
else
    msg_header << ""
end

puts
puts
cursor = TTY::Cursor

begin
    one_s_queue = []
    one_s_filled = false
    ten_s_queue = []
    ten_s_filled = false

    while true
        start_time  = Time.now
        has_new_samples = false
        while port_reader.read_new && (Time.now - start_time) < min_reporting_period
            has_new_samples = true
            one_s_queue.unshift Time.now
            ten_s_queue.unshift Time.now
        end

        now = Time.now
        one_s = compute_frequency(now, one_s_queue, 1)
        ten_s = compute_frequency(now, ten_s_queue, 10)

        cursor.restore

        one_s_progress.unshift one_s
        ten_s_progress.unshift ten_s
        one_s_progress.pop
        ten_s_progress.pop

        print cursor.prev_line
        print cursor.prev_line
        print cursor.clear_lines(3, :down)
        print cursor.prev_line
        print cursor.prev_line
        table = TTY::Table.new(
            header: msg_header,
            rows: [["Past 1s", *one_s_progress.compact.map { |n| "%.1f" % [n] }],
                   ["Past 10s", *ten_s_progress.compact.map { |n| "%.1f" % [n] }]])
        print table.render(:basic)

        if !has_new_samples
            sleep 0.1
        end
    end
rescue Interrupt
end
