#--
#******************************************************************************
#* Copyright 1990-2007 MBARI
#* MBARI Proprietary Information. All rights reserved.
#******************************************************************************
#* Summary  : Provide control of the All Motion EZSV23 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 : ezservo.rb
#* Author   : A. Chase
#* Project  : Benthic Rover
#* Version  : 
#* Created  : May-06  Initial functionality
#* Modified : Numerous, stablized Mar-07
#******************************************************************************
#++

require 'rover_environment'
require 'posix/serialport'
require 'rover_utils'
require 'rover_thread'
require 'monitor'
require 'configman'

# Ezservo class provides interface to All Motion EZServo controller
#
class Ezservo < Monitor
  include LogHelper
  attr_reader :thread, :response_packets, :serial_port
  attr_accessor :active_motor_num, :log_moves, :stop_wait
  @@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] }
                  
  #NOTE: This should probably come from a config file
  STARBOARD_MOTOR_NUM = 2
  PORT_MOTOR_NUM = 1

  # Initiale takes SerialPort config object and ezservo serial port name
  # Typical usage: ez = Ezservo.new(nil)
  #
  def initialize(config, portname='/dev/ezservo') 
    super()
    unless config
      config = SerialPort::Configuration.default
      #SerialPort has default baud of 38.4k, let's change it to 9600
      config.baud = 9600
    end

    @serial_port = Posix::SerialPort.new portname, config
    @response_packets = Array.new
   
    #the default motor number
    self.active_motor_num = "A"
    #self.encoder_position = 2000
    self.log_moves = false
    self.stop_wait = false
    #self.encoder_position
  end

  #configuration steps that are needed for testing at my (andrew's) desk
  def Ezservo.lab_test
    ez = Ezservo.new nil, '/dev/ttyS1'
    ez.start_listener
    ez.proportional_gain = 1000
    ez
  end

  # Helper initializer for Rover configuration
  #  
  def Ezservo.rover
    ez = Ezservo.new nil, '/dev/ezservo'
    ez.start_listener
    LogHelper.syslog("_ after start listener")
    LogHelper.syslog("_ Listener started")
    ez.active_motor_num = "A"
    LogHelper.syslog("_ active motor number set to A")
    `stty -F /dev/ezservo min 1 time 0`
    ez
  end 

  # Helper initializer for mini-Rover configuration
  #  
  def Ezservo.mini_rover
    ez = Ezservo.new nil, '/dev/ttyAM0'
    ez.start_listener
    ez.active_motor_num = "A"
   # `stty -F /dev/ezservo min 0 time 20`
    ez
  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
  end
  
  #returns the ready/busy status as well as any error conditions
  def move_abs position
    send_message "A" << position.to_s << "R"
  end

  #a blocking move command that will block until the move is complete
  def forward(amount)
  
    # Hack: Make sure velocity and current limit are set for slower move
    # Used on WF cruise Sept07 as an attempt to get avoid motor failure
    #
    syslog("_ moving #{amount}")
