#include <stdio.h>
#include <iostream>
#include "shader.h"
#include <cmath>
#include <GL/glew.h>
#include <GLFW/glfw3.h>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
GLFWwindow* window;

#define PI                  3.141592    // PI approximate value
#define HEIGHT              0.0025       // step length for numerical integration
#define ROD_LENGHT          0.5         // length of rod
#define GRAVITY             9.81        // gravitational constant
#define theta_0             PI / 2      // Initial angle
#define omega_0    0           // Initial angular velocity
#define time_0     0           // Initial time
#define RADIUS              0.15        // Radius of pendulum circle

#define A                   1.4         // Amplitude of the driving force
#define k                   0.67         // Frequency of the driving force

// Function for angular velocity (f function for numerical integration)
float f(float time, float theta, float omega) {
    return omega;
}

// Function for angular acceleration (g function for numerical integration)
float g(float time, float theta, float omega) {
    return -(GRAVITY / ROD_LENGHT) * sin(theta);
}

// Function to draw the circle (pendulum ball)
void drawCircle(float array[]) {
    int corner_one, corner_two, corner_three; // corners of triangles. GL_TRIANGLES starts to draw counterclockwise.
    corner_one = -6;
    corner_two = -4;
    corner_three = -2;

    for (int angle = 1; angle <= 360; angle++) {
        corner_one = corner_one + 6;
        corner_two = corner_two + 6;
        corner_three = corner_three + 6;

        array[corner_one] = 0.0f;
        array[corner_one + 1] = 0.0f;

        array[corner_two] = RADIUS * cos((angle - 1) * PI / 180);
        array[corner_two + 1] = RADIUS * sin((angle - 1) * PI / 180);

        array[corner_three] = RADIUS * cos(angle * PI / 180);
        array[corner_three + 1] = RADIUS * sin(angle * PI / 180);
    }
}

void RungeKuttaIntegration(float& theta, float& omega, float& time) {
    float h = HEIGHT; // Step size
    float k1_theta = h * f(time, theta, omega);
    float k1_omega = h * g(time, theta, omega);
    float k2_theta = h * f(time + h / 2, theta + k1_theta / 2, omega + k1_omega / 2);
    float k2_omega = h * g(time + h / 2, theta + k1_theta / 2, omega + k1_omega / 2);
    float k3_theta = h * f(time + h / 2, theta + k2_theta / 2, omega + k2_omega / 2);
    float k3_omega = h * g(time + h / 2, theta + k2_theta / 2, omega + k2_omega / 2);
    float k4_theta = h * f(time + h, theta + k3_theta, omega + k3_omega);
    float k4_omega = h * g(time + h, theta + k3_theta, omega + k3_omega);

    // Update theta and omega
    theta += (k1_theta + 2 * k2_theta + 2 * k3_theta + k4_theta) / 6;
    omega += (k1_omega + 2 * k2_omega + 2 * k3_omega + k4_omega) / 6;

    // Keep theta in the range of -2PI to 2PI
    if (theta > 2 * PI) theta -= 2 * PI;
    if (theta < -2 * PI) theta += 2 * PI;

    time += h; // Increment time
}

void EulerIntegration(float& theta, float& omega, float& time) {
    float h = HEIGHT; // Step size
    float theta_new = theta + h * f(time, theta, omega);
    float omega_new = omega + h * g(time, theta, omega);

    // Update theta and omega
    theta = theta_new;
    omega = omega_new;

    // Keep theta in the range of -2PI to 2PI
    if (theta > 2 * PI) theta -= 2 * PI;
    if (theta < -2 * PI) theta += 2 * PI;

    time += h; // Increment time
}

void VerletIntegration(float& theta, float& omega, float& time) {
    float h = HEIGHT; // Step size
    float theta_new = theta + h * omega + 0.5 * h * h * g(time, theta, omega);
    float omega_new = (theta_new - theta) / h;

    // Update theta and omega
    theta = theta_new;
    omega = omega_new;

    // Keep theta in the range of -2PI to 2PI
    if (theta > 2 * PI) theta -= 2 * PI;
    if (theta < -2 * PI) theta += 2 * PI;

    time += h; // Increment time

}

