--[[------------------------------------------------------------------------------------------------
Helicopter movement
Андрущенко Иван

--------------------------------------------------------------------------------------------------]]
local heli_flyer = {}

function get_heli_flyer(obj)
    if heli_flyer[obj:id()] == nil then
        heli_flyer[obj:id()] = heli_fly(obj)
    end
    return heli_flyer[obj:id()]
end

class "heli_fly"

function heli_fly:__init(obj)
	self.obj            = obj
    self.block_flook    = false
    self.dist_by_look   = 0
    self.heliLAccFW     = 6
    self.heliLAccBW     = 4
    self.max_velocity   = 0
    self.point_arr      = {}
    self.dest_point     = nil 
    self.point_by_look  = vector():set(0,0,0)
end


function heli_fly:fly_on_point_with_vector(dest_point, dest_direction, dest_velocity, flag_to_wp_callback, flag_by_null_velocity)

--'	printf("fly_on_point_with_vector()")

	local heli = self.obj:get_helicopter()
	local curr_heli_position = self.obj:position()						-- текущая позиция
	local curr_heli_direction = self.obj:direction()					-- единичный вектор текущего направления
	local curr_heli_velocity = heli:GetCurrVelocity()					-- текущая скорость
	dest_velocity = dest_velocity * 1000 / 3600
	if not flag_to_wp_callback then
--------------Расчеты------------------------------------------------------------------------------------------------------------------------------------------------------

		local a_speed = 0
		local time_by_fly = 0
		local rez_point = vector():set(0,0,0)								-- точка, через которую пролетим, что-бы попасть в заданную точку, и выйти на заданную скорость
		local d_path                                                        -- d_path - допуск расстояния попадания в точку
	-------------------------------------------------------------
		if dest_velocity >= curr_heli_velocity then
	-------------------------------------------------------------
			a_speed=  self.heliLAccFW										                -- ускорение разгона, берем из helicopter.ltx
	        d_path = curr_heli_velocity * 2 / a_speed
    -------------------------------------------------------------
		else
	-------------------------------------------------------------
			a_speed= - self.heliLAccBW	    									                -- ускорение торможения, берем из helicopter.ltx
            d_path = - curr_heli_velocity * 2 / a_speed
	-------------------------------------------------------------
		end

		time_by_fly = (dest_velocity - curr_heli_velocity)/a_speed					-- t=(v2-v1)/a -- t - время, за которое затормозим/разгонимся. a - ускорение торможения/разгона, берем из helicopter.ltx
		local delta = curr_heli_velocity * time_by_fly + a_speed * time_by_fly * time_by_fly / 2	-- расстояние для выхода на заданную скорость	s=v0t+(at^2)/2	
		if delta >= d_path then
            self.point_arr[0] = curr_heli_position
            self.point_arr[1] = dest_point
            self.point_arr[2] = curr_heli_direction

            rez_point = self:calc_point()
            if not self.block_flook then
                rez_point.x = rez_point.x + curr_heli_direction.x * delta / 2
                rez_point.z = rez_point.z + curr_heli_direction.z * delta / 2
            end
			flag_to_wp_callback = true
		else
			rez_point = dest_point
			flag_to_wp_callback = false
		end
        self.dest_point = rez_point
---------------------------------------------------------------------------------------------------------------------------------------------------------------------------

	else
        self.dest_point = dest_point
		flag_to_wp_callback = false
	end
    heli:SetDestPosition( self.dest_point ) 
    self:correct_velocity()
    if flag_by_null_velocity then
        heli:SetSpeedInDestPoint(0)
    else
        heli:SetSpeedInDestPoint(heli:GetMaxVelocity())
    end
	return flag_to_wp_callback
end

function heli_fly:get_block_flook()
    return self.block_flook
end

function heli_fly:calc_point()

    local rez_point = vector():set(0,0,0)    
    local xxArr = {}
        xxArr[0] = self.point_arr[0].x
        xxArr[1] = self.point_arr[1].x
        xxArr[2] = self.point_arr[2].x
    local yyArr = {}
        yyArr[0] = self.point_arr[0].y
        yyArr[1] = self.point_arr[1].y
        yyArr[2] = self.point_arr[2].y
    local zzArr = {}
        zzArr[0] = self.point_arr[0].z
        zzArr[1] = self.point_arr[1].z
        zzArr[2] = self.point_arr[2].z
    rez_point.y = (self.point_arr[0].y + self.point_arr[1].y)/2
    if rez_point.y == self.point_arr[0].y then
        rez_point.z = (self.point_arr[0].z + self.point_arr[1].z)/2
        if rez_point.z == self.point_arr[0].z then
            rez_point.x = (self.point_arr[0].x + self.point_arr[1].x)/2
            rez_point.z = self:lagrange(rez_point.x, xxArr, zzArr)
        else
            rez_point.x = self:lagrange(rez_point.z, zzArr, xxArr)
        end
    else
        rez_point.x = self:lagrange(rez_point.y, yyArr, xxArr)
        rez_point.z = self:lagrange(rez_point.y, yyArr, zzArr)
    end

--'    printf("fly_point[x=%d; y=%d; z=%d;]",rez_point.x, rez_point.y, rez_point.z);
    
    return rez_point

end

function heli_fly:lagrange(x, xArr, yArr)

    local i, j
    local m, s
    s = 0
    for i=0,2 do
        m = yArr[i]
        for j=0,2 do
            if j ~= i then 
                m = m * (x - xArr[j]) / (xArr[i] - xArr[j])
            end
        end
        s = s + m
    end

    return s

end

function heli_fly:correct_velocity()
	local heli = self.obj:get_helicopter()
	local curr_heli_velocity = heli:GetCurrVelocity()					--' текущая скорость
    local dist_to_dest_point = heli:GetDistanceToDestPosition()
    local a_speed =  self.heliLAccFW
    local dest_velocity
    dest_velocity = ((2*a_speed*dist_to_dest_point + curr_heli_velocity^2)/3)^(1/2)
    if self.max_velocity*1000/3600 < dest_velocity then
        dest_velocity = self.max_velocity*1000/3600
    end
	heli:SetMaxVelocity(dest_velocity)
--'  printf("dist_to_dest_point %s", dist_to_dest_point);  
--'	printf("dest_velocity end = %d", dest_velocity);

end

function heli_fly:look_at_position()
    if self.block_flook then
        local heli = self.obj:get_helicopter()
        heli:LookAtPoint( self.point_by_look, true )
    end
end

function heli_fly:set_block_flook(fl_block)
    self.block_flook = fl_block
end

function heli_fly:set_look_point(l_point)
    self.point_by_look = l_point
end