local DRIFT_CMD = 86
local DEFAULT_DRIFT_M = 10  -- (m) allowable drift distance if not specified
local DRIFT_MAV_NAME = 'DRIFT'
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_INFO = 6
local ROVER_MODE_AUTO = 10
local RUN_INTERVAL_MS = 200

local last_id = -1
local drift_start_loc
local script_time_msn_index
local allowable_drift_dist = DEFAULT_DRIFT_M
local drift_start_ms
local drift_bearing_deg
local drift_dist_m = 0
local drift_vel_mps

-- file logging stuff (SD card)
local first_file = 1
local file_name = "DRIFT_DATA"
local file

local function write_to_file()
  if not file then
    error("Could not open file")
  end

  -- write data
  -- separate with comas and add a carriage return
  -- lat/long are in degrees * 1e7 as an integer.
  file:write(tostring(millis())..","..tostring(gps:time_week(0))..","..tostring(gps:time_week_ms(0))..","..tostring(drift_start_loc:lat())..","..tostring(drift_start_loc:lng())..","..tostring(drift_vel_mps)..","..tostring(drift_bearing_deg).."\n")

  -- make sure file is up to date
  file:flush()
end

function file_exists(name)
   local f = io.open(name, "r")
   return f ~= nil and io.close(f)
end

function do_script_time()
    -- zero steering/throttle using guided mode binding
    vehicle:set_steering_and_throttle (0, 0)

    -- compute drift distance and send to GCS
    local current_loc = assert(ahrs:get_location(), 'AHRS location error!')
    drift_dist_m = current_loc:get_distance(drift_start_loc)
	drift_vel_mps = (drift_dist_m/(millis():tofloat()-drift_start_ms))*1000.0  --can div by zero!
	drift_bearing_deg = drift_start_loc:get_bearing(current_loc)*57.29578
    gcs:send_named_float(DRIFT_MAV_NAME, drift_dist_m)
	-- for some reason no spaces between the commas in the function call below?!?
	logger:write('DRFT','dist, vel, bearing','fff','mnd','---',drift_dist_m,drift_vel_mps,drift_bearing_deg)

    -- exit script time on waypoint change, mode change, or disarm
    if not arming:is_armed() or
       vehicle:get_mode() ~= ROVER_MODE_AUTO or
       mission:get_current_nav_index() ~= script_time_msn_index or 
	   drift_dist_m > allowable_drift_dist then

		-- reset for next time
        vehicle:nav_script_time_done(last_id)
        allowable_drift_dist = DEFAULT_DRIFT_M
        gcs:send_text(MAV_SEVERITY_NOTICE, ("Drift: "..tostring(drift_vel_mps).."m/s at "..tostring(drift_bearing_deg)).." deg" )
		
		-- log data to file on SD card
		write_to_file()
		
        return await_script_time, RUN_INTERVAL_MS
    end

    return do_script_time, RUN_INTERVAL_MS
end

function await_script_time()
    -- enter script time routine if DRIFT_CMD is received
    -- first argument is allowable drift distance in meters
    local id, cmd, arg1, arg2, arg3, arg4 = vehicle:nav_script_time()
    if id and cmd == DRIFT_CMD then
        drift_start_loc = assert(ahrs:get_location(), 'AHRS location error!')
		drift_start_ms = millis():tofloat()
        if arg1 and arg1 > 0 then allowable_drift_dist = arg1 end
        last_id = id
        script_time_msn_index = mission:get_current_nav_index()
        gcs:send_text(MAV_SEVERITY_NOTICE, ('Allowing %.1f m drift...'):format(allowable_drift_dist))
        --gcs:send_text(MAV_SEVERITY_NOTICE, ('Time %.1f ms ...'):format(drift_start_ms))
        return do_script_time, RUN_INTERVAL_MS
    end
	
	-- close existing file and open new file if CLOSE_CMD is received (at end of mission or whatever)
	-- only do this once each time it is triggered with script_time waypoint 
	
	

    -- report -1 for drift distance if script time is inactive
    gcs:send_named_float(DRIFT_MAV_NAME, -1)
	logger:write('DRFT','dist,vel,bearing','fff','mnd','---',-1,-1,0)

    return await_script_time, RUN_INTERVAL_MS
end

gcs:send_text(MAV_SEVERITY_INFO, 'Allow drift script 3 loaded')

-- open drift data file
local fname = (tostring(file_name)..tostring(first_file)..".csv")

while ((file_exists(fname)) and (first_file < 99))
do
	first_file=first_file+1
	fname = (tostring(file_name)..tostring(first_file)..".csv")
end

file = io.open(fname, "w")
if not file then
	gcs:send_text(MAV_SEVERITY_INFO, ("ERROR opening "..fname))
else
	gcs:send_text(MAV_SEVERITY_INFO, ("opened "..fname))
end

return await_script_time()