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

include LogHelper
@move_heading = 0
@move_distance = 400  # 400 counts ~ 0.5 meters

#
# Measurement phase method
#
def doMeasure

  syslog("Starting Measurement phase")

  #
  # Create a Measure object and run it
  #
  m = Measure.new
  m.run
  mt = m.thr

  measureTime = 0
  sleepInterval = 30
  pct = 0
  #
  # Allow 18 minutes for the measurement phase
  # Wake-up every so often (sleepInterval) and check
  # to see if the phase is complete
  #
  while (measureTime < 18*60 && pct < 100) do

    sleep(sleepInterval)
    pct = m.percentComplete
    syslog("Measure #{pct} complete")
    measureTime += sleepInterval
  
  end
  syslog("Measurement time was #{measureTime} seconds")

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

#
# Movement phase
#
def doMove
  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 = 2500
  sleep 1
  motors.velocity = 1000
  sleep 1
  
  # Create a control system and give it the compass
  # data. The plan is to move in a square pattern.
  # Move forward in whatever direction we're pointing in,
  # then 90 to the right, ...
  #
#  control_system = ControlSystem.new motors, acm
#  syslog "moving 2000 counts at #{@move_heading} degree heading"
#  control_system.forward @move_heading, @move_distance
 motors.forward 2000
  syslog("Movement done!")
  relay.motorsOff
  @move_heading += 90 
  @move_heading = @move_heading % 360
  syslog "Stopping the acm"
  acm.close
  acm = nil
end

############################# Mission Main ######################


syslog("Starting mission!")
sleep 3

# Launch phase, just sleep and wake-up
# 90 minutes launch time (launch, Ventana to reach bottom)
#
launchTime = 1*60 
syslog("Launch phase, sleep for #{launchTime} seconds")
sleep(launchTime)

# Turn the 12-volt system on and do the
# Measurement-Movement cycles.
# 7 measurements, 6 moves
#
Rover.instance.relay.v12On
sleep 5
for i in 1..6
  doMeasure
  doMove
end

doMeasure
Rover.instance.relay.v12Off