#include <GL/freeglut.h>  
#include <math.h>
#include "vector3d.h"
#include "vector4d.h"
#include "matrix4x4.h"
#include <vector>

#define M_PI 3.14
using namespace std;

/* Global variables */
char title[] = "Avion";

Vector3D camera(3.0, 3.0, -1.0);
int FPS = 60;
double WINDOW_HEIGHT = 480;
double aspectRatio = 640.0f / 480.0f;
vector<vector<Vector3D>> planeBody;
Vector3D planeTop;
vector<vector<Vector3D>> bigWing;
vector<vector<Vector3D>> smallWing;
vector<vector<Vector3D>> building;
Vector3D planePos = Vector3D(-2.0, 2.0, 0.0);
Vector3D buildingPos = Vector3D(1.0, 0.0, 1.0);
Vector3D trenPos = Vector3D(planePos);

Vector3D moveVector = Vector3D();

double speed = 0.005;

/* Initialize OpenGL Graphics */

void createPlaneBody(double r, int n, double d, double h, vector<vector<Vector3D>> &planeBody, Vector3D &planeTop)
{
	planeTop = Vector3D(0.0, 0.0, 0.0);

	vector<Vector3D> circle1, circle2;
	double angle = 2 * M_PI / n;
	Matrix4x4 mRot, mTran, mTranMinus;
	mRot.loadRotateX(angle);
	mTran.loadTranslate(d, 0.0, 0.0);
	mTranMinus.loadTranslate(-d, 0.0, 0.0);
	Vector3D tren = Vector3D(h, 0.0, r);

	for (int i = 0; i < n; i++)
	{
		circle1.push_back(tren);
		tren = mTran * tren;
		circle2.push_back(tren);
		tren = mTranMinus * tren;
		tren = mRot * tren;
	}

	planeBody.push_back(circle1);
	planeBody.push_back(circle2);
}

void createWings(double a1, double b1, double c1, double a2, double b2, double c2, vector<vector<Vector3D>> &bigWing, vector<vector<Vector3D>> &smallWing)
{
	double x1 = (planeTop.X() + planeBody[1][0].X()) / 2 - b1 / 2.0;
	cout << planeBody[1][0] << endl;
	double x2 = planeBody[1][0].X() - b2;

	vector<Vector3D> rect1, rect2, rect3, rect4;
	
	rect1.push_back(Vector3D(x1, -c1/2, a1/2));
	rect1.push_back(Vector3D(x1, c1 / 2, a1 / 2));
	rect1.push_back(Vector3D(x1, c1 / 2, -a1 / 2));
	rect1.push_back(Vector3D(x1, -c1 / 2, -a1 / 2));

	rect2.push_back(Vector3D(x1 + b1, -c1 / 2, a1 / 2));
	rect2.push_back(Vector3D(x1 + b1, c1 / 2, a1 / 2));
	rect2.push_back(Vector3D(x1 + b1, c1 / 2, -a1 / 2));
	rect2.push_back(Vector3D(x1 + b1, -c1 / 2, -a1 / 2));

	rect3.push_back(Vector3D(x2, -c2 / 2, a2 / 2));
	rect3.push_back(Vector3D(x2, c2 / 2, a2 / 2));
	rect3.push_back(Vector3D(x2, c2 / 2, -a2 / 2));
	rect3.push_back(Vector3D(x2, -c2 / 2, -a2 / 2));

	rect4.push_back(Vector3D(x2 + b2, -c2 / 2, a2 / 2));
	rect4.push_back(Vector3D(x2 + b2, c2 / 2, a2 / 2));
	rect4.push_back(Vector3D(x2 + b2, c2 / 2, -a2 / 2));
	rect4.push_back(Vector3D(x2 + b2, -c2 / 2, -a2 / 2));

	bigWing.push_back(rect1);
	bigWing.push_back(rect2);
	smallWing.push_back(rect3);
	smallWing.push_back(rect4);
}

