=begin
 ******************************************************************************
 * Copyright 1990-2010 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Provide control of the All Motion EZ17 servo motor controller
            : over a RS-485 half duplex mult-drop bus.
            : The OEM communication protocol is the only protocol supported at
            : this time. It has the same functionality as the DT protocol, but
            : supports checksums as well.
 * Filename : ezstepper.rb
 * Author   : Henthorn
 * Project  : SES
 * Version  : 
 * Created  : June 10  Initial functionality
 ******************************************************************************
=end

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

require 'rubygems'
require 'serialport'
require 'utils/misc'
require 'utils/rover_thread'
require 'utils/datalog'
require 'monitor'

$CMD_TIMEOUT = 5.0

$HOME_POS = 0
$COLLECTION_POS = 6720
$FLUORO_POS = 36000
$CAMERA_POS = 70575
$CLEAR_POS = 134500

###################### Contructors and Set-up ####################################
#
# Ezstepper class provides interface to All Motion EZStepper controller
#
class Ezstepper
  include SyslogWriter

  attr_reader :thread, :response_packets, :serial_port
  attr_accessor :active_motor_num, :log_moves, :stop_wait, :position_interval, \
  :callback_method

  @@cr = 0xD #ascii carriage return
  
  #this hash maps the motor banks to the actual motors each bank controls
  #note that only motor 1 is mapped to the global address "_"
  #
  @@motor_banks = {"A" => [1,2], "C" => [3,4], "E" => [5,6], "G" => [7,8], \
                  "I" => [9,':'], "K" => [';','<'], "M" => ['=','>'], \
                  "O" => ['?','@'], "Q" => [1,2,3,4], "U" => [5,6,7,8], \
                  "Y" => ['9',':',';','<'], "]" => ['=','>','?','@'], \
                  "_" => [1] }
                  
  MOTOR_NUM = "1"

  # Initiale takes SerialPort config object and ezstepper serial port name
  # Typical usage: ez = Ezstepper.new(nil)
  #
  def initialize(portname='/dev/ezstepper') 

    #config = SerialPort::Configuration.default
    #SerialPort has default baud of 38.4k, let's change it to 9600
    #config.baud = 9600
    # @serial_config = config
    @portname = portname

    #@serial_port = Posix::SerialPort.new(@portname, @serial_config)
    @serial_port = SerialPort.new(@portname)
    @response_packets = Array.new
    @position_interval = @counts_per_tooth
    @callback_method = nil
    @counts_per_tooth = 960

    #the default motor number
    self.active_motor_num = MOTOR_NUM
    #self.encoder_position = 2000
    self.log_moves = false
    self.stop_wait = false
    #self.encoder_position
    @seektest = false
  end

  # Helper initializer for Rover configuration
  #  
  def Ezstepper.ses
    ez = Ezstepper.new '/dev/ezstepper'

    ez
  end 

  def reinit
    syslog("ezstepper reinit")
    kill_listener
    active_motor_num = MOTOR_NUM
    #`stty -F /dev/ezstepper min 1 time 0 icrnl -icanon opost`
    start_listener
    sleep(0.5)
    velocity=1500
    sleep(0.5)
    self.acceleration=1
    sleep(0.5)
    self.current_limit=70
    sleep(0.5)
    send_message("f1R")
    sleep(0.5)
  end

  def port_reset
    self.close
    # @serial_port = Posix::SerialPort.new(@portname, @serial_config)
    @serial_port = SerialPort.new(@portname)
    `stty -F #{@portname} min 1 time 0`
    self.start_listener
  end
        
