-- Vehicle Class
#include "PID_controller.lua"
#include "helpers/mathfr.lua"

c_vehicle = {}
function c_vehicle:new(in_id)
	local obj = {
		id 				= in_id,
		t 				= Transform(),
		velocity 		= Vec(0,0,0),
		preemption_pos 	= Vec(0,0,0),
		forward_vec 	= Vec(0,0,0),
		right_vec 		= Vec(0,0,0),
		raytrace_ignore = {
			shapes_to_ignore 	= {},
			shapes_not_vehicles = {}
		},
		control = {
			steering_controller 	= {},
			acceleration_controller = {},
			drive 					= 0.0,
			drive_multiplier 		= 1.0,
			steering 				= 0.0,
			handbrake 				= false,
			Collect = function(self)
				local new_drive = self.drive
				if new_drive > 0 then
					new_drive = new_drive * self.drive_multiplier
				end
				return new_drive, self.steering, self.handbrake
			end
		},

		Tick = function(self, dt)
			self.t 				= GetVehicleTransform(self.id)
			self.velocity 		= GetBodyVelocity(GetVehicleBody(self.id))
			self.preemption_pos = VecAdd(self.t.pos, VecScale(self.velocity, 0.5))
			self.forward_vec 	= QuatRotateVec(self.t.rot, Vec(0,0,-1))
			self.right_vec 		= QuatRotateVec(self.t.rot, Vec(1,0,0))
		end,

		SetSteeringController = function(self, PID_coeff)
			self.control.steering_controller = c_PID:new()
			if #PID_coeff > 0 then
				self.control.steering_controller:SetParams(PID_coeff[1], PID_coeff[2], PID_coeff[3])
			else
				self.control.steering_controller:SetParams(1.5, 0.7, 0.5)
			end
		end,

		SetAccelerationController = function(self, PID_coeff)
			self.control.acceleration_controller = c_PID:new()
			if #PID_coeff > 0 then
				self.control.acceleration_controller:SetParams(PID_coeff[1], PID_coeff[2], PID_coeff[3])
			else
				self.control.acceleration_controller:SetParams(1, 0.2, 0.1)
			end
		end,

		Drive = function(self)
			DriveVehicle(self.id, self.control:Collect())
		end,

		UpdateSpeed = function(self, dt, desired_speed)
			self.control.drive = self.control.acceleration_controller:PIDUpdateSpeed(dt, desired_speed, self:GetVehicleVelocity())
		end,

		TurnToPositionByController = function(self, dt, wp_next, skip_y)
			self.control.steering = -self.control.steering_controller:UpdateRotation(dt, VecCopy(wp_next), self.t, skip_y)
		end,

		FindAngleWithTransform = function(self, transform)
			local rot_to_target = QuatLookAt(self.t.pos, transform.pos)
			local _,y1,_ 		= GetQuatEuler(self.t.rot)
			local _,y2,_ 		= GetQuatEuler(rot_to_target)
			return math:SignedAnglesDelta(y1, y2)
		end,

		TurnToPosition = function(self, pos)
			local rot_to_target = QuatLookAt(self.t.pos, pos)
			local _,y1,_ 		= GetQuatEuler(self.t.rot)
			local _,y2,_ 		= GetQuatEuler(rot_to_target)
			local delta_angle 	= math:SignedAnglesDelta(y1, y2)
			self:TurnToAngle(delta_angle)
		end,

		TurnToAngle = function(self, angle)
			local steering_sign = 0.0
			local steering_val = 0.0

			if angle > 0.0 then
				steering_sign = -1.0
			else
				steering_sign = 1.0
			end

			if math.abs(angle) > 30 then
				steering_val = 1.0
			else
				steering_val = math.abs(angle) / 30
			end

			self.control.steering = steering_sign * steering_val
		end,

		GetVehicleVelocity = function(self)
			local vel 	= TransformToLocalVec(self.t, self.velocity)
			local speed = math.abs(vel[3])
			return math.floor(speed * 3.6)
		end,

		IsTargetOnRight = function(self, target)
			local cross_prod_vec = self:VecCrossByForwardVec(target)
			return cross_prod_vec[2] > 0 and true or false, cross_prod_vec[2]
		end,

		VecCrossByForwardVec = function(self, target)
			local start 			= Vec(0.0, 0.0, 0.0)
			local tmp_target 	 	= VecCopy(target)
			local tmp_forward 	 	= VecCopy(self.forward_vec)
			local to_target_vec  	= VecNormalize(VecSub(tmp_target, self.t.pos))
			to_target_vec[2] 		= start[2]
			tmp_forward[2]			= start[2]
			return VecCross(to_target_vec, tmp_forward)
		end,

		PointAtDirByVehicle = function(self, FrwOrBkw, RightOrLeft)
			-- Forward = 1, Backward = -1.
			-- Right = 1, Left = -1.
			local direction = VecAdd(VecScale(self.forward_vec, FrwOrBkw), VecScale(self.right_vec, RightOrLeft))
			local point = VecScale(direction, 10.0)
			return VecAdd(self.t.pos, point)
		end,
	}

	setmetatable(obj, {__index = self})
	return obj
end