int main()
{
    if( !glfwInit() )
    {
        fprintf( stderr, "Failed to initialize GLFW\n" );
        getchar();
        return -1;
    }

    glfwWindowHint(GLFW_SAMPLES, 4);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

    window = glfwCreateWindow( 630, 600, "ZADANIE 1", NULL, NULL);
    if( window == NULL ){
        fprintf( stderr, "Failed to open GLFW window. If you have an Intel GPU, they are not 3.3 compatible. Try the 2.1 version of the tutorials.\n" );
        getchar();
        glfwTerminate();
        return -1;
    }

    glfwMakeContextCurrent(window);
    glewExperimental = true;
    if (glewInit() != GLEW_OK) {
        fprintf(stderr, "Failed to initialize GLEW\n");
        getchar();
        glfwTerminate();
        return -1;
    }

    glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);
    glClearColor(1.0f, 0.8f, 0.0f, 0.0f);

    Shader myshader("pendulum_vs.glsl" , "pendulum_fs.glsl");
    unsigned int shaderProgram = myshader.programID();
    float vertices[2160];
    float vertices2[] = { //vertices2 gives us the rod of the pendulum.
            -0.01f, 0.0f,
            0.01f, 0.0f,
            0.01f, 0.8f,
            -0.01f, 0.8f,
            -0.01, 0.0f
    };

    drawCircle(vertices); //draws the pendulum ball.

    unsigned int VBO, VAO, VAO2;
    glGenVertexArrays(1, &VAO);
    glGenBuffers(1, &VBO);
    glBindVertexArray(VAO);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    //position attribute for 'vertices'(ball of the pendulum)
    glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 2 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);

    glGenVertexArrays(1, &VAO2);
    glGenBuffers(1, &VBO);
    glBindVertexArray(VAO2);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices2), vertices2, GL_STATIC_DRAW);

    //position attribute for 'vertices2'(rod of the pendulum)
    glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 2 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);

    float theta= theta_0;
    float omega= omega_0;
    float time = time_0;

    float k1,k2,k3,k4,l1,l2,l3,l4;
    float driving_force;

    //Initialize f and g functions.
    f(time,theta,omega);
    g(time,theta,omega);
    float current_angle;
    do{
        driving_force = A * cos(k * time);
        current_angle = theta * 180 / PI; //converts theta(radian) to degree

        glClear( GL_COLOR_BUFFER_BIT );
        glUseProgram(shaderProgram);

        glm::mat4 model = glm::mat4(1.0f);
        glm::mat4 projection = glm::mat4(1.0f);
        glm::mat4 view = glm::mat4(1.0f);
        view = glm::rotate(view, glm::radians(current_angle), glm::vec3(0.0f, 0.0f, 1.0f));
        view = glm::translate(view, glm::vec3(0.0f, -0.8f, 0.0f));

        glUniformMatrix4fv(glGetUniformLocation(shaderProgram, "projection"), 1, GL_FALSE, glm::value_ptr(projection));
        glUniformMatrix4fv(glGetUniformLocation(shaderProgram, "view"), 1, GL_FALSE, glm::value_ptr(view));
        glUniformMatrix4fv(glGetUniformLocation(shaderProgram, "model"), 1, GL_FALSE, glm::value_ptr(model));

//        RungeKuttaIntegration(theta, omega, time);
//        VerletIntegration(theta, omega, time);
        EulerIntegration(theta, omega, time);

        glBindVertexArray(VAO);
        glDrawArrays(GL_TRIANGLES, 0, 1080);
        glBindVertexArray(VAO2);
        glDrawArrays(GL_TRIANGLE_STRIP, 0, 5);

        glfwSwapBuffers(window);
        glfwPollEvents();
    }
    while( glfwGetKey(window, GLFW_KEY_ESCAPE ) != GLFW_PRESS &&
           glfwWindowShouldClose(window) == 0);

    glfwTerminate();

    return 0;
}