1 files changed, 35 insertions, 62 deletions

diff --git a/src/main.c b/src/main.c
index 53b4edf..3fd6447 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,92 +1,65 @@
 #include <stdio.h>
+#include <time.h>
 #include <math.h>
 #include "display.h"
 
 #define W 1600
 #define H 900
-#define FPS 60
-#define RFPS 1/600
+#define FPS 460
+#define RFPS 1/FPS
 #define PI 3.141592654
 
-typedef struct{
-    float x;
-    float y;
-} vec;
+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
 
-
-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;
-// }
+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;
 
-    // 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;
+    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;
+    }
 
-    // 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;
+    secs += (float)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;
+    float px = sinf(t) * l;
+    float py = cosf(t) * l;
     dspl_draw_line(px, py, 0, 0, 0);
-    dspl_draw_circle(px, py, 5);
+    dspl_draw_circle(px, py, m*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;