void createBuilding(double a, double b, double c, Vector3D &pos, vector<vector<Vector3D>> &building)
{
	vector<Vector3D> rect1, rect2;

	rect1.push_back(Vector3D(pos.X() - a / 2, pos.Y(), pos.Z() + c / 2));
	rect1.push_back(Vector3D(pos.X() - a / 2, pos.Y() + b, pos.Z() + c / 2));
	rect1.push_back(Vector3D(pos.X() - a / 2, pos.Y() + b, pos.Z() - c / 2));
	rect1.push_back(Vector3D(pos.X() - a / 2, pos.Y(), pos.Z() - c / 2));

	rect2.push_back(Vector3D(pos.X() + a / 2, pos.Y(), pos.Z() + c / 2));
	rect2.push_back(Vector3D(pos.X() + a / 2, pos.Y() + b, pos.Z() + c / 2));
	rect2.push_back(Vector3D(pos.X() + a / 2, pos.Y() + b, pos.Z() - c / 2));
	rect2.push_back(Vector3D(pos.X() + a / 2, pos.Y(), pos.Z() - c / 2));

	building.push_back(rect1);
	building.push_back(rect2);
}

void PositionThePlane(vector<vector<Vector3D>> &planeBody, Vector3D &planeTop, vector<vector<Vector3D>> &bigWing, vector<vector<Vector3D>> &smallWing, Vector3D &planeTopPos, Vector3D &buildingPos)
{
	//postavljanje aviona u polozaj vektora AB

	moveVector = planePos - buildingPos;

	double angle = moveVector.AngleBetween(Vector3D::AxisX);
	Vector3D cross = moveVector.Cross(Vector3D::AxisX);
	
	Matrix4x4 m, rot = Matrix4x4();
	Matrix4x4 tran = Matrix4x4();
	
	rot.loadRotationMatrix(Vector3D(0,0,0), -cross, angle);

	tran.loadTranslate(planePos.X(), planePos.Y(), planePos.Z());

	m = tran*rot;

	planeBody = m.Transform(planeBody);
	planeTop = m.Transform(planeTop);
	bigWing = m.Transform(bigWing);
	smallWing = m.Transform(smallWing);

	//rotacija tako da bude paralelan sa ravni xOz

	Vector3D wing = bigWing[0][0] - bigWing[0][3];
	
	Vector3D wingProj = Vector3D(wing.X(), 0, wing.Z());

	double fi = wing.AngleBetween(wingProj);

	m.loadRotationMatrix(planePos, -moveVector, fi);

	planeBody = m.Transform(planeBody);
	planeTop = m.Transform(planeTop);
	bigWing = m.Transform(bigWing);
	smallWing = m.Transform(smallWing);

	//podesavanje vectora moveVector

	moveVector.Normalize();
	moveVector *= -1 * speed;
}