###################### Operational Commands ####################################
#
# commands:
#   stop_command -- stops the currently executing string of commands sent to the
#                   servo, but does not stop the command actually executing
#   halt, stop -- aliases for stop_command
#   reset! -- Resets the servo, returning it to the same state as on power-up 
#   move_abs [int count] -- Moves the motor to the specified encoder count
#   forward [int count] -- Moves the motor forward "count" encoder counts
#   back [int count] -- Moves the motor backward "count" encoder counts
#   recover -- Recover Servo, turns the servo on or off to current encoder
#              position. Useful in the case where an overload error has occurred.
#              Typically this command is used in a recovery script triggered by
#              the n512 mode.
# read only values:
#   current_status? -- Retrieves the current error message
#   commanded_position? -- Retrieves the position the servo has been commanded to 
#   message -- The last message sent back from the servo
#   error_mesage -- Error from the last response from the servo
# read/write values:
#   encoder_position -- Setting this value will change the encoder position 
#     without moving the motor
#   proportional_gain, integral_gain, differential_gain -- PID Loop variables
#   velocity
# write only values:
#   current [0-100 percent] -- Sets the max current allowed during a move
#   acceleration [0-65000] -- Sets acceleration in (encoder ticks/32.768) per
#              second squared
#   overload_retry_count [1-65000] -- Only executes when n512 and n1024 modes
#              are activated, after exceeding the retry count stored program
#              12 is executed
#   overload_timeout [1-25000 milliseconds] -- Time to wait before shutting down
#              when an overload condition has been detected.
#
################################################################################

  # Stop hogging the port and close it. Used frequently during missions that
  # must cycle power on the controllers.
  #
  def close
    syslog("_ killing listener and closing serial port")
    kill_listener
    self.serial_port.close unless self.serial_port.closed?
  end
  
  #returns the ready/busy status as well as any error conditions
  def move_abs(steps)
    return unless steps
    syslog("_ moving to #{steps} steps")

    send_message "A" << steps.to_s << "R"
    
    return wait_for_move_to_complete
  end

  # enable callback during a move
  # 1st parameter is a reference to an object method,
  # the 2nd is the position interval
  # The default is to call the callback method every so number of steps
  #
  def set_move_callback(callback, every=@steps_per_call)
    if (callback)
      @callback_method = callback
      @position_interval = every if (every && every > 0)
      syslog("_ will call #{@callback_method} every #{@position_interval} steps")
    end
  end

  def reset_move_callback
    @callback_method = nil
    @position_interval = @counts_per_meter
  end

  #a blocking move command that will block until the move is complete
  def forward(steps)
    return false unless steps
  
    syslog("_ moving #{steps} steps")

    send_message "P" << steps.to_i.to_s << "R"
    
    return wait_for_move_to_complete
  end
  
  #a blocking move command that will block until the move is complete
  def back(amount)
    return unless amount
    send_message "D" << amount.to_s << "R"
    wait_for_move_to_complete
  end
  
  # Query controller and return status
  #
  def current_status?
    packets = do_query("Q")
    
    return unless packets 
    packets.each do |packet|
      packet.error_message
    end
  end
  
  def wait_for_move_to_complete(sleep_interval = 1.5)
    self.stop_wait = false
    syslog("_ initializing wait_for_move_to_complete")
    
    sleep(1)
    positions = self.encoder_position
    sleep(4)
    new_positions = self.encoder_position
    
    update = "position: "
    start_pos = 0
    new_positions.each do |pos|
      update << pos.to_s << ", "
      start_pos = pos
    end

    # Keep checking until the positions are equal or until we abort out of
    # wait cycle.
    #
    while (positions != new_positions && !self.stop_wait) do
      sleep(sleep_interval)
      
