=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Benthic resp chamber rack assembly simplified interface
 * Filename : new_rack.rb
 * Author   : Henthorn
 * Project  : Benthic Rover
 * Version  : 2
 * Created  : Jul-09  Initial functionality
 ******************************************************************************
=end

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

require 'posix/serialport'
require 'utils/misc'
require 'utils/rover_thread'
require 'utils/datalog'
require 'device/rover'

# Motor designations used in commands match
# ezservo active_motor_num designations
#
Stbd = "1"
Port = "2"

$STBD_RACK = "1"
$PORT_RACK = "2"
$INSERT_SPEED = "3000"
$HOME_SPEED = "2300"
$CURR_LIMIT = "50"
$COUNTS_PER_METER = 855
$CMD_TIMEOUT = 5
$HOME_CMD = "Z20000R"
$INSERT_CMD = "gP100S11e2G0R"
$PORT_INSERT_POS = 11100
  
# Abstract representation of Rover rack controller
#
class Rack
  include SyslogWriter
  
  attr_reader :dev
  
  NUM_RACK_RETRIES = 3    # number of attempts to make at performing a rack
                          # insertion or home

  # Standard initializer
  #
  def initialize(portname='/dev/rack') 
    cfg = SerialPort::Configuration.default
    cfg.baud = 9600
    @portname = portname
    @dev = Posix::SerialPort.new(@portname, cfg)
  end

  # Initializer used to create rack object in main rover object.
  # Do not assume that rack controller has its' power on 
  #
  def Rack.rover
    r = Rack.new('/dev/rack')
    `stty -F /dev/rack min 1 time 0`
    r
  end 


  # Initialize/Reinitialize controller. Must pass in which rack to
  # operate on.
  #
  def reinit(rack_id)
    return nil unless rack_id == $STBD_RACK || rack_id == $PORT_RACK

    # Cycle power to the rack controller before setting controller
    # parameters. Delays are used to allow controller to initialize
    # communications.
    #
    if (rack_id == $STBD_RACK)
      Rover.instance.relay.s_rackOff
      sleep(0.5)
      Rover.instance.relay.s_rackOn
    else
      Rover.instance.relay.p_rackOff
      sleep(0.5)
      Rover.instance.relay.p_rackOn
    end
    sleep(0.5)

    # Set H-E limit switches (for chamber homing and insertion).
    # first send a message to flush the buffer
    # because there could be junk in it if the rack was not
    # on when this object was first initialized
    #
    send_message(rack_id, "badR")
    sleep(0.5)
    send_message(rack_id, "f1R")   # set home flag and limit polarity
    sleep(0.5)
    send_message(rack_id, "n2R")   # enable limit switches
    sleep(0.5)
    send_message(rack_id, "f1R")   # set home flag and limit polarity, again (first
                          # time the controller responds with "bad operand")
    sleep(0.5)
    send_message(rack_id, "s2R")   # store null program for use in finding insert switch
    sleep(0.5)
    send_message(rack_id, "V3000R")
    sleep(0.5)
  end

  # Set motor speed of a rack motor
  #
  def set_rack_speed(rack_id, counts_per_sec)
    msg = "V" + counts_per_sec.to_s + "R"
    syslog("_ Setting rack speed: #{msg}")
    send_message(rack_id, msg)
  end
  
  # Set motor current limit
  # Percentage 1-100
  #
  def set_rack_current_limit(rack_id, percentage)
    msg = "m" + percentage.to_s + "R"
    syslog("_ Setting rack current limit: #{msg}")
    send_message(rack_id, msg)
  end
  
  #####
  # Usage: home
  # Tell the controller to home a chamber using the home limit switch
  #
  def home(rack_id)
    return false unless set_rack_current_limit(rack_id, "99")
    return false unless set_rack_speed(rack_id, $HOME_SPEED)
    send_message(rack_id, $HOME_CMD)

    # Wait for 3 minutes then check the home switch
    #
    syslog("_ Executing home, sleep for 3 minutes without monitoring controller")
    sleep(3*60)
    self.halt
    if (home_switch(rack_id))
      syslog("_ Controller says home is done")
      return true
    else
      syslog("_ Controller says home failed")
      return false
    end
  end
  #####
 
  def insert_port_rack
    unless (home_switch($PORT_RACK))
      return false unless (home($PORT_RACK))
    end
    
    send_message("P#{$PORT_INSERT_POS}")
    sleep(3*60)
  end
  
  # Usage: insert_chamber($STBD_RACK)
  # Tell the controller to insert the chamber using the insertion limit switch
  #
  def insert_chamber(rack_id)
    return false unless set_rack_current_limit(rack_id, $CURR_LIMIT)
    return false unless set_rack_speed(rack_id, $INSERT_SPEED)
    
    if (rack_id == $PORT_RACK)
      return insert_port_rack
    else
      send_message(rack_id, $INSERT_CMD)
    end

    # Wait for 3 minutes then check the home switch
    #
    syslog("_ Executing stbd rack hack, sleep for 3 minutes without monitoring controller")
    sleep(3*60)
    self.halt
    if (insert_switch(rack_id))
      syslog("_ Controller says insert is done")
      return true
    else
      syslog("_ Controller says insert failed")
      return false
    end
  end

  # Home the rack using the generalized move method
  # allowing for multiple attempts.
  #
  def move_home(rack_id, retry_count = NUM_RACK_RETRIES)
    return true if (home_query(rack_id))

    for i in 1..retry_count
      syslog("_ Attempt \##{i} to move rack by :home (#{rack_id})")
      move_rack(rack_id, :home)

      return true if home_switch(rack_id)

      # Re-init rack and try again
      self.reinit(rack_id)
    end

    return false
  end
  
  # Insert the rack using the generalized move method
  # allowing for multiple attempts.
  # 
  def move_to_insert(rack_id, retry_count = NUM_RACK_RETRIES)
    insert_query(rack_id)

    for i in 1..retry_count
      syslog("_ Attempt \##{i} to move rack by :insert_chamber (#{rack_id})")
      move_rack(rack_id, :insert_chamber)

      return true if insert_switch(rack_id)

      # Re-init rack and try again
      self.reinit(rack_id)
    end

    return false
  end
  
  # Query the insert switch for the rack.
  # Return true if switch is activated.
  #
  def insert_switch(rack)
    nval = switch_query(rack, 4)
    return false unless (nval)

    val = (nval < 2000)? true : false
    syslog("_ insert_switch? #{val}")
    return val
  end

  # Query the home switch for the rack.
  # Return true if switch is activated.
  #
  def home_switch(rack)
    nval = switch_query(rack, 2)
    return false unless (nval)

    val = (nval < 2000)? true : false
    syslog("_ home_switch? #{val}")
    return val
  end

  # Query the lower limit switch for the rack.
  # Return true if switch is activated.
  #
  def low_switch(rack)
    nval = switch_query(rack, 1)
    return false unless (nval)

    val = (nval < 2000)? true : false
    syslog("_ low_switch? #{val}")
    return val
  end

  # Query the controller switches, return the value
  # of the requested switch (1st one in the string
  # corresponds to the switch #4 - it's backward)
  #
  def switch_query(rack_id, switch)
    ret = nil
    if (switch < 1 || switch > 4)
      syslog("! Invalid switch passed to switch_query (#{switch}")
      return ret
    end
 
    # Send the command to the controller and get the response
    # 
    max_tries = 3
    for iw in 1..max_tries do
      #@dev.purge
      query = "/#{rack_id}?aa"
      @dev.write(query+"\r")
      resp = @dev.gets
      syslog("_ switch_query(#{query}) returned \"#{resp}\"")

      # The response should contain 4 values seperated by commas,
      # so 3 commas 
      if (resp && (resp.count(",") == 3))
        resp = resp[4..resp.size]       # strip off the control characters
        resp_ary = resp.split(",")      # split the values into an array
        if (4 == resp_ary.size)         # there better be 4 elements
          switch -= 1
          ret = resp_ary[switch] # get the requested element
        end
      end

      if (0 == ret.to_i)
        syslog("! switch value not an integer? (#{ret})")
        next
      else
        ret = ret.to_i
      end
    
      return ret
    end
    
  end

  def send_message(rack_id, msg)
    @dev.purge
    cmd = "/#{rack_id}#{msg}"
    @dev.write(cmd+"\r")
    resp = @dev.gets
    syslog("_ send_message(#{cmd}) returned \"#{resp}\"")
  end
