require "rover"
require "measure"
require "control_system"
require "rover_utils"
require "acm"
require "navigator"

# RoverMission class
# Purpose is to encapsulate the "old" rover mission script into
# a callable, repeatable object.
#
# May07 - modified to account for 2 rack assemblies (double measure time)
# 
class RoverMission

  include LogHelper
  
  SIMPLE_FORWARD_DISTANCE = 1000 # The distance for the rover to go forward
                                 # in the case of a simple (no current 
                                 # detection) move (in counts)
                                 
  SLEEP_ABORT_TIME = 120  #Seconds to sleep between wakeups to check for abort
  DEFAULT_SETTLE_TIME = 5 #Time, in minutes, to wait for dust to settle before
                          #transitioning from movement to measurement phase
  
  def initialize 
    @move_heading = nil
    @current_timeout = 60 # wait an hour for a favorable current direction
    @current_deviation = 10 # favorable current is desired current +- 10 degrees
    @move_distance = 0   # in meters, value of zero ensures no movement
    @iterations = 1      # recording iterations in measure phase (not cycles)
    @launch_time = 120   # in minutes, duration of "dive and wait" phase
    @mission_start = nil # record time mission starts
    @cycle_start = nil   # record time each cycle starts
    @num_cycles = 2      # number of move-measure cycles in mission
    @measure = nil       # measure phase is handled by Measure class
    @current_cycle = 0
    @abort = false
    @current_phase = "Initialized"
    @mission_thr = nil
    @settle_time = DEFAULT_SETTLE_TIME
  end

  # Accessors and mutators for configurable parameters
  #

  def mission_thread
    @mission_thr
  end
    
  def movement_distance
    @move_distance
  end
  
  def record_iterations
    @iterations
  end
  
  def launch_time
    @launch_time
  end
  
  def movement_distance=dist
    if(dist < 0)
      syslog("_ movement distance < 0 for RoverMission.movement_distance has no effect")
      return
    end
    @move_distance = dist
  end
  
  def move_heading=heading
    if(heading < 0 || heading > 360)
      syslog("_ movement heading must be >= 0 and <= 360")
      return
    end
    @move_heading = heading
  end

  def current_timeout=to
    @current_timeout = to if to > 0
  end
    
  def current_deviation=cd
    @current_timeout = cd if cd > 0
  end
    
  def record_iterations=iters
    if(iters <= 0)
      syslog("! record_iterations value of less than zero received. has no effect")
      return
    end
    @iterations = iters
  end
  
  def launch_time=lt
    if(lt < 0)
      syslog("! launch_time value of less than zero has no effect.")
      return
    end
    @launch_time = lt
  end
  
  def settle_time=st
    if(st < 0)
      syslog("! settle_time value of less than zero has no effect.")
      return
    end
    @settle_time = st
  end
  
  def number_of_cycles
    @num_cycles
  end
  
  def number_of_cycles=n
    if(n <= 0)
      syslog("! number_of_cycles < 0 has no effect.")
    end
    @num_cycles = n
  end
  
