////////////////////////////////////////////////////////////////////////////////
// VTOLAI.cpp
// AI for vehicles that have control over their altitude (helicopters, vector-thrust vehicles, submarines)
// Copyright Bjoern Ganster
// Developed since 2021
////////////////////////////////////////////////////////////////////////////////

#include "FileList.h"
#include "VTOLAI.h"
#include "MechSim.h"

#include <iostream>

#define FILE_NUMBER FILE_NO_VTOL_AI_CPP

////////////////////////////////////////////////////////////////////////////////
// Constructor

VTOLAI::VTOLAI(const char* className, Vehicle* vehicle)
: AbstractVehicleAI(className, GroundBasedVehicleAIID, vehicle),
  aimAccuracy(10.0),
  mTriggerRange(300.0),
  mLastFired(0),
  mCurrentGoal(0)
{
}

////////////////////////////////////////////////////////////////////////////////
// Destructor

VTOLAI::~VTOLAI()
{
}

////////////////////////////////////////////////////////////////////////////////
// Plan movement for this frame

void VTOLAI::planMovement(DWORD now)
{
	if (mVehicle == nullptr) {
		return;
	}

	SET_SCOPE;
	if (mVehicle->getStatus() != VehicleStatus::engineRunning) {
		mVehicle->m_turnDelta = 0.0;
		mVehicle->m_speedDelta = - 0.8 * getSpeed();
		checkWakeUp();
		mVehicle->mAcceleration = -0.1 * mVehicle->mMovement;
		RETURN;
	}

	SET_OP;
	if (mTarget != nullptr) {
		if (!mTarget->isDestroyed()) {
			fight(now);
		} else {
			mTarget = nullptr;
		}
	} else if (mCurrentGoal >= mGoals.size()) {
		SET_OP;
		mVehicle->setStatus(VehicleStatus::shutDown);
	} else {
		SET_OP;
		auto goalType = mGoals[mCurrentGoal].type;
		switch(goalType) {
			case SubObjectiveType::sleep:
				checkWakeUp();
				break;
			case SubObjectiveType::attack:
				doAttack();
				break;
			case SubObjectiveType::defend:
				doDefend();
				break;
			case SubObjectiveType::inspect:
				doInspect();
				break;
			case SubObjectiveType::visit:
				doVisit();
				break;
		}
	}

	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Choose heading and speed to walk towards a target

void VTOLAI::moveTowards(const Point3D& aimGoal)
{
	if (mVehicle == nullptr) {
		return;
	}

	// Choose heading
	SET_SCOPE;
	Point3D ownPos = mVehicle->getPosition();
	Point3D distV = aimGoal - ownPos; // reverse for fleeing ...
	Point3D distHoriz(distV.x, 0.0, distV.z);
	double dist = distHoriz.length();
	double temp = (distHoriz * Point3D(0.0, 0.0, 1.0)) / dist;
	double desiredDirection = 180.0 * acos(temp) / M_PI;

	if (distV.x > 0.0) {
		desiredDirection = -desiredDirection;
	}

	// Choose walking speed
	// todo: check if a collision is possible in the near future. If so, evade or slow down.
	//double walkingHexes = mVehicle->getDouble(Vehicle::Info_WalkingHexes);
	double runningHexes = mVehicle->getDouble(Vehicle::Info_RunningHexes);
	double desiredSpeed = mVehicle->getMaxSpeed();

	// Store speed delta
	SET_OP;
	mVehicle->m_speedDelta = desiredSpeed - getSpeed();
	if (mVehicle->m_speedDelta > 1.0) {
		mVehicle->m_speedDelta = 5.0;
	} else if (mVehicle->m_speedDelta < -1.0) {
		mVehicle->m_speedDelta = -5.0;
	}

	while (mVehicle->mHeading > 180.0)
		mVehicle->mHeading -= 360;
	while (mVehicle->mHeading < -180.0)
		mVehicle->mHeading += 360;

	// Choose walking direction
	if (abs(mVehicle->mHeading-desiredDirection) < 5) {
		mVehicle->m_turnDelta = 0.0;
	} else {
		SET_OP;
		double leftTurnDist = 0.0;
		double rightTurnDist = 0.0;
		if (mVehicle->mHeading > 0.0) {
			leftTurnDist  = -mVehicle->mHeading;
			rightTurnDist = 360 - mVehicle->mHeading;
		} else {
			leftTurnDist  = -(360 + mVehicle->mHeading);
			rightTurnDist = -mVehicle->mHeading;
		}
		if (desiredDirection > 0.0) {
			leftTurnDist  += desiredDirection - 360.0;
			rightTurnDist += desiredDirection;
		} else {
			leftTurnDist  += desiredDirection;
			rightTurnDist += 360 + desiredDirection;
		}

		while(leftTurnDist < -360)
			leftTurnDist += 360;
		while (rightTurnDist > 360.0)
			rightTurnDist -= 360.0;

		if (leftTurnDist < -360 || leftTurnDist > 0
		||  rightTurnDist < 0.0 || rightTurnDist > 360)
			COUT << "Something's wrong" << std::endl;

		if (abs(leftTurnDist) < abs(rightTurnDist)) {
			mVehicle->m_turnDelta = leftTurnDist;
		} else {
			mVehicle->m_turnDelta = rightTurnDist;
		}

		double maxTurnPerSecond = mVehicle->getMaxTurnPerSecond();
		if (mVehicle->m_turnDelta > maxTurnPerSecond)
			mVehicle->m_turnDelta = maxTurnPerSecond;
		if (mVehicle->m_turnDelta < -maxTurnPerSecond)
			mVehicle->m_turnDelta = -maxTurnPerSecond;
	}

	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Aim at target

void VTOLAI::aimAndFire(const Point3D& aimGoal, DWORD now)
{
	if (mTarget == nullptr 
	||  mVehicle == nullptr) 
	{
		return;
	}

	// Debug code ...
	/*MechSim* mechSim = getMechSim();
	if (mechSim->getMechSimDebug()) {
		COUT << "DEBUG NOW" << std::endl;
	}*/

	SET_SCOPE;
	const MountedWeapon* activeWeapon = mVehicle->getActiveWeapon();
	if (activeWeapon == nullptr) {
		mVehicle->cycleWeapon(false);
		mLastFired = now; // to delay AI shooting
		RETURN;
	}

	Matrix m;
	int weaponSection = activeWeapon->section;
	int vehiclePart = mVehicle->section2part(activeWeapon->section, mVehicle->preferLowerArms);
	mVehicle->getPartMatrix(vehiclePart, m);
	Point3D firePoint = m.Transform(activeWeapon->position);

	Point3D distV = aimGoal - firePoint;
	Point3D distHoriz(distV.x, 0.0, distV.z);
	double heightDiff = distV.y;
	double dist = distHoriz.length();
	double temp = (distHoriz * Point3D(0.0, 0.0, 1.0)) / dist;
	double desiredTorsoTwist = 180.0 * acos(temp) / M_PI;

	if (distV.x > 0.0) {
		desiredTorsoTwist = -desiredTorsoTwist;
	}

	// Warning: m_walkingDirectionDegrees and desiredTorsoTwist count cw, while bipedMech->torsoTwist counts ccw ...
	SET_OP;
	auto currentTilt = mVehicle->getAimTilt();
	auto currentTurn = mVehicle->getAimTurn();
	double desiredTorsoTilt = 180 * atan(heightDiff / dist) / M_PI;
	bool readyToFire = mVehicle->canFireWeapon(mVehicle->mActiveWeapon, now);

	if (!readyToFire) {
		mVehicle->cycleWeapon(false);
		mLastFired = now; // to delay AI shooting
	}

	if (abs(desiredTorsoTwist + currentTurn - mVehicle->mHeading) < aimAccuracy
	&&  abs(desiredTorsoTilt - currentTilt) < aimAccuracy
	&&  readyToFire
	&&  mLastFired + 1000.0 * mTriggerSpeed < now) 
	{
		mVehicle->fireWeapons(false, now);
		mLastFired = now;
	} else {
		mVehicle->adjustAim(mVehicle->mHeading - desiredTorsoTwist, desiredTorsoTilt, 1.0);
	}

	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Handle goal types

void VTOLAI::fight(DWORD now)
{
	SET_SCOPE;
	double desiredDirection = mVehicle->mHeading;
	double desiredSpeed = 0.0;
	Point3D walkingGoal, aimGoal;
	Point3D ownPos = mVehicle->getPosition();

	if (mTarget != nullptr) {
		// targetPos is where the AI wants to go ...
		// The AI should try to go behind the enemy. Spots slightly sideway should make sure the AI doesn't bump into the other mech while going there ...
		SET_OP;
		Vehicle* targetVehicle = dynamic_cast<Vehicle*>(mTarget);
		Point3D targetPosition = mTarget->getCenterOfMass();
		Point3D aimGoal = targetPosition;
		if (targetVehicle != nullptr) {
			Point3D sweetSpot1 = targetPosition - 50.0 * targetVehicle->p_front + 50.0 * targetVehicle->p_left;
			Point3D sweetSpot2 = targetPosition - 50.0 * targetVehicle->p_front - 50.0 * targetVehicle->p_left;
			walkingGoal = sweetSpot1;
			if ((ownPos-sweetSpot1).length() > (ownPos-sweetSpot2).length()) {
				walkingGoal = sweetSpot2;
			}
		} else {
			BoundingBox targetBB = mTarget->getTransformedBB();
			walkingGoal = Point3D(targetBB.max().x + 20.0, targetPosition.y, targetPosition.z);
		}
		moveTowards(walkingGoal);
		aimAndFire(aimGoal, now);
	}

	RETURN;
}

void VTOLAI::checkWakeUp()
{
	SET_SCOPE;
	if (mVehicle->getStatus() == VehicleStatus::shutDown) {
		// Check if an enemy is within wakeup/attack range
		MechSim* mechSim = getMechSim();
		RenderableObject* scene = mechSim->getScene();
		if (scene != nullptr) {
			for (size_t i = 0; i < scene->getObjectCount(); ++i) {
				RenderableObject* object = scene->getObject(i);
				Vehicle* vehicle = dynamic_cast<Vehicle*>(object);
				if (vehicle != nullptr && vehicle != mVehicle) {
					BGCPString vehicleFaction = vehicle->getFaction();
					double dist = (vehicle->getPosition()-mVehicle->getPosition()).length();
					if (dist < mTriggerRange
					&&  !vehicleFaction.empty()
					&&  vehicleFaction != mVehicle->getFaction()
					&&  vehicle->getStatus() != VehicleStatus::destroyed) {
						setTarget(vehicle);
					}
				}
			}

			// Check if a sleep timer has run out
			// todo: this only works if the first goal is a sleep goal, because the sleep time is counted from mission start here
			// To solve this properly, we would have to keep track of when an AI started on the current goal
			SET_OP;
			if (mCurrentGoal < mGoals.size()) {
				const auto& goal = mGoals[mCurrentGoal];
				if (goal.type == SubObjectiveType::sleep && (int)mechSim->missionTime() > 1000 * goal.duration)
				{
					mVehicle->setStatus(engineRunning);
					++mCurrentGoal;
				} else if (goal.type != SubObjectiveType::sleep) {
					mVehicle->setStatus(engineRunning);
				}
			}
		}
	}

	RETURN;
}

void VTOLAI::doAttack()
{
	SET_SCOPE;
	BGCPString targetID;
	if (mCurrentGoal >= mGoals.size()) {
		RETURN;
	}

	targetID = mGoals[mCurrentGoal].target;
	MechSim* mechSim = getMechSim();
	RenderableObject* scene = mechSim->getScene();
	GameObject* gameObject = findGameObject(scene, targetID);
	if (gameObject != nullptr) {
		if (gameObject->isDestroyed()) {
			++mCurrentGoal;
		} else {
			setTarget(gameObject);
		}
	}

	RETURN;
}

void VTOLAI::doDefend()
{
	// todo
}

void VTOLAI::doInspect()
{
	// todo
}

void VTOLAI::doVisit()
{
	SET_SCOPE;
	if (mCurrentGoal < mGoals.size() && mVehicle != nullptr) {
		const AIGoal& goal = mGoals[mCurrentGoal];
		Point3D vehiclePos = mVehicle->getPosition();
		double dist = (goal.coords - vehiclePos).length();

		if (dist > 100.0) {
			moveTowards(goal.coords);
		} else {
			++mCurrentGoal;
			MechSim* mechSim = getMechSim();
			mechSim->updateObjectStatus(goal.trigger, SubObjectiveType::event, false);
		}
	}
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Search the scene recursively for an object with the given id

GameObject* VTOLAI::findGameObject(RenderableObject* ro, const BGCPString& id)
{
	if (id.empty()) {
		return nullptr;
	}

	SET_SCOPE;
	GameObject* go = dynamic_cast<GameObject*>(ro);
	if (go != nullptr) {
		if (go->getID() == id) {
			RETURN go;
		}
	}

	if (ro != nullptr) {
		for (size_t i = 0; i < ro->getObjectCount(); ++i) {
			GameObject* go = findGameObject(ro->getObject(i), id);
			if (go != nullptr) {
				RETURN go;
			}
		}
	}

	RETURN nullptr;
}

void VTOLAI::setTriggerRange(double newVal)
{
	mTriggerRange = newVal;
}

void VTOLAI::addAttackGoal(const BGCPString& target)
{
	SET_SCOPE;
	AIGoal newGoal;
	newGoal.type = SubObjectiveType::attack;
	newGoal.target = target;
	mGoals.push_back(newGoal);
	RETURN;
}

// Notify AI of an event, like a destroyed object
void VTOLAI::updateObjectStatus(const BGCPString& id, SubObjectiveType newStatus)
{
	SET_SCOPE;
	if (mCurrentGoal >= mGoals.size()
	||  mVehicle == nullptr
	||  mVehicle->isDestroyed()) {
		RETURN;
	}

	SET_OP;
	auto& goal = mGoals[mCurrentGoal];
	if (goal.type == SubObjectiveType::sleep
	&&  goal.trigger == id) 
	{
		++mCurrentGoal;
		if (mVehicle->getStatus() == shutDown) {
			mVehicle->setStatus(engineRunning);
		}
	}
	RETURN;
}

void VTOLAI::setAim(double dx, double dy)
{
}

bool VTOLAI::weaponActive(int i, bool groupFire) const
{
	return false;
}

// Set AI goals
void VTOLAI::addGoals(const std::vector<AIGoal>& goals, bool clearExisting)
{
	SET_SCOPE;
	for (size_t i = 0; i < goals.size(); ++i) {
		mGoals.emplace_back(goals[i]);
	}
	RETURN;
}