#    syslog("_ setting velocity to 500")
#    velocity = 500
#    syslog("_ setting current limit to 100")
#    send_message "m" << 100.to_s << "R"
    
    send_message "P" << amount.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
    send_message "D" << amount.to_s << "R"
    wait_for_move_to_complete
  end
  
  #turn the two motors simultaneously with optionally different
  #values for distance and different values for direction
  #Legal values for direction are :forward and :backward
  #
  def turn(starboard_dir, starboard_dist, port_dir, port_dist)
    syslog("_ received command to move starboard #{starboard_dir} : #{starboard_dist}" <<
           " and port #{port_dir} : #{port_dist}")
    old_active = self.active_motor_num

    #first, check the encoder position to make sure any backward moves
    #won't cause position to be less than zero and halt prematurely.
    #
    if starboard_dir == :backward || port_dir == :backward
      self.active_motor_num = "A"
      positions = self.encoder_position
      syslog("_ encoder positions are: #{positions[0]} and #{positions[1]}")
      if starboard_dir == :backward && positions[0] < (starboard_dist + 1)
        self.active_motor_num = "A"
        #BUG! The starboard motor controller can not handle this 
        #20061130 This bug should now be fixed.
        #the motor controller firmware should now be updated and this should work
        syslog("_ Setting starboard motors position forward to accomadate for upcoming " +
               "backward move")
        self.encoder_position = starboard_dist + 500
        #HACK! This is to get around the problem with the ezservo going backward
        starboard_dist = 0
        sleep 1
      end
      if port_dir == :backward && positions[1] < (port_dist + 1)
        self.active_motor_num = "A"
        syslog("_ Setting port motors position forward to accomadate for upcoming " +
               "backward move")
        self.encoder_position = port_dist + 500
        port_dist = 0
        sleep 1
      end
    end
    
    # Set-up motor directions and execute move
    #
    starboard_dir = direction_translation(starboard_dir)
    port_dir = direction_translation(port_dir)
    self.active_motor_num = STARBOARD_MOTOR_NUM
    syslog("_ in turn, moving starboard dir #{starboard_dir} #{starboard_dist.to_s}")
    send_message "#{starboard_dir}" << starboard_dist.to_s 
    
    self.active_motor_num = PORT_MOTOR_NUM
    syslog("_ in turn, moving port dir #{port_dir} #{port_dist.to_s}")
    send_message "#{port_dir}" << port_dist.to_s
    self.active_motor_num = "A"
    send_message "R"
    wait_for_move_with_timeout
    self.active_motor_num = old_active
  end
  
  # Query controller and return status
  #
  def current_status?
    packets = do_query "Q" 
    packets.each do |packet|
      packet.error_message
    end
  end
  
  # Keep checking motor positions until two consecutive values are equal.
  #
  def wait_for_move_to_complete(sleep_interval = 1.5)
    self.stop_wait = false
    
    sleep 1
    syslog("_ beginning wait_for_move_to_complete")
    positions = self.encoder_position
    sleep(2.5)
    new_positions = self.encoder_position
    
    update = "position: "
    new_positions.each do |pos|
      update << pos.to_s << ", "
    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)
      
      # Flag error condition
# 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)) )
      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

      old_update = update
      positions = new_positions
      new_positions = self.encoder_position
      update = "motors: #{self.active_motor_num} positions: "
      new_positions.each do |pos|
        update << pos.to_s << ", "
      end
#      syslog("_ old: " + old_update) 
      syslog("_ new: " + update)
    end
    
    if (self.stop_wait)
      syslog("_ wait was stopped")
    else
      syslog("_ positions are equal")
    end
    
    # 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
  
  def wait_for_move_with_timeout(timeout = 180)
    t = RoverThread.new do
      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 = "A"
      self.halt
    end
  end
  
  #translate from a direction symbol (:forward or :backward) to the
  #corresponding ezservo direction (P or D)
  def direction_translation direction
    if direction == :forward
      return "P"
    elsif direction == :backward
      return "D"
    else
      syslog("! ERROR: Bad direction received in direction_translation function") 
      syslog("! Direction symbol received: #{direction}")
      syslog("! Returning :forward direction rather than throwing an error.")
      return "P"
    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
  
  # 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
    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
    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
    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
    send_message "y" << new_gain.to_s << "R"
  end

  # Set the motor velocity
  def velocity= 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_acceleration
    send_message "L" << new_acceleration.to_s << "R"
  end

  # Set motor current limit
  def current_limit= new_current
    send_message "m" << new_current.to_s << "R"
    syslog("_ motor current set to #{new_current}")
  end
  
  # Set overload retry count and timeout values
  def overload_retry_count= new_count
    send_message "au" << new_count.to_s << "R"
  end
  def overload_timeout= new_timeout
    send_message "u" << new_timeout.to_s << "R"
  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 = "A"
    send_message "T"
    self.active_motor_num = old_active
    self.stop_wait = true
  end
  alias halt stop_command
  alias stop stop_command 

  #perform a processor reset
  def reset!
    send_message "ar5073R"
  end

  #NOTE: This needs to be tested, the numeric value can be 1 or 0
  def recover
    send_message "r1R"
  end
  
  #get the most recent error message
  def error_message
    @response_packets[-1].error_message
  end

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

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

