require 'rover_environment'
require 'posix/serialport'
require 'rover_thread'

class Tcm2Compass
  
  def initialize config, portname = '/dev/tts/9'
    unless config
      config = SerialPort::Configuration.default
      config.baud = 9600
    end
    @dev = Posix::SerialPort.new portname, config
    @num_average = 4
    @headings = [0, 0, 0, 0, 0, 0, 0, 0, 0]
    @avg_index = 0
  end
  
  def acquire_data
    @dev.write("c?\r")
    sleep .1
    @raw = @dev.gets
    #clear the prompt
    @dev.gets
    @raw =~ /\$C(\d+\.\d).*/
    @heading = $1.to_f
    @headings[@avg_index] = @heading
    @avg_index += 1
    @avg_index = 0 unless @avg_index < @num_average
    add_offsets @headings
    
    acc = 0
    @headings.each {|h| acc += h}
    @avg_head = acc/@headings.length
    @avg_head -= 360 if @avg_head > 360
    @heading
  end
  
  def add_offsets headings
    greatest = 0
    headings.each do |x|
      greatest = x if x > greatest
    end
    if greatest > 270
      headings.collect! do |x|
        if x < 90
          x += 360
        else
          x
        end
      end
    end
    headings
  end

  
  def dev
    @dev
  end
  
  def run
    @thread = RoverThread.new do
      while true
        acquire_data
        sleep .5
      end
    end
  end
  
  def stop
    @thread.kill
  end
  
  def dev
    @dev
  end
  
  def heading
    acquire_data
  end
  
  def average_heading
    self.heading
  end
end