--
-- ExtendedTillage
-- Specialization for ploughs and cultivators
--
-- @author    PES 4ever
-- @date      26/10/12
-- @version   1.0
-- @history   1.0 - Initial version
--
-- Copyright (C) PES 4ever, All Rights Reserved.
--

ExtendedTillage = {};

ExtendedTillage.debugRendering = false;

function ExtendedTillage.prerequisitesPresent(specializations)
    if not SpecializationUtil.hasSpecialization(Cylindered, specializations) then print("Warning: Specialization ExtendedTillage needs the specialization Cylindered."); end;
	
	return SpecializationUtil.hasSpecialization(Cylindered, specializations);
	-- return true; --SpecializationUtil.hasSpecialization(cultivator, specializations);
end;

function ExtendedTillage:load(xmlFile)

	self.setVehicleMinRpm = SpecializationUtil.callSpecializationsFunction("setVehicleMinRpm");
	self.forceRpmResetOnLeave = true;
    self.saveMinRpm = 0;

	self.depthLimiters = {};
	local i = 0;
	while true do
		local baseName = string.format("vehicle.depthLimiters.depthLimiter(%d)", i);
		if not hasXMLProperty(xmlFile, baseName) then
			break;
		end;
		local node = Utils.indexToObject(self.components, getXMLString(xmlFile, baseName.."#index"));
		if node ~= nil then
			local entry = {};
			entry.node = node;
			entry.depthMax = Utils.getNoNil(getXMLFloat(xmlFile, baseName.."#depthMax"), 0.1);
			entry.depthCur = 0;
			entry.bladeAngle = math.rad(Utils.getNoNil(getXMLFloat(xmlFile, baseName.."#bladeAngle"), 30));
			local x, y, z = getTranslation(node);
			entry.startTranslation = y;
			table.insert(self.depthLimiters, entry);
		end;
		i = i + 1;
	end;

	self.blades = {};
	local rotMin = math.rad(Utils.getNoNil(getXMLFloat(xmlFile, "vehicle.blades#rotMin"), 0));
	local rotMax = math.rad(Utils.getNoNil(getXMLFloat(xmlFile, "vehicle.blades#rotMax"), 20));
	local i = 0;
	while true do
		local baseName = string.format("vehicle.blades.blade(%d)", i);
		if not hasXMLProperty(xmlFile, baseName) then
			break;
		end;		
		local node = Utils.indexToObject(self.components, getXMLString(xmlFile, baseName.."#index"));
		local point = Utils.indexToObject(self.components, getXMLString(xmlFile, baseName.."#point"));
		local group = Utils.indexToObject(self.components, getXMLString(xmlFile, baseName.."#group"));
		if node ~= nil and point ~= nil and group ~= nil then
			local entry = {};
			entry.node = node;
			entry.point = point;
			entry.group = group;
			entry.vectorToPoint = {worldToLocal(entry.group, getWorldTranslation(entry.point))};
			entry.rotMin = rotMin;
			entry.rotMax = rotMax;
			table.insert(self.blades, entry);
		end;
		i = i + 1;
	end;
	
	self.gridSizeX = 9; -- in m
	self.gridSizeZ = 3; -- in m
	self.gridAngle = math.rad(30);
	
	self.depthStones = -0.1; -- in m
	self.activeStones = 0;
	
	self.forceFactor = 1; -- (force in plums, unit of force in the FS)
	
	self.lastWorldPosition = nil;

end;

function ExtendedTillage:delete()
	--removeTrigger(self.playerTrigger);
	--removeDeleteListener(self.playerTrigger, self.deleteListenerId);	
end;

function ExtendedTillage:readStream(streamId, connection)
	--self:setIsForkProtectionVisible(streamReadInt8(streamId), true);
end;

function ExtendedTillage:writeStream(streamId, connection)
	--streamWriteInt8(streamId, self.forkProtectionIsVisible);
end;

function ExtendedTillage:mouseEvent(posX, posY, isDown, isUp, button)
end;

function ExtendedTillage:keyEvent(unicode, sym, modifier, isDown)
end;