############################# Mission Main ######################

  def run(keepSimple)
    @valveThread = nil
    
    @mission_thr = RoverThread.new do
      @abort = false
      eventlog("_ Starting mission!")

    if nil      
      syslog("_ raising rack to home position")
      Rover.instance.relay.s_rackOn
      Rover.instance.rack.reinit($STBD_RACK)
      Rover.instance.rack.move_home($STBD_RACK)
      Rover.instance.relay.s_rackOff
      
      Rover.instance.relay.p_rackOn
      Rover.instance.rack.reinit($PORT_RACK)
      Rover.instance.rack.move_home($PORT_RACK)
      Rover.instance.relay.p_rackOff
    end
    
      puts("Starting mission!")
      @measure = Measure.new(@iterations)
	
      # Launch phase, just sleep and wake-up
      # @launch_time minutes launch time (launch, ROV to reach bottom)
      #
      @current_phase = "Launch"
      @mission_start = Time.new
      launchTimeSec = @launch_time*60 
      eventlog("_ Launch phase, sleep for #{launchTimeSec} seconds")

      # Toggle camera lights for 10 minutes while on deck, just to
      # reassure the team that I'm actually running :)
      #
      if (@launch_time > 10)
        syslog("_ Blink lights at start for everyone")
        for i in 1..8
          Rover.instance.relay.camerasOn
          sleep(2)
          Rover.instance.relay.camerasOff
          sleep(2)
          launchTimeSec -= 4
        end

      end
      
      # Allow air in chambers to escape and reset relays
      #
      purge_time = self.purgeChambers
      Rover.instance.relay.allOff
      
      # Collect current data on the way to the bottom
      #
      Rover.instance.relay.acmOn
      sleep(3)
      Rover.instance.acm.run
      
      if(self.abort?)
        eventlog("_ Aborting mission while in run method, before sleep")
        Rover.instance.relay.allOff
        Thread.current.exit
      end

      # Sleep during dive, wake-up 10 minutes early and blink again
      #
      launchTimeSec = @launch_time > 20? launchTimeSec - 600 : launchTimeSec
      #until the issues with the v12/video are fixed, the acm should only
      #be running when the video is not. use acm.stop_thread/acm.run to toggle
      #Rover.instance.acm.run

      #wake up and check for an abort command
      sleepPeriods = (launchTimeSec / SLEEP_ABORT_TIME).to_i
      syslog("_ Sleep #{@launchTimeSec} seconds during dive")
      syslog("_ Wake-up every #{SLEEP_ABORT_TIME} seconds to check for abort")
      syslog("_ Waking up #{sleepPeriods} times")
      while(sleepPeriods > 0)
        Rover.instance.rt.rSleep(SLEEP_ABORT_TIME)
        if(self.abort?)
          eventlog("_ Aborting mission while sleeping in launch phase")
          Rover.instance.relay.allOff
          Thread.current.exit
        end
        sleepPeriods -= 1
      end
      #the rest of the time to sleep
      sleepRemainder = launchTimeSec % SLEEP_ABORT_TIME
      Rover.instance.rt.rSleep(sleepRemainder)

      tn = Time.now().strftime("%Y%m%d-%H:%M:%S")
      Rover.instance.modem.write("#{tn}: Rover awake and preparing for Movement")
      if (@launch_time > 20)
        syslog("_ Blink lights at finish for everyone")
        for i in 1..5
          Rover.instance.relay.camerasOn
          sleep(2)
          Rover.instance.relay.camerasOff
          sleep(2)
        end
      end

      # Stop Acm data collection and reset all relays
      #
      Rover.instance.acm.stop_thread
      Rover.instance.relay.allOff
      
      
      if(self.abort?)
        eventlog("_ Aborting mission after sleeping, before measure/move cycle")
        Thread.current.exit
      end

      # Do the Measurement-Movement cycles.
      #
      for @current_cycle in 1..@num_cycles
        @cycle_start = Time.new

        break if self.abort?  # Abort could have been signaled during measure

        eventlog("_ Movement phase #{@current_cycle} of #{@num_cycles}")
        @current_phase = "Movement"
        tn = Time.now().strftime("%Y%m%d-%H:%M:%S")
        Rover.instance.modem.write("#{tn}: Rover beginning Movement phase")

        # Move into the current or simply move foward
        #
        if(keepSimple)
          goForward
        else
          doMove
        end 
      
        break if self.abort?  # Abort could have been signaled during move

        # Sleep for the specified dust settle time.
        # Wake-up periodically to check abort flag.
        #
        unless (Rover::TEST)
          eventlog("_ Waiting for #{@settle_time} minutes for dust to settle.")
          st = @settle_time
          while (st > 0)
            Rover.instance.rt.rSleep(5*60)
            break if self.abort?
            st -= 5
          end
        end

        break if self.abort?  # Abort could have been requested during wait

        eventlog("_ Measure phase #{@current_cycle} of #{@num_cycles}")
        @current_phase = "Measure"

        tn = Time.now().strftime("%Y%m%d-%H:%M:%S")
        Rover.instance.modem.write("#{tn}: Rover beginning Measure phase")
        doMeasure
      end

      if self.abort?
        eventlog("_ Aborted and done.")
      else
        eventlog("_ Done with scheduled phases")
      end
  
      @current_phase = "Completed"
      eventlog("_ Standing by...")
      Rover.instance.relay.allOff

    end     # Thread do
  end     # run method

  def purgeChambers
    st = 0
    # Open chamber valves to allow water to pass through
    # during first 10 minutes of launch - ballasting issue
    #
    unless (Rover.instance.clo.launch_time)
      relay = Rover.instance.relay
      ref = Rover.instance.ref_optode
      stb = Rover.instance.starboard_optode
      port = Rover.instance.port_optode
      
        syslog("_ waiting for vehicle to get wet before purging chambers")
        # Start data collection on the optodes and grab a
        # saturation value in air
        # 
        relay.s_optodeOn
        relay.p_optodeOn
        ref.run
        stb.run
        port.run
        sleep(3)
        dry_sat = ref.sat
        
        # Wait max of 20 minutes to get vehicle in water
        # which we determine as a saturation value change
        # of 4 or more percentage points
        #
        sat_diff = 0.0
        while (sat_diff.abs <= 4 && st < 1200)
          sat_diff = ref.sat - dry_sat
          Rover.instance.rt.rSleep(10)
          st += 10
        end
        
        # When we're in the water, open the valves for 2.5 minutes
        eventlog("_ Okay, we're in the drink, so chamber valves")
          
        relay.s_valveOn
        relay.p_valveOn
        Rover.instance.rt.rSleep(150)
        st += 150
        relay.s_valveOff
        relay.p_valveOff

        relay.s_optodeOff
        relay.p_optodeOff
        ref.stop
        stb.stop
        port.stop

        eventlog("_ purging complete after #{st} seconds")
    end
    return st
  end
  
  # A rack throw is used in a final attempt to purge the chamber
  # volume before inserting into sediment
  def throw_rack(rack_id)
    syslog("_ Throwing rack #{rack_id}")
    unless true
      relay.p_valveOn
      rack.reinit(rack_id)
      rack.set_rack_speed(rack_id, 5000)
      result = rack.forward(1000)
    end
  end

  #
  # Measurement phase method
  #
  def doMeasure
    @current_phase = "Measure"
    eventlog("_ Starting Measurement phase")

    #
    # Create a Measure object and run it
    #
    @measure.run(@current_cycle)
    mt = @measure.thr

    #
    # Allow 15 minutes for each iteration of the measurement phase
    # Wake-up every so often (wakeInterval) and check
    # to see if the phase is complete
    #
    wakeInterval = 5   #minutes
    cm = Rover.instance.cm
    ci = cm.get("MeasurementProgressCheckInterval")
    if (ci)
      wakeInterval = ci.to_f
    end
    syslog("_ Measure check interval is #{wakeInterval} minutes")
    
    measureTime = 0     #minutes
    totalMeasureTime = (@iterations*     \
                       (@measure.record_time+@measure.record_interval)) + 30
    pct = 0
    while (measureTime < totalMeasureTime && pct < 100) do

      sleep(wakeInterval*60)
      pct = @measure.percentComplete
      syslog("_ Measurement: #{measureTime} of #{totalMeasureTime} minutes ")
      eventlog("_ #{@measure.currentAction}, #{pct}% complete")

      if (self.abort?)
        @measure.abort
        eventlog("_ Aborting within doMeasure method while loop")
        return
      end
    
      measureTime += wakeInterval
  
    end
    eventlog("Measurement time was #{measureTime} minutes")

    if (100 == pct)
      eventlog("_ Measurement Done")
    else
      eventlog("_ Measurement timed-out!")
      @measure.abort
      while (@measure.percentComplete < 100) do
        eventlog("_ Aborting Measurement process")
        sleep(30)
      end
    end
  end

  # Method returns true if both the starboard and port chambers
  # are in the home position
  #
  def chambersHome
    ret = false
    rack = Rover.instance.rack

    # Query the starboard home switch
    # Make an attempt to home the rack if the switch is off
    #
    rack.reinit($STBD_RACK)
    s_home = rack.home_query($STBD_RACK)
    if (!s_home || s_home > 2000)
      syslog("_ Try to move stbd rack home")
      rack.move_home($STBD_RACK)
      s_home = rack.home_query($STBD_RACK)
    end
    Rover.instance.relay.s_rackOff

    # Query the port home switch
    # Make an attempt to home the rack if the switch is off
    #
    rack.reinit($PORT_RACK)
    p_home = rack.home_query($PORT_RACK)
    if (!p_home || p_home > 2000)
      syslog("_ Try to move port rack home")
      rack.move_home($PORT_RACK)
      p_home = rack.home_query($PORT_RACK)
    end
    Rover.instance.relay.p_rackOff

    # Determine the return value (2000 seems to be a fair value)
    #
    if (s_home && s_home < 2000 && p_home && p_home < 2000)
      ret = true
    else
      eventlog("! Racks not at home after homing attempt (stbd=#{s_home}, port=#{p_home}")
    end
    return ret

  end
  
  # Method returns true if the measured current direction is
  # within the given deviation of @move_heading, otherwise false.
  # 
  def favorable_current(deviation)
    favorable = false
    acm = Rover.instance.acm
    # Calculate desired current heading (180 degrees from move heading)
    #
    dch = @move_heading + 180
    dch = dch % 360

    ch = acm.avg_current_heading().to_f # current measured average heading

    # Handle desired current heading within #{deviation} degress of 0
    # if necessary. Otherwise, just check if the current heading
    # is within #{deviation} degrees of the desired heading
    #
    if ( (dch >= (360-deviation)) && (dch <= 360) )
      #
      # Desired heading is nearly 360
      #
      favorable = true if (ch >= (dch-deviation))  # the slice up to 360
      favorable = true if (ch <= ( deviation - (360-dch) )) # slice on the other side
      #
    elsif ( ((dch <= deviation) && (dch >= 0)) )
      #
      # Desired heading is just above 0
      #
      favorable = true if (ch <= (dch+deviation)) # the slice on the 0+ side
      favorable = true if (ch >= ( 360 - (deviation-dch) )) # slice on the other side
      #
    else
      favorable = true if ((ch <= (dch+deviation)) && (ch >= dch-deviation))
    end

    # If favorable direction, check magnitude
    #
    if (favorable)
      mag = acm.avg_magnitude_of_current
      if (mag > 5)
        syslog("_ Current is favorable: reading heading of #{ch} at #{mag}")
        return true
      else
        syslog("_ Current is favorable, but mag is too small: #{mag}")
      end
    else
      syslog("_ Current not favorable: looking for ~#{dch} and reading #{ch}")
    end
    false
  end
  
  def doMove
    eventlog("_ Starting Forward movement phase")

    return unless @move_distance > 0

    # Only move if chambers are in the home position
    #
    return nil unless chambersHome

    # Examine the current direction - turn on the acm and
    # run the driver thread.
    # Make sure video is off before powering up acm
    #
    Rover.instance.relay.videoOff
    Rover.instance.relay.acmOn
    sleep(3)
    Rover.instance.acm.run
    sleep(1)

    syslog("_ Sleeping 60 seconds to let the acm accumulate data")
    Rover.instance.rt.rSleep(60)
    direction = Rover.instance.acm.avg_current_heading().to_f
    direction += 180
    direction = direction % 360

    # If a heading is specified, we will wait for a period of time
    # for the current to flow in roughly the opposite direction.
    # 
    if (@move_heading)
      dch = @move_heading + 180
      dch = dch % 360
      syslog("_ Looking to move at a heading of #{@move_heading}")
      syslog("_ Waiting for current to obtain heading of roughly #{dch}")
      wm = 1   # minutes we've been waiting
      while (true)
        if (wm > @current_timeout)
          syslog("_ Time-out waiting for current to be around #{dch}")
          break
        end
        if (favorable_current(@current_deviation))
          # Set direction here if we want a heading of @move_heading only if
          # we have favorable current (otherwise head into current)
          ch = Rover.instance.acm.avg_current_heading().to_f
          direction = @move_heading
          syslog("_ Current has heading of #{ch}, close enough to #{dch}")
          break
        end      
        Rover.instance.rt.rSleep(60)
        wm += 1
        
        if(self.abort?)
          eventlog("_ Aborting from within doMove")
          return
        end
      end
      # Set direction here if we want a heading of @move_heading whether
      # we have favorable current or not.
      direction = @move_heading
    end
    
    Rover.instance.relay.motorsOn
    navigator = Navigator.new(Rover.instance)

    eventlog("_ Moving #{@move_distance} meters, heading #{direction}")
    Rover.instance.rt.rSleep(10)
    
    navigator.forward(direction, @move_distance)
    Rover.instance.acm.stop_thread
    Rover.instance.relay.motorsOff
  end

  #go forward
  def goForward
    eventlog("_ Starting Forward movement phase")

    # Only move if chambers are in the home position
    #
    return nil unless chambersHome
    
    #syslog("_ skipping move")
    #return
    return unless @move_distance > 0
  
    motors = Rover.instance.motors
    motors.close
  
    relay=Rover.instance.relay
    relay.motorsOn
    sleep(2)

    Rover.instance.motors = Ezservo.rover
    motors = Rover.instance.motors

    #set up motor gains
    motors.proportional_gain = 1000
    sleep 1
    motors.integral_gain = 0
    sleep 1
    motors.velocity = 500
    sleep 1

    #send motors forward 2000 counts 
    navigator = Navigator.new(Rover.instance)
    eventlog("_ Moving forward #{@move_distance} meters")
    navigator.move_forward(@move_distance)
    eventlog("_ Movement done")
    relay.motorsOff
  end                     

  # Movement phase
  #
  def doMoveOld
    return unless @move_distance > 0
    counts = 800 * @move_distance  # 800 counts per meter
  
    syslog("_ Starting the Movement phase")
    syslog("_ Starting the acm")
  
    # Get some current and compass data for nav
    #
    acm = Acm.new nil, "/dev/acm"
    acm.run
    relay = Rover.instance.relay

    # Since we have to power-cycle the motors again
    # and again, closing the port avoids comm problems
    #
    relay.motorsOn
    motors = Rover.instance.motors
    motors.close

    # Turn the motors on set them up for our purposes
    #
    relay.motorsOn
    sleep(3)
    Rover.instance.motors = Ezservo.rover
    motors = Rover.instance.motors
    motors.proportional_gain = 1000
    sleep 1
    motors.integral_gain = 0
    sleep 1
    motors.differential_gain = 1000
    sleep 1
    motors.velocity = 500
    sleep 1
  
    # The heading we want to take is opposite the heading of the current
    #
    ah = acm.average_heading
    ach = acm.avg_current_heading
    syslog("_ avg_heading = #{ah}")
    syslog("_ avg_current_heading = #{ach}")
    @move_heading = 180
  
    #
    # Create a control system and give it the compass data
    #
    control_system = ControlSystem.new motors, acm
    syslog "_ moving #{@move_distance} meters at #{@move_heading} degree heading"
    control_system.forward @move_heading, counts

 
    syslog("_ Movement done!")
    relay.motorsOff
    syslog "_ Stopping the acm"
    acm.close
    acm = nil
  end

  def abort
    eventlog("_ Aborting mission...")
    @abort = true
  end

  def abort?
    @abort
  end

  def status
    if @mission_start
      duration = Time.new - @mission_start
    else
      duration = 0
    end
    
    h = sprintf("%.3f", duration / 3600)
    status_message = String.new ""
    status_message << "Mission Time:#{h} hours:Phase #{@current_phase}"
    if @current_phase == "Measure"
      status_message << ":Cycle #{@current_cycle}/#{@num_cycles}"
      status_message << ":" << @measure.action
    end
  
    if @current_phase == "Movement"
    end
  
    status_message
  end

end