# Logic to force an overload error for testing purposes
#      if ( (new_positions[0] > 300 && new_positions[0] < 600) ||
#           (new_positions[1] > 300 && new_positions[1] < 600) ||
#           (self.has_error && ("overload error" == self.error_message)) )

      # Flag and handle error conditions
      #
      if ( (self.has_error && ("overload error" == self.error_message)) )
        syslog("! error overload detected while waiting for move to complete")
        self.halt    # Perhaps the decision to halt should be up to the caller?
        return false
      end

      positions = new_positions
      new_positions = self.encoder_position
      update = "motors: #{self.active_motor_num} positions: "
      new_pos = start_pos
      new_positions.each do |pos|
        update << pos.to_s << ", "
      	new_pos = pos
      end
      syslog("_ new: " + update)

      # Trigger the callback?
      if (@callback_method && ((new_pos - start_pos) > @position_interval))
        start_pos = new_pos - 50
      	@callback_method.call
      end
    end
    
    if (self.stop_wait)
      syslog("_ wait was stopped")
    else
      syslog("_ positions are equal")
    end
    
    reset_move_callback()

    # Flag error condition
    if self.has_error
      syslog("! error detected while waiting for move to complete")
      syslog("! error: #{self.error_message}")
      return false
    end

    self.stop_wait = false
    syslog("_ move has completed")
    
    return true
  end
  
  # Keep checking motor positions until two consecutive values are equal, BUT
  # only wait for so long before halting the move.
  #
  def wait_for_move_with_timeout(timeout = 180)
    t = RoverThread.new do
      Thread.current["name"] = "ezstepper_#{@serial_port.name} wait_for_move"
      status = wait_for_move_to_complete
    end
    run_time = 0
    while(t.alive? && run_time < timeout)
      run_time += 3
      sleep(3)
    end
    if(t.alive?)
      self.stop_wait = true 
      sleep 3
      if(t.alive?)
        t.kill
      end
      self.stop_wait = false
      self.active_motor_num = MOTOR_NUM
      self.halt
    end
  end
  
  # Method used to compensate for an overload error before attempting
  # another move
  #
  def adjust_for_overload
    # Cut the velocity in half and try again
    #
    low = 1000      # Maximum will be 1000 before halving it
    vel = self.velocity
    
    # Find motor with lowest velocity setting
    vel.each do |v|
      low = v if v < low
    end
    
    self.velocity=low/2
    return
  end

  # Go to the Home position
  #
  def go_home
    # Go fast until we find the home switch
    #
    send_message("gP2500S03GD2500R")
    wait_for_move_to_complete

    # Now slower
    #
    send_message("gP10S03GR")
    return wait_for_move_to_complete
  end


  # Set the current position to zero
  #
  def reset_position
    self.send_message("z0R")
  end
  
  # Save the position for startup
  #
  def save_position_for_startup(pos=nil)
    pos = encoder_position[0] unless (pos)

    self.send_message("s0z#{pos.to_i}R")
  end
  
  # Return last controller commanded position
  #
  def commanded_position?
    numeric_response_query("?0")
  end
  
  # Return current motor position in counts
  #
  def encoder_position
    numeric_response_query("?8")
  end
  
  # change position without moving (sets position to given value)
  #
  def encoder_position=(new_position)
    return unless new_position
    send_message "z" << new_position.to_s << "R"
  end
  
  # Return controller proportional gain
  def proportional_gain
    numeric_response_query("?w")
  end

  # Set controller proportional gain
  def proportional_gain=(new_gain)
    return unless new_gain
    send_message "w" << new_gain.to_s << "R"
  end
 
  # Return controller integral gain
  def integral_gain
    numeric_response_query("?x")
  end

  # Set controller integral gain
  def integral_gain=(new_gain)
    return unless new_gain
    send_message "x" << new_gain.to_s << "R"
  end

  # Return controller differential gain
  def differential_gain
    numeric_response_query("?y")
  end

  # Return controller differential gain
  def differential_gain=(new_gain)
    return unless new_gain
    send_message "y" << new_gain.to_s << "R"
  end

  # Set the motor velocity
  def velocity=(new_velocity)
    return unless new_velocity
    syslog("_ setting velocity to #{new_velocity}")
    send_message "V" << new_velocity.to_s << "R"
  end

  # Return motor velocity
  def velocity?
    numeric_response_query("?2")
  end

  # Set motor acceleration
  def acceleration=(new_acc)
    return unless new_acc
    send_message "L" << new_acc.to_s << "R"
  end

  # Set motor current limit
  def current_limit=(new_current)
    return unless new_current
    send_message "m" << new_current.to_s << "R"
    syslog("_ motor current set to #{new_current}")
  end
  
  #stop the command currently executing. This stop request is likely
  #to be ignored...
  def stop_command
    old_active = self.active_motor_num
    self.active_motor_num = MOTOR_NUM
    send_message("T")
    self.active_motor_num = old_active
    self.stop_wait = true
  end

  alias halt stop_command
  alias stop stop_command 

  #get the most recent error message
  def error_message
    return unless @response_packets
    @response_packets[-1].error_message
  end

  # Return true if last packet contains error code  
  def has_error
    return unless @response_packets
    if @response_packets[-1]
      return @response_packets[-1].error_code != 0
    end
  end

  #get the message segment of the last response_packet
  def message
    return unless @response_packets
    @response_packets[-1].message
  end

