#include "MassSpringSystemSimulator.h"
#include <stdio.h>

MassSpringSystemSimulator::MassSpringSystemSimulator()
{
	m_iTestCase = 0;
	m_fMass = 10;
	m_fStiffness = 40;
	m_iIntegrator = EULER;
}

const char* MassSpringSystemSimulator::getTestCasesStr()
{
	return "Demo1,Demo2,Demo3,Demo4";
}

void MassSpringSystemSimulator::initUI(DrawingUtilitiesClass* DUC)
{
	this->DUC = DUC;
	switch (m_iTestCase)
	{
		case 0:
			break;
		case 1:
			break;
		case 2:
			break;
		default:
			break;
	}
}

void MassSpringSystemSimulator::reset()
{
	m_mouse.x = m_mouse.y = 0;
	m_trackmouse.x = m_trackmouse.y = 0;
	m_oldtrackmouse.x = m_oldtrackmouse.y = 0;

	springs.clear();
	masspoints.clear();
}

void MassSpringSystemSimulator::drawFrame(ID3D11DeviceContext* pd3dImmediateContext)
{
	
	for (size_t i = 0; i < springs.size(); i++) {
		auto sp = springs.at(i);
		if (!sp.isValid())
		{
			springs.erase(springs.begin() + i);
			continue;
		}
		auto mp1 = sp.mp1.lock();
		auto mp2 = sp.mp2.lock();
		DUC->setUpLighting(Vec3(), 0.4 * Vec3(1, 1, 1), 100, 0.6 * Vec3(0.97, 0.86, 1));
		
		DUC->drawSphere(mp1->position, Vec3(0.01));
		DUC->drawSphere(mp2->position, Vec3(0.01));

		DUC->beginLine();
		DUC->drawLine(mp1->position, Vec3(1,0,0), mp2->position, Vec3(0,1,0));
		DUC->endLine();
	}
}

void MassSpringSystemSimulator::notifyCaseChanged(int testCase)
{
	m_iTestCase = testCase;
	system("cls");
	reset();

	switch (m_iTestCase)
	{
	case 0: {
		cout << "Demo 1 !\n";
		setMass(10);
		setStiffness(40);

		int first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
		int second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);

		addSpring(first, second, 1);
		cout << "\t -- INITIAL --\n";
		printSpring(springs.at(0));
		cout << "--------------------------------------------------" << std::endl;

		//calculate Euler for one step and print results
		setIntegrator(EULER);
		cout << "\n\n\t -- EULER RESULT --\n";
		simulateTimestep(1);
		printSpring(springs.at(0));

		cout << "--------------------------------------------------" << std::endl;

		reset();
		first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
		second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);

		addSpring(first, second, 1);

		//calculate Midpoint for one step and print results
		setIntegrator(MIDPOINT);
		cout << "\n\n\t -- MIDPOINT RESULT --\n";
		simulateTimestep(1);
		printSpring(springs.at(0));
		break;
	}
	
	case 1: {
		cout << "Demo 2 !\n";
		reset();
		int first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
		int second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);
		addSpring(first, second, 1.0);

		cout << "\t -- INITIAL --\n";
		printSpring(springs.at(0));

		cout << "--------------------------------------------------" << std::endl;

		//calculate Euler for a timestep of 0.005 and print results
		setIntegrator(EULER);
		cout << "\n\n\t -- EULER RESULT--\n";
		simulateTimestep(0.005);
		printSpring(springs.at(0));

		break;
	}

	case 2: {
		cout << "Demo 3 !\n";
		reset();
		int first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
		int second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);
		addSpring(first, second, 1.0);

		cout << "\t -- INITIAL --\n";
		printSpring(springs.at(0));

		cout << "--------------------------------------------------" << std::endl;

		//calculate Midpoint for a timestep of 0.005 and print results
		setIntegrator(MIDPOINT);
		cout << "\n\n\t -- MIDPOINT RESULT --\n";
		simulateTimestep(0.005);
		printSpring(springs.at(0));
		break;
	}
	case 3: {
		cout << "Demo 4 !\n";
		break;
	}
	default:
		break;
	}
}

