#--
#******************************************************************************
#* Copyright 1990-2007 MBARI
#* MBARI Proprietary Information. All rights reserved.
#******************************************************************************
#* Summary  : Real and simulated drivers for the ACM current meter
#* Filename : acm.rb
#* Author   : Henthorn
#* Project  : Benthic Rover
#* Version  :
#* Created  : Aug-06 Basic functions incl logging
#* Modified : Aug-06 Implement running average for heading
#*            Aug-07 Improved by using separate read/write threads
#*            Aug-07 Get instrument info info from ConfigMan
#******************************************************************************
#++

require 'rover_environment'
require 'rover_thread'
require 'rover_utils'
require 'configman.rb'
require 'posix/serialport'

# Implements data acquisition, processing, and logging of data from ACM
# current meter. Recalculates average heading and current direction after each
# data record is read.
#
class Acm 
  include LogHelper

  attr_reader :date, :time, :x_tilt, :y_tilt, :north_curr, :east_curr, \
              :head, :avg_head, :avg_curr
    
  # Typical usage: op = Acm.new(nil, '/dev/acm')
  # Default serial port configuration: 9600,8,N,1
  #
  def initialize config, portname = '/dev/acm'
    unless config
      config = SerialPort::Configuration.default
      config.baud = 9600
    end

    @d_count = 0
    @dev = Posix::SerialPort.new portname, config
    @name = portname[(portname.rindex("/")+1)..(portname.length-1)]
    
    @RAWDATA_ELEMENTS_A = 9
    @RAWDATA_ELEMENTS_B = 6
    @NUM_HEAD_AVG = 4

    # Log file name is derived from port name    
    @logfilename = @name + ".csv"

    # Init other elements
    @model = "Current Meter"
    @sn    = "xyz"
    @rate = 0.5
    cm = ConfigMan.instance
    if cm
      @model = "Current Meter" unless @model = cm.get(@name, "model")
      @sn    = "xyz"           unless @sn    = cm.get(@name, "sn")
      if (rt = cm.get(@name, "rate"))
        @rate = rate.to_f
      end
    end

    @thread = @wthread = nil
    @heading = @volt = @temp = -1
    @h_place = @current_place = @time = @date = @x_tilt = @y_tilt = \
               @north_curr = @east_curr = @avg_head = @avg_curr = 0

    # Variables for calculating current speed and dir
    @headings = Array.new
    @current_array_length = 60
    @eastings = Array.new
    @northings = Array.new
    @eastings_sum = 0.0
    @northings_sum = 0.0
 
    # Write a header to the log file
    datawrite("Rover Timestamp (UTC) , Mission Time (sec), ACM time , " <<
     "ACM date , " <<
     "heading (degress 0-360) , voltage (volts) , " <<
     "tilt-x (degrees) , tilt-y (degrees) , " <<
     "north current (degrees 0-360) , " <<
     "east current (degrees 0-360) , temp (celcius) , " <<
     "average heading (degrees 0-360)," << "average current (degrees 0-360)," <<
     ",#{@name}  #{@model}  #{@sn} \n, \n", @logfilename)
  end

  # Perform a blocking read on the data port. When data is read it is parsed,
  # logged, and processed.
  #
  def acquire_data
    # ACM responds to CRLF, returning data string
    #
#    a = Array.new(1,@dev)
#    return unless(IO.select(a, nil, nil, 0.25))
#    @rawdata = @dev.gets
    @rawdata = @dev.gets
    
    # Process data
    #
    @data = @rawdata.split(/,\s+/)
    
    # We can handle formats with 9 or 6 components in the data record.
    #
    if @data.length == @RAWDATA_ELEMENTS_A
      @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 == @RAWDATA_ELEMENTS_B
      @time = 0  # No date, time, or voltage elements but that's OK
      @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_A}")
      syslog("! #{@rawdata}")

      return false
    end
    @east_curr = 0.0 - @east_curr
    
    # log it
    #
    if (@d_count == 0)
      log_data
    end

    @d_count += 1
    @d_count %= 7 # if (@d_count == 5)

    # Calculate a new boxcar average heading
    #    
    @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
    @avg_curr = self.avg_current_heading

    # Update eastings and northings array for magnitude and dir of current
    #
    if(@northings.length >= @current_array_length)
      @northings_sum -= @northings.pop
      @eastings_sum -= @eastings.pop
    end
    
    @northings.unshift @north_curr
    @eastings.unshift @east_curr
    @northings_sum += @north_curr
    @eastings_sum += @east_curr
    
    return true
  end

  # Attempts to normalize headings that are near 0/360 when the highest
  # compass reading is near 360. This makes calculating an average heading
  # a little more straight-forward.
  #
  def add_offsets headings
    greatest = 0
    headings.each do |x|
      greatest = x if x > greatest
    end
    
    # Normalize compass reading around 360
    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
    datalog("#{@rawdata.gsub(/\s+/, '')},#{@avg_head},#{@avg_curr}", @logfilename)
  end

  # Poke the ACM so it returns data
  # Intended to run in a separate thread to remove latency.
  #
  def poke
    com = "\n"
    @dev.write(com)
