-- Circle Mode Lua Script Draft 2

local button_number = 1 -- the button number we want to read, as defined in AP_Button
local button_active_state = true -- the 'pressed' state of the button
local last_button_state
local RUN_INTERVAL_MS = 100
local ROVER_MODE_GUIDED = 15
local ROVER_MODE_AUTO = 5 

local circle_anlge = 0
local circle_angle_increment = 1 -- increment the target angle by 1 degree every 0.1 sec (10deg/sec)
local circle_speed = 1
local last_mode = vehicle:get_mode()



function update(locate button_new_state) -- the loop which periodically runs
	local button_new_state = button:get_button_state(button_number) ==button_active_state

-- the button has changed since the last loop

if button_new_state ~= last_button_state then
	last_button_state = button_new_state

	if button_new_state then
		gcs:send_text(0, "LUA: Button pressed")

	else
		gcs:send_text(0, "LUA:Button released")
	end




function do_circle()
	local button_new_state = button:get_button_state(button_number) == button_active_state

	if button_new_state ~= last_button_state then
	circle_angle = 0
	vehicle:set_mode(ROVER_MODE_GUIDED)
	end

	if mode~= ROVER_MODE_GUIDED then
		if ROVER_MODE_AUTO then
		last_mode = mode
		end

		return do_circle, RUN_INTERVAL_MS
	end

-- calculate velocity vector

circle_angle = circle_angle + circle_angle_increment
if (circle_angle >=360) then
	vehicle:set_mode(last_mode)
	return do_circle, RUN_INTERVAL_MS
end
local target_vel = Vectr3f()
target_vel:x(math.sin(math.rad(circle_angle)) * circle_speed)
target_vel:y(math.cos(math.rad(circle_angle)) * circle_speed)
target_vel:z(0)

--send velocity request
if not (vehicle:set_target_velocity_NED(target_vel)) then
	gcs:send_text(0, "failed to execute velocity command")
end
return do_circle, RUN_INTERVAL_MS
end

return do_circle()