void MassSpringSystemSimulator::externalForcesCalculations(float timeElapsed) 
{
	Point2D mouseDiff;
	mouseDiff.x = m_trackmouse.x - m_oldtrackmouse.x;
	mouseDiff.y = m_trackmouse.y - m_oldtrackmouse.y;
	if (mouseDiff.x != 0 || mouseDiff.y != 0)
	{
		Mat4 worldViewInv = Mat4(DUC->g_camera.GetWorldMatrix() * DUC->g_camera.GetViewMatrix());
		worldViewInv = worldViewInv.inverse();
		Vec3 inputView = Vec3((float)mouseDiff.x, (float)-mouseDiff.y, 0);
		Vec3 inputWorld = worldViewInv.transformVectorNormal(inputView);
		// find a proper scale!
		float inputScale = 0.001f;
		inputWorld = inputWorld * inputScale;
		
		//m_vfMovableObjectPos = m_vfMovableObjectFinalPos + inputWorld;
	}
	else {
		//m_vfMovableObjectFinalPos = m_vfMovableObjectPos;
	}

}

void MassSpringSystemSimulator::simulateTimestep(float timeStep)
{
	//update current setup for each frame
	for (size_t i = 0; i < springs.size(); i++) {
		auto sp = springs.at(i);
		if (!sp.isValid())
		{
			springs.erase(springs.begin() + i);
			continue;
		}

		if (m_iIntegrator == EULER) {

			Euler(sp, timeStep);
		}
		else if (m_iIntegrator == MIDPOINT) {
			Midpoint(sp, timeStep);
		}
		else if (m_iIntegrator == LEAPFROG) {
			//TODO: Add Leapfrog
		}

	}
}

void MassSpringSystemSimulator::onClick(int x, int y)
{
	m_trackmouse.x = x;
	m_trackmouse.y = y;
}

void MassSpringSystemSimulator::onMouse(int x, int y)
{
	m_oldtrackmouse.x = x;
	m_oldtrackmouse.y = y;
	m_trackmouse.x = x;
	m_trackmouse.y = y;
}

void MassSpringSystemSimulator::setMass(float mass)
{
	m_fMass = mass;
}

void MassSpringSystemSimulator::setStiffness(float stiffness)
{
	m_fStiffness = stiffness;
}

void MassSpringSystemSimulator::setDampingFactor(float damping)
{
	m_fDamping = damping;
}

int MassSpringSystemSimulator::addMassPoint(Vec3 position, Vec3 Velocity, bool isFixed)
{
	MassPoint masspoint;
	masspoint.position = position;
	masspoint.velocity = Velocity;
	masspoint.isFixed = isFixed;
	masspoints.push_back(std::make_shared<MassPoint>(masspoint));

	return masspoints.size() - 1;
}

void MassSpringSystemSimulator::addSpring(int masspoint1, int masspoint2, float initialLength)
{
	auto mp1 = masspoints.at(masspoint1);
	auto mp2 = masspoints.at(masspoint2);

	Spring spring;
	spring.mp1 = mp1;
	spring.mp2 = mp2;
	spring.initialLength = initialLength;

	springs.push_back(spring);
}

int MassSpringSystemSimulator::getNumberOfMassPoints()
{
	return masspoints.size();
}

int MassSpringSystemSimulator::getNumberOfSprings()
{
	return springs.size();
}

Vec3 MassSpringSystemSimulator::getPositionOfMassPoint(int index)
{
	auto mp = masspoints.at(index);
	return mp->position;
}

Vec3 MassSpringSystemSimulator::getVelocityOfMassPoint(int index)
{
	auto mp = masspoints.at(index);
	return mp->velocity;
}

void MassSpringSystemSimulator::applyExternalForce(Vec3 force)
{
	//eine Vorstellung: for all Masspoints, update force
}

int MassSpringSystemSimulator::LengthCalculator(Vec3 position1, Vec3 position2) {

	Vec3 PosVector = position1 - position2;
	//wurzel aus Vektor
	int length = sqrt(pow(PosVector.x, 2) + pow(PosVector.y, 2) + pow(PosVector.z, 2));
	
	return length;
}



Vec3 MassSpringSystemSimulator::calculatePositionTimestepEuler(Vec3 oldPosition, float timestep, Vec3 velocity)
{
	return oldPosition + timestep * velocity;
}

