###############################################################################
# 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
#        (e) BR$HOME   - return the rover to its initial state. Send rack home,
#                        turn all relays off, etc
#        (f) BR$LCOMPLEX, BR$SCOMPLEX
#                      - These two missions are the same as BR$SHORT and BR$LONG
#                        except they try to use the current detection algorithm
#                        for movement rather than simply going straight.
#
#    (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"
  GO_HOME       =  "BR$HOME"
  

  # 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 # For deployment purposes (36hr*4/hr)
  SHORT_NCYCLES = 3             # Each cycle about 15 minutes
  LONG_NCYCLES =  1             # Each cycle about 36 hours
  LONG_SETTLE_TIME = 60
  LAUNCH_TIME = 180             # Dive time before anything happens (minutes)
  MOVE_DISTANCE = 10            # meters
  if(Rover::TEST)
    MOVE_DISTANCE = 0
    LAUNCH_TIME = 0
  end

  STATUS_INTERVAL = 300         # seconds
  ABORT_WAIT_TIME = 300

  def initialize()
    # Create directory for data and logs
    # 
    unless LogDir.instance.newlogdir("/cf/rover/logs")
      syslog("! Could not create new log dir")
    end
    
    @modem = Rover.instance.modem
    @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.launch_time=1 if ($LAUNCH_TIME_1_MIN)
    rm.movement_distance=MOVE_DISTANCE
    rm
  end

  #launch the pre-configured mission and then go into the modem listening loop
  def run
  
    # Parse command line options
    #
    simple = false
    long_mission = false
    ARGV.each {|arg| simple = true if (arg == "-simple") }
    ARGV.each {|arg| long_mission = true if (arg == "-long_mission") }
    ARGV.each {|arg| $CLEAN_FIRST = 1 if (arg == "-cf") }
    ARGV.each {|arg| $LAUNCH_TIME_1_MIN = 1 if (arg == "-lt1") }
    ARGV.each {|arg| $SAFE_MOVE = true if (arg == "-safe_move") }

    @currentMission = initial_mission()

    if simple
      syslog("_ Will not attempt to move into current")
    else
      syslog("_ Attempt to move into current")
    end

    if long_mission
      @currentMission.number_of_cycles=LONG_NCYCLES
      syslog("_ Long mission: #{LONG_NCYCLES} cycles")
      @currentMission.record_iterations=LONG_RECORD_ITERATIONS
      syslog("_ Long mission: #{LONG_RECORD_ITERATIONS} iterations")
      @currentMission.settle_time=LONG_SETTLE_TIME
      syslog("_ Long mission: #{LONG_SETTLE_TIME} minute settle time")
    end
    
    if ($SAFE_MOVE)
      syslog("_ Will attempt to do a safe move")
    else
      syslog("_ Will not do a safe move")
    end
    
    if ($LAUNCH_TIME_1_MIN)
      syslog("_ 1 minute launch time")
    else
      syslog("_ Normal dive launch time")
    end
    
    if ($CLEAN_FIRST)
      syslog("_ Start cleaning pumps before lifting chambers")
    else
      syslog("_ Start cleaning pumps after lifting chambers")
    end
    
    begin
      start_mission(simple)
    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}")
          t = Time.now().strftime("%Y%m%d-%H:%M:%S")
          @modem.write("Rover copy message [#{msg}] at #{t}\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. Thank you sir, may I have another?\n")
        end
      end
  
      unless @missionThread == nil
        if (Time.new - lastStat) >= STATUS_INTERVAL
          lastStat = Time.new
          t = Time.now().strftime("%Y%m%d-%H:%M:%S")
          @modem.write("#{t}: " << @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
    elsif msg.eql?(GO_HOME)
      go_home
    else
      syslog("! invalid modem command received. No action taken")
      @modem.write("Invalid command: [#{msg}]")
    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, check back soon...")
        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

  #this method should be used to return rover to it's starting state
  #right now that means turning all relays off and sending the rack home  
  def go_home()
     if @missionThread != nil
      syslog("! mission still running, please abort mission before trying to go home.")
      @modem.write("! Mission still running, abort before going home.")
    else
      Rover.instance.rack.reinit($STBD_RACK)
      result = Rover.instance.rack.move_home($STBD_RACK)
      Rover.instance.relay.s_rackOff
      unless(result)
        syslog("! error sending starboard rack to home")
      end
      
      Rover.instance.rack.reinit($PORT_RACK)
      result = Rover.instance.rack.move_home($PORT_RACK)
      Rover.instance.relay.p_rackOff
      unless(result)
        syslog("! error sending port rack to home")
      end
      
      Rover.instance.relay.allOff
      final_status = "go_home routine completed, all relays turned off, "
      if(result)
        final_status << "rack successfully sent to home"
      else
        final_status << "unable to send rack home"
      end
      @modem.write(final_status)
    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()