require 'rover_environment'
require 'rover_utils'
require 'posix/serialport'

################################################################
# MBARI Rover
#
# Driver for 2D-ACM current meter from FSI
#
# Change history:
#    Date       Who    What
#    ---------  ---    -------------------------------------
#    08-Aug-06  RGH    V1.0 - basic functions incl logging
#    09-Aug-06  RGH    Implement running average for heading
#
################################################################
class Acm 
  include LogHelper

  # Default configuration
  # /dev/acm 9600,8,N,1
  #
  def initialize config, portname = '/dev/acm'
    unless config
      config = SerialPort::Configuration.default
      config.baud = 9600
    end
    @dev = Posix::SerialPort.new portname, config
    
    @RAWDATA_ELEMENTS = 9
    @NUM_HEAD_AVG = 4
    
    datalog "#ACM current meter data log"
    datalog "#time,date,heading,voltage,tilt-x,tilt-y,north current," <<
             "east current,temp,average heading"
    datalog "#"
    
    @headings = Array.new
    @h_place = 0
    @time = 0
    @date = 0
    @heading = -1
    @volt = -1
    @x_tilt = 0
    @y_tilt = 0
    @north_curr = 0
    @east_curr = 0
    @temp = -1
    @avg_head = 0
    
  end

  def acquire_data
    # ACM responds to CRLF, returning data string
    #
    com = "\n"
    @dev.purge
    @dev.write(com)
    @rawdata = @dev.gets
    
    # Process data
    #
    @data = @rawdata.split(/,\s+/)
    
    # There should be 9 or 6 components in the data string
    #
    
    if @data.length == @RAWDATA_ELEMENTS
      @time = @data[0]
      @date = @data[1]
      @head = @data[2].to_f
      @volt = @data[3].to_f
      @x_tilt = @data[4].to_f
      @y_tilt = @data[5].to_f
      @north_curr = @data[6].to_f
      @east_curr = @data[7].to_f
      @temp = @data[8].to_f
    elsif @data.length == 6
      @time = 0
      @date = 0
      @head = @data[0].to_f
      @volt = 0
      @x_tilt = @data[1].to_f
      @y_tilt = @data[2].to_f
      @north_curr = @data[3].to_f
      @east_curr = @data[4].to_f
      @temp = @data[5].to_f
    else
      syslog "Error parsing rawdata, @data array length is: " +
             "#{@data.length} and should be #{@RAWDATA_ELEMENTS}"
      elements_string = "["
      @data.each {|el| elements_string += el + ", "}
      elements_string += "]"
      syslog "elements in @data array: #{elements_string}"
      return false
    end
    #puts "h_place = #{@h_place}"
    #puts "heading[h_place] = #{@head}"
    @headings[@h_place] = @head
    @h_place += 1
    @h_place = 0 unless @h_place < @NUM_HEAD_AVG
    add_offsets @headings
    
    acc = 0
    @headings.each {|h| acc+=h}
    @avg_head = acc/@headings.length
    @avg_head -= 360 if @avg_head > 360
        
    return true
  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
  
  # Write data to log file
  #
  def log_data
    #puts "#{@rawdata.gsub(/\s+/, '')}"
    datalog "#{@rawdata.gsub(/\s+/, '')},#{@avg_head}"
  end

  # Run in thread
  #
  def run
    @thread = Thread.new do
      while true
        log_data if acquire_data
        sleep(.5)
      end
    end
  end
  
  def is_running?
    @thread.alive?
  end
  
  def close
    @thread.kill
    @dev.close
  end
  
  def heading
    @head
  end
  
  def average_heading
    @avg_head
  end
  
  def date
    @date
  end
 
  def time
    @time
  end
 
  def xtilt
    @x_tilt
  end
 
  def ytilt
    @y_tilt
  end
  
  def north_current
    @north_curr
  end
  
  def east_current
    @east_curr
  end
end