=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Acm class implements interface to ACM 2-D Current Meter
 * Filename : acm.rb
 * Author   : Henthorn
 * Project  : Benthic Rover
 * Version  : 2
 * Created  : Nov 08
 * Modified : 
 ******************************************************************************
=end

# Refer to Rover-code home directory as base
#
require "#{ENV['ROVER_HOME']}/utils/rover_environment"
#

require 'utils/datalog'
require 'utils/rover_thread'
require 'utils/run_avg'
require 'posix/serialport'

AcmData = Struct.new(:time, :date, :head, :volt, :x, :y, :north, :east, :temp)

##########
# Acm class based on Acm class from early Rover implementations.
# This version relies on the condition that the ACM is already set-up
# to return the correct data items (se below)
# before a deployment.
#
# Operating mode:
#   Passive - Assumes the ACM is set up to return data sets with the
#             items below. The run method simply commands the ACM to
#             begin streaming data continously.
#
# Device parameters must be ON:
#
#    "ATIME",
#    "HDNG" ,
#    "BATT" ,
#    "TX"   ,
#    "TY"   ,
#    "VN"   ,
#    "VE"   ,
#    "STEMP"
#
class Acm
  include SyslogWriter

  attr_reader :name, :thread, :vehicle_averager, :current_averager, :mags, :dev

  ##
  # Initialize the object. Not to be confused with initializing the driver.
  # Does not assume that power to the acm is on.
  #
  def initialize(p_portname="/dev/acm")
    config = Posix::SerialPort::Configuration.default
    config.baud = 9600
    @dev = Posix::SerialPort.new(p_portname, config)

    # Basic stuff
    #
    @thread = nil
    @name = File.basename(p_portname)

    # Data and calculators
    #
    @data = AcmData.new("","","","","","","","","")
    @vehicle_averager = RunAvg.new(10)
    @current_averager   = RunAvg.new(10)
    @mags = []
  end
  #####


  NoComms = "no comms"
  BadData = "bad data"

  #####
  # Perform a basic health check-up by attempting to acquire data.
  # Assume that power to the acm was just applied so there will
  # be data in the buffer.
  # Return nil if check-up succeeded. Otherwise, return a string
  # describing cause of failure.
  #
  def check_up
    while (IO.select([@dev], nil, nil, 0.1))
      r = @dev.gets
    end
    ##
    # Execute in a separate thread to catch comms errors.
    # Timeout after 10 seconds.
    #
    @dev.write("SC\r") if (@dev)  # start continuous data stream
    sleep(0.1)
    @dev.write("SC\r") if (@dev)  # start continuous data stream

    got_data = nil
    t = RoverThread.new do
      while (true)
        got_data = read_data
      end
    end
    sleep(10)
    @dev.write("S\r") if (@dev)  # stop continuous data stream

    ##
    # If check-up thread still running, kill it
    #
    if (t.alive?)
      t.kill
    end
    t.join

    ##
    # A return value of false from read_data means
    # the information from the device is malformed. A value
    # of true means all is well.
    #
    if (got_data)
      if (@data.head == 0)
        return BadData
      else
        return nil
      end
    elsif (got_data == false)
      return BadData
    else
      return NoComms
    end
  end 
  #####


  #####
  # Reset the averagers
  #
  def reset()
    @vehicle_averager.clear
    @current_averager.clear
  end
  #####


  #####
  # Run the driver and log data
  #
  def run(data_dir=DataLog.acm_logs)

    # TODO: Raise an exception here
    #
    if (@thread && @thread.active)
      syslog("! Already running")
      return
    end

    # Clear calculators
    #
    @vehicle_averager.clear
    @current_averager.clear
    @mags = []

    # May be a different deployment. Open log files just in case.
    #
    unless (data_dir && File.exist?(data_dir) && File.directory?(data_dir))
      data_dir = "."
    end

    @rawlog = AcmRawLog.new(data_dir + "/acm_raw.csv")
    @calclog = AcmCalcLog.new(data_dir + "/acm_calc.csv")

    # Initialize the instrument and run the data acquisition
    # and log thread.
    #
    run_passive_mode

  end
  #####


  ##
  # Initialize the acm driver for passive operation.
  # This mode requires that the driver only read from the port.
  #
  def run_passive_mode
    sleep(0.1)
    @dev.write("SC\r") if (@dev)  # start continuous data stream
    sleep(0.1)
    # Start the worker thread
    #
    @thread = RoverThread.new do
      while (true) do
	log_data if read_data
      end
    end
  end
  #####


  #####
  # Return latest raw data
  #
  def latest_data(acm_data=nil)
    acm_data = AcmData.new("","","","","","","","","") unless acm_data
    acm_data.time = @data.time
    acm_data.date = @data.date
    acm_data.head = @data.head
    acm_data.volt = @data.volt
    acm_data.x    = @data.x
    acm_data.y    = @data.y
    acm_data.north= @data.north
    acm_data.east = @data.east
    acm_data.temp = @data.temp
    return acm_data
  end
  #####
  

  ##
  # Stop the worker thread and stop the acm
  #
  def stop
    @thread.kill if (@thread && @thread.alive?)
    @thread = nil
    @dev.write("S\r") if (@dev)
    @dev.purge if (@dev)
  end
  #####


  ##
  # Read data record from the device
  #
  def read_data
    #raw = "09:59:15 2008/10/28 102.32 9.16 -0.47 3.32 -11.47 -3.74 3.93";sleep(1)
    raw = @dev.gets
    syslog(raw.strip)
    
    rawdata = raw.split
 
    if (rawdata.size == 9)
      @data.time = rawdata[0].gsub(",", "")
      @data.date = rawdata[1].gsub(",", "") #.gsub!("-",":")
      @data.head = rawdata[2].to_f
      @data.volt = rawdata[3].to_f
      @data.x    = rawdata[4].to_f
      @data.y    = rawdata[5].to_f
      @data.north= rawdata[6].to_f
      @data.east = rawdata[7].to_f
      @data.temp = rawdata[8].to_f

      # Run calculations on raw data
      #
      calc_data

      return true
    else
      return false
    end
  end

  ##
  # calc_data: Use latest data record in calculating headings & magnitude
  #
  def calc_data
    cavg = calc_current_direction(@data.east, @data.north)
    #syslog("_ calc_data: current_avg(#{cavg}) heading_avg(#{@data.head})")

    cavg = @current_averager.avg(cavg)
    vavg = @vehicle_averager.avg(@data.head)
    syslog("_ current_avg = #{cavg} heading_avg = (#{vavg})")

    @mags.push(Math.sqrt((@data.east*@data.east)+(@data.north*@data.north)))
    @mags.shift if (@mags.length > 10)
  end

  ##
  # Return a heading (0-359.9999) given easting and northing
  #
  def calc_current_direction(p_easting, p_northing)
    val = Math.atan2(p_easting, p_northing) * 180.0 / Math::PI
    if (val < 0.0)
      val += 360
    elsif (val > 360.0)
      val -= 360
    end
    return val  
  end

  ##
  # Calculate average of magnitudes
  #
  def current_magnitude
    return 0.0 unless (@mags.length > 0)

    mag = 0.0
    @mags.each do |m|
      mag += m
    end
    return mag/@mags.length

  end

  ##
  # Return vehicle heading
  #
  def vehicle_heading
    return @vehicle_averager.avg
  end

  ##
  # Return vehicle heading
  #
  def current_heading
    return @current_averager.avg
  end


  ##
  # Prompt and read data record from the device
  #
  def acquire_data
    @dev.write("\r") if (@dev)
    return read_data
  end


  ##
  # Log data record
  #
  def log_data
    @rawlog.write("#{@data.date} #{@data.time}", @data.head, @data.volt,
                  @data.x, @data.y, @data.north, @data.east, @data.temp)
    @calclog.write(@data.head, @data.north, @data.east, self.current_magnitude,
                   @vehicle_averager.avg, @current_averager.avg)
  end

