////////////////////////////////////////////////////////////////////////////////
// GroundVehicleAI.cpp
// AI for ground vehicles (biped and quad mechs, wheeled, tracked vehicles, hovercraft
// Copyright Bjoern Ganster
// Developed since 2019
////////////////////////////////////////////////////////////////////////////////

#include "FileList.h"
#include "GroundVehicleAI.h"
#include "MechSim.h"

#include <iostream>

#define FILE_NUMBER FILE_NO_GROUND_VEHICLE_AI_CPP

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

GroundVehicleAI::GroundVehicleAI(const char* className, Vehicle* vehicle)
: AbstractVehicleAI(className, GroundBasedVehicleAIID, vehicle),
  mTriggerRange(300.0),
  mLastFired(0),
  mCurrentGoal(0),
  mLastAimFrame(0),
  mLastMoveFrame(0)
{
}

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

GroundVehicleAI::~GroundVehicleAI()
{
}

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

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

	SET_SCOPE;
	if (mVehicle->isDestroyed()) {
		mVehicle->m_turnDelta = 0.0;
		RETURN;
	}

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

	// todo: see if there things to avoid. Yank code from FighterAI for this?

	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:
				mVehicle->setSpeed(0.0); // Remain stationary while waiting
				mVehicle->setStatus(shutDown); // in case this is not the first goal (whit)
				checkWakeUp();
				break;
			case SubObjectiveType::attack:
				doAttack();
				break;
			case SubObjectiveType::defend:
				doDefend();
				break;
			case SubObjectiveType::inspect:
				doInspect();
				break;
			case SubObjectiveType::visit:
				doVisit();
				break;
			case SubObjectiveType::joinFormation:
				doJoinFormation();
				break;
		}
	}

	RETURN;
}

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

void GroundVehicleAI::moveTowards(const Point3D& aimGoal)
{
	// 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();

	if (dist < 0.1) {
		RETURN; // if we are where we want to be, don't change a thing (dist near zero can lead to serious problems below)
	}

	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;
	}

	// todo: check if a collision is possible in the near future. If so, evade or slow down.

	// Choose walking speed
	double runningHexes = mVehicle->getDouble(Vehicle::Info_RunningHexes);
	double desiredSpeed = mVehicle->getMaxSpeed();

	// Store speed delta
	SET_OP;
	mVehicle->m_speedDelta = desiredSpeed - mVehicle->getSpeed();
	if (mVehicle->m_speedDelta > 1.0) {
		mVehicle->m_speedDelta = 1.0;
	} else if (mVehicle->m_speedDelta < -1.0) {
		mVehicle->m_speedDelta = -1.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 GroundVehicleAI::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);

	// Horizontal aim
	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 + frand(-mAimError, mAimError);

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

	// Vertical aim
	// Warning: m_walkingDirectionDegrees and desiredTorsoTwist count cw, while bipedMech->torsoTwist counts ccw ...
	auto currentTilt = mVehicle->getAimTilt();
	auto currentTurn = mVehicle->getAimTurn();
	double desiredTorsoTilt = 180 * atan(heightDiff / dist) / M_PI + frand(-mAimError, mAimError);
	
	// See if a weapon can be fired
	bool readyToFire = mVehicle->canFireWeapon(mVehicle->mActiveWeapon, now);
	if (!readyToFire) {
		mVehicle->cycleWeapon(false);
		mLastFired = now; // to delay AI shooting
		RETURN;
	}

	if (abs(desiredTorsoTwist + currentTurn - mVehicle->mHeading) < mAimingAccuracy
	&&  abs(desiredTorsoTilt - currentTilt) < mAimingAccuracy
	&&  readyToFire
	&&  mLastFired + 1000.0 * mTriggerSpeed < now) 
	{
		SET_OP;
		double weaponRange = mVehicle->getActiveWeaponRange();
		if (dist < weaponRange) {
			mVehicle->fireWeapons(false, now);
			mLastFired = now;
		} else {
			mVehicle->nextWeapon();
		}
	} else {
		mVehicle->adjustAim(mVehicle->mHeading - desiredTorsoTwist, desiredTorsoTilt, 1.0);
	}

	RETURN;
}

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

