-- AutoPilot 
-- Plugin for X-Plane 10, 11
----------------------------------------------------------------------------------

require("graphics")

----key definition
create_command("autopilot/toggle", "autopilot toggle", "acp_set = acp_set + 1", "", "")
create_command("autopilot/interface_toggle", "interface_toggle", "acp_vis = acp_vis + 1", "", "")
add_macro("AutoPilot - Show", "acp_vis = 1", "acp_vis = 0", "activate")

----controls
DataRef( "acp_yoke_x", "sim/cockpit2/controls/yoke_roll_ratio", "readonly" )
DataRef( "acp_yoke_y", "sim/cockpit2/controls/yoke_pitch_ratio", "readonly" )
DataRef("acp_yoke_t", "sim/cockpit2/engine/actuators/throttle_ratio_all", "readonly")

----flight parameters
DataRef("acp_heading", "sim/flightmodel/position/true_psi", "readonly")
DataRef("acp_t_heading", "sim/flightmodel/position/hpath", "readonly")
DataRef("acp_speed", "sim/flightmodel/position/indicated_airspeed", "readonly")
DataRef("acp_TAS", "sim/flightmodel/position/true_airspeed", "readonly")
DataRef("acp_altitude", "sim/flightmodel/position/elevation", "readonly")
DataRef("acp_vspeed", "sim/flightmodel/position/vh_ind", "readonly")
DataRef("acp_latitude", "sim/flightmodel/position/latitude", "readonly")
DataRef("acp_longitude", "sim/flightmodel/position/longitude", "readonly")
DataRef("acp_weight", "sim/flightmodel/weight/m_total", "readonly")
--- position
DataRef("acp_pitch", "sim/flightmodel/position/theta", "readonly")
DataRef("acp_roll", "sim/flightmodel/position/true_phi", "readonly")

--- additional
DataRef("acp_truespeed", "sim/flightmodel/position/true_airspeed", "readonly")
DataRef("acp_paused", "sim/time/sim_speed", "readonly")
DataRef("acp_frame_period", "sim/time/framerate_period", "readonly")
DataRef("acp_paused", "sim/time/sim_speed", "readonly")

-------------------------
---- settings
acp_vis = 1
acp_set = 0
local acp_test = 0
local acp_test2 = 0
local acp_test3 = 0
local acp_test4 = 0

local acp_TIMER_rate = 100
local acp_TIMER_curtime = 0
local acp_TIMER_cycletime = 0
local acp_TIMER_cycleshift = 0

local acp_show_ap = 1
local acp_add_vis = 0
local acp_add2_vis = 0
local acp_on = 0
local acp_start = true

local acp_status_horizont = "course"
local acp_status_vertical = "alt"
local acp_status_throttle = "off"
local acp_nav_source = "none"
local acp_editparam = false
local acp_oldres = 0
local acp_drag_stat = false
local acp_ap_pitch_control = true

local ln_adjust = 0
local ln_drag_x = 0
local ln_drag_y = 0
local ln_width = 366
local ln_height = 44
local ln_add_height = 74
local ln_add2_height = 170
local ln_drag_add_height = 0
----- saved settings. DO NOT CHANGE ORDER!
local ln_x = 37
local ln_y = 244
local acp_accuracy = 0.5
local acp_throttle_accuracy = 0.45
local acp_limit_dyn_heading = 5
local acp_limit_roll = 20
local acp_limit_dyn_altitude = 2.5
local acp_limit_min_throttle = 0.1
------ for reset settings
local old_ln_x = ln_x
local old_ln_y = ln_y
local acp_old_accuracy = acp_accuracy
local acp_old_throttle_accuracy = acp_throttle_accuracy
local acp_old_limit_dyn_heading = acp_limit_dyn_heading
local acp_old_limit_roll = acp_limit_roll
local acp_old_limit_dyn_altitude = acp_limit_dyn_altitude
local acp_old_limit_min_throttle = acp_limit_min_throttle
---- adjust
local acp_limit_dyn_roll = 12
local acp_limit_dyn_speed = 5
local acp_limit_dyn_pitch = 0.25
---- definitions
local acp_dyn_heading = 0
local acp_dyn_speed = 0
local acp_dyn_altitude = 0
local acp_dyn_vspeed = 0
local acp_dyn_pitch = 0
local acp_dyn_roll = 0

local acp_old_heading = acp_heading
local acp_old_speed = acp_speed
local acp_old_altitude = acp_altitude
local acp_old_vspeed = acp_vspeed
local acp_old_pitch = acp_pitch
local acp_old_roll = acp_roll

local acp_limits = {}
local acp_target_heading = 0
local acp_diff_heading = 0
local acp_target_dyn_heading = 0
local acp_override_dyn_heading = false
local acp_target_roll = 0
local acp_diff_roll = 0
local acp_target_dyn_roll = 0
local acp_diff_dyn_roll = 0
local acp_target_dyn_yoke_x = 0
local acp_new_yoke_x = 0

local acp_target_altitude = 0
local acp_diff_altitude = 0
local acp_target_dyn_altitude = 0
local acp_override_dyn_altitude = false
local acp_target_dyn_pitch = 0
local acp_diff_dyn_pitch = 0
local acp_target_dyn_yoke_y = 0
local acp_new_yoke_y = 0

local acp_target_speed = 0
local acp_diff_speed = 0
local acp_target_dyn_speed = 0
local acp_target_dyn_throttle = 0
local acp_diff_dyn_speed = 0
local acp_new_yoke_t = 0

-------------------------
local route_points = {}
local count_points = 0
local cur_point = 0
local target_point = 0

local acp_NAV_opt_R = 0
local acp_NAV_opt_W = 0
local acp_NAV_opt_W_deg = 0
local acp_NAV_An = 0
--------------------------------------
--------------------------------------

function acp_FRAME()
	acp_TIMER(acp_TIMER_rate)
	if acp_set == 1 then
		if acp_on == 0 then
			acp_ParamInit()
		end
		acp_on = 1
	else
		acp_on = 0
		acp_set = 0
	end
	if acp_on == 1 and acp_paused ~= 0 then
		acp_NAV_SOLVER()
		acp_SOLVER()
	end
end

