#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>

#include <chrono>
#include <thread>

GLFWwindow *window;

#define GLM_ENABLE_EXPERIMENTAL

#include <glm/gtx/string_cast.hpp>

#define PI                 3.141592    // PI approximate value
#define RADIUS              1     // Radius of pendulum circle
#define GRAVITY 9.81

#define CIRCLE_AMOUNT 1
#define CIRCLE_SEGMENTS 12

#define INIT_SPEED 20
#define INIT_ANGLE (PI / 4)

#define TIME_STEP 0.0025


double f_x(double time) {
    return INIT_SPEED * time * cos(INIT_ANGLE);
}

double f_y(double time) {
    return INIT_SPEED * time * sin(INIT_ANGLE) - (GRAVITY * pow(time, 2) / 2.0f);
}

std::vector<glm::vec3> vertices;

glm::vec3 compute_ball_position_rk(glm::vec3 currentPosition, double time) {


    glm::vec3 k1 = glm::vec3(f_x(time),
                             f_y(time),
                             0.0f);

    glm::vec3 k2 = glm::vec3(f_x(time) + TIME_STEP / 2,
                             f_y(time) + k1.z / 2,
                             0.0f);
    glm::vec3 k3 = glm::vec3(f_x(time) + TIME_STEP / 2,
                             f_y(time) + k2.z / 2,
                             0.0f);
    glm::vec3 k4 = glm::vec3(f_x(time),
                             f_y(time) + k3.z,
                             0.0f);
    glm::vec3 newPosition = currentPosition + (k1 + 2.0f * k2 + 2.0f * k3 + k4) * glm::vec3(TIME_STEP / 6.0f);

//    std::cout << glm::to_string(newPosition) << std::endl;

    return newPosition;
}


// Function to draw the circle (pendulum ball)
void buildCircles() {
    float angle = 360.0f / CIRCLE_SEGMENTS;

    int triangleCount = CIRCLE_SEGMENTS - 2;

    // positions
    for (int c = 0; c < CIRCLE_AMOUNT; c++) {
        std::vector<glm::vec3> temp;

        float center_x = -0.75f;
        float center_y = RADIUS * (static_cast<float>(c) * 2.0f - CIRCLE_AMOUNT);

        for (int i = 0; i < CIRCLE_SEGMENTS; i++) {
            float currentAngle = angle * i;
            float x = center_x + RADIUS * cos(glm::radians(currentAngle));
            float y = center_y + RADIUS * sin(glm::radians(currentAngle)) + RADIUS * c;
            float z = 0.0f;

            glm::vec3 point = glm::vec3(x, y, z);
            std::cout << glm::to_string(point) << std::endl;
            temp.push_back(point);

        }

        for (int i = 0; i < triangleCount; i++) {
            vertices.push_back(temp[0]);
            vertices.push_back(temp[i + 1]);
            vertices.push_back(temp[i + 2]);
        }

    }
}

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

    GLFWmonitor *MyMonitor = glfwGetPrimaryMonitor();

    const GLFWvidmode *mode = glfwGetVideoMode(MyMonitor);
    int SCR_WIDTH = mode->width;
    int SCR_HEIGHT = mode->height;

    window = glfwCreateWindow(SCR_WIDTH, SCR_HEIGHT, "ZADANIE 2", 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();
    buildCircles();

    unsigned int VBO, VAO;
    glGenVertexArrays(1, &VAO);
    glGenBuffers(1, &VBO);
    glBindVertexArray(VAO);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3) * vertices.size(), &vertices[0], GL_STATIC_DRAW);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), (void *) 0);
    glEnableVertexAttribArray(0);


    float current_time = 1.0f;

    glm::vec3 ballPosition(0.0f, 0.0f, 0.0f);

    do {


        glClear(GL_COLOR_BUFFER_BIT);

        glm::mat4 model = glm::mat4(0.15f);
        glm::mat4 projection = glm::perspective(glm::radians(90.0f), (float)SCR_WIDTH / (float)SCR_HEIGHT, 0.1f, 10.0f);


        ballPosition = compute_ball_position_rk(ballPosition, current_time);

        // Set the view matrix to move the ball to the new position
        glm::mat4 view = glm::translate(glm::mat4(0.0f), ballPosition);

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

        glBindVertexArray(VAO);
        glUseProgram(shaderProgram);
        glDrawArrays(GL_TRIANGLES, 0, vertices.size() * CIRCLE_AMOUNT);


        glfwSwapBuffers(window);
        glfwPollEvents();

        current_time += TIME_STEP;

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

    glfwTerminate();

    return 0;
}