/*
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <fstream>
#include "mpi.h"
#include <GL/glut.h>
#include <cstdlib>

using namespace std;
*/

#include <stdio.h>
#include <GL/glut.h>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <math.h>
#include <fstream>
#include <chrono>
#include <thread>

using namespace std;

double uavX[15], uavY[15], uavZ[15], velX[15], velY[15], velZ[15];
double aMax = 20;

// Whole space is defined in meters


double squared(double in)
{
    return (in * in);
}

double getMagnitude(double in1, double in2, double in3)
{
    double magnitude = sqrt(squared(in1) + squared(in2) + squared(in3));
    return magnitude;
}

double getDistance(double x1, double y1, double z1, double x2, double y2, double z2)
{
    double magnitude = sqrt(squared((x1 - x2)) + squared((y1 - y2)) + squared((z1 - z2)) );
    return magnitude;
}

/*
* a function that determines the total speed of a UAV
* @param vX: velocity in X direction
* @param vY: velocity in Y direction
* @param vZ: velocity in Z direction
* @return: double giving total speed
*/
double getSpeed(double vX, double vY, double vZ)
{
    double magnitude = sqrt(vX*vX + vY*vY + vZ*vZ);
    return magnitude;
}


// Yards to meters
double yToM(double yards)
{
    return (yards * 0.9144);
}

// Meters to yards
double mToY(double meters)
{
    return (meters * 1.09361);
}

// Get spherical coordinates
double getPhi(double uavX, double uavY, double uavZ)
{
    double numerator = sqrt(squared(uavX) + squared(uavY));
    double phi = atan2(numerator, 50.0);
    return phi;
}

double getTheta(double uavX, double uavY)
{
    if (uavX == 0 && uavY == 0)
    {
        return 0.0;
    }
    else
    {
        return atan2(-uavY, -uavX);
    }

}

double newVelo(double origV, double acc)
{
    return (origV + acc);
}

void init(void)
{
    // Set initial parameters
    glDepthMask(GL_TRUE);
    glMatrixMode(GL_PROJECTION);

    // Set black background
    glClearColor(0.0, 0.0, 0.0, 0.0);

    // Set smooth objects and maintain colors
    glShadeModel(GL_SMOOTH);
    glEnable(GL_LIGHTING);
    glEnable(GL_COLOR_MATERIAL);
    glEnable(GL_DEPTH_TEST);
}


/*
* This function will define the initial perspective and
* viewport for the assignment.
* No parameters and no return values.
*/
void resize(int w, int h)
{
    glViewport(0, 0, (GLsizei)w, (GLsizei)h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0, ((w + 0.0) / (h + 0.0)), 1.0, 200);
}


void display(void)
{
        int count = 0;
        double distance = 0;
        double phi, theta;
        double xPos, yPos, zPos, xVel[15], yVel[15], zVel[15];
        double vMag, xVel0, yVel0, zVel0;
        double vel1[15], vel2[15], vel3[15];
        double aX, aY, aZ;

        // Setup beginning uav values
        for (int j = 0; j < 5; ++j)
        {
            for (int i = 0; i < 3; ++i)
            {
                uavX[count] = yToM(26.667 - (26.667 * i));
                uavY[count] = yToM(-50.0 + (25 * j));
                uavZ[count] = 0;
                xVel[count] = 0;
                yVel[count] = 0;
                zVel[count] = 0;
                ++count;
            }
        }

    count = 0;

    for (int time = 0; time < 1000; ++time)
    {
        // Clear and set matrix mode
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glMatrixMode(GL_MODELVIEW);

        // Draw scene and look at football field
        glLoadIdentity();
        gluLookAt(120.0, 0.0, 120.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);

        // Draw the football field
        glColor3f(0.0, 1.0, 0.0);
        glPushMatrix();
            glBegin(GL_POLYGON);
                glTranslatef(0.0, 0.0, 0.0);
                glVertex3f(yToM(-26.667), yToM(-50.0), 0.0);
                glVertex3f(yToM(-26.667), yToM(50.0), 0.0);
                glVertex3f(yToM(26.667), yToM(50.0), 0.0);
                glVertex3f(yToM(26.667), yToM(-50.0), 0.0);
            glEnd();

        glColor3f(1.0, 1.0, 0.0);
        glPushMatrix();
            glTranslatef(0.0, 0.0, 50.0);
            glutWireSphere(10.0, 50.0, 50.0);
        glPopMatrix();

        glColor3f(1.0, 0.0, 0.0);

        count = 0;

        for (int c = 0; c < 15; ++c)
        {
            xPos = uavX[c];
            yPos = uavY[c];
            zPos = uavZ[c];
            glPushMatrix();
                glTranslatef(xPos, yPos, zPos);
                glutSolidTetrahedron();
            glPopMatrix();

            vMag = getMagnitude(velX[c], velY[c], velZ[c]);
            distance = getDistance(xPos, yPos, zPos, 0.0, 0.0, 50.0);
            if (distance <= 10)
            {
                // MAKE REALISTIC!!!!
                velX[c] -= velX[c];
                velY[c] -= velY[c];
                velZ[c] -= velZ[c];
            }
            else
            {
                if (vMag < 0.001)
                {
                    phi = getPhi(xPos, yPos, zPos);
                    theta = getTheta(xPos, yPos);
                    aX = 10 * sin(phi) * cos(theta);
                    aY = 10 * sin(phi) * sin(theta);
                    aZ = 10 * cos(phi) + 10;
                    aZ -= 10;
                    uavX[c] = xPos + (velX[c] * 0.1) + (aX * 0.1 * 0.1 / 2);
                    uavY[c] = yPos + (velY[c] * 0.1) + (aY * 0.1 * 0.1 / 2);
                    uavZ[c] = zPos + (velZ[c] * 0.1) + (aZ * 0.1 * 0.1 / 2);
                    velX[c] = aX * 0.1;
                    velY[c] = aY * 0.1;
                    velZ[c] = aZ * 0.1;
                }
                else
                {
                    uavX[c] = xPos + (velX[c] * 0.1);
                    uavY[c] = yPos + (velY[c] * 0.1);
                    uavZ[c] = zPos + (velZ[c] * 0.1);
                }
            }
        }
        glutSwapBuffers();
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

}


void change(void)
{
    glutPostRedisplay();
}





int main(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);

    // Define initial position and look angle
    glutInitWindowPosition(100,100);
    glutInitWindowSize(400,400);
    glutCreateWindow ("ECE 4122 Final Project");
    glEnable(GL_DEPTH_TEST);

    init();
    glutReshapeFunc(resize);
    glutDisplayFunc(display);
    glutMainLoop();

    return 0;
}