function acp_SOLVER()
--- horizontal
	if acp_status_horizont ~= "none" then
		if acp_status_horizont == "course" then			
			acp_diff_heading = acp_DIFF_ANGLE(acp_heading, acp_target_heading)
			if math.abs(acp_diff_heading) > (1 - acp_accuracy) * 0.8 then
				if acp_diff_heading > 90 then 
					acp_diff_heading = 90
				end
				if acp_diff_heading < -90 then
					acp_diff_heading = -90
				end
				acp_diff_heading = acp_SPLINE_BOOST(acp_diff_heading, 90)
				acp_diff_heading = acp_SQR_BOOST(acp_diff_heading, 1)
				if acp_override_dyn_heading == false then				
					acp_target_dyn_heading  = (acp_diff_heading / 90) * acp_NAV_opt_W_deg * acp_limits[1] --- (1.4|1.2|0.8)
				end
				if acp_diff_heading * acp_dyn_heading > 0 then
					if math.abs(acp_target_dyn_heading) > acp_limit_dyn_heading and acp_limit_dyn_heading < 10 then
						acp_target_dyn_heading = math.abs(acp_limit_dyn_heading) * acp_SIGN(acp_target_dyn_heading)
					end	
				else
					acp_target_dyn_heading  = acp_target_dyn_heading / 2
				end			
			end
		end
		---		
		acp_target_roll = math.deg(math.atan((acp_target_dyn_heading * acp_TAS) / 561.3))		
		--acp_test = math.deg(math.atan((acp_TAS * math.rad(acp_target_dyn_heading)) / 9.8))
		if acp_target_roll * acp_roll > 0 then
			if math.abs(acp_target_roll) > acp_limit_roll then
				acp_target_roll = math.abs(acp_limit_roll) * acp_SIGN(acp_target_roll)
			end
		end			
		---
		acp_diff_roll = acp_target_roll - math.deg(math.atan((acp_dyn_heading * acp_TAS) / 561.3))
		acp_target_dyn_roll = acp_diff_roll * acp_limits[2]					-- (c172: 0.6 | an-2: 0.45 | an-24: 0.3)
		if math.abs(acp_target_dyn_roll) > (1 - acp_accuracy) * 0.004 then										-- 0.002
			if acp_diff_roll * acp_dyn_roll > 0 then
				if math.abs(acp_target_dyn_roll) > acp_limit_dyn_roll then
					acp_target_dyn_roll = math.abs(acp_limit_dyn_roll) * acp_SIGN(acp_target_dyn_roll)
				end
			else
				acp_target_dyn_roll = acp_target_dyn_roll / 2 				
			end
			---
			acp_diff_dyn_roll = acp_target_dyn_roll - acp_dyn_roll
			acp_target_dyn_yoke_x = acp_diff_dyn_roll * acp_limits[3]   				-- (c172: 0.06 | an-2: 0.045 | an-24: 0.03)
			acp_new_yoke_x = acp_yoke_x + (acp_target_dyn_yoke_x * acp_frame_period)
		else
			acp_new_yoke_x = acp_yoke_x
		end
		if acp_new_yoke_x > 1 then 
			acp_new_yoke_x = 1
		end
		if acp_new_yoke_x < -1 then
			acp_new_yoke_x = -1
		end
		set("sim/cockpit2/controls/yoke_roll_ratio", acp_new_yoke_x)
	end
--- vertical
	if acp_status_vertical ~= "none" then
		if acp_status_vertical == "alt" then			
			acp_diff_altitude = acp_target_altitude - acp_altitude
			if math.abs(acp_diff_altitude) > (1 - acp_accuracy) * 0.6 then								-- 0.3 | |
				if acp_diff_heading > 30 then 
					acp_diff_heading = 30
				end
				if acp_diff_heading < -30 then
					acp_diff_heading = -30
				end				
				acp_diff_altitude = acp_SQR_BOOST(acp_diff_altitude, 30)
				acp_diff_altitude = acp_SQRT_BOOST(acp_diff_altitude, 3)
				if acp_override_dyn_altitude == false then
					acp_target_dyn_altitude  = (acp_diff_altitude / 30) * acp_NAV_opt_W_deg * acp_limits[4] --acp_limits[4] --- limits (1.6|1.4|1)
				end
				if acp_target_dyn_altitude * acp_dyn_altitude < 0 then
					acp_target_dyn_altitude  = acp_target_dyn_altitude / 2
				end
				if math.abs(acp_target_dyn_altitude) > acp_limit_dyn_altitude and acp_limit_dyn_altitude < 10 then
					acp_target_dyn_altitude = math.abs(acp_limit_dyn_altitude) * acp_SIGN(acp_target_dyn_altitude)						
				end
			end
		end
		---		
		acp_target_dyn_pitch = (acp_target_dyn_altitude - acp_dyn_altitude) * 0.2 -- acp_limits[5] -- --acp_limits[5] 1.8 1.6	-- (c172: 1.2 | an-2: 0.8 | an-24: 1.2)
		if math.abs(acp_target_dyn_pitch) > (1 - acp_accuracy) * 0.04 then										-- 0.02 | |
			if acp_target_dyn_pitch * acp_dyn_pitch < 0 then
				acp_target_dyn_pitch = acp_target_dyn_pitch / 2
			end
			if math.abs(acp_target_dyn_pitch) > acp_limit_dyn_pitch then
				acp_target_dyn_pitch = math.abs(acp_limit_dyn_pitch) * acp_SIGN(acp_target_dyn_pitch)
			end
			---
			acp_diff_dyn_pitch = acp_target_dyn_pitch - acp_dyn_pitch
			acp_target_dyn_yoke_y = acp_diff_dyn_pitch * acp_limits[6] -- (c172: 0.06 | an-2: 0.045 | an-24: 0.03)
			acp_new_yoke_y = acp_yoke_y + (acp_target_dyn_yoke_y * acp_frame_period)
		else
			acp_new_yoke_y = acp_yoke_y
		end
		if acp_new_yoke_y > 1 then 
			acp_new_yoke_y = 1
		end
		if acp_new_yoke_y < -1 then
			acp_new_yoke_y = -1
		end			
		set("sim/cockpit2/controls/yoke_pitch_ratio", acp_new_yoke_y)
	end
--- throttle
	if acp_status_throttle == "on" then
		acp_diff_speed = acp_target_speed - acp_speed
		if math.abs(acp_diff_speed) > (1 - acp_throttle_accuracy) * 2 then							-- | 2 |
			acp_target_dyn_speed  = (acp_diff_speed * acp_limits[7])						-- acp_limits[7](c172: 0.06 | an-2: 0.04 | an-24: 0.008)
			if acp_diff_speed * acp_dyn_speed < 0 then
				acp_target_dyn_speed  = (acp_diff_speed * acp_limits[7])			
			end
			if math.abs(acp_target_dyn_speed) > acp_limit_dyn_speed then
				acp_target_dyn_speed = math.abs(acp_limit_dyn_speed) * acp_SIGN(acp_target_dyn_speed)						
			end	
			if acp_dyn_pitch < 0 and acp_target_dyn_speed > 0 then
				acp_target_dyn_speed = acp_target_dyn_speed / (1 + (-acp_dyn_pitch))
			end
			---
			acp_diff_dyn_speed = acp_target_dyn_speed - acp_dyn_speed
			acp_target_dyn_throttle = acp_diff_dyn_speed * acp_limits[8]--acp_limits[8] 					-- (c172: 0.12 | an-2: 0.15 | an-24: 0.04)
			acp_new_yoke_t = acp_yoke_t + acp_target_dyn_throttle * acp_frame_period
		else
			acp_new_yoke_t = acp_yoke_t
		end
		if acp_new_yoke_t > 1 then 
			acp_new_yoke_t = 1
		end
		if acp_new_yoke_t < acp_limit_min_throttle then
			acp_new_yoke_t = acp_limit_min_throttle
		end
		set("sim/cockpit2/engine/actuators/throttle_ratio_all", acp_new_yoke_t)		
	end
end

function acp_NAV_SOLVER()
	nav_on = false
	acp_override_dyn_heading = false
	acp_override_dyn_altitude = false
	--acp_test = 0