#    @dev.flush
  end
  
  # Run data acquisition and logging functions in separate threads so
  # the data is gathered and logged so very close to the desired
  # frequency. Single-threaded implementation resulted in less than 50
  # samples per minute, rather than 60.
  #
  def run
    syslog("_ acm thread running")
    @d_count = 0
    # Assume acm was just powered-up and has lines of stuff in the buffer
    #
    @dev.purge

    # Continuous operation rather than request mode
    #
    @dev.write("SC\n")
        
    # Start the main thread
    #
    @thread = RoverThread.new do
      Thread.current["name"] = "acm log"
      # Start another thread to just
      # prod the ACM to return data at
      # the prescribed rate. This technique
      # used to remove latency that quickly
      # accumulates to an undesired amount.
      #
      
      # Using Contiuous operation mode now
      #
      
#      @wthread = RoverThread.new do
#	       Thread.current["name"] = "acm poke"
#        while true
#          sleep(1/@rate)
#          poke
#        end
#      end

      # Now back in the main thread, record the data as it arrives
      #
      while true
        acquire_data
      end
    end
    
  end
  
  # Indicates whether ACM driver is running
  def is_running?
    @thread != nil && @thread.alive?
  end

  # Stops both threads if they are alive
  #  
  def stop_thread
    if is_running?
      @dev.write("S\n")
      @wthread.kill if @wthread != nil
      @thread.kill  if @thread  != nil
    end
    syslog("_ acm thread halted")
  end

  def stop
    stop_thread
  end

  # Closes the data port by first stopping the read/write threads
  # then closing the port.
  #
  def close
    stop_thread
    @dev.close
  end
  
  # Synonym for :north_curr accessor
  def north_current
    @north_curr
  end
  
  # Synonym for :east_curr accessor
  def east_current
    @east_curr
  end

  # Synonym for :head accessor
  def heading
    @head
  end
  
  # Synonym for :avg_head accessor
  def average_heading
    @avg_head
  end

  # Intermediate calculators
  #  
  def avg_magnitude_of_current
    avg_northing = @northings_sum / @northings.length
    avg_easting = @eastings_sum / @eastings.length
    return magnitude_of_current(avg_easting, avg_northing)
  end
  
  def avg_current_heading
    avg_northing = @northings_sum / @northings.length
    avg_easting = @eastings_sum / @eastings.length
    return heading_of_current(avg_easting, avg_northing)
  end

  def magnitude_of_current(easting, northing)
    return  Math.sqrt((northing * northing) + (easting * easting))
  end

  def heading_of_current(easting, northing)
    val = Math.atan2(easting, northing) * 180.0 / Math::PI
    val+= 360 if val < 0
    #syslog("Math.atan2(#{easting}, #{northing}) * 180.0 / Math::PI = #{val}")
    return val  
  end

  private :magnitude_of_current, :heading_of_current

end

# Implements a simulated Acm to be used without the Rover
#
class SimAcm < Acm

  def initialize(config, portname = '/dev/null')
#    super(config, portname)
    @running = false
    @index = 0
    @head = 0
    @indexDir = :forward
    @avgCurrentHeadings = [230, 231, 230, 232, 233, 234, 234, 234, 233, 234, 235,
                         235, 236, 236, 237, 237, 238, 237, 238, 239, 240, 240,
                         241, 242, 242, 243, 242, 244, 245, 246, 246, 247, 248,
                         249, 249, 250, 251, 252, 252, 253, 254, 254, 255, 256,
                         257, 258, 257, 258, 259, 260, 261, 262, 263, 264, 265,
                         266, 267, 267, 266, 268, 269, 270, 271, 271, 273, 274]  
  end

  #returns a range of values starting at 230, going up to 274, then going
  #back down to 230.
  def avg_current_heading()
    val = @avgCurrentHeadings[@index]
    if(@indexDir == :forward)
      @index += 1
    else
      @index -= 1
    end
    if(@index >= @avgCurrentHeadings.length)
      @indexDir = :backward
      @index -= 1
    elsif(@index <= 0)
      @indexDir = :forward
    end
    val
  end

  #spins around the compass, gives values incrementing from 0 to 359 than goes back to 0
  def average_heading()
    val = @head
    @head += 1
    @head = @head % 360
    return val
  end

  def run()
    @running = true
    syslog("_ SimAcm running")
    #do nothing.
  end

  # Indicates whether Sim ACM driver is running
  def is_running?
    @running
  end

  # Stops driver
  #  
  def stop_thread
    @running = false
    syslog("_ SimAcm stopping")
    return
  end

  # Closes by stopping driver
  #
  def close
    stop_thread
  end
  
end