void drawPlane()
{
	glColor3f(0.0, 1.0, 0.0);

	glBegin(GL_TRIANGLE_FAN);

	glVertex3f(planeTop.X(), planeTop.Y(), planeTop.Z());

	for (int i = 1; i < planeBody[0].size(); i++)
	{
		glVertex3f(planeBody[0][i - 1].X(), planeBody[0][i - 1].Y(), planeBody[0][i - 1].Z());
		glVertex3f(planeBody[0][i].X(), planeBody[0][i].Y(), planeBody[0][i].Z());
	}

	glVertex3f(planeBody[0][planeBody[0].size() - 1].X(), planeBody[0][planeBody[0].size() - 1].Y(), planeBody[0][planeBody[0].size() - 1].Z());
	glVertex3f(planeBody[0][0].X(), planeBody[0][0].Y(), planeBody[0][0].Z());

	glEnd();

	glColor3f(1.0, 1.0, 0.0);

	for (int i = 1; i < planeBody[1].size(); i++)
	{
		glBegin(GL_POLYGON);

		glVertex3f(planeBody[0][i - 1].X(), planeBody[0][i - 1].Y(), planeBody[0][i - 1].Z());
		glVertex3f(planeBody[0][i].X(), planeBody[0][i].Y(), planeBody[0][i].Z());
		glVertex3f(planeBody[1][i].X(), planeBody[1][i].Y(), planeBody[1][i].Z());
		glVertex3f(planeBody[1][i - 1].X(), planeBody[1][i - 1].Y(), planeBody[1][i - 1].Z());

		glEnd();
	}

	glBegin(GL_POLYGON);

	glVertex3f(planeBody[0][planeBody[1].size() - 1].X(), planeBody[0][planeBody[1].size() - 1].Y(), planeBody[0][planeBody[1].size() - 1].Z());
	glVertex3f(planeBody[0][0].X(), planeBody[0][0].Y(), planeBody[0][0].Z());
	glVertex3f(planeBody[1][0].X(), planeBody[1][0].Y(), planeBody[1][0].Z());
	glVertex3f(planeBody[1][planeBody[1].size() - 1].X(), planeBody[1][planeBody[1].size() - 1].Y(), planeBody[1][planeBody[1].size() - 1].Z());

	glEnd();

	glBegin(GL_POLYGON);

	for (int i = 0; i < planeBody[1].size(); i++)
		glVertex3f(planeBody[1][i].X(), planeBody[1][i].Y(), planeBody[1][i].Z());

	glEnd();

	glColor3f(0.0, 0.0, 1.0);

	glBegin(GL_POLYGON);

	for (int i = 0; i < 4; i++) glVertex3f(smallWing[0][i].X(), smallWing[0][i].Y(), smallWing[0][i].Z());

	glEnd();

	glBegin(GL_POLYGON);

	for (int i = 0; i < 4; i++) glVertex3f(smallWing[1][i].X(), smallWing[1][i].Y(), smallWing[1][i].Z());

	glEnd();

	for (int i = 1; i < 4; i++)
	{
		glBegin(GL_POLYGON);
		glVertex3f(smallWing[0][i-1].X(), smallWing[0][i-1].Y(), smallWing[0][i-1].Z());
		glVertex3f(smallWing[0][i].X(), smallWing[0][i].Y(), smallWing[0][i].Z());
		glVertex3f(smallWing[1][i].X(), smallWing[1][i].Y(), smallWing[1][i].Z());
		glVertex3f(smallWing[1][i - 1].X(), smallWing[1][i - 1].Y(), smallWing[1][i - 1].Z());
		glEnd();
	}

	glBegin(GL_POLYGON);
	glVertex3f(smallWing[0][4 - 1].X(), smallWing[0][4 - 1].Y(), smallWing[0][4 - 1].Z());
	glVertex3f(smallWing[0][0].X(), smallWing[0][0].Y(), smallWing[0][0].Z());
	glVertex3f(smallWing[1][0].X(), smallWing[1][0].Y(), smallWing[1][0].Z());
	glVertex3f(smallWing[1][4 - 1].X(), smallWing[1][4 - 1].Y(), smallWing[1][4 - 1].Z());
	glEnd();

	glColor3f(1.0, 0.0, 0.0);

	glBegin(GL_POLYGON);

	for (int i = 0; i < 4; i++) glVertex3f(bigWing[0][i].X(), bigWing[0][i].Y(), bigWing[0][i].Z());

	glEnd();

	glBegin(GL_POLYGON);

	for (int i = 0; i < 4; i++) glVertex3f(bigWing[1][i].X(), bigWing[1][i].Y(), bigWing[1][i].Z());

	glEnd();

	for (int i = 1; i < 4; i++)
	{
		glBegin(GL_POLYGON);
		glVertex3f(bigWing[0][i - 1].X(), bigWing[0][i - 1].Y(), bigWing[0][i - 1].Z());
		glVertex3f(bigWing[0][i].X(), bigWing[0][i].Y(), bigWing[0][i].Z());
		glVertex3f(bigWing[1][i].X(), bigWing[1][i].Y(), bigWing[1][i].Z());
		glVertex3f(bigWing[1][i - 1].X(), bigWing[1][i - 1].Y(), bigWing[1][i - 1].Z());
		glEnd();
	}

	glBegin(GL_POLYGON);
	glVertex3f(bigWing[0][4 - 1].X(), bigWing[0][4 - 1].Y(), bigWing[0][4 - 1].Z());
	glVertex3f(bigWing[0][0].X(), bigWing[0][0].Y(), bigWing[0][0].Z());
	glVertex3f(bigWing[1][0].X(), bigWing[1][0].Y(), bigWing[1][0].Z());
	glVertex3f(bigWing[1][4 - 1].X(), bigWing[1][4 - 1].Y(), bigWing[1][4 - 1].Z());
	glEnd();

	
}

void drawBuilding()
{
	glColor3f(0.0, 1.0, 0.0);

	glBegin(GL_POLYGON);

	for (int i = 0; i < 4; i++) glVertex3f(building[0][i].X(), building[0][i].Y(), building[0][i].Z());

	glEnd();

	glBegin(GL_POLYGON);

	for (int i = 0; i < 4; i++) glVertex3f(building[1][i].X(), building[1][i].Y(), building[1][i].Z());

	glEnd();

	for (int i = 1; i < 4; i++)
	{
		glBegin(GL_POLYGON);
		glVertex3f(building[0][i - 1].X(), building[0][i - 1].Y(), building[0][i - 1].Z());
		glVertex3f(building[0][i].X(), building[0][i].Y(), building[0][i].Z());
		glVertex3f(building[1][i].X(), building[1][i].Y(), building[1][i].Z());
		glVertex3f(building[1][i - 1].X(), building[1][i - 1].Y(), building[1][i - 1].Z());
		glEnd();
	}

	glBegin(GL_POLYGON);
	glVertex3f(building[0][4 - 1].X(), building[0][4 - 1].Y(), building[0][4 - 1].Z());
	glVertex3f(building[0][0].X(), building[0][0].Y(), building[0][0].Z());
	glVertex3f(building[1][0].X(), building[1][0].Y(), building[1][0].Z());
	glVertex3f(building[1][4 - 1].X(), building[1][4 - 1].Y(), building[1][4 - 1].Z());
	glEnd();
}