------ gps
	if acp_nav_source == "gps" and get("sim/cockpit2/radios/indicators/gps_nav_id") ~= "" then		
		if cur_point ~= -1 then
			nav_on = true
			if route_points[cur_point].finish == false then
				target_point = cur_point + 1				
				acp_target_heading = acp_NAV_GET_TARGET_HEADING(acp_DIFF_ANGLE(route_points[cur_point].dir, route_points[target_point].head), route_points[target_point].dist, route_points[cur_point].dir)
				if math.abs(acp_DIFF_ANGLE(route_points[target_point].head, route_points[cur_point].head)) < math.abs(acp_DIFF_ANGLE(route_points[target_point].head, acp_target_heading)) then
					acp_target_heading = route_points[cur_point].head					
				end
			else
				target_point = cur_point				
				if count_points ~= 1 then
					cur_point = cur_point - 1
				else
					acp_target_heading = route_points[target_point].head
				end
			end
			lim_next_route = 200
			if route_points[target_point].finish == false then
				lim_next_route = math.abs(acp_DIFF_ANGLE(route_points[target_point].head, acp_NORMDEG(route_points[target_point].dir + 180))) / 2
				lim_next_route = acp_NAV_opt_R / math.tan(math.rad(lim_next_route))
				if lim_next_route > 3200 then
					lim_next_route = 3200
				end
			end
			if route_points[target_point].dist < lim_next_route or route_points[target_point].dist < 185 then
				acp_SET_NEXT_POINT()
			end
		end
	end	
------ vor 1 / 2
	if acp_nav_source == "vor1" or acp_nav_source == "vor2" then
		if acp_nav_source == "vor1" and get("sim/cockpit2/radios/indicators/nav1_nav_id") ~= "" then
			shift_heading = get("sim/cockpit/radios/nav1_hdef_dot") * 4
			dme_dist = get("sim/cockpit/radios/nav1_dme_dist_m") * 1852
			sel_dir = get("sim/cockpit/radios/nav1_obs_degt")
			nav_on = true
		end
		if acp_nav_source == "vor2" and get("sim/cockpit2/radios/indicators/nav2_nav_id") ~= "" then
			shift_heading = get("sim/cockpit/radios/nav2_hdef_dot") * 4
			dme_dist = get("sim/cockpit/radios/nav2_dme_dist_m") * 1852
			sel_dir = get("sim/cockpit/radios/nav2_obs_degt")
			nav_on = true
		end
		if nav_on == true then
			acp_target_heading = acp_NAV_GET_TARGET_HEADING(shift_heading, dme_dist, sel_dir)
		end
	end
------ adf 1 / 2
	if acp_nav_source == "adf1" or acp_nav_source == "adf2" then
		if acp_nav_source == "adf1" and get("sim/cockpit2/radios/indicators/adf1_nav_id") ~= "" then
			dir_leg = get("sim/cockpit/radios/adf1_dir_degt")
			nav_on = true
		end
		if acp_nav_source == "adf2" and get("sim/cockpit2/radios/indicators/adf2_nav_id") ~= "" then
			dir_leg = get("sim/cockpit/radios/adf2_dir_degt")
			nav_on = true
		end
		if nav_on == true then			
			acp_target_heading = acp_NORMDEG(acp_heading + dir_leg)
		end
	end
	
------ kln
	if acp_nav_source == "kln" and XPLMFindDataRef("sim/custom/kln_power") ~= nil then
		nav_on = true
		leg_shift = get("sim/cockpit/radios/gps_hdef_dot") * 2
		point_dist = get("sim/cockpit/radios/gps_dme_dist_m") * 1852
		leg_dir = get("sim/cockpit/autopilot/nav_steer_deg_mag")
		if math.abs(leg_shift * 1852) < (acp_NAV_opt_R) and math.abs(leg_shift) < 5 then			
			shift_heading = ((acp_DIFF_ANGLE(acp_heading, leg_dir) / 90) + (leg_shift / 2)) * acp_NAV_opt_W_deg * acp_limits[1]
			shift_heading = acp_SQRT_BOOST(shift_heading, 0.3)
			acp_target_dyn_heading = shift_heading
			acp_override_dyn_heading = true
		else
			acp_target_heading = acp_NORMDEG(leg_dir)
		end
	end
------ ap
	if acp_nav_source == "ap" then
		shift_heading =  get("sim/cockpit2/autopilot/flight_director_roll_deg")	
		shift_heading = (shift_heading / 30) * acp_NAV_opt_W_deg * acp_limits[1]		
		shift_heading = acp_REV_SQR_BOOST(shift_heading, 30)
		--shift_heading = acp_SQR_BOOST(shift_heading, 3)
		shift_heading = acp_SQR_BOOST(shift_heading, 1)
		acp_target_dyn_heading = shift_heading
		acp_override_dyn_heading = true
		if acp_ap_pitch_control == true then
			shift_altitude =  get("sim/cockpit2/autopilot/flight_director_pitch_deg")
			shift_altitude = (shift_altitude / 2.5) * acp_NAV_opt_W_deg * acp_limits[4]
			shift_altitude = acp_SQR_BOOST(shift_altitude, 2.5)
			acp_test = shift_altitude
			acp_target_dyn_altitude = shift_altitude
			acp_override_dyn_altitude = true
		end
	end
end

function acp_DRAW()
	if acp_vis == 2 then
		acp_vis = 0
	end
	if acp_vis == 1 then
		if acp_show_ap == 1 then
			glColor4f(0.14,0.15,0.2,0.65)
			glRectf(ln_x, ln_y, ln_x + ln_width, ln_y + ln_height)
			draw_string(ln_x + 282, ln_y + 34, "AutoPilot  X", "grey" )
-------- main button
			if acp_on == 1 then
				glColor4f(0.35,0.35,0.35,1)
				glRectf(ln_x + 6, ln_y + 6, ln_x + 44, ln_y + 38)
				glColor4f(0,1,0,1)
				draw_string_Helvetica_18(ln_x + 10, ln_y + 15, "ON")
			else
				glColor4f(0.4,0.4,0.4,1)
				glRectf(ln_x + 6, ln_y + 6, ln_x + 44, ln_y + 38)
				glColor4f(0.6,0.6,0.6,1)
				draw_string_Helvetica_18(ln_x + 10, ln_y + 15, "ON")
			end
