###############################################################################
# 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 = 2             # Each cycle about 15 minutes
  LONG_NCYCLES =  1             # Each cycle about 36 hours
  LONG_SETTLE_TIME = 5
  SHORT_SETTLE_TIME = 5
  LAUNCH_TIME = 90             # Dive time before anything happens (minutes)
  MOVE_DISTANCE = 10            # meters
  if(Rover::TEST)
    MOVE_DISTANCE = 0
    LAUNCH_TIME = 0
  end

  STATUS_INTERVAL = 240         # seconds
  ABORT_WAIT_TIME = 120

  def initialize()
  
    rv = Rover.instance
    @long_mission = rv.clo.long_mission
    cm = rv.cm

    if (rv.clo.config_file)
      cm.configfile = rv.clo.config_file
    else
      cm.configfile = "RoverCfg.xml"
    end

    unless cm.load
      puts "Unable to load config file!"
      puts cm.last_error
    end
    
    # Create directory for data and logs
    # 
    log_dir_prefix = cm.get("LogDirPrefix")
    
    unless LogDir.instance.newlogdir("/cf/rover/logs", log_dir_prefix)
      eventlog("! Could not create new log dir")
    end
    
    @modem = rv.modem
    @modem.init
    @currentMission = nil
    @missionThread = nil
    @abortCount = 0 		#abort command needs to be sent twice, so we keep count
    
    if (ci = cm.get("StatusReportInterval"))
      @stat_interval = ci.to_i 
    else
      @stat_interval = STATUS_INTERVAL #seconds
    end
    if (ci = cm.get("SurfaceCommandCheckInterval"))
      @check_interval = ci.to_f
    else
      @check_interval = 60  #seconds
    end
  end

  # Method to start mission and save the thread reference
  #
  def start_mission(keepSimple = false) # pass in RoverMission object
#    LogDir.instance.newlogdir("/cf/rover/logs")
    syslog("_ running Rover Mission from main")
    syslog("_ keepSimple parameter set to #{keepSimple}")
    
    if @long_mission
      cm = Rover.instance.cm
      if (ci = cm.get("LongCycles"))
        @currentMission.number_of_cycles=ci.to_i
        syslog("_ Long mission: #{ci} cycles")
      else
        @currentMission.number_of_cycles=LONG_NCYCLES
        syslog("_ Long mission: #{LONG_NCYCLES} cycles")
      end
      if (ci = cm.get("LongIterations"))
        @currentMission.record_iterations=ci.to_i
        syslog("_ Long mission: #{ci} iterations")
      else
        @currentMission.record_iterations=LONG_RECORD_ITERATIONS
        syslog("_ Long mission: #{LONG_RECORD_ITERATIONS} iterations")
      end
      if (ci = cm.get("DustSettleTime"))
        @currentMission.settle_time=ci.to_f
        syslog("_ Long mission: #{ci} minute settle time")
      else
        @currentMission.settle_time=LONG_SETTLE_TIME
        syslog("_ Long mission: #{LONG_SETTLE_TIME} minute settle time")
      end
      if (ci = cm.get("Distance"))
        @currentMission.movement_distance=ci.to_f
      else
        @currentMission.movement_distance=MOVE_DISTANCE
      end
    
    end
    
    if (Rover.instance.clo.launch_time)
      eventlog("_ #{Rover.instance.clo.launch_time} minute launch time")
    else
      eventlog("_ Normal dive launch time")
    end
    
    Rover.instance.battery.run
    
    @currentMission.run(keepSimple)
    # Return thread ref
    @missionThread = @currentMission.mission_thread
  end

  # Set-up for a short mission
  #
  def initial_mission
    eventlog(LogDir.instance.mst.to_s)    
    rm = RoverMission.new   # mission object can be re-used

    rv = Rover.instance
    cm = rv.cm
    if (ci = cm.get("ShortCycles"))
      rm.number_of_cycles=ci.to_i
    else
      rm.number_of_cycles=SHORT_NCYCLES
    end

    if (ci = cm.get("ShortIterations"))
      rm.record_iterations=ci.to_i
    else
      rm.record_iterations=SHORT_RECORD_ITERATIONS
    end
    
    if (ci = cm.get("LaunchDuration"))
      rm.launch_time=ci.to_f
    else
      rm.launch_time=LAUNCH_TIME
    end
 
    # A heading less than zero means we don't have a general heading,
    # just move into the current.
    #
    if (ci = cm.get("Heading"))
      rm.move_heading=ci.to_f if (ci.to_f >= 0)
    end
 
    if (ci = cm.get("FavorableCurrentDeviation"))
      rm.current_deviation=ci.to_f if (ci.to_f > 0)
    end
 
    if (ci = cm.get("FavorableCurrentWaitTime"))
      rm.current_timeout=ci.to_f if (ci.to_f > 0)
    end
 
    # Command-line option overrides config file
    #   
    rm.launch_time=rv.clo.launch_time if (rv.clo.launch_time)

    if (ci = cm.get("Distance"))
      rm.movement_distance=ci.to_f
    else
      rm.movement_distance=MOVE_DISTANCE
    end
    
    if (ci = cm.get("ShortDustSettleTime"))
      rm.settle_time=ci.to_f
      syslog("_ initial mission: #{ci} minute settle time")
    else
      rm.settle_time=SHORT_SETTLE_TIME
      syslog("_ initial mission: #{SHORT_SETTLE_TIME} minute settle time")
    end

    if rv.clo.simple
      if (ci = cm.get("SimpleForwardDistance"))
        rm.movement_distance=ci.to_f
      end
      eventlog("_ Will not attempt to move into current")
    else
      eventlog("_ Attempt to move into current")
    end

    rm
  end

  #launch the pre-configured mission and then go into the modem listening loop
  def run
  
    @currentMission = initial_mission()

    begin
      start_mission(Rover.instance.clo.simple)
    rescue Exception => e
      eventlog("! 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
        eventlog("! 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(@check_interval)
  
      # 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
          eventlog("_ 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
          eventlog("_ 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) >= @stat_interval #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)
    #
    # Need 2 abort commands in succession
    #
    if msg.eql?(ABORT_CMD)
      if @missionThread == nil
        eventlog("! received an abort command from modem while no mission was running")
        @modem.write("! Mission not running")
        @abortCount = 0
      else
        @abortCount += 1
        abort_mission
      end
    else
      @abortCount = 0
    end
    
    if 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
      eventlog("! invalid modem command received. No action taken")
      @modem.write("Invalid command: [#{msg}]")
    end
  end

  def abort_mission
    if @missionThread == nil
      eventlog("! received an abort command from modem while no mission was running")
      @modem.write("! Mission not running")
    else
      if @abortCount > 1
        eventlog("_ 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?)
            sleep(30)
            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")
        eventlog("_ 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)
    eventlog("_ 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")
      @long_mission = nil
      @currentMission = initial_mission()
      @currentMission.launch_time=1
      LogDir.instance.newlogdir("/cf/rover/logs")
      start_mission(keepSimple)
    end
  end

  def run_long_mission(keepSimple)
    eventlog("_ 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")
      @long_mission = true
      @currentMission = initial_mission()
      @currentMission.number_of_cycles=LONG_NCYCLES
      @currentMission.record_iterations=LONG_RECORD_ITERATIONS
      @currentMission.movement_distance=MOVE_DISTANCE
      LogDir.instance.newlogdir("/cf/rover/logs")
      start_mission(keepSimple)
    end
  end

  def send_status
    eventlog("_ 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()