#include <stdio.h>
#include <math.h>
#include "display.h"

#define W 1600
#define H 900
#define FPS 60
#define RFPS 1/600
#define PI 3.141592654

typedef struct{
    float x;
    float y;
} vec;


const float s = (PI/180 * 10); // rad
const int l = 250; // mm
const int m = 25;  // kg
// const vec g = {0.0f, -9.81f}; // m/s^2
const float g = -9.81f;

// vec p;
// vec v;
// vec a;
float t;
float v;
float a;

// vec rotate_grav(float angle, int d) // d = 0-clock, 1-anti
// {
//     // vec rg = {0, g.y*m*sinf(angle)};
//     vec rg = {0, g.y};
//     if(d == 0) {
//         rg.x = (rg.y*sinf(angle));
//         rg.y = -(rg.y*cosf(angle));
//     } else if(d == 1) {
//         rg.x = -(rg.y*sinf(angle));
//         rg.y = (rg.y*cosf(angle));
//     }

//     printf("grav transform: %f %f %f\n", rg.x, rg.y, sqrt((rg.x*rg.x) + (rg.y*rg.y)));
//     return rg;
// }

void update()
{

    // float sinamp = p.x / l;
    // vec rg = rotate_grav((PI/2) - asinf(sinamp), (p.x > 0) ? 0 : 1);

    // printf("amp: %f\n", asinf(sinamp)*180/PI);
    // printf("lenght: %f\n", sqrt((p.x*p.x) + (p.y*p.y)));

    // p.x = sinf(t) * l;
    // p.y = cosf(t) * l;

    // a.x += rg.x;
    // a.y += rg.y;
    // v.x += a.x * RFPS;
    // v.y += a.y * RFPS;
    // p.x += v.x * RFPS;
    // p.y += v.y * RFPS;

    // dspl_draw_line(p.x, p.y, v.x + p.x, v.y + p.y, 0);
    // dspl_draw_line(p.x, p.y, a.x + p.x, a.y + p.y, 1);
    // dspl_draw_line(p.x, p.y, 0, 0, 0);
    // dspl_draw_circle(p.x, p.y, 5);

    a += g * cosf(t) * m;
    printf("%f %f\n", sinf(t), a);
    v += a * RFPS;
    t += v * RFPS;


    int px = sinf(t) * l;
    int py = cosf(t) * l;
    dspl_draw_line(px, py, 0, 0, 0);
    dspl_draw_circle(px, py, 5);

}

int main(void)
{
    // p.x = sinf(t) * l;
    // p.y = cosf(t) * l;
    // v.x = 0; v.y = 0;
    // a.x = 0; a.y = 0;

    t = s;
    v = 0;
    a = 0;

    dspl_createinfo info = {0};
    info.width = W;
    info.height = H;
    info.name = "Pendulum";
    info.fps = FPS;
    info.update_func = &update;

    dspl_create(info);
    dspl_start();
    dspl_destroy();
    return 0;
}