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.
#
class RoverMission

  include LogHelper
  
  SIMPLE_FORWARD_DISTANCE = 4300 #The distance for the rover to go forward
                                 #in the case of a simple (no current detection)
                                 #move
  SLEEP_ABORT_TIME = 300 #Time in seconds to sleep between wakeups to check for an abort
  SETTLE_TIME = 10       #Time, in minutes, to wait for dust to settle before
                         #transitioning from movement to measurement phase
  
  def initialize 
    @move_heading = 0
    @move_distance = 2   # in meters, value of zero ensures no movement
    @iterations = 5      # recording iteration in measurement phase (not cycles)
    @launch_time = 240   # 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 = 4      # 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
  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 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 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)
    @mission_thr = RoverThread.new do
      @abort = false
      syslog("_ Starting mission!")
      syslog("_ raising rack to home position")
      Rover.instance.relay.rackOn
      sleep(2)
      Rover.instance.rack.reinit
      sleep(2)
      Rover.instance.rack.home
      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 
      syslog("_ Launch phase, sleep for #{launchTimeSec} seconds")

      Rover.instance.relay.v12On

      # 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..5
          Rover.instance.relay.videoOn
          sleep(60)
          Rover.instance.relay.videoOff
          sleep(60)
          launchTimeSec -= 120
        end
      end

      if(self.abort?)
        syslog("_ Aborting mission while in run method, before sleep")
        Rover.instance.relay.allOff
        return
      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
      sleepPeriod = (launchTimeSec / SLEEP_ABORT_TIME).to_i
      if(sleepPeriod > 0)
        sleep(SLEEP_ABORT_TIME)
        if(self.abort?)
          syslog("_ Aborting mission while sleeping in launch phase")
          Rover.instance.relay.allOff
          return
        end
      end
      #the rest of the time to sleep
      sleepRemainder = launchTimeSec % SLEEP_ABORT_TIME
      sleep(sleepRemainder)
    
      if (@launch_time > 20)
        syslog("_ Blink lights at finish for everyone")
        for i in 1..5
          Rover.instance.relay.videoOn
          sleep(60)
          Rover.instance.relay.videoOff
          sleep(60)
        end
      end

      if(self.abort?)
        syslog("_ Aborting mission after sleeping, before measure/move cycle")
        Rover.instance.relay.allOff
        return
      end

      # Do the Measurement-Movement cycles.
      #
      sleep 3

      for @current_cycle in 1..@num_cycles
        @cycle_start = Time.new

        break if self.abort?
        syslog("_ Movement phase #{@current_cycle} of #{@num_cycles}")
        @current_phase = "Movement"
        #make sure video is off before powering up acm
        Rover.instance.relay.videoOff
        Rover.instance.relay.v12On
        sleep(1)
        Rover.instance.acm.run
        syslog("_ Sleeping 60 seconds to let the acm accumulate data")
        sleep 60
        if(keepSimple)
          goForward
        else
          doMove
        end 
        Rover.instance.acm.stop_thread
      
        break if self.abort?
        unless (Rover::TEST)
          syslog("_ Waiting for #{RoverMission::SETTLE_TIME} minutes for dust to settle.")
          sleep (RoverMission::SETTLE_TIME * 60).to_i
        end
        break if self.abort?
        syslog("_ Measure phase #{@current_cycle} of #{@num_cycles}")
        @current_phase = "Measure"
        doMeasure
      end

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

    end     # Thread do
  end     # run method

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

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

    measureTime = 0
    wakeInterval = 60
    pct = 0
    #
    # 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
    #
    while (measureTime < (@iterations*15 + 5)*60 && pct < 100) do

      sleep(wakeInterval)
      pct = @measure.percentComplete
      syslog("_ #{@measure.currentAction}, #{pct} complete")

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

    if (100 == pct)
      syslog("_ Measurement Done")
    else
      syslog("_ Measurement timed-out!")
    end
  end

  def doMove
    return unless @move_distance > 0
    
    Rover.instance.relay.motorsOn
    navigator = Navigator.new(Rover.instance)
    direction = Rover.instance.acm.getAvgCurrentHead().to_f
    direction += 180
    direction = direction % 360

    if(self.abort?)
      syslog("_ Aborting from within doMove")
      return
    end

    syslog("_ Moving #{@move_distance} meters, heading #{direction}")
    sleep(10)
    
    navigator.forward direction, @move_distance
  end

  #go forward
  def goForward
    syslog("_ Starting Forward movement phase")
    relay=Rover.instance.relay
  
    relay.motorsOn
    motors = Rover.instance.motors
    motors.close
  
    relay.motorsOn
    sleep(3)
    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 = 1000
    sleep 1
 
    #send motors forward 2000 counts 
    syslog("_ Moving motors #{SIMPLE_FORWARD_DISTANCE} counts")
    motors.forward SIMPLE_FORWARD_DISTANCE
    syslog("_ 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 = 1000
    sleep 1
  
    # The heading we want to take is opposite the heading of the current
    #
    @move_heading = 90
  
    #
    # 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
    syslog("_ 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