########################## Supporting Commands ################################
  #warning, this is 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
        receiving_response = true
        collector << c
        if collector[-1] == 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] != 2
            collector = collector[1..-1]
          end
          if collector[0] != 2
            syslog("! WARNING: start character not seen at beginning of response")
          end
          #grab the checksum...
          checksum = @serial_port.getc
          if Ezservo.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 ezservo port for responses to commands
  # and error conditions
  #
  def start_listener
    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.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
  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
    command = ""
    command << OEM_START_CHAR 
    command << @active_motor_num.to_s	#message address
    command << "1"			#sequence character
    command << message
    command << OEM_END_CHAR 
    checksum = Ezservo.compute_checksum(command)
    command << checksum
    syslog("_ command #{message} sent [#{command}]")
    num_responses = @response_packets.size
    @serial_port.write(command)
    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 = 3
      while new_num_responses == num_responses && sleeptimer > 0
        sleep 1
        sleeptimer -= 1 
        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 Ezservo.compute_checksum(string)
    checksum = 0
    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 listner thread.
  #
  def do_query(query_string)
    responses = @response_packets.size
    #self.synchronized do
    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 = 3
      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
    #end #end syncrhonized call
    #since the last time the arrays size was queried.
    @response_packets.slice(responses..-1)
  end

  def numeric_response_query 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("" << Ezservo::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[1][0] || 5
    else
      result = 5
      t = Time.now
      t.gmtime
      msg = "_ ezservo::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? 
    if self.status_character & 2**5 != 0
      true
    else
      false
    end
  end

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

  def error_code
    self.status_character & 15
  end

  def error_message 
    ERROR_NAMES[self.error_code]
  end
end


# Class SimEzservo provides a simple simulated Ezservo interface for
# mission simulations.
#
class SimEzservo
  include LogHelper

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

  def SimEzservo.rover
    ez = SimEzservo.new nil
    ez
  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")
      encoderCounts = 0
    end
    
    st = encoderCounts/30
    syslog("_ Sim Ezservo sleeping #{st} seconds to simulate forward move")
    Rover.instance.rt.rSleep(st)
    if(self.active_motor_num == "A")
      self.encoder_position = [encoderCounts, encoderCounts]
    else
      self.encoder_position = encoderCounts
    end
  end

  def turn(starboard_dir, starboard_dist, port_dir, port_dist)
    if(starboard_dir != :forward && starboard_dir != :backward)
      raise("unknown turn direction for starboard_dir")
    end
    if(port_dir != :forward && port_dir != :backward)
      raise("unknown turn direction for port_dir")
    end
    if(starboard_dir == :backward || port_dir == :backward)
      raise("backward directions are currently buggy, not supported in sim")
    end
    syslog("_ SimEzservo told to turn port #{port_dir} #{port_dist} and " <<
         "starboard #{starboard_dir} #{starboard_dist}")

    # Rover motors move about 30 counts per second, so sleep
    # time is adjusted accordingly.
    #
    encoderCounts = port_dist
    encoderCounts = starboard_dist if (starboard_dist > port_dist)
    if (encoderCounts < 0)
      syslog("! turn method works best when distance > 0")
      encoderCounts = 0
    end
    st = encoderCounts/30

    syslog("_ SimEzservo sleeping #{st} seconds to simulate turn")
    Rover.instance.rt.rSleep(st)
    @encoderPos[0] += starboard_dist
    @encoderPos[1] += port_dist
  end

  def stop_command
    syslog("_ SimEzservo 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