/*
 * Step 4: Water Flow Simulation (Memory Optimized)
 * 아두이노 우노 메모리 최적화 버전
 */

#include <LedControl.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// 상수
constexpr uint8_t NUM_PARTICLES = 3;
constexpr float GRAVITY_SCALE = 8.0; // 중력 강화 (2.5 -> 8.0)
constexpr float PARTICLE_RADIUS = 0.45;
constexpr float BOUNCE_DAMPING = 0.85;   // 바운스 강화 (0.7 -> 0.85)
constexpr float COLLISION_DAMPING = 0.9; // 충돌 반발 강화 (0.8 -> 0.9)
constexpr float FRICTION = 0.995;        // 마찰 감소 (0.98 -> 0.995)
constexpr float MIN_VELOCITY = 0.02;     // 최소 속도 증가
constexpr float ACCEL_THRESHOLD = 0.15;  // 임계값 감소 (더 민감하게)

// 객체
LedControl matrix = LedControl(11, 13, 10, 1);
Adafruit_MPU6050 mpu;

// 입자 구조체
struct Particle
{
    float x, y;
    float vx, vy;
    float ax, ay;
};

Particle p[NUM_PARTICLES];

// 중력
struct
{
    float x, y;
} gravity;

// 자이로 기반 회전 각도 (라디안)
float rotation_x = 0.0; // X축 회전 (좌우 기울기)
float rotation_y = 0.0; // Y축 회전 (앞뒤 기울기)

// 타이밍
uint32_t prev_update = 0;
uint32_t prev_sensor = 0;

void setup()
{
    Serial.begin(115200);
    Serial.println(F("\n=== Water Flow ==="));

    // LED Matrix
    matrix.shutdown(0, false);
    matrix.setIntensity(0, 8);
    matrix.clearDisplay(0);

    // 테스트 패턴
    for (uint8_t i = 0; i < 8; i++)
        matrix.setLed(0, i, i, true);
    delay(300);
    matrix.clearDisplay(0);
    Serial.println(F("Matrix OK"));

    // MPU6050
    if (!mpu.begin())
    {
        Serial.println(F("No sensor"));
        gravity.x = 0;
        gravity.y = 1;
    }
    else
    {
        mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
        mpu.setGyroRange(MPU6050_RANGE_500_DEG);
        mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
        Serial.println(F("Sensor OK"));
    }

    // 입자 초기화 (화면 상단 중앙에 배치)
    for (uint8_t i = 0; i < NUM_PARTICLES; i++)
    {
        p[i].x = 3.0 + i * 1.0; // 3, 4, 5 위치
        p[i].y = 1.0 + i * 0.5; // 상단에서 시작
        p[i].vx = p[i].vy = 0;
        p[i].ax = p[i].ay = 0;
    }
    Serial.println(F("Start\n"));
}

void loop()
{
    uint32_t now = millis();

    // 센서 읽기
    if (now - prev_sensor >= 50)
    {
        prev_sensor = now;
        read_sensor();
    }

    // 물리 업데이트
    if (now - prev_update >= 50)
    {
        prev_update = now;
        update_physics();
        render();
    }
}

void read_sensor()
{
    static uint32_t last_time = 0;
    uint32_t now = millis();
    float dt = (now - last_time) / 1000.0;
    if (last_time == 0)
        dt = 0.05;
    last_time = now;

    sensors_event_t a, g, temp;
    if (!mpu.getEvent(&a, &g, &temp))
        return;

    // 각도 드리프트 보정 (가속도계 기반 - 더 강한 보정)
    float ax = a.acceleration.x;
    float ay = a.acceleration.y;
    float az = a.acceleration.z;
    float accel_angle_x = atan2(ay, sqrt(ax * ax + az * az));
    float accel_angle_y = atan2(-ax, sqrt(ay * ay + az * az));

    // 자이로스코프 회전 속도를 적분하여 회전 각도 계산
    rotation_x += g.gyro.x * dt; // rad/s * s = rad
    rotation_y += g.gyro.y * dt;

    // 상보 필터: 자이로 90% + 가속도 10% (드리프트 보정 강화)
    rotation_x = rotation_x * 0.90 + accel_angle_x * 0.10;
    rotation_y = rotation_y * 0.90 + accel_angle_y * 0.10;

    // 각도를 -60° ~ 60° 범위로 제한 (실제 사용 범위)
    constexpr float MAX_ANGLE = 1.05; // 약 60도
    rotation_x = constrain(rotation_x, -MAX_ANGLE, MAX_ANGLE);
    rotation_y = constrain(rotation_y, -MAX_ANGLE, MAX_ANGLE);

    // 회전 각도를 중력 벡터로 변환
    // X축 회전 -> 좌우 중력
    // Y축 회전 -> 앞뒤 중력 (LED 매트릭스에서는 Y축)
    float tilt_x = sin(rotation_y) * GRAVITY_SCALE;
    float tilt_y = sin(rotation_x) * GRAVITY_SCALE;

    // 기본 중력(1.0) + 기울기 성분
    gravity.x = tilt_x;
    gravity.y = cos(rotation_x) * cos(rotation_y) * GRAVITY_SCALE;

    // 중력 크기 제한
    float mag = sqrt(gravity.x * gravity.x + gravity.y * gravity.y);
    if (mag > GRAVITY_SCALE)
    {
        gravity.x = gravity.x / mag * GRAVITY_SCALE;
        gravity.y = gravity.y / mag * GRAVITY_SCALE;
    }
}