end


class SimRack
  def initialize(portname='/dev/rack')
    syslog("_ intializing sim rack to portname #{portname}")
  end
  
  def SimRack.rover
    r = SimRack.new('/dev/rack')
  end 

  def reinit(rack_id)
    if (set_active_rack(rack_id))
      syslog("_ rack #{rack_id} re-inited")
    else
      syslog("_ rack #{rack_id} NOT re-inited")
    end
    return "sim command"
  end
  
  def home(rack_id)
    syslog("_ sending rack #{rack_id} home")
    sleep(2)
    true
  end

  def insert_chamber(rack_id)
    syslog("_ inserting rack #{rack_id} chamber")
    sleep(2)
    true
  end

  def move_to_insert(rack_id, retry_count = 1)
    sleep(2)
    syslog("_ Rack moved by move_to_insert(#{rack_id}) after 1 attempts")
    true
  end

  def move_home(rack_id, retry_count = 1)
    sleep(2)
    syslog("_ Rack moved by move_home(#{rack_id}) after 1 attempts")
    true
  end

  def set_rack_speed(rack_id, counts_per_sec)
    syslog("_ Setting rack speed to #{counts_per_sec}")
    return "sim command"
  end

  def home_switch(rack)
    return true
  end
  
  def low_switch(rack)
    return true
  end
  
  def insert_switch(rack)
    return true
  end
  
  def encoder_position
    return 2000
  end
end