###############################################################################
# Rover main()program.
#
# This script:
#    (1) Creates and runs a mission using the RoverMission class in mission.rb.
#        This is the default mission. Note: You may not be able to run another.
#
#    (2) Enters loop listening for commands from surface every 30 seconds
#          (a) BR$ABORT   - stop current mission, stopping in a safe state.
#                           Request must be received two times.
#          (b) BR$STATUS  - send status of Rover right now
#          (c) BR$SHORT   - start a short mission 45-60 minutes
#          (d) BR$LONG    - start a long mission, 36-72 hours
#
#    (3) Rover will send messages to surface
#          (a) Status - STATUS_INTERVAL controls how often to send unsolicited
#                       status messages
#          (b) Acknowledge - Rover will acknowledge messages from surface
#          (c) Action - Rover will inform surface when a mission is starting
#                       and when a mission has completed and ready for another.
#
###############################################################################

require "rover_utils"
require "modem"
require "mission"
require "rover"

class Main

  include LogHelper

  ABORT_CMD     =  "BR$ABORT"
  STATUS_CMD    =  "BR$STATUS"
  RUN_LONG_CMD  =  "BR$LONG"
  RUN_LONG_COMPLEX_CMD = "BR$LCOMPLEX"
  RUN_SHORT_CMD =  "BR$SHORT"
  RUN_SHORT_COMPLEX_CMD = "BR$SCOMPLEX"

  # These variables set-up Short and Long missions
  #
  # Iterations - more than 1 will encur a 14-minute waiting period
  SHORT_RECORD_ITERATIONS = 1   # Short measurement phases for testing purposes
  LONG_RECORD_ITERATIONS  = 144 # Long measurement phases for deployment purposes
  SHORT_NCYCLES = 3             # Each cycle about 15 minutes
  LONG_NCYCLES =  1             # Each cycle about 36 hours
  LAUNCH_TIME = 240             # Dive time before anything happens (minutes)
  MOVE_DISTANCE = 5             # meters
  if(Rover::TEST)
    MOVE_DISTANCE = 0
    LAUNCH_TIME = 0
  end

  STATUS_INTERVAL = 600         # seconds
  ABORT_WAIT_TIME = 600

  def initialize()
    @modem = BenthosModem.rover
    @modem.init
    @currentMission = nil
    @missionThread = nil
    @abortCount = 0 		#abort command needs to be sent twice, so we keep count
  end

  # Method to start mission and save the thread reference
  #
  def start_mission(keepSimple = false) # pass in RoverMission object
    syslog "running Rover Mission from main"
    syslog "keepSimple parameter set to #{keepSimple}"
    @currentMission.run(keepSimple)
    # Return thread ref
    @missionThread = @currentMission.mission_thread
  end

  def initial_mission
    rm = RoverMission.new   # mission object can be re-used
    rm.number_of_cycles=SHORT_NCYCLES
    rm.record_iterations=SHORT_RECORD_ITERATIONS
    rm.launch_time=LAUNCH_TIME
    rm.movement_distance=MOVE_DISTANCE
    rm
  end

  #launch the pre-configured mission and then go into the modem listening loop
  def run
    @currentMission = initial_mission()
    begin
      start_mission(true)
    rescue Exception => e
      syslog("An exception has been caught from the initial mission.")
      syslog("No other action will be taken, as the modem_loop should still " +
             "be running")
      syslog("The exception message is #{e}")
      backtrace = e.backtrace.join("\n")
      syslog("Exception backtrace: #{backtrace}")
    end
    while(true)
      begin
        modem_loop()
      rescue Exception => e
        syslog("An exception has been caught in the modem loop")
        syslog("The modem loop will be restarted")
        syslog("The exception message is #{e}")
        backtrace = e.backtrace.join("\n")
        syslog("Exception backtrace: #{backtrace}")
      end
    end
  end

  # continuously check for messages and possibly interrupt mission,
  # or start another mission
  #
  def modem_loop()
    @abortCount = 0
    lastStat = Time.new
    while (true)
      syslog("_ listening for commands")
      sleep(30)
  
      # If data is present on the modem port, read it and process
      if @modem.dataPresent?
        msg = @modem.readData(1)
        if msg.length == 0
          syslog "No new data on modem"
        else
          msg.strip!
          len = msg.length
          syslog "Received data on modem: #{msg}, #{len}"
          @modem.write("Rover copy message #{msg}\n")
          dispatch_command(msg)
        end
      end

      if @missionThread != nil 
        unless @missionThread.alive?
          @abortCount = 0
          @missionThread.join
          syslog "Mission completed"
          @missionThread = nil
          @modem.write("Mission completed. What now, Princess?\n")
        end
      end
  
      unless @missionThread == nil
        if (Time.new - lastStat) >= STATUS_INTERVAL
          lastStat = Time.new
          @modem.write(@currentMission.status)
        end
      end
  
    end  # listening while
  end

  def dispatch_command(msg)
    if msg.eql?(ABORT_CMD)
      abort_mission        
    elsif msg.eql?(RUN_LONG_COMPLEX_CMD)
      run_long_mission(false)
    elsif msg.eql?(RUN_LONG_CMD)
      run_long_mission(true)
    elsif msg.eql?(RUN_SHORT_CMD)
      run_short_mission(true)
    elsif msg.eql?(RUN_SHORT_COMPLEX_CMD)
      run_short_mission(false)
    elsif msg.eql?(STATUS_CMD)
      send_status
    else
      syslog "invalid modem command received. No action taken"
      @modem.write("Invalid command")
    end
  end

  def abort_mission
    if @missionThread == nil
      syslog "received an abort command from modem while no mission was running"
      @modem.write("! Mission not running")
    else
      @abortCount += 1
      if @abortCount > 1
        @abortCount = 0
        syslog "abort command received from modem, aborting mission"
        @modem.write("Roger that. Rover aborting mission\n")
        @currentMission.abort  # may take a while for rover to reach safe state
        abortTimer = 0
        while(abortTimer < ABORT_WAIT_TIME)
          if(@missionThread.alive?)
            abortTimer += 30
          else
            break
          end
        end
        if(abortTimer >= ABORT_WAIT_TIME)
          syslog("Abort command took longer than #{ABORT_WAIT_TIME} seconds, abort status uncertain")
          @modem.write("abort not verified, may need more time, may be stuck...")
        else
          @modem.write("abort succeeded")
        end
      else
        @modem.write("Valid ABORT command. Send again to verify\n")
        syslog "abort command received, abortCount incremented. taking no further action."
      end
    end 
  end

  def run_short_mission(keepSimple)
    syslog "received command to run a short mission"
    @abortCount = 0
    if @missionThread != nil
      syslog "mission still running, not starting a new short mission."
      @modem.write("! Mission still running")
    else
      @modem.write("Valid RUN SHORT command. Executing short mission\n")
      @currentMission.number_of_cycles=SHORT_NCYCLES
      @currentMission.record_iterations=SHORT_RECORD_ITERATIONS
      @currentMission.launch_time=1
      @currentMission.movement_distance=MOVE_DISTANCE
      start_mission(keepSimple)
    end
  end

  def run_long_mission(keepSimple)
    syslog "received command to run a long mission"
    @abortCount = 0
    if @missionThread != nil
      syslog "mission still running, not starting a new long mission."
      @modem.write("! Mission still running")
    else
      @modem.write("Valid RUN LONG command. Executing long mission\n")
      @currentMission.number_of_cycles=LONG_NCYCLES
      @currentMission.record_iterations=LONG_RECORD_ITERATIONS
      @currentMission.launch_time=1
      @currentMission.movement_distance=MOVE_DISTANCE
      start_mission(keepSimple)
    end
  end

  def send_status
    syslog "received command to send status message."
    @abortCount = 0
    if @missionThread == nil
      syslog "no mission in progress to send status."
      @modem.write("! Mission not running")
    else
      @modem.write("Valid STATUS command.\n")
      lastStat = Time.new
      @modem.write(@currentMission.status)
      syslog "mission status sent."
    end
  end
end # end class definition

Main.new().run()