########################## Supporting Commands ################################
#

  # Read response from motor controller, place in packet object
  # warning, this method uses a blocking call...
  #
  def get_response_packet
      collector = ""

      #use this variable to track when a response is coming across the wire
      @receiving_response = false
      while true
        c = @serial_port.getc  # Blocking call
        unless (@receiving_response)
          @receiving_response = true
          #syslog("listener receiving response...")
        end

        # syslog("listener char #{c[0].bytes.to_a[0]}")
        collector << c
        
        if collector[-1].bytes.to_a[0] == 3
          #remove the RS485 line turn around character, this character
          #should be 255, but that could be corrupted due to line turn around
          #transients, so just test that the first character isn't the 'start'
          #character
          if collector[0].bytes.to_a[0] != 2
            collector = collector[1..-1]
          end
          if collector[0].bytes.to_a[0] != 2
            syslog("! WARNING: start character not seen at beginning of response")
          end

          # grab the checksum and verify
          # 
          checksum = @serial_port.getc
          #if Ezstepper.compute_checksum(collector) != checksum
          #  syslog("! WARNING: Checksums do not match...")
          #end

          packet = ResponsePacket.new(collector)
          return packet
        end
      end
  end

  # Start thread to listen to the ezstepper port for responses to commands
  # and error conditions
  #
  def start_listener
    return if (@thread && @thread.alive?)
    syslog("_ starting listener thread")
    #as far as i can tell, if a thread is launched from an
    #initialize method, the initialize method will not continue
    #if the thread makes a blocking call. For instance, it will
    #not continue if the thread calls sleep until the sleep
    #is done. calling Thread.pass seems to bypass this problem
    @thread = RoverThread.new do
      Thread.current["name"] = "ezstepper listener"
      Thread.pass
      while true
        packet = get_response_packet
        unless packet.error_code == 0
          syslog("! ERROR returned from servo: #{packet.error_message}")
        end
        @response_packets.push packet
        syslog("_ new response : [#{packet}]")
        sleep(0.5)
      end
    end
    syslog("_ thread created")
  end

  def kill_listener
    @thread.kill if @thread
    @thread = nil
  end

  #format a message to the servo using the (more complex) OEM
  #protocol of the EZ Servo.
  #start character is Ctrl-B, end character is Ctrl-C
  #See appendix 4 (page 21) of the EZ Servo Communications Protocol Document
  #for a full description of the OEM protocol.
  #
  OEM_START_CHAR = 0x2
  OEM_END_CHAR = 0x3
  def send_message(message)

    # Header info first
    #
    command = ""
    command << OEM_START_CHAR 
    command << @active_motor_num.to_s	 #message address
    command << "1"			               #sequence character
    
    # Message data next
    #
    command << message
    command << OEM_END_CHAR
    
    # Conclude with checksum
    #     
    checksum = Ezstepper.compute_checksum(command)
    command << checksum
    
    #syslog("_ command #{message} sending [#{command}]")
    num_responses = @response_packets.size
    
    # Look for and handle "Illegal seek" exception
    #
    max_tries = 3
    for iw in 1..max_tries do
      begin
        #@serial_port.purge
        @serial_port.flush_input
        @serial_port.seek(-1) if @seektest
        @serial_port.write(command)
        break    # worked fine, break out of loop
      rescue Exception
        @seektest = false
  	    syslog("! #{$!} on #{iw} of 2")
  	    if (iw < max_tries)
  	      self.port_reset
          next
        else
          raise #{$!}
        end
      end
      
    end
    new_num_responses = @response_packets.size

    #sleep a bit so that another command doesn't come along
    #after this one and collide with it.
    sleep(0.5)
    if @active_motor_num.to_s != "A"
      sleeptimer = $CMD_TIMEOUT
      while new_num_responses == num_responses && sleeptimer > 0
        sleep 0.5
        sleeptimer -= 0.5 
        new_num_responses = @response_packets.size
      end
      if(sleeptimer <= 0)
        syslog("! timed out waiting for command response")
      end
    end
    command
  end

  # Compute checksum of given string
  #
  def Ezstepper.compute_checksum(string)
    checksum = 0
    return checksum unless string 
    string.each_byte do |byte|
      checksum = checksum ^ byte
    end
    checksum
  end


  #Returns the most recent response packet after sending the query to the
  #motor
  #
  #This method assumes a single threaded mode of operation, with the only
  #other thread operating on the serial port being the listener thread.
  #
  def do_query(query_string)
    responses = @response_packets.size
    unless thread && thread.alive?
      syslog("_ listener thread not currently active. Starting listener...")
      self.start_listener
    end
    #spin the wheels while the line clears
    count = 0
    while @receiving_response && count < 10 do
      sleep(0.2)
      count += 1
    end
    @receiving_response = false
    response_counter = responses
    originally_active_motor = self.active_motor_num
    motors_to_query = @@motor_banks[self.active_motor_num] ||= [self.active_motor_num]
    motors_to_query.each do |motor|
      self.active_motor_num = motor
      syslog("_ query sent to motor #{motor}")
      send_message(query_string)
      #wait until the new response has been added to the stack
      timeoutCounter = $CMD_TIMEOUT
      while response_counter == @response_packets.size do
        sleep(0.5)
        timeoutCounter -= 0.5
        if(timeoutCounter <= 0)
          syslog("! timed out waiting for query response")
          break
        end
      end
      response_counter += 1 
    end
    self.active_motor_num = originally_active_motor

    #since the last time the arrays size was queried.
    @response_packets.slice(responses..-1)
    
  end

  # Check return value against nil unless yuo're passing in a
  # string literal (i.e., "Q")
  #
  def numeric_response_query(message)
    return nil unless message
    packets = do_query(message)
    packets.collect! {|x| x.message.to_i }
    packets
  end