void update_physics()
{
    float dt = 0.05;

    // 각 입자
    for (uint8_t i = 0; i < NUM_PARTICLES; i++)
    {
        // 중력
        p[i].ax = gravity.x;
        p[i].ay = gravity.y;

        // 마찰
        p[i].vx *= FRICTION;
        p[i].vy *= FRICTION;

        if (abs(p[i].vx) < MIN_VELOCITY)
            p[i].vx = 0;
        if (abs(p[i].vy) < MIN_VELOCITY)
            p[i].vy = 0;

        // 속도/위치
        p[i].vx += p[i].ax * dt;
        p[i].vy += p[i].ay * dt;
        p[i].x += p[i].vx * dt;
        p[i].y += p[i].vy * dt;

        // 벽 충돌
        if (p[i].x < 0)
        {
            p[i].x = 0;
            p[i].vx = -p[i].vx * BOUNCE_DAMPING;
        }
        if (p[i].x >= 8)
        {
            p[i].x = 7.99;
            p[i].vx = -p[i].vx * BOUNCE_DAMPING;
        }
        if (p[i].y < 0)
        {
            p[i].y = 0;
            p[i].vy = -p[i].vy * BOUNCE_DAMPING;
        }
        if (p[i].y >= 8)
        {
            p[i].y = 7.99;
            p[i].vy = -p[i].vy * BOUNCE_DAMPING;
        }
    }

    // 입자간 충돌
    for (uint8_t i = 0; i < NUM_PARTICLES; i++)
    {
        for (uint8_t j = i + 1; j < NUM_PARTICLES; j++)
        {
            float dx = p[j].x - p[i].x;
            float dy = p[j].y - p[i].y;
            float dist = sqrt(dx * dx + dy * dy);

            if (dist < PARTICLE_RADIUS * 2 && dist > 0)
            {
                // 충돌 해결
                float nx = dx / dist;
                float ny = dy / dist;

                float dvx = p[j].vx - p[i].vx;
                float dvy = p[j].vy - p[i].vy;
                float dvn = dvx * nx + dvy * ny;

                if (dvn < 0)
                {
                    p[i].vx += dvn * nx * COLLISION_DAMPING;
                    p[i].vy += dvn * ny * COLLISION_DAMPING;
                    p[j].vx -= dvn * nx * COLLISION_DAMPING;
                    p[j].vy -= dvn * ny * COLLISION_DAMPING;

                    // 위치 보정
                    float overlap = (PARTICLE_RADIUS * 2) - dist;
                    float sep = overlap / 2 + 0.01;
                    p[i].x -= nx * sep;
                    p[i].y -= ny * sep;
                    p[j].x += nx * sep;
                    p[j].y += ny * sep;
                }
            }
        }
    }
}

void render()
{
    matrix.clearDisplay(0);

    for (uint8_t i = 0; i < NUM_PARTICLES; i++)
    {
        int8_t px = (int8_t)p[i].x;
        int8_t py = (int8_t)p[i].y;

        if (px >= 0 && px < 8 && py >= 0 && py < 8)
        {
            matrix.setLed(0, py, px, true);
        }
    }

    // 간단한 디버그 (30 프레임마다)
    static uint32_t count = 0;
    if (++count % 30 == 0)
    {
        Serial.print(F("Rot:"));
        Serial.print(rotation_x * 57.3, 0); // 라디안 -> 도
        Serial.print(F("°,"));
        Serial.print(rotation_y * 57.3, 0);
        Serial.print(F("° G:"));
        Serial.print(gravity.x, 1);
        Serial.print(F(","));
        Serial.print(gravity.y, 1);
        Serial.print(F(" |"));
        for (uint8_t i = 0; i < NUM_PARTICLES; i++)
        {
            Serial.print(F(" P"));
            Serial.print(i);
            Serial.print(F("("));
            Serial.print((int)p[i].x);
            Serial.print(F(","));
            Serial.print((int)p[i].y);
            Serial.print(F(")"));
        }
        Serial.println();
    }
}