#include "vector.h"
#include <GyverOLED.h>

template<typename T>
void PRINT(const T& arg) {
  Serial.print(arg);
  Serial.print(',');
}

template<typename... Args>
void DEBUG(const Args&... args) {
  (PRINT(args), ...);
  Serial.println();
}

#define PART_R 2
#define PART_SHIFT 4
#define DSHIFT (PART_R + PART_SHIFT + 1)

float damp = 0.8;
int16_t R = (1 << PART_R) << PART_SHIFT;
int16_t D = R * 2;
uint16_t width = 128 << PART_SHIFT;
uint16_t height = 64 << PART_SHIFT;
const uint8_t amount = 50;

template <typename T>
class Vect {
  public:
    Vect (T x = 0, T y = 0): x(x), y(y) {}
    T x;
    T y;
};

GyverOLED<SSD1306_128x64, OLED_BUFFER> oled;

class Particle {
  public:
    Particle(int16_t x = 0, int16_t y = 0, int8_t vx = 0, int8_t vy = 0) {
      pos.x = x;
      pos.y = y;
      vel.x = vx;
      vel.y = vy;
    }

    Vect<int16_t> pos;
    Vect<int8_t> vel;

    void move() {
      vel.x *= damp;
      vel.y *= damp;
      pos.x += vel.x;
      pos.y += vel.y;
    }

    void edges() {
      if (pos.x < R) {
        pos.x = R;
        vel.x = 0;
        // vel.x *= -damp;
      } else if (pos.x > width - R - 1) {
        pos.x = width - R - 1;
        vel.x = 0;
        // vel.x *= -damp;
      }

      if (pos.y < R) {
        pos.y = R;
        vel.y = 0;
        // vel.y *= -damp;
      } else if (pos.y > height - R - 1) {
        pos.y = height - R - 1;
        vel.y = 0;
        // vel.y *= -damp;
      }
    }

    void collision(Particle* particles, uint8_t i, uint8_t amount) {
      uint16_t tx = pos.x >> DSHIFT;
      uint16_t ty = pos.y >> DSHIFT;

      for (uint8_t p = 0; p < amount; p++) {
        if (i == p) continue;
        Particle& neigh = particles[p];
        uint16_t ox = neigh.pos.x >> DSHIFT;
        uint16_t oy = neigh.pos.y >> DSHIFT;
        ox = abs(ox - tx);
        oy = abs(oy - ty);

        if (ox <= 1 && oy <= 1) {
          Vect<int32_t> norm(neigh.pos.x - pos.x, neigh.pos.y - pos.y);
          uint32_t dsq = norm.x * norm.x + norm.y * norm.y;

          if (dsq && dsq < D * D) {
            float ds = sqrt(dsq);
            Vect<float> norm1(norm.x / ds, norm.y / ds);
            // Vect<float> rel(neigh.vel.x - vel.x, neigh.vel.y - vel.y);
            // float scalar = norm1.x * rel.x + norm1.y * rel.y;
            // Vect<float> proj(norm1.x * scalar, norm1.y * scalar);

            // vel.x += proj.x;
            // vel.y += proj.y;
            // neigh.vel.x -= proj.x;
            // neigh.vel.y -= proj.y;

            vel.x *= damp;
            vel.y *= damp;
            neigh.vel.x *= damp;
            neigh.vel.y *= damp;

            Vect<float> shift(norm1.x * (D - ds) / 2, norm1.y * (D - ds) / 2);
            pos.x -= shift.x;
            pos.y -= shift.y;
            neigh.pos.x += shift.x;
            neigh.pos.y += shift.y;
          }
        }
      }
    }

    void show(int n) {
      // oled.circle(pos.x, pos.y, R, OLED_STROKE);
      oled.circle(pos.x >> PART_SHIFT, pos.y >> PART_SHIFT, R >> PART_SHIFT, OLED_STROKE);
      // oled.setCursorXY((pos.x) - 2, (pos.y) - 2);
      // oled.print(n);
    }
};

Particle pars[amount];

void setup() {
  Serial.begin(115200);
  oled.init();
  Wire.setClock(800000);
  for (uint8_t i = 0; i < amount; i++) {
    // pars[i] = Particle(random(0, Particle::width), random(0, Particle::height));
    pars[i] = Particle(width / 2, height / 2, random(-5, 5), random(-5, 5));
  }
}

void loop() {
  oled.clear();
  float angle = radians((uint16_t)analogRead(0) * 360.0 / 1023);
  float mag = 10;
  Vector2<float> G(mag * cos(angle), mag * sin(angle));
  uint32_t ms = millis();
  for (uint8_t i = 0; i < amount; i++) {
    // pars[i].vel.x += G.x;
    // pars[i].vel.y += G.y;
    pars[i].move();
    pars[i].collision(pars, i, amount);
    pars[i].vel.x += G.x;
    pars[i].vel.y += G.y;
    pars[i].edges();
  }
  Serial.println(millis() - ms);
  for (uint8_t i = 0; i < amount; i++) pars[i].show(i);
  oled.update();
  // delay(10);
}