end
##########


##########
class SimAcm < Acm

  def initialize(p_portname="/dev/acm")
    # Basic stuff
    #
    @thread = nil
    @name = File.basename(p_portname)

    # Data and calculators
    #
    @data = AcmData.new("","","","","","","","","")
    @vehicle_averager = RunAvg.new(10)
    @current_averager   = RunAvg.new(10)
    @mags = []
  end
  #####

  ##
  # Read data record from the device
  #
  def read_data
    rawdata = "09:59:15 2008/10/28 102.32 9.16 -0.47 3.32 -11.47 -3.74 3.93"
    rawdata = rawdata.split
    sleep(0.66)

    @data.time = rawdata[0]
    @data.date = rawdata[1].gsub!("/","-")
    @data.head = rawdata[2].to_f
    @data.volt = rawdata[3].to_f
    @data.x    = rawdata[4].to_f
    @data.y    = rawdata[5].to_f
    @data.north= rawdata[6].to_f
    @data.east = rawdata[7].to_f
    @data.temp = rawdata[8].to_f

    # Run calculations on raw data
    #
    calc_data

    return true
  end

end
##########


##########
# AcmRawLog specialized versions of DataLog class.
# Contains raw data from the ACM.
#
class AcmRawLog < DataLog

  # Column headings for the log
  #
  @@cols = [
    "ACM Timestamp",
    "Vehicle Heading (degrees)",
    "Voltage (V)",
    "Tilt X (degrees)",
    "Tilt Y (degrees)",
    "Northing (cm/sec)",
    "Easting (cm/sec)",
    "Temp (C)"
  ]

  def AcmRawLog.items
    @@cols
  end

  def initialize(p_filename)
    super(p_filename)
    write_heading if @need_heading
  end

  def write_heading
    @file.write("Rover Timestamp (ISO601),#{@@cols.join(',')}\n")
    @file.flush
  end
end
##########


##########
# AcmCalcLog specialized versions of DataLog class.
# Contains calculated data from the ACM.
#
class AcmCalcLog < DataLog

  # Column headings for the log
  #
  @@cols = [
    "Vehicle Heading (degrees)",
    "Northing (cm/sec)",
    "Easting (cm/sec)",
    "Current Magnitude (cm/sec)",
    "Avg Vehicle Heading (degrees)",
    "Avg Current Heading (degrees)"
  ]

  def AcmCalcLog.items
    @@cols
  end

  def initialize(p_filename)
    super(p_filename)
    write_heading if @need_heading
  end

  def write_heading
    @file.write("Rover Timestamp (ISO601),#{@@cols.join(',')}\n")
    @file.flush
  end
end
##########


##
# Standalone unit test (recommended)
# Include at the end of the file as a hook to run a unit test of
# the code from the command line (e.g., "$ ruby  my_class.rb")
#
if __FILE__ == $0
  # Test code here
end