end

################################################################################
# EZ Servos respond to commands by sending messages addressed to the master
# device (\0), following the device address is a "status character" which is
# a byte that encodes various information in each of it's bytes. the bits are:
# 7 - reserved
# 6 - always set
# 5 - ready bit -- Set wehn EZ Servo is ready to accept a command
# 4 - reserved
# 3-0 combined to form an error code from 0 to 15
# The error codes are as follows:
# 0 = no error                  1 = InitError   2 = Bad Command (illegal command)
# 3 = Bad Operand (range error) 4 = n/a         5 = Communications Error (internal)
# 6 = n/a                       7 = Not Inited  8 = n/a
# 9 = Overload Error (physical system could not keep up with command position)
# 10= n/a                       11= Move not allowed
# 12-14 = n/a                   15= Command Overflow
#
# For more detailed documentation see appendix 7 of the ez servo command set
################################################################################
class ResponsePacket
  ERROR_NAMES = ["no error", "init error", "bad command", "bad operand", "n/a", \
                 "communications error", "n/a", "not initialzed", "n/a", \
                 "overload error", "n/a", "move not allowed", "n/a", "n/a", "n/a", \
                 "command overflow"]

  def initialize response_packet_string
    @response_packet_string = response_packet_string
  end

  def to_s
    "readybit = " + is_ready_bit_set?.to_s + " error message = " + error_message
  end  
    
  def status_character
    status_regex = Regexp.new("" << Ezstepper::OEM_START_CHAR << "0(.).*")
    matchs = status_regex.match @response_packet_string
    #the first index into match gets the regex () match, the second index
    #takes the first (and only) character's byte value from the string
    if matchs
      #result = matchs.bytes.to_a[0] || 5
      result = matchs[1][0] || 5
    else
      result = 5
      t = Time.now
      t.gmtime
      msg = "_ ezstepper::status_character regex match was nil"
      puts t.strftime("%Y%m%d%H%M%S , ") << msg 
    end
    return result
  end

  def is_ready_bit_set? 
    # return true
    if self.status_character.bytes.to_a[0] & 2**5 != 0
      true
    else
      false
    end
  end

  def message
    return nil unless @response_packet_string
    #the . after the 0 is the status_character
    message_regex = Regexp.new("" << Ezstepper::OEM_START_CHAR << "0.(.*)" << \
                               Ezstepper::OEM_END_CHAR)
    match = message_regex.match @response_packet_string
    match[1]
  end

  def error_code
    return nil unless self.status_character
    self.status_character.bytes.to_a[0] & 15
  end

  def error_message 
    ERROR_NAMES[self.error_code]
  end