-------- alt
			if acp_status_vertical == "alt" then
				ili_draw_button(ln_x + 54, ln_y + 23, 112, "Hold Altitude", 0, 1, 0, 0.35, 0.35, 0.35)
			else
				ili_draw_button(ln_x + 54, ln_y + 23, 112, "Hold Altitude", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
			end
-------- vs
			if acp_status_vertical == "vs" then
				ili_draw_button(ln_x + 54, ln_y + 6, 112, "Hold Vertical speed", 0, 1, 0, 0.35, 0.35, 0.35)
			else
				ili_draw_button(ln_x + 54, ln_y + 6, 112, "Hold Vertical speed", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
			end
-------- course
			if acp_status_horizont == "course" then
				ili_draw_button(ln_x + 176, ln_y + 23, 96, "Hold Course", 0, 1, 0, 0.35, 0.35, 0.35)
			else
				ili_draw_button(ln_x + 176, ln_y + 23, 96, "Hold Course", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
			end
-------- turn
			if acp_status_horizont == "turn" then
				ili_draw_button(ln_x + 176, ln_y + 6, 96, "Hold turn speed", 0, 1, 0, 0.35, 0.35, 0.35)
			else
				ili_draw_button(ln_x + 176, ln_y + 6, 96, "Hold turn speed", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
			end
-------- throttle
			if acp_status_throttle == "on" then
				ili_draw_button(ln_x + 282, ln_y + 6, 78, "AutoThrottle", 0, 1, 0, 0.35, 0.35, 0.35)
			else
				ili_draw_button(ln_x + 282, ln_y + 6, 78, "AutoThrottle", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
			end
------------------------------
-------- additional menu 1
			if acp_add_vis == 1 then
				glColor4f(0.11,0.12,0.17,0.65)
				glRectf(ln_x, ln_y, ln_x + 366, ln_y - ln_add_height)
				glColor4f(0.18,0.20,0.24,0.32)
				graphics.draw_filled_arc(ln_x + 183, ln_y, 90, 270, 12)
				glColor4f(0.7,0.7,0.7,1)
				graphics.set_width(1)
				graphics.draw_line(ln_x + 178, ln_y - 5, ln_x + 183, ln_y - 1)
				graphics.draw_line(ln_x + 183, ln_y - 1, ln_x + 188, ln_y - 5)
				graphics.draw_line(ln_x + 178, ln_y - 8, ln_x + 183, ln_y - 4)
				graphics.draw_line(ln_x + 183, ln_y - 4, ln_x + 188, ln_y - 8)
----------------
				draw_string(ln_x + 117, ln_y - 24, "Navigation data source", 0.1,0.6,0.1)
----------------Autopilot output
				if acp_nav_source == "vor1" then
					ili_draw_button(ln_x + 6, ln_y - 50, 111, "VOR 1", 0, 1, 0, 0.35, 0.35, 0.35)
				else
					ili_draw_button(ln_x + 6, ln_y - 50, 111, "VOR 1", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
				end				
				if acp_nav_source == "vor2" then
					ili_draw_button(ln_x + 127, ln_y - 50, 111, "VOR 2", 0, 1, 0, 0.35, 0.35, 0.35)
				else
					ili_draw_button(ln_x + 127, ln_y - 50, 111, "VOR 2", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
				end
				if acp_nav_source == "adf1" or acp_nav_source == "adf2" then
					if acp_nav_source == "adf1" then
						ili_draw_button(ln_x + 248, ln_y - 50, 111, "ADF 1", 0, 1, 0, 0.35, 0.35, 0.35)
					else
						ili_draw_button(ln_x + 248, ln_y - 50, 111, "ADF 2", 0, 1, 0, 0.35, 0.35, 0.35)
					end
				else
					ili_draw_button(ln_x + 248, ln_y - 50, 111, "ADF 1 / 2", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
				end
				if acp_nav_source == "gps" then
					ili_draw_button(ln_x + 6, ln_y - 68, 111, "FMS plane", 0, 1, 0, 0.35, 0.35, 0.35)
				else
					ili_draw_button(ln_x + 6, ln_y - 68, 111, "FMS plane", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
				end
				if acp_nav_source == "kln" then
					ili_draw_button(ln_x + 127, ln_y - 68, 111, "BendixKing KLN-90b", 0, 1, 0, 0.35, 0.35, 0.35)
				else
					ili_draw_button(ln_x + 127, ln_y - 68, 111, "BendixKing KLN-90b", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
				end
				if acp_nav_source == "ap" then
					ili_draw_button(ln_x + 248, ln_y - 68, 111, "Autopilot output", 0, 1, 0, 0.35, 0.35, 0.35)
				else
					ili_draw_button(ln_x + 248, ln_y - 68, 111, "Autopilot output", 0.7, 0.7, 0.7, 0.4, 0.4, 0.4)
				end
------------------------------
-------- additional menu 2
				if acp_add2_vis == 1 then
					glColor4f(0.11,0.12,0.17,0.65)
					glRectf(ln_x, ln_y - ln_add_height, ln_x + 366, ln_y - ln_add_height - ln_add2_height)
					glColor4f(0.18,0.20,0.24,0.32)
					graphics.draw_filled_arc(ln_x + 183, ln_y - ln_add_height, 90, 270, 12)
					glColor4f(0.7,0.7,0.7,1)
					graphics.set_width(1)
					graphics.draw_line(ln_x + 178, ln_y - ln_add_height - 5, ln_x + 183, ln_y - ln_add_height - 1)
					graphics.draw_line(ln_x + 183, ln_y - ln_add_height - 1, ln_x + 188, ln_y - ln_add_height - 5)
					graphics.draw_line(ln_x + 178, ln_y - ln_add_height - 8, ln_x + 183, ln_y - ln_add_height - 4)
					graphics.draw_line(ln_x + 183, ln_y - ln_add_height - 4, ln_x + 188, ln_y - ln_add_height - 8)
----------------
					draw_string(ln_x + 117, ln_y - 100, "Adjustments and Limits", 0.1,0.6,0.1)
---------------- adjustments
					ili_draw_slider(acp_accuracy * 100, 1, 100, ln_x + 6, ln_y - 122, 140, "Pitch and Roll accuracy")
					ili_draw_slider(acp_throttle_accuracy * 100, 1, 100, ln_x + 190, ln_y - 122, 140, "Throttle accuracy")
					ili_draw_slider_sp(acp_limit_dyn_heading * 2, 2, 2, 20, ln_x + 6, ln_y - 157, 140, "Heading limit (deg/sec)")
					ili_draw_slider(acp_limit_roll, 1, 90, ln_x + 190, ln_y - 157, 140, "Bank angle limit (degrees)")
					ili_draw_slider_sp(acp_limit_dyn_altitude * 2, 2, 1, 20, ln_x + 6, ln_y - 194, 140, "V/Speed limit (meters/sec)")
					ili_draw_slider(acp_limit_min_throttle * 100, 1, 100, ln_x + 190, ln_y - 194, 140, "Min throttle level (percent)")
---------------- reset button
					ili_draw_button(ln_x + 65, ln_y - 238, 44, "RESET", 0.7, 0.7, 0.7, 0.35, 0.35, 0.35)
---------------- save button
					if acp_editparam == false then
						ili_draw_button(ln_x + 257, ln_y - 238, 44, "SAVE", 0.5, 0.5, 0.5, 0.35, 0.35, 0.35)
					else
						ili_draw_button(ln_x + 257, ln_y - 238, 44, "SAVE", 0, 1, 0, 0.35, 0.35, 0.35)
					end					
				else
					glColor4f(0.11,0.12,0.17,0.65)
					graphics.draw_filled_arc(ln_x + 183, ln_y - ln_add_height, 90, 270, 12)
					glColor4f(0.7,0.7,0.7,1)
					graphics.set_width(1)
					graphics.draw_line(ln_x + 178, ln_y - ln_add_height - 1, ln_x + 183, ln_y - ln_add_height - 5)
					graphics.draw_line(ln_x + 183, ln_y - ln_add_height - 5, ln_x + 188, ln_y - ln_add_height - 1)
					graphics.draw_line(ln_x + 178, ln_y - ln_add_height - 4, ln_x + 183, ln_y - ln_add_height - 8)
					graphics.draw_line(ln_x + 183, ln_y - ln_add_height - 8, ln_x + 188, ln_y - ln_add_height - 4)							
				end
			else
				glColor4f(0.11,0.12,0.17,0.65)
				graphics.draw_filled_arc(ln_x + 183, ln_y, 90, 270, 12)
				glColor4f(0.7,0.7,0.7,1)
				graphics.set_width(1)
				graphics.draw_line(ln_x + 178, ln_y - 1, ln_x + 183, ln_y - 5)
				graphics.draw_line(ln_x + 183, ln_y - 5, ln_x + 188, ln_y - 1)
				graphics.draw_line(ln_x + 178, ln_y - 4, ln_x + 183, ln_y - 8)
				graphics.draw_line(ln_x + 183, ln_y - 8, ln_x + 188, ln_y - 4)			
			end
		else
			draw_string(ln_x + 286, ln_y + 34, "AutoPilot", 0.85,0.85,0.85)
		end
	end

		--draw_string(150, 230, "shift_heading: "..acp_test.." |shift_heading2 "..acp_test4)
		--draw_string(150, 220, "shift_heading: "..acp_test.." |shift_heading2"..acp_test2)
		--draw_string(150, 210, "acp_NAV_opt_R: "..acp_NAV_opt_R.." |acp_target_dyn_heading "..acp_target_dyn_heading)
		--draw_string(150, 200, "acp_target_dyn_heading: "..acp_target_dyn_heading.." | acp_target_heading: "..acp_target_heading)
end

function acp_CLICK()
	if acp_vis == 2 then
		acp_vis = 0
	end
	if acp_vis == 1 then
		if acp_show_ap == 1 then
-------- hide		
			if MOUSE_X > ln_x + 282 and MOUSE_X < ln_x + 366 and MOUSE_Y > ln_y +32 and MOUSE_Y < ln_y + 44 and MOUSE_STATUS == "down" then
				acp_show_ap = 0
				return
			end
-------- main button
			if MOUSE_X > ln_x + 6 and MOUSE_X < ln_x + 44 and MOUSE_Y > ln_y +6 and MOUSE_Y < ln_y + 38 and MOUSE_STATUS == "down" then
				acp_set = acp_set + 1
				return
			end
-------- alt			
			if ili_check_button(ln_x + 54, ln_y + 23, 112) == true then
				if acp_status_vertical == "alt" then
					acp_status_vertical = "none"
				else
					acp_target_altitude = acp_altitude
					acp_status_vertical = "alt"
				end
				return
			end
-------- vs			
			if ili_check_button(ln_x + 54, ln_y + 6, 112) == true then
				if acp_status_vertical == "vs" then
					acp_status_vertical = "none"
				else
					acp_target_dyn_altitude = acp_dyn_altitude
					acp_status_vertical = "vs"
				end
				return
			end
-------- course			
			if ili_check_button(ln_x + 176, ln_y + 23, 96) == true then
				if acp_status_horizont == "course" then
					acp_status_horizont = "none"
				else
					acp_target_heading = acp_heading
					acp_status_horizont = "course"
				end
				return
			end
-------- turn			
			if ili_check_button(ln_x + 176, ln_y + 6, 96) == true then
				if acp_status_horizont == "turn" then
					acp_status_horizont = "none"
				else
					acp_target_dyn_heading = acp_dyn_heading
					acp_status_horizont = "turn"
				end
				return
			end
-------- throttle			
			if ili_check_button(ln_x + 282, ln_y + 6, 78) == true then
				if acp_status_throttle == "on" then
					acp_status_throttle = "none"
				else
					acp_target_speed = acp_speed
					acp_status_throttle = "on"
				end
				return
			end
----------------------------------------
-------------	additioal menu 1
			if MOUSE_X > ln_x + 174 and MOUSE_X < ln_x + 193 and MOUSE_Y < ln_y - 0 and MOUSE_Y > ln_y - 12 and MOUSE_STATUS == "down" then
				if acp_add_vis == 0 then
					acp_add_vis = 1
				else
					acp_add_vis = 0
				end
				return
			end
-------- vor 1
			if ili_check_button(ln_x + 6, ln_y - 50, 111) == true then
				if acp_nav_source == "vor1" then
					acp_nav_source = "none"
					acp_target_heading = acp_heading
				else
					acp_nav_source = "vor1"
				end
				return
			end			
-------- vor 2		
			if ili_check_button(ln_x + 127, ln_y - 50, 111) == true then
				if acp_nav_source == "vor2" then
					acp_nav_source = "none"
					acp_target_heading = acp_heading
				else
					acp_nav_source = "vor2"
				end
				return
			end			
-------- adf 1 / 2
			if ili_check_button(ln_x + 248, ln_y - 50, 111) == true then
				if acp_nav_source == "adf1" or acp_nav_source == "adf2" then
					if acp_nav_source == "adf1" then
						acp_nav_source = "adf2"
					else
						acp_nav_source = "none"
						acp_target_heading = acp_heading
					end
				else
					acp_nav_source = "adf1"
				end
				return
			end		
-------- fms
			if ili_check_button(ln_x + 6, ln_y - 68, 111) == true then
				if acp_nav_source == "gps" then
					acp_nav_source = "none"
					acp_target_heading = acp_heading
				else
					acp_nav_source = "gps"
					acp_FP_INIT(acp_nav_source)
					cur_point = acp_FIND_NEAREST_POINT()
					if cur_point ~= -1 then
						if route_points[cur_point].start == false and route_points[cur_point].finish == false then					
							if math.abs(acp_DIFF_ANGLE(route_points[cur_point].head, route_points[cur_point - 1].dir)) + 15 < math.abs(acp_DIFF_ANGLE(route_points[cur_point].head, acp_NORMDEG(route_points[cur_point].dir + 180))) then
							cur_point = cur_point - 1
							end
						end
					end
				end
				return
			end
-------- kln
			if ili_check_button(ln_x + 127, ln_y - 68, 111) == true then
				if acp_nav_source == "kln" then
					acp_nav_source = "none"
					acp_target_heading = acp_heading
				else
					acp_nav_source = "kln"
					set("sim/cockpit2/radios/actuators/HSI_source_select_pilot", 2)
				end
				return
			end
-------- ap
			if ili_check_button(ln_x + 248, ln_y - 68, 111) == true then
				if acp_nav_source == "ap" then
					acp_nav_source = "none"
				else
					acp_nav_source = "ap"
				end
				return
			end					
----------------------------------------
-------------	additioal menu 2
			if MOUSE_X > ln_x + 174 and MOUSE_X < ln_x + 193 and MOUSE_Y < ln_y - ln_add_height - 0 and MOUSE_Y > ln_y - ln_add_height - 12 and MOUSE_STATUS == "down" and  acp_add_vis == 1 then
				if acp_add2_vis == 0 then
					acp_add2_vis = 1
				else
					acp_add2_vis = 0
				end
				return
			end
------------- Pitch and Roll accuracy
			if acp_add2_vis == 1 then
				acp_oldres = acp_accuracy
				acp_accuracy = ili_check_slider(acp_accuracy * 100, 1, 100, ln_x + 6, ln_y - 122, 140) / 100
				if acp_oldres ~= acp_accuracy then
					acp_editparam = true
				end
			end
------------- Throttle accuracy
			if acp_add2_vis == 1 then
				acp_oldres = acp_throttle_accuracy
				acp_throttle_accuracy = ili_check_slider(acp_throttle_accuracy * 100, 1, 100, ln_x + 190, ln_y - 122, 140) / 100
				if acp_oldres ~= acp_throttle_accuracy then
					acp_editparam = true
				end
			end
------------- Heading limit
			if acp_add2_vis == 1 then
				acp_oldres = acp_limit_dyn_heading
				acp_limit_dyn_heading = ili_check_slider(acp_limit_dyn_heading * 2, 2, 20, ln_x + 6, ln_y - 157, 140) / 2
				if acp_oldres ~= acp_limit_dyn_heading then
					acp_editparam = true
				end
			end
------------- Bank angle limit
			if acp_add2_vis == 1 then
				acp_oldres = acp_limit_roll
				acp_limit_roll = ili_check_slider(acp_limit_roll, 1, 90, ln_x + 190, ln_y - 157, 140)
				if acp_oldres ~= acp_limit_roll then
					acp_editparam = true
				end
			end
------------- V/Speed limit
			if acp_add2_vis == 1 then
				acp_oldres = acp_limit_dyn_altitude
				acp_limit_dyn_altitude = ili_check_slider(acp_limit_dyn_altitude * 2, 1, 20, ln_x + 6, ln_y - 194, 140) / 2
				if acp_oldres ~= acp_limit_dyn_altitude then
					acp_editparam = true
				end
			end
------------- Min throttle level
			if acp_add2_vis == 1 then
				acp_oldres = acp_limit_min_throttle
				acp_limit_min_throttle = ili_check_slider(acp_limit_min_throttle * 100, 1, 100, ln_x + 190, ln_y - 194, 140) / 100
				if acp_oldres ~= acp_limit_min_throttle then
					acp_editparam = true
				end
			end
--------------
---------------- reset button
			if ili_check_button(ln_x + 65, ln_y - 238, 44) == true then
				acp_reset_settings()		
				return
			end
---------------- save button
			if ili_check_button(ln_x + 257, ln_y - 238, 44) == true then
				acp_saveparam()		
				return
			end		
----------------------------------------			
------------- drag
			if acp_add_vis == 1 then
				if acp_add2_vis == 1 then
					ln_drag_add_height = ln_add_height + ln_add2_height
				else
					ln_drag_add_height = ln_add_height
				end
			else
				ln_drag_add_height = 0
			end
			if MOUSE_STATUS == "down" then
				ln_drag_x = MOUSE_X - ln_x
				ln_drag_y = MOUSE_Y - ln_y
			end
			if MOUSE_X > ln_x and MOUSE_X < ln_x + ln_width and MOUSE_Y > ln_y - ln_drag_add_height and MOUSE_Y < ln_y + ln_height and MOUSE_STATUS == "drag" and acp_drag_stat == false then
				ln_adjust = 1
				acp_drag_stat = true
			end
			if MOUSE_STATUS == "up" then
				ln_adjust = 0
				acp_drag_stat = false
			end
			if MOUSE_STATUS == "drag" and ln_adjust == 1 then
				ln_x = MOUSE_X - ln_drag_x
				ln_y = MOUSE_Y - ln_drag_y
				if ln_x < 0 then
					ln_x = 0
					ln_drag_x = MOUSE_X - ln_x
				end
				if ln_x > (SCREEN_WIDTH - ln_width) then
					ln_x = SCREEN_WIDTH - ln_width
					ln_drag_x = MOUSE_X - ln_x
				end
				if ln_y < 0 then
					ln_y = 0
					ln_drag_y = MOUSE_Y - ln_y
				end
				if ln_y > (SCREEN_HIGHT - ln_height) then
					ln_y = SCREEN_HIGHT - ln_height
					ln_drag_y = MOUSE_Y - ln_y
				end
			end
			if MOUSE_X > ln_x and MOUSE_X < ln_x + ln_width and MOUSE_Y > ln_y - ln_drag_add_height and MOUSE_Y < ln_y + ln_height then
				RESUME_MOUSE_CLICK = true
			end
		else
-------- show
			if MOUSE_X > ln_x + 282 and MOUSE_X < ln_x + 348 and MOUSE_Y > ln_y +30 and MOUSE_Y < ln_y + 44 and MOUSE_STATUS == "down" then
				acp_show_ap = 1
			end
		end
	end
end

function acp_ParamInit()
	acp_old_heading = acp_heading
	acp_old_speed = acp_speed
	acp_old_altitude = acp_altitude
	acp_old_vspeed = acp_vspeed
	acp_old_pitch = acp_pitch
	acp_old_roll = acp_roll
	
	acp_SET_LIMITS(acp_weight)
	
	acp_target_heading = acp_heading
	acp_target_dyn_heading = acp_dyn_heading
	acp_target_altitude = acp_altitude
	acp_target_dyn_altitude = acp_dyn_altitude
	acp_target_speed = acp_speed
end

function acp_ParamReader()
	acp_dyn_heading = (acp_heading - acp_old_heading) * (1000 / acp_TIMER_cycletime)
	acp_dyn_speed = (acp_speed - acp_old_speed) * (1000 / acp_TIMER_cycletime)
	acp_dyn_altitude = (acp_altitude - acp_old_altitude) * (1000 / acp_TIMER_cycletime)
	acp_dyn_vspeed = (acp_vspeed - acp_old_vspeed ) * (1000 / acp_TIMER_cycletime)
	acp_dyn_pitch = (acp_pitch - acp_old_pitch) * (1000 / acp_TIMER_cycletime)
	acp_dyn_roll = (acp_roll - acp_old_roll) * (1000 / acp_TIMER_cycletime)

	acp_old_heading = acp_heading
	acp_old_speed = acp_speed
	acp_old_altitude = acp_altitude
	acp_old_vspeed = acp_vspeed
	acp_old_pitch = acp_pitch
	acp_old_roll = acp_roll
end

function acp_TIMER(msec)
	acp_TIMER_curtime = acp_TIMER_curtime + (acp_frame_period * 1000)
	if acp_TIMER_curtime >= msec then
		acp_TIMER_cycletime = acp_TIMER_curtime - acp_TIMER_cycleshift
		acp_TIMER_curtime = acp_TIMER_curtime - msec		
		---------
		acp_ParamReader()
		acp_NAV_UPDATE()
		---------
		acp_TIMER_cycleshift = acp_TIMER_curtime
	end
end

function acp_NAV_GET_TARGET_HEADING(ang, dist, direct)
	ngth_shift_heading = ang
	acp_test2 = ang
	if dist ~= 0 then				
		ngth_shift_dist = dist * math.sin(math.rad(ang))
		ngth_shift_heading = (ngth_shift_dist / (acp_NAV_opt_R * 2.5)) * 10
	end
	if ngth_shift_heading > 10 then 
		ngth_shift_heading = 10
	end
	if ngth_shift_heading < -10 then
		ngth_shift_heading = -10
	end
	acp_test = ngth_shift_dist
	
	ngth_shift_t_heading = acp_DIFF_ANGLE(acp_t_heading, acp_heading)	
	ngth_shift_heading = (ngth_shift_heading * 9)
	acp_test3 = ngth_shift_heading
	--ngth_shift_heading = acp_SPLINE_BOOST(ngth_shift_heading, 90)
	--ngth_shift_heading = acp_SQR_BOOST(ngth_shift_heading, 1)
	ngth_shift_heading = acp_SQR_BOOST(ngth_shift_heading, 90)
	ngth_shift_heading = acp_REV_SQR_BOOST(ngth_shift_heading, 5)
	ngth_shift_heading = acp_REV_SQR_BOOST(ngth_shift_heading, 5)
	
	ngth_shift_heading = ngth_shift_heading + ngth_shift_t_heading + 1.6
	acp_test4 = ngth_shift_heading
	ngth_result = acp_NORMDEG(direct + ngth_shift_heading)
	if math.abs(acp_DIFF_ANGLE(ngth_result, acp_heading)) > 180 then
		ngth_result = acp_NORMDEG(direct - (90 * acp_SIGN(ngth_shift_heading)))
	end
	return ngth_result			
end

function acp_FP_INIT(FPtype)
	if FPtype == "gps" then		
		count_points = XPLMCountFMSEntries()
		if count_points ~= 0 then
		route_points.length = 0;
			for i = 0, count_points - 1 do
				my_nav_type, my_nav_name, my_nav_ID,  my_nav_altitude, my_nav_lat, my_nav_lon = XPLMGetFMSEntryInfo(i)
				route_points[i] = {}
				route_points[i].ID = my_nav_name
				if i ~= 0 then
					route_points[i].start = false
				else
					route_points[i].start = true
				end
				if i ~= count_points - 1 then
					route_points[i].finish = false
				else
					route_points[i].finish = true
				end
				route_points[i].lat = my_nav_lat
				route_points[i].lon = my_nav_lon
			end			
			for i = 0, count_points - 1 do
				if i ~= count_points - 1 then
					route_points[i].dir, route_points[i].len = ini_get_dir_and_len(route_points[i].lat, route_points[i].lon, route_points[i+1].lat, route_points[i+1].lon)
				end
			end
		end
	end
end

function acp_SET_NEXT_POINT()
	if cur_point ~= -1 then
		if route_points[cur_point + 1].finish == false then
			cur_point = cur_point + 1
			acp_UPDATE_POINT(cur_point)
		else	
			acp_target_heading = route_points[cur_point].head
			cur_point = -1
		end
	end
end

function acp_FIND_NEAREST_POINT()
	res = -1	
	if count_points ~= 0 then		
		old = 0
		len = 0		
		for i = 0, count_points - 1 do
			acp_UPDATE_POINT(i)
			len = route_points[i].dist			
			if len < old or i == 0 then
				old = len
				res = i
			end
		end
	end
	return res
end

function acp_UPDATE_POINT(num)
	route_points[num].head, route_points[num].dist = ini_get_dir_and_len(acp_latitude, acp_longitude, route_points[num].lat, route_points[num].lon)
	route_points[cur_point].head, route_points[cur_point].dist = ini_get_dir_and_len(acp_latitude, acp_longitude, route_points[cur_point].lat, route_points[cur_point].lon)

end

function acp_NAV_UPDATE()
	W = math.rad(math.abs(acp_dyn_heading))
	V = acp_speed * 0.5144
	acp_NAV_An = W * V
	if acp_NAV_An > 0 then
		acp_NAV_opt_W = 4.7 / V --An 4.7
		if math.deg(acp_NAV_opt_W) > acp_limit_dyn_heading then
			acp_NAV_opt_W = math.rad(acp_limit_dyn_heading)
		end		
		acp_NAV_opt_R = V / acp_NAV_opt_W 
		acp_NAV_opt_W_deg = math.deg(acp_NAV_opt_W)
	end	
	if acp_nav_source == "gps" and nav_on == true and cur_point ~= -1 then
		acp_UPDATE_POINT(target_point)
		acp_UPDATE_POINT(cur_point)
	end
end

function acp_DIFF_ANGLE(ang1, ang2)
	local res = 0
	res = ang2 - ang1
	result = res
	if math.abs(res) > 180 then
		if res < 0 then
			result = (res + 360)
		end
		if res > 0 then
			result = (360 - res) * -1
		end
	end
	return result
end

function round(num, idp)
	--workaround for the SASL bug
	local mult = string.format("%f", 10^(idp or 0))
	--local mult = 10^(idp or 0)
	return math.floor(num * mult + 0.5) / mult
end

function acp_NORMDEG(d)
	if d < -360 then
		d = d + 360
	end
	if d > 360 then
		d = d - 360
	end
	if d < 0 then
		d = d + 360
	end
	return d
end
				
function acp_SET_LIMITS(weight)
	acp_limits[1] = acp_GET_LIM(1.4, 0.8, weight) -- acp_target_dyn_heading
	acp_limits[2] = acp_GET_LIM(0.6, 0.3, weight) -- acp_target_dyn_roll
	acp_limits[3] = acp_GET_LIM(0.06, 0.03, weight) -- acp_target_dyn_yoke_x
	acp_limits[4] = acp_GET_LIM(1.6, 1, weight) -- acp_target_dyn_altitude
	acp_limits[5] = acp_GET_LIM(0.8, 0.4, weight) -- acp_target_dyn_pitch
	acp_limits[6] = acp_GET_LIM(0.08, 0.04, weight) -- acp_target_dyn_yoke_y
	acp_limits[7] = acp_GET_LIM(0.06, 0.02, weight) -- acp_target_dyn_speed
	acp_limits[8] = acp_GET_LIM(0.12, 0.04, weight) -- acp_target_dyn_throttle

end

function acp_GET_LIM(hi,lo, weight)
	start = 1000
	finish = 13000
	if weight < start then
		weight = start
	end
	if weight > finish then
		weight = finish
	end
	result = (((math.sqrt((weight - start) / (finish - start)) * -1) +1) * (hi -lo)) + lo
	return result
end

function acp_SQR_BOOST(cur, lim)
	result = cur
	if math.abs(cur) < lim then
		result = ((math.abs(cur) / lim) ^ 2) * lim
		result = result * acp_SIGN(cur)
	end		
	return result
end

function acp_SQRT_BOOST(cur,lim)
	result = cur
	if math.abs(cur) < lim then
		result = math.sqrt(math.abs(cur) / lim) * lim
		result = result * acp_SIGN(cur)
	end		
	return result
end

function acp_REV_SQR_BOOST(cur,lim)
	result = cur
	if math.abs(cur) < lim then
		result = (1 - (1 - math.abs(cur) / lim) ^ 2) * lim
		result = result * acp_SIGN(cur)
	end		
	return result
end

function acp_SPLINE_BOOST(cur,lim)
	result = cur
	if math.abs(cur) < lim then
		result = (((2 * ((math.abs(cur) / lim) - 0.5) ^ 3) / math.abs((math.abs(cur) / lim) - 0.5)) + 0.5) * lim
		result = result * acp_SIGN(cur)
	end		
	return result
end

function acp_SIGN(val)
	result = 0
	if val ~= 0 then
		result = math.abs(val) / val
	end
	return result
end

function ili_draw_slider(val, minval, maxval, x, y, width, caption)
	glColor4f(0.05,0.3,0.05,1)
	glRectf(x + 15, y - 6, x + 15 + width, y - 8)
	glColor4f(0.35,0.35,0.35,1)
	glRectf(x, y, x + 15, y - 15)
	glRectf(x + width + 15, y, x + width + 30, y - 15)
	stepval = (width - 15) / (maxval - minval)
	glRectf(x + 15 + math.floor(stepval * (val - minval)), y, x + 30 + math.floor(stepval * (val - minval)), y - 15)
	draw_string(x + 2, y - 9, "—", 0.7,0.7,0.7)
	draw_string(x + width + 15 + 3, y - 10, "+", 0.7,0.7,0.7)
	
	draw_string(x + 15 + math.floor(stepval * (val - minval)) + (8 - (measure_string(val) / 2)), y - 9, val, 0.7,0.7,0.7)
	draw_string(x + 15 + math.floor(stepval * (val - minval)) + 3, y - 13, "...", 0.2,0.2,0.2)
	captx = ((width + 30) / 2) - (measure_string(caption) / 2)
	draw_string(x + captx, y + 5, caption, 0.75,0.75,0.75)
end
function ili_draw_slider_sp(val, seg, minval, maxval, x, y, width, caption)
	glColor4f(0.05,0.3,0.05,1)
	glRectf(x + 15, y - 6, x + 15 + width, y - 8)
	glColor4f(0.35,0.35,0.35,1)
	glRectf(x, y, x + 15, y - 15)
	glRectf(x + width + 15, y, x + width + 30, y - 15)
	stepval = (width - 15) / (maxval - minval)
	glRectf(x + 15 + math.floor(stepval * (val - minval)), y, x + 30 + math.floor(stepval * (val - minval)), y - 15)
	draw_string(x + 2, y - 9, "—", 0.7,0.7,0.7)
	draw_string(x + width + 15 + 3, y - 10, "+", 0.7,0.7,0.7)
	if val == maxval then
		draw_string(x + 15 + math.floor(stepval * (val - minval)) + (8 - (measure_string("∞") / 2)), y - 9, "∞", 0.7,0.7,0.7)
	else
		draw_string(x + 15 + math.floor(stepval * (val - minval)) + (8 - (measure_string(val / seg) / 2)), y - 9, val / seg, 0.7,0.7,0.7)
	end
	draw_string(x + 15 + math.floor(stepval * (val - minval)) + 3, y - 13, "...", 0.2,0.2,0.2)
	captx = ((width + 30) / 2) - (measure_string(caption) / 2)
	draw_string(x + captx, y + 5, caption, 0.75,0.75,0.75)
end
function ili_draw_button(x, y, width, caption, r, g, b, bgr, bgg, bgb)
	glColor4f(bgr, bgg, bgb,1)
	glRectf(x, y, x + width, y + 15)
	draw_string(x + ((width  / 2) - (measure_string(caption) / 2)), y + 4, caption, r, g, b)
end
function ili_check_slider(val, minval, maxval, x, y, width)
	result = val
	stepval = (width - 15) / (maxval - minval)
	if MOUSE_X > x and MOUSE_X < x + 15 and MOUSE_Y < y and MOUSE_Y > y - 15 and MOUSE_STATUS == "down" then
		result = result - 1
		if result < minval then
			result = minval
		end
	return result
	end
	if MOUSE_X > x + width + 15 and MOUSE_X < x + width + 30 and MOUSE_Y < y and MOUSE_Y > y - 15 and MOUSE_STATUS == "down" then
		result = result + 1
		if result > maxval then
			result = maxval
		end
	return result
	end
	if MOUSE_X > x + 15 + math.floor(stepval * (val - minval)) and MOUSE_X < x + 30 + math.floor(stepval * (val - minval)) and MOUSE_Y < y and MOUSE_Y > y - 15 and MOUSE_STATUS == "drag" then
		dragst = 1
		acp_drag_stat = true
	end
	if MOUSE_STATUS == "up" then
		dragst = 0
		acp_drag_stat = false
	end
	if MOUSE_X > x + 15 and MOUSE_X < x + 15 + width and MOUSE_Y < y and MOUSE_Y > y - 15 and dragst == 1 and MOUSE_STATUS == "drag" then
		result = math.floor((MOUSE_X - x - 22) / stepval) + minval
		if result < minval then
			result = minval
		end
		if result > maxval then
			result = maxval
		end
		return result
	end
	if MOUSE_X > x + 15 + math.floor(stepval * (val - minval)) and MOUSE_X < x + 30 + math.floor(stepval * (val - minval)) and MOUSE_Y < y and MOUSE_Y > y - 15 then
		RESUME_MOUSE_CLICK = true
	end
	if MOUSE_X > x + 15 and MOUSE_X < x + 15 + width and MOUSE_Y < y and MOUSE_Y > y - 10 and MOUSE_STATUS == "down" then
		result = math.floor((MOUSE_X - x - 22) / stepval) + minval
				if result < minval then
			result = minval
		end
		if result > maxval then
			result = maxval
		end
		return result
	end
	return result
end
function ili_check_button(x, y, width)
	if MOUSE_X > x and MOUSE_X < x + width and MOUSE_Y > y and MOUSE_Y < y + 15 and MOUSE_STATUS == "down" then
		return true
	end
	return false
end
-----------------
function ini_get_dir_and_len(lat1, lon1, lat2, lon2)
earth_r = 6372795
--earth_r = 6371e3

lat1_R = math.rad(lat1)
lon1_R = math.rad(lon1)
lat2_R = math.rad(lat2)
lon2_R = math.rad(lon2)

lat1_SIN = math.sin(lat1_R)
lat2_SIN = math.sin(lat2_R)
lat1_COS = math.cos(lat1_R)
lat2_COS = math.cos(lat2_R)

D_SIN = math.sin(lon2_R - lon1_R)
D_COS = math.cos(lon2_R - lon1_R)

lat12CS = lat1_COS * lat2_SIN
lat12SC = lat1_SIN * lat2_COS

a = math.sqrt((lat2_COS * D_SIN) ^ 2 + (lat12CS - (lat12SC * D_COS)) ^ 2)
b = (lat1_SIN * lat2_SIN) + (lat1_COS * lat2_COS * D_COS)

y = D_SIN * lat2_COS
x =  lat12CS - lat12SC * D_COS

brng_R = math.atan2(y, x)
brng = math.deg(brng_R)
dist = math.atan2(a, b) * earth_r
	
return acp_NORMDEG(brng), dist
end

function acp_saveparam()
	if acp_editparam == true then
		local savevars = {}
		local replres = ""
		local filename = SCRIPT_DIRECTORY .. "autopilot"
		savevars["ln_x"] = ln_x
		savevars["ln_y"] = ln_y
		savevars["acp_accuracy"] = acp_accuracy
		savevars["acp_throttle_accuracy"] = acp_throttle_accuracy
		savevars["acp_limit_dyn_heading"] = acp_limit_dyn_heading
		savevars["acp_limit_roll"] = acp_limit_roll
		savevars["acp_limit_dyn_altitude"] = acp_limit_dyn_altitude
		savevars["acp_limit_min_throttle"] = acp_limit_min_throttle		
		local tempfile = io.open(filename..".tmp", "w")
		for line in io.lines(filename..".lua") do
			for key, val in pairs(savevars) do
				if val ~= false and val ~= nil then
					replres = replval(line, key, val)
					if replres ~= false then
						savevars[key] = nil
						line = replres
					end
				end
			end
			tempfile:write(line, "\n")
		end
		tempfile:close()
		os.remove(filename..".lua")
		os.rename(filename..".tmp", filename..".lua")
		old_ln_x = ln_x
		old_ln_y = ln_y
		acp_old_accuracy = acp_accuracy
		acp_old_throttle_accuracy = acp_throttle_accuracy
		acp_old_limit_dyn_heading = acp_limit_dyn_heading
		acp_old_limit_roll = acp_limit_roll
		acp_old_limit_dyn_altitude = acp_limit_dyn_altitude
		acp_old_limit_min_throttle = acp_limit_min_throttle		
		acp_editparam = false	
	end
end

function replval(str, var, val)
	local start = string.find (str, var)
	if start ~= nil then
		local finish = string.find(str, "--",1, true)
		local strres = string.sub(str, 1, start - 1)
		strres = strres..var.." = "..val
		if finish ~= nil then
			strres = strres.." "..string.sub(str, finish)
		end
	return strres
	end
	return false
end

function acp_reset_settings()
	acp_accuracy = acp_old_accuracy 
	acp_throttle_accuracy = acp_old_throttle_accuracy
	acp_limit_dyn_heading = acp_old_limit_dyn_heading 
	acp_limit_roll = acp_old_limit_roll
	acp_limit_dyn_altitude = acp_old_limit_dyn_altitude
	acp_limit_min_throttle = acp_old_limit_min_throttle
end

-----------------
-----------------
acp_ParamInit()

do_every_frame("acp_FRAME()")
do_every_draw("acp_DRAW()")
do_on_mouse_click("acp_CLICK()")