function ExtendedTillage:updateTick(dt)
end;

function ExtendedTillage:update(dt)
    local joint = self.componentJoints[2];
		setJointFrame(joint.jointIndex, 0,joint.jointNode);
		 local joint = self.componentJoints[3];
		setJointFrame(joint.jointIndex, 0,joint.jointNode);
		 local joint = self.componentJoints[4];
		setJointFrame(joint.jointIndex, 0,joint.jointNode);
		 local joint = self.componentJoints[5];
		setJointFrame(joint.jointIndex, 0,joint.jointNode);
	if self:getIsActive() then
	
		if ExtendedTillage.debugRendering then
			if self:getIsActiveForInput() then
				if InputBinding.hasEvent(InputBinding.DEV_INCREASE_FORCE) then
					self.forceFactor = self.forceFactor + 50;
				end;
				if InputBinding.hasEvent(InputBinding.DEV_DECREASE_FORCE) then
					self.forceFactor = self.forceFactor - 50;
				end;
			end;
			renderText(0.5, 0.31, 0.025, string.format("forceFactor: %1.2f", self.forceFactor));

			local xp, yp, zp = getWorldTranslation(self.components[1].node);
			
			-- Transform point coordinates from I-System into rotated K-system
			local Kxp, Kyp, Kzp = ExtendedTillage.transformationKI(xp, yp, zp, self.gridAngle);
			
			local Kxd = math.floor(Kxp) % self.gridSizeX;
			local Kzd = math.floor(Kzp) % self.gridSizeZ;
			
			for i = -5, 5 do
				for j = -5, 5 do
					-- Stone grid coordinates in K-system
					local Kxs = math.floor(Kxp) - Kxd + i * self.gridSizeX;
					local Kys = 0;
					local Kzs = math.floor(Kzp) - Kzd + j * self.gridSizeZ;
					
					-- Stone grid coordinates in I-system
					local xs, ys, zs = ExtendedTillage.transformationIK(Kxs, Kys, Kzs, self.gridAngle);
					local terrainHeight = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, xs, ys, zs);
					ys = terrainHeight + 0.1;
					--ys = terrainHeight - self.depthStones;
					
					drawDebugPoint(xs, ys, zs, 1, 0, 0, 1);
				end;
			end;
		end;
		
		-- Ground contact of cultivator or plough
		local hasGroundContact = self.cultivatorHasGroundContact or self.ploughHasGroundContact;
		
		-- Last moved distance of cultivator or plough in z-direction
		local lastMovedDistance = 0;
		local xw, yw, zw = getWorldTranslation(self.components[1].node);
		if self.lastWorldPosition ~= nil then
			--local dx, dy, dz = worldToLocal(self.rootNode, self.lastWorldPosition);
			-- Last moved distance of the main component projected on its z-axis 
			local Cdx, Cdy, Cdz = worldDirectionToLocal(self.components[1].node, xw - self.lastWorldPosition[1], yw - self.lastWorldPosition[2], zw - self.lastWorldPosition[3]);
			lastMovedDistance = Cdz;
		end;
		self.lastWorldPosition = {xw, yw, zw};	
		
		-- Handle stones
		self.activeStones = 0;
		for _, blade in pairs(self.blades) do
		
			-- Transform point coordinates from I-System into rotated K-system
			local xp, yp, zp = getWorldTranslation(blade.point); -- Point in I-System
			local Kxp, Kyp, Kzp = ExtendedTillage.transformationKI(xp, yp, zp, self.gridAngle); -- Point in K-System
			
			-- Stone coordinate in K-System
			local Kxs = self.gridSizeX * math.floor(Kxp / self.gridSizeX + 0.5);
			local Kys = 0; -- terrain height calculated later in I-System
			local Kzs = self.gridSizeZ * math.floor(Kzp / self.gridSizeZ + 0.5);
			
			-- Stone coordinate transformed into I-System
			local xs, ys, zs = ExtendedTillage.transformationIK(Kxs, Kys, Kzs, self.gridAngle);
			ys = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, xs, ys, zs) - self.depthStones;
			
			-- 
			local Pxs, Pys, Pzs = worldToLocal(blade.point, xs, ys, zs);
			if _ == 1 then
				--renderText(0.5, 0.40, 0.025, string.format("vector2Length(xs - xp, zs - zp): %1.2f", Utils.vector2Length(xs - xp, zs - zp)));
				--renderText(0.5, 0.37, 0.025, string.format("yp - ys: %1.2f", yp - ys));
				--renderText(0.5, 0.34, 0.025, string.format("vector2Length(Pys, Pzs): %1.2f", Utils.vector2Length(Pys, Pzs)));
				--renderText(0.5, 0.31, 0.025, string.format("Pzs: %1.2f", Pzs));
			end;			
			
			-- Check circle area (x-z plane) and y-distance to stone
			if Utils.vector2Length(xs - xp, zs - zp) <= 0.2 and yp - ys <= 0 then--and Pzs >= -0.05 then				
				
				-- Geometry of blade
				local Bxp, Byp, Bzp = worldToLocal(blade.node, getWorldTranslation(blade.point));
				local length = Utils.vector2Length(Byp, Bzp);
				local alpha = math.acos(Bzp / length) * Utils.sign(Byp);
				--renderText(0.5, 0.31, 0.025, string.format("alpha: %1.2f", math.deg(alpha)));
				
				-- Stone coordinates transformed into G-System (blade group)
				local Gxs, Gys, Gzs = worldToLocal(blade.group, xs, ys, zs);
				
				local beta = math.acos(math.min(math.max(Gzs, -length), length) / length) * Utils.sign(Gys);
				--renderText(0.5, 0.34, 0.025, string.format("beta: %1.2f", math.deg(beta)));
				local gamma = math.min(math.max(alpha - beta, blade.rotMin), blade.rotMax);
				--renderText(0.5, 0.37, 0.025, string.format("gamma: %1.2f", math.deg(gamma)));

				local rx, ry, rz = getRotation(blade.node);
				-- 
				if rx == blade.rotMin and lastMovedDistance < 0 then
					gamma = blade.rotMin;
				end;
			
				setRotation(blade.node, gamma, ry, rz);
				
				if ExtendedTillage.debugRendering then
					if _ == 1 then
						drawDebugPoint(xs, ys + self.depthStones + 0.1, zs, 0, 1, 0, 1);
					end;
				end;
				self.activeStones = self.activeStones + 1;
			else
				-- Spring back
				local rot = {getRotation(blade.node)};
				local newRot = Utils.getMovedLimitedValues(rot, {blade.rotMax, 0, 0}, {blade.rotMin, 0, 0}, 3, 300, dt, true);
				setRotation(blade.node, unpack(newRot));
				if ExtendedTillage.debugRendering then
					if _ == 1 then
						drawDebugPoint(xs, ys + self.depthStones + 0.1, zs, 0, 0, 1, 1);
					end;
				end;
			end;
			self:setMovingToolDirty(blade.node);
		end;

	
		if self.attacherVehicle ~= nil then
		
			-- Speed factor
			local speed = self.attacherVehicle.lastSpeed * 3600;
			local speedFactor = math.min(60, math.max(0, math.abs(speed))) / 60;			
			
			-- Add force
			if self:isLowered() and hasGroundContact and lastMovedDistance > 0.001 then
				local force = math.pow(speedFactor, 0.25) * self.depthLimiters[1].depthCur * self.forceFactor;
				local forceX, forceY, forceZ = localDirectionToWorld(self.components[1].node, 0, 0, -force);
				--local forceX, forceY, forceZ = localDirectionToWorld(self.attacherVehicle.components[1].node, 0, 0, -force);				
				local positionX, positionY, positionZ = getWorldTranslation(self.attacherJoint.node);				
				
				if self.isServer then
					--addForce(self.components[1].node, forceX, forceY, forceZ, positionX, positionY, positionZ, false);
					addForce(self.attacherVehicle.components[1].node, forceX, forceY, forceZ, positionX, positionY, positionZ, false);
				end;
				
				if ExtendedTillage.debugRendering then
					--local x, y, z = localToWorld(self.components[1].node, 0, 1, -1);
					local x, y, z = positionX, positionY, positionZ;
					drawDebugPoint(x, y, z, 1, 0, 0, 1);
					drawDebugLine(x, y, z, 1, 0, 0, x + (forceX * 2/50), y + (forceY * 2/50), z + (forceZ * 2/50), 1, 0, 0);
					renderText(0.1, 0.15, 0.025, string.format("force: %1.2f", force));					
				end;
			end;
			
			--renderText(0.1, 0.12, 0.025, string.format("self.attacherVehicle.movingDirection: %d", self.attacherVehicle.movingDirection));
			--renderText(0.1, 0.09, 0.025, string.format("lastMovedDistance: %1.5f", lastMovedDistance));
			--renderText(0.1, 0.06, 0.025, string.format("speed: %1.2f", speed));			
			
			
			-- Change min rpm
			if ExtendedTillage.debugRendering then
				renderText(0.1, 0.03, 0.025, string.format("minRpm: %1.5f", self.attacherVehicle.motor.minRpm));
			end;

			if self:isLowered() and lastMovedDistance > 0.001 and hasGroundContact then --self.attacherVehicle.movingDirection == 1
				--self:setVehicleMinRpm(dt, 1000 - self.activeStones * 300);
				self:setVehicleMinRpm(dt, 0.42 - self.activeStones * 0.15);
			else
				--self:setVehicleMinRpm(dt, -100);
				self:setVehicleMinRpm(nil, 0);
			end;
			
			-- Move depth limiters
			if self.isClient then
				for _, depthLimiter in pairs(self.depthLimiters) do				
				
					local x, y, z = getTranslation(depthLimiter.node);
					
					if self:isLowered() then
						if hasGroundContact then
						
							--[[ Last moved distance of depth limiter in z-direction
							local distance = 0;
							if depthLimiter.lastWorldPosition ~= nil then
								distance = math.abs(ExtendedTillage.projectOnAxis(depthLimiter.node, depthLimiter.lastWorldPosition, 2));
							end;
							depthLimiter.lastWorldPosition = {getWorldTranslation(depthLimiter.node)};
							--local depthDelta = self.movingDirection * speedFactor / 25; 
							depthDelta = 0.2 * distance * self.movingDirection;--self.lastMovedDistance * 0.1;--math.sin(depthLimiter.bladeAngle);
							]]
				
							local depthDelta = 0.2 * lastMovedDistance;							
							depthLimiter.depthCur = math.min(math.max(depthLimiter.depthCur + depthDelta, 0), depthLimiter.depthMax);
							
							--print(depthLimiter.depthCur);
							--renderText(0.5, 0.31, 0.025, string.format("depthLimiter.depthCur: %1.2f", depthLimiter.depthCur));
							--renderText(0.5, 0.34, 0.025, string.format("self.lastMovedDistance: %1.2f", self.lastMovedDistance));
							--renderText(0.5, 0.34, 0.025, string.format("distance: %1.5f", distance));							
						end;
					else
						-- Move depth limiter to original position when tool is lifted by attacher vehicle
						depthLimiter.depthCur = Utils.getMovedLimitedValues({0, depthLimiter.depthCur, 0}, {0, depthLimiter.depthCur, 0}, {0, 0, 0}, 3, 1000, dt, true)[2];
						depthLimiter.lastWorldPosition = nil;
					end;
					y = depthLimiter.startTranslation + depthLimiter.depthCur;
					setTranslation(depthLimiter.node, x, y, z);
					self:setMovingToolDirty(depthLimiter.node);					
				end;
			end;
		end;
	else
		if self.forceRpmResetOnLeave then
			self:setVehicleMinRpm(nil, 0);
		end;
	end;
	
