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

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

const float s = (PI/180 * 25); // rad
const int l = 500; // mm
const int m = 1;  // kg
const float g = -9810.0f; // mm/s^2

float t;
float v;
float a;

float secs = 0.0f;
int cycles = 20;

int has_reached_starting_amp(float old_t)
{
    if(cycles % 2 == 0 && t < 0) {
        if(old_t < t) return 1;
    } else if(cycles % 2 == 1 && t > 0) {
        if(old_t > t) return 1;
    } return 0;
}
void update()
{
    float old_t = t;
    a = (g/l) * sinf(t);
    v += a * RFPS;
    t += v * RFPS;

    if(has_reached_starting_amp(old_t)) {
        puts("new cycle");
        cycles += 1;
    }
    if(cycles == 20) {
        printf("sec: %f\n", secs);
        secs = 0.0f;
        cycles = 0;
    }

    secs += (float)RFPS;



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


}

int main(void)
{
    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;
}