4 files changed, 77 insertions, 17 deletions

diff --git a/src/stm32/TODO/hh_entity.c b/src/stm32/TODO/hh_entity.c
new file mode 100644
index 0000000..fa550d5
--- /dev/null
+++ b/src/stm32/TODO/hh_entity.c
@@ -0,0 +1,41 @@
+#include <stdbool.h>
+
+#include "hh_entity.h"
+#include "maths.h"
+
+/*
+    PLAYER: (pos on X)
+    ,___,
+    |   |
+    | X |
+    |___|
+
+*/
+
+bool hh_collision(vec2* pos1, vec2* pos2){
+    if (pos2->x == CLAMP(pos2->x,pos1->x,pos1->x+1.0f)){// hit x
+        return true;
+    }
+
+    if (pos2->y == CLAMP(pos2->y,pos1->y,pos1->y+0.99f)){// hit y
+        return true;
+    }
+    return false;
+}
+
+void hh_solve_collision(vec2* pos_environment, hh_entity* entity){
+    if (entity->vec.x > 0.0f){
+        entity->pos.x = MIN(entity->pos.x,pos_environment->x-1.0f);
+        entity->vec.x = 0.0f;
+    } else if (entity->vec.x < 0.0f){
+        entity->pos.x = MAX(entity->pos.x,pos_environment->x+1.0f);
+        entity->vec.x = 0.0f;
+    } else if (entity->vec.y > 0.0f){
+        entity->pos.x = MIN(entity->pos.x,pos_environment->x-1.0f);
+        entity->vec.x = 0.0f;
+    } else if (entity->vec.y < 0.0f){
+        entity->pos.x = MAX(entity->pos.x,pos_environment->x+1.0f);
+        entity->vec.x = 0.0f;
+    }
+}
+
diff --git a/src/stm32/TODO/hh_entity.h b/src/stm32/TODO/hh_entity.h
index 82d75c5..fdbeb8a 100644
--- a/src/stm32/TODO/hh_entity.h
+++ b/src/stm32/TODO/hh_entity.h
@@ -1,20 +1,24 @@
+#pragma once
+
+#include <stdint.h>
+
 #include "maths.h"
 
-typedef struct hh_entity
-{
-    vec2 pos, direction;
+typedef struct {
+    vec2 pos, vec;
+    bool is_grounded;
     int8_t hp;
     //armor/block?
-};
+}hh_entity;
 
 /// @brief detect for collision enity and eviroment
-/// @param environment position of tile to be checked
-/// @param entity position entity
+/// @param pos1 position of environment tile to be checked
+/// @param pos2 position entity
 /// @return true if collision between enity and environment
-bool hh_collision(const vec2& environment, const vec2& entity);
+bool hh_collision(vec2* pos1, vec2* pos2);
 
 /// @brief solve collisions
 /// @param environment position
 /// @param entity position
 /// @return solved new entity position
-vec2 hh_solve_collision(const vec2& environment, const vec2& entity);
+void hh_solve_collision(vec2* pos_environment, hh_entity* entity);
diff --git a/src/stm32/TODO/maths.c b/src/stm32/TODO/maths.c
new file mode 100644
index 0000000..2f4444a
--- /dev/null
+++ b/src/stm32/TODO/maths.c
@@ -0,0 +1,10 @@
+#include "maths.h"
+
+float clamp( float* x, float *min, float *max ){
+    if (*x < *min)
+        return *min;
+    else if (*x > *max)
+        return *max;
+    else
+        return *x;
+}
diff --git a/src/stm32/TODO/maths.h b/src/stm32/TODO/maths.h
index 7b68156..0889c47 100644
--- a/src/stm32/TODO/maths.h
+++ b/src/stm32/TODO/maths.h
@@ -1,11 +1,16 @@
-#include <math.h>
+#pragma once
 
-typedef struct vec2{
-    float x,y;
-};
+// #include <math.h>
 
-/// @brief clamps x between min and max
-/// @param x value to clamp
-/// @param min 
-/// @param max 
-float clamp( float& x, float &min, float &max );
+typedef struct {
+    u_int32_t x,y;
+} vec2;
+
+typedef vec2 vec_cen;//centered
+typedef vec2 vec_cor;//left upper corner
+
+#define HH_MATH_FIXED_POINT 7 //fixed point at decimal 7lsb (world positions in pixels (with fixed decimal point))
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+#define CLAMP(N,LOWER,UPPER) (MIN(MAX(LOWER, N), UPPER))