#--
#******************************************************************************
#* Copyright 1990-2008 MBARI
#* MBARI Proprietary Information. All rights reserved.
#******************************************************************************
#* Summary  : Handles execution of launch phase
#* Filename : launch.rb
#* Author   : Henthorn
#* Project  : Benthic Rover
#* Version  : 
#* Created  : 03/19/08
#* Modified :
#******************************************************************************

require 'rover_environment'
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

  # Initialize Main object:
  #   Instantiate the Rover object
  #   Load config file attributes
  #   Create log directory
  #
  def initialize()
  
    # Rover is a Singleton object
    #
    rv = Rover.instance
    @long_mission = rv.clo.long_mission
    cm = rv.cm

    # Load the config file
    #
    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

    # Initialize modem comms and object variables
    #
    @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)
    syslog("_ running Rover Mission from main")
    syslog("_ keepSimple parameter set to #{keepSimple}")

    # Set appropriate mission parameters if long mission was specified
    #    
    if @long_mission
      cm = Rover.instance.cm  # shorthand for ConfigManager

      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
    
    # Command-line launch duration overrides the duration value in config
    #
    if (Rover.instance.clo.launch_time)
      eventlog("_ #{Rover.instance.clo.launch_time} minute launch time")
    else
      eventlog("_ Normal dive launch time")
    end
    
    # Log Battery voltage throughout
    #
    Rover.instance.battery.run
    
    # Let'er rip
    #
    @currentMission.run(keepSimple)
    # Return thread ref
    @missionThread = @currentMission.mission_thread
  end

  # Initialize a RoverMission object and set it up for
  # default (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
    
    # Default mission is the short option
    #
    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
 
    # Command-line option overrides launch duration from config file
    #   
    rm.launch_time=rv.clo.launch_time if (rv.clo.launch_time)

    # 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
 
    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

    # Command-line option for simple mission (no current following)
    #
    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 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
    
    # This loop runs until the main ruby process is terminated externally.
    # FIXME! Perhaps we could have some better options instead
    #    (1) Exit-on-completion option
    #    (2) Use this main loop to load and run other mission files (configs)
    #
    while(true)
      begin
        # Modem loop listens for commands from surface (i.e., abort)
        #
        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

      # Mission is over if thread has expired
      #
      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
  
      # Log mission status
      #
      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

  # Parse and execute valid commands
  #
  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
   
    # Execute command handler
    #
    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

  # Set about aborting the mission processes and moving
  # components to safe position
  #
  def abort_mission
  
    # Ensure that user really wants to abort
    #
    if @missionThread == nil
      eventlog("! received an abort command from modem while no mission was running")
      @modem.write("! Mission not running")
    else
    
      # User must confirm abort command
      #
      if @abortCount > 1
      
        # Abort command confirmed
        #
        eventlog("_ abort command received from modem, aborting mission")
        @modem.write("Roger that. Rover aborting mission\n")
        
        # Tell the mission object to abort, then wait for it
        # to clean-up after itself.
        #
        @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

  # Run a mission using the "short mission" parameter
  #
  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

  # Run a mission using the "long mission" parameter
  #
  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

  # Send a mission status message to the surface station
  #
  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 entry point
Main.new().run()