void GroundVehicleAI::fight(DWORD now)
{
	if (mTarget == nullptr) {
		return;
	}

	SET_SCOPE;
	const MountedWeapon* activeWeapon = mVehicle->getActiveWeapon();
	if (activeWeapon == nullptr) {
		RETURN;
	}

	bool attackGround = false;
	#ifdef MECHVM
	if (activeWeapon->weaponType == WeaponType::IS1_Long_Tom) {
		attackGround = true;
	}
	#endif

	double desiredDirection = mVehicle->mHeading;
	double desiredSpeed = 0.0;
	Point3D walkingGoal;
	Point3D ownPos = mVehicle->getPosition();

	// 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/vehicle while going there ...
	SET_OP;
	Vehicle* targetVehicle = dynamic_cast<Vehicle*>(mTarget);
	Point3D targetPosition = mTarget->getCenterOfMass();
	Point3D aimGoal = targetPosition;
	if (targetVehicle != nullptr) {
		SET_OP;
		double desiredDistance = std::min(0.5 * mVehicle->getActiveWeaponRange(), 50.0);
		Point3D sweetSpot1 = targetPosition - desiredDistance * targetVehicle->p_front + desiredDistance * targetVehicle->p_left;
		Point3D sweetSpot2 = targetPosition - desiredDistance * targetVehicle->p_front - desiredDistance * targetVehicle->p_left;
		walkingGoal = sweetSpot1;
		if ((ownPos-sweetSpot1).length() > (ownPos-sweetSpot2).length()) {
			walkingGoal = sweetSpot2;
		}
		aimGoal = targetVehicle->getAttackSpot(attackGround);
	} else {
		SET_OP;
		// Choose a position slightly beside the object so that the vehicle does not run directly into the object ...
		BoundingBox targetBB = mTarget->getTransformedBB();
		double distx = ownPos.x - targetPosition.x;
		double distz = ownPos.z - targetPosition.z;
		if (fabs(distx) > fabs(distz)) {
			if (distx > 0) {
				walkingGoal = Point3D(targetBB.max().x + 50.0, targetPosition.y, targetPosition.z);
			} else {
				walkingGoal = Point3D(targetBB.min().x - 50.0, targetPosition.y, targetPosition.z);
			}
		} else {
			if (distz > 0) {
				walkingGoal = Point3D(targetPosition.x, targetPosition.y, targetBB.max().z + 50.0);
			} else {
				walkingGoal = Point3D(targetPosition.x, targetPosition.y, targetBB.min().z - 50.0);
			}
		}
	}

	if (mLastMoveFrame + mMsBetweenMoveFrames < now) {
		SET_OP;
		moveTowards(walkingGoal);
		mLastMoveFrame = now;
	}
	if (mLastAimFrame + mMsBetweenAimFrames < now) {
		SET_OP;
		aimAndFire(aimGoal, now);
		mLastAimFrame= now;
	}

	RETURN;
}

void GroundVehicleAI::checkWakeUp()
{
	SET_SCOPE;
	if (mVehicle->getStatus() != VehicleStatus::shutDown) {
		// No reason to to keep the engine running while we're supposed to be waiting?
		mVehicle->setStatus(shutDown);
		RETURN;
	}

	// 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()) {
		auto missionTime = (int)mechSim->missionTime();
		const auto& goal = mGoals[mCurrentGoal];
		if (goal.type == SubObjectiveType::sleep && missionTime > 1000 * goal.duration)
		{
			mVehicle->setStatus(engineRunning);
			++mCurrentGoal;
			mechSim->updateObjectStatus(goal.trigger, SubObjectiveType::event, false);
		} else if (goal.type != SubObjectiveType::sleep) {
			mVehicle->setStatus(engineRunning);
		}

		// Check if the event we're waiting happened already ...
		if (goal.type == SubObjectiveType::sleep 
		&& !goal.trigger.empty()
		&& mechSim->eventHappened(goal.trigger)) 
		{
			mVehicle->setStatus(engineRunning);
			++mCurrentGoal;
		}
	}

	RETURN;
}

void GroundVehicleAI::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 GroundVehicleAI::doDefend()
{
	// todo
}

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

void GroundVehicleAI::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;
}

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

void GroundVehicleAI::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 GroundVehicleAI::updateObjectStatus(const BGCPString& id, SubObjectiveType newStatus)
{
	SET_SCOPE;
	if (mCurrentGoal >= mGoals.size()
	||  mVehicle == nullptr
	||  mVehicle->isDestroyed()) {
		RETURN;
	}

	// whit: would lead to mCurrentGoal being incremented twice, skipping the sleep objective 
	SET_OP;
	auto& goal = mGoals[mCurrentGoal];
	if (goal.type == SubObjectiveType::sleep
	&&  goal.trigger == id
	&&  !goal.trigger.empty()) 
	{
		++mCurrentGoal;
		if (mVehicle->getStatus() == shutDown) {
			mVehicle->setStatus(engineRunning);
		}
	}
	RETURN;
}

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

// Check if a weapon is currently active - used by the HUD
bool GroundVehicleAI::weaponActive(int i, bool groupFire) const
{
	return false;
}

// Add goals for AI
void GroundVehicleAI::addGoals(const std::vector<AIGoal>& goals, bool clearExisting)
{
	SET_SCOPE;
	if (clearExisting) {
		mGoals.clear();
	}

	for (size_t i = 0; i < goals.size(); ++i) {
		mGoals.emplace_back(goals[i]);
	}
	RETURN;
}

// Set skills (will be reflected by other variables));
void GroundVehicleAI::setPiloting(int newVal)
{
	// For perfect AI, the values below should be 0
	mPiloting = newVal;
	newVal = putInRange(0, newVal, 6);
	mMsBetweenMoveFrames = 80 * newVal;
}

void GroundVehicleAI::setGunnery(int newVal)
{
	// For perfect AI, the values below should be 0
	mGunnery = newVal;
	newVal = putInRange(0, newVal, 6);
	mMsBetweenAimFrames = 20 * newVal;
	mAimError = 2 * newVal;
}

void GroundVehicleAI::doJoinFormation()
{
	SET_SCOPE;
	MechSim* mechSim = getMechSim();
	Vehicle* playerVehicle = mechSim->getPlayerVehicle();

	if (playerVehicle == nullptr) {
		RETURN;
	}

	// todo: depending on whether we are starmate1 or starmate2, assume different positions ...
	double x = 50;
	if (mVehicle->getID().equalsNoCase("Starmate2")) {
		x = -50;
	}

	Point3D targetPos = playerVehicle->getPosition() + x * playerVehicle->p_left - 50 * playerVehicle->p_front;
	moveTowards(targetPos);
	RETURN;
}

void GroundVehicleAI::setTarget(GameObject* gameObject)
{
	// At skill levels beyond 6, the vehicle will just follow its course
	SET_SCOPE;
	if (mGunnery > 6) {
		return;
	}

	AbstractVehicleAI::setTarget(gameObject);

	RETURN;
}
