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

#define W 1600
#define H 900
#define FPS 1000
#define SCALE 1000
#define SWING_SCALE 15

#define RFPS 1/FPS
#define PI 3.141592654

const float s = (PI/180 * 25); // rad
const float l = 0.5 * SCALE; // m
const int m = 7;  // kg
const float g = -9.81f * SCALE; // m/s^2

float t;
float v;
float a;

float secs = 0.0f;
int swings = 2 * SWING_SCALE;

int has_reached_starting_amp(float old_t)
{
    if(swings % 2 == 0 && t < 0) {
        if(old_t < t) return 1;
    } else if(swings % 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 swing");
        swings += 1;
    }
    if(swings >= 2 * SWING_SCALE) {
        printf("period: %f\n", secs/SWING_SCALE);
        secs = 0.0f;
        swings = 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);
}

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