Vec3 MassSpringSystemSimulator::calculateVelocityTimestepEuler(Vec3 oldVelocity, float timestep, Vec3 acceleration)
{
	return oldVelocity + acceleration * timestep;
}

Vec3 MassSpringSystemSimulator::calculateAcceleration(Vec3 force, float mass)
{
	return force / mass;
}

void MassSpringSystemSimulator::Midpoint(Spring& spring, float timestep) {
	//here some implementation about Midpoint
	auto massPoint1 = spring.mp1.lock();
	auto massPoint2 = spring.mp2.lock();

	//old position
	auto mp = massPoint1->position;
	auto mp2 = massPoint2->position;
	//old Velocity
	auto mOld_v = massPoint1->velocity;
	auto m2Old_v = massPoint2->velocity;

	Vec3 PosVector = mp - mp2;
	//Abstand ausrechnen
	int d = LengthCalculator(mp, mp2);
	//normalize
	Vec3 PosNorm1 = PosVector / d;
	Vec3 PosNorm2 = -1 * PosNorm1;

	Vec3 Force = -m_fStiffness * (d - spring.initialLength) * PosNorm1;
	Vec3 Force2 = -1 * Force;

	Vec3 oldAcc = calculateAcceleration(Force,m_fMass);
	Vec3 oldAcc2 = calculateAcceleration(Force2, m_fMass);

	//Midpoint calculator
	//Pos of Midstep
	Vec3 PosOfMidstep = mp + 0.5 * timestep * mOld_v;
	Vec3 PosOfMidstep2 = mp2 + 0.5 * timestep * m2Old_v;
	//Vel at Midstep
	Vec3 VelAtMidstep = mOld_v + 0.5 * timestep * oldAcc;
	Vec3 VelAtMidstep2 = m2Old_v + 0.5 * timestep * oldAcc2;
	
	Vec3 NewPos = mp + timestep * VelAtMidstep;
	Vec3 NewPos2 = mp2 + timestep * VelAtMidstep2;

	Vec3 NewVel = mOld_v + timestep * oldAcc;
	Vec3 NewVel2 = m2Old_v + timestep * oldAcc2;
	//cout << NewPos;
	//cout << NewVel;
	massPoint1->position = NewPos;
	massPoint1->velocity = NewVel;
	massPoint2->position = NewPos2;
	massPoint2->velocity = NewVel2;
}

void MassSpringSystemSimulator::Euler(Spring& spring, float timestep)
{
	auto massPoint1 = spring.mp1.lock();
	auto massPoint2 = spring.mp2.lock();

	//take old position and send to calculatePositionTimestepEuler
	auto PosVector = massPoint1->position - massPoint2->position;
	auto lengthVector = sqrt(PosVector.x * PosVector.x + PosVector.y * PosVector.y + PosVector.z * PosVector.z);
	auto normalized = PosVector / lengthVector;

	// Actual Calculation
	// Force of spring is -k * (l - L) * normalizedVector [for P2 we can take -F1)
	auto force = -m_fStiffness * (lengthVector - spring.initialLength) * normalized;
	auto foreP2 = -1 * force;
	auto veloc = calculatePositionTimestepEuler(massPoint1->velocity, timestep, calculateAcceleration(force, 10.));
	auto pos = calculatePositionTimestepEuler(massPoint1->position, timestep, veloc);

	auto veloc2 = calculateVelocityTimestepEuler(massPoint2->velocity, timestep, calculateAcceleration(foreP2, 10.));
	auto pos2 = calculatePositionTimestepEuler(massPoint2->position, timestep, veloc2);

	// Update Positions and Velocity
	massPoint1->position = pos;
	massPoint1->velocity = veloc;

	massPoint2->position = pos2;
	massPoint2->velocity = veloc2;
}

void MassSpringSystemSimulator::printSpring(const Spring& spring)
{
	auto mp1 = spring.mp1.lock();
	auto mp2 = spring.mp2.lock();
	printf("Masspoint 1:\nPosition: %s \nVelocity: %s\n\n", mp1->position.toString().c_str(), mp1->velocity.toString().c_str());
	printf("Masspoint 2:\nPosition: %s \nVelocity: %s\n", mp2->position.toString().c_str(), mp2->velocity.toString().c_str());
}