void drawAxis()
{
	glColor3f(1.0, 0.0, 0.0);
	glBegin(GL_LINES);
		glVertex3f(0.0, 0.0, 0.0);
		glVertex3f(2*Vector3D::AxisX[0], 2*Vector3D::AxisX[1], 2*Vector3D::AxisX[2]);
	glEnd();

	glColor3f(0.0, 1.0, 0.0);
	glBegin(GL_LINES);
		glVertex3f(0.0, 0.0, 0.0);
		glVertex3f(2*Vector3D::AxisY[0], 2*Vector3D::AxisY[1], 2*Vector3D::AxisY[2]);
	glEnd();

	glColor3f(0.0, 0.0, 1.0);
	glBegin(GL_LINES);
		glVertex3f(0.0, 0.0, 0.0);
		glVertex3f(2*Vector3D::AxisZ[0], 2*Vector3D::AxisZ[1], 2*Vector3D::AxisZ[2]);
	glEnd();
}

void movePlane()
{
	Matrix4x4 m;

	if (!(((planeTop.X() >= building[0][0].X() && planeTop.X() <= building[1][0].X()) || (planeTop.X() <= building[0][0].X() && planeTop.X() >= building[1][0].X())) &&
		((planeTop.Z() >= building[0][1].Z() && planeTop.Z() <= building[0][2].Z()) || (planeTop.Z() <= building[0][1].Z() && planeTop.Z() >= building[0][2].Z())) &&
		((planeTop.Y() >= building[0][0].Y() && planeTop.Y() <= building[0][1].Y()) || (planeTop.Y() <= building[0][0].Y() && planeTop.Y() >= building[0][1].Y()))))
	{
		m.loadTranslate(moveVector.X(), moveVector.Y(), moveVector.Z());

		planeTop = m.Transform(planeTop);
		planeBody = m.Transform(planeBody);
		bigWing = m.Transform(bigWing);
		smallWing = m.Transform(smallWing);
	}
}


void display()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear color and depth buffers

	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

	
	glColor3f(1.0, 1.0, 1.0);
	glBegin(GL_LINES);
	glVertex3f(planePos.X(), planePos.Y(), planePos.Z());
	glVertex3f(buildingPos.X(), buildingPos.Y(), buildingPos.Z());
	glEnd();
	
	drawAxis();
	drawBuilding();
	drawPlane();
	movePlane();

	glutSwapBuffers();  // Swap the front and back frame buffers (double buffering)

	glutPostRedisplay();
}

void SetCamera()
{
	glViewport(0, 0, WINDOW_HEIGHT*aspectRatio, WINDOW_HEIGHT);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(80.0f, aspectRatio, 0.1f, 50.0f);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(camera.m_x, camera.m_y, camera.m_z,
		0.0, 0.0, 0.0,
		0.0, 1.0, 0.0);
}

void initGL()
{
	glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // Set background color to black and opaque
	glShadeModel(GL_FLAT);

	glEnable(GL_DEPTH_TEST);   // Enable depth testing for z-culling
}

int main(int argc, char** argv)
{
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE);
	glutInitWindowSize(WINDOW_HEIGHT * aspectRatio, WINDOW_HEIGHT);
	glutInitWindowPosition(50, 50);
	glutCreateWindow(title);

	glutDisplayFunc(display);
	SetCamera();
	initGL();
	
	createBuilding(0.5,0.5,0.5, buildingPos, building);
	createPlaneBody(0.1, 100, 0.7, 0.2, planeBody, planeTop);
	createWings(1.0, 0.3, 0.01, 0.5, 0.1, 0.01, bigWing, smallWing);
	PositionThePlane(planeBody, planeTop, bigWing, smallWing, planePos, buildingPos);

	glutMainLoop();

	return 0;
}