end;

function ExtendedTillage:draw()
end;

function ExtendedTillage:onAttach(attacherVehicle)

	self.attacherMotorVehicle = nil; -- only vehicles with motor
	self.saveMinRpm = nil;
	if attacherVehicle.motor ~= nil and attacherVehicle.motor.minRpm ~= nil then
		self.attacherMotorVehicle = attacherVehicle;
		self.saveMinRpm = attacherVehicle.motor.minRpm;
	end;

end;

function ExtendedTillage:onDetach()

	if self.attacherMotorVehicle ~= nil and self.saveMinRpm ~= nil then
		self.attacherMotorVehicle.motor.minRpm = self.saveMinRpm;
	end;
	self.attacherMotorVehicle = nil;
	self.saveMinRpm = nil;

end;

function ExtendedTillage:setVehicleMinRpm(dt, rpmRangeFactor)

	-- if self.attacherVehicle ~= nil then
		-- self.attacherVehicle.motorSoundPitchOffset = math.abs(minRpm/ self.attacherVehicle.motor.minRpm);
	-- end;
	if self.attacherVehicle ~= nil and self.attacherVehicle.motor ~= nil and self.saveMinRpm ~= nil then		
		local motor = self.attacherVehicle.motor;
		if dt ~= nil then		
			if rpmRangeFactor ~= nil then
				rpmRangeFactor = math.max(math.min(rpmRangeFactor, 1), 0);
				--if rpmRangeFactor >= 0 and rpmRangeFactor <= 1 then
				local minRpm = self.saveMinRpm + rpmRangeFactor * (motor.maxRpm[3] - self.saveMinRpm);
				if rpmRangeFactor ~= 0 then
					minRpm = -minRpm;
				end;
				if minRpm > motor.minRpm then
					--delta = dt * 2;
					motor.minRpm = math.min(motor.minRpm + dt * 2, minRpm);
				elseif minRpm < motor.minRpm then
					--delta = -dt * 5;
					motor.minRpm = math.max(motor.minRpm - dt * 5, minRpm);
				end;

				--motor.minRpm = math.max(math.min(motor.minRpm + delta, minRpm), minRpm);
				--motor.lastMotorRpm = motor.minRpm;
				--end;
			end;
		else
			motor.minRpm = self.saveMinRpm;
		end;
		if self.attacherVehicle.isMotorStarted then
			local fuelUsed = 0.0000012 * math.abs(motor.minRpm);
			self.attacherVehicle:setFuelFillLevel(self.attacherVehicle.fuelFillLevel - fuelUsed);
			g_currentMission.missionStats.fuelUsageTotal = g_currentMission.missionStats.fuelUsageTotal + fuelUsed;
			g_currentMission.missionStats.fuelUsageSession = g_currentMission.missionStats.fuelUsageSession + fuelUsed;
		end;
	end;
end;

function ExtendedTillage.transformationIK(Ix, Iy, Iz, angle)

	-- Transformationmatrix from rotated (y-axis) system K to initial vehicle system I
	-- A_IK =
	-- [  cos(angle)  0  sin(angle) ]
	-- [      0       1      0      ]
	-- [ -sin(angle)  0  cos(angle) ]
	
	return Ix * math.cos(angle) + Iz * math.sin(angle), Iy, -Ix * math.sin(angle) + Iz * math.cos(angle);

end;

function ExtendedTillage.transformationKI(Kx, Ky, Kz, angle)

	-- Transformationmatrix from initial vehicle system I to rotated (y-axis) system K 
	-- A_KI =
	-- [ cos(angle)  0  -sin(angle) ]
	-- [     0       1       0      ]
	-- [ sin(angle)  0   cos(angle) ]
	
	-- Comment: A_KI(angle) = A_IK(-angle)
	
	return Kx * math.cos(angle) - Kz * math.sin(angle), Ky, Kx * math.sin(angle) + Kz * math.cos(angle);

end;

function ExtendedTillage.consoleCommandToggleDebugRendering(unusedSelf)
	ExtendedTillage.debugRendering = not ExtendedTillage.debugRendering;
	return "ExtendedTillageDebugRendering = "..tostring(ExtendedTillage.debugRendering);
end;

addConsoleCommand("gsExtendedTillageToggleDebugRendering", "Toggles the debug rendering of the extended tillage tools.", "ExtendedTillage.consoleCommandToggleDebugRendering", nil);