end


# Class SimEzstepper provides a simple simulated Ezstepper interface for
# mission simulations.
#
class SimEzstepper
  include RoverTime
  include SyslogWriter

  attr_accessor :active_motor_num, :log_moves, :stop_wait, :velocity
  
  def initialize(portname='/dev/ezstepper') 
    syslog("_ initializer called with portname = #{portname}")
    @position_interval = 850
    @encoderPos = [0, 0]  
    @active_motor_num = "A"
  end  

  def SimEzstepper.rover
    ez = SimEzstepper.new nil
    ez
  end 

  def set_move_callback(callback, every=@counts_per_meter)
    if (callback)
      @callback_method = callback
      @position_interval = every if (every && every > 0)
      syslog("_ will call #{@callback_method} every #{@position_interval} counts")
    end
  end

  def encoder_position
    if(self.active_motor_num == "A")
      return @encoderPos
    elsif(self.active_motor_num == "1")
      return @encoderPos[0]
    elsif(self.active_motor_num == "2")
      return @encoderPos[1]
    else
      raise "Unknown active motor number"
    end
  end
  
  def encoder_position= new_position
    if(self.active_motor_num == "A")
      unless(new_position.is_a?(Array))
        raise("encoder position should be an array!")
      end
      @encoderPos = new_position
    elsif(self.active_motor_num == "1")
      @encoderPos[0] = new_position
    elsif(self.active_motor_num == "2")
      @encoderPos[1] = new_position
    else
      raise "Unknown active motor number"
    end
  end

  def forward(encoderCounts)
    # Rover motors move about 30 counts per second, so sleep
    # time is adjusted accordingly.
    #
    if (encoderCounts < 0)
      syslog("! forward method not effective unless encoderCounts > 0")
      return
    end
    
    st = encoderCounts/15
    syslog("_ Sim Ezstepper sleeping #{st} seconds to simulate forward move")
    r_sleep(st)
    if(self.active_motor_num == MOTOR_NUM)
      self.encoder_position = [encoderCounts, encoderCounts]
    else
      self.encoder_position = encoderCounts
    end

    @callback_method.call if @callback_method

  end

  def stop_command
    syslog("_ SimEzstepper received stop command.")  
  end

  def close
    syslog("_ closing serial port")  
  end

  def proportional_gain= new_gain
    return "proportional"
  end

  def integral_gain= new_gain
    return "integral"
  end

end