#include <SDL2/SDL.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "chain.c"

#define WINDOW_WIDTH 800
#define WINDOW_HEIGHT 600
#define CHAIN_LENGTH 10
#define PARTICLE_SPACING 10.0f

static void
project(float x, float y, float z, int *screen_x, int *screen_y) {
  *screen_x = (int)(x + WINDOW_WIDTH / 2.0f);
  *screen_y = (int)(-y + WINDOW_HEIGHT / 2.0f);
}
// Helper: Rotate vector by inv_quat and translate by -chain_pos (scalar for simplicity)
static void quat_rotate_inv_translate(float wx, float wy, float wz,  // world pos
                                      float cx, float cy, float cz,  // chain pos
                                      float qx, float qy, float qz, float qw,  // chain quat
                                      float *mx, float *my, float *mz) {  // out model pos
  // Translate: world - chain
  float tx = wx - cx, ty = wy - cy, tz = wz - cz;
  // Quat conj: (qw, -qx, -qy, -qz)
  float cqw = qw, cqx = -qx, cqy = -qy, cqz = -qz;
  // t = conj * (0, tx,ty,tz)
  float tw = cqx*tx + cqy*ty + cqz*tz;
  float t_x = cqw*tx + cqy*tz - cqz*ty;
  float t_y = cqw*ty + cqz*tx - cqx*tz;
  float t_z = cqw*tz + cqx*ty - cqy*tx;
  // model = t * quat (vec part)
  *mx = qw*t_x + qy*t_z - qz*t_y + tw*qx;
  *my = qw*t_y + qz*t_x - qx*t_z + tw*qy;
  *mz = qw*t_z + qx*t_y - qy*t_x + tw*qz;
}
extern int
main(int argc, char *argv[]) {
  if (SDL_Init(SDL_INIT_VIDEO) < 0) {
    printf("SDL_Init failed: %s\n", SDL_GetError());
    return 1;
  }
  SDL_Window *window = SDL_CreateWindow("Chain Simulation", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, WINDOW_WIDTH, WINDOW_HEIGHT, 0);
  SDL_Renderer *renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED);
  if (!window || !renderer) {
    printf("SDL_CreateWindow/Renderer failed: %s\n", SDL_GetError());
    SDL_Quit();
    return 1;
  }
  struct chn_sim sim;
  chn_sim_init(&sim);

  struct chn_cfg cfg = {
    .damping = 0.5f,
    .stiffness = 0.9f,
    .friction = 0.5f,
    .linear_inertia = 0.9f,       // Tune 0.5f for more tangential lag
    .angular_inertia = 0.9f,
    .centrifugal_inertia = 0.9f,   // Tune 0.5f for more outward bow
    .bend_stiffness = 0.8f,
  };
  float rest_pos[CHAIN_LENGTH * 4];
  for (int i = 0; i < CHAIN_LENGTH; i++) {
    rest_pos[i*4 + 0] = 0.0f;
    rest_pos[i*4 + 1] = -i * PARTICLE_SPACING;
    rest_pos[i*4 + 2] = 0.0f;
    rest_pos[i*4 + 3] = i == 0 ? 0.0f : 1.0f;
  }
  int chn = chn_sim_add(&sim, &cfg, rest_pos, CHAIN_LENGTH);
  chn_sim_grav(&sim, chn, (float[]){0.0f, -1.0f, 0.0f});  // Reduced for spin test
  chn_sim_tel(&sim, chn, (float[]){50.0f, 0.0f, 0.0f}, (float[]){0.0f, 0.0f, 0.0f, 1.0f});  // Start at circle edge

  // Fixed world spheres (triangle outside circle)
  float world_spheres[12] = {  // 3 spheres * 4 floats (x,y,z,r)
    60.0f,  0.0f, 0.0f, 10.0f,   // Sphere 0: right
    -30.0f, 52.0f, 0.0f, 10.0f,  // Sphere 1: top-left
    -30.0f,-52.0f, 0.0f, 10.0f   // Sphere 2: bottom-left
  };

  // Circular motion params
  float time = 0.0f;
  float circle_radius = 50.0f;
  float angular_speed = 2.0f * M_PI / 5.0f;  // Full circle every 5s
  float dt = 0.016f;
  int running = 1;
  SDL_Event event;

  // Debug: Check gravity and masses
  int p_base = chn * PTC_PER_CHN;
  while (running) {
    while (SDL_PollEvent(&event)) {
      if (event.type == SDL_QUIT) {
        running = 0;
      }
    }
    time += dt;

    // Circular anchor motion
    float anchor_x = circle_radius * cosf(angular_speed * time);
    float anchor_y = circle_radius * sinf(angular_speed * time);
    chn_sim_mov(&sim, chn, (float[]){anchor_x, anchor_y, 0.0f}, (float[]){0.0f, 0.0f, 0.0f, 1.0f});

    // Transform fixed world spheres to current model space
    float model_spheres[12];
    for (int s = 0; s < 3; ++s) {
      float wx = world_spheres[s*4 + 0], wy = world_spheres[s*4 + 1], wz = world_spheres[s*4 + 2];
      float r = world_spheres[s*4 + 3];
      float cx = sim.chn_pos_x[chn], cy = sim.chn_pos_y[chn], cz = sim.chn_pos_z[chn];
      float qx = sim.chn_quat_x[chn], qy = sim.chn_quat_y[chn], qz = sim.chn_quat_z[chn], qw = sim.chn_quat_w[chn];
      float mx, my, mz;
      quat_rotate_inv_translate(wx, wy, wz, cx, cy, cz, qx, qy, qz, qw, &mx, &my, &mz);
      model_spheres[s*4 + 0] = mx;
      model_spheres[s*4 + 1] = my;
      model_spheres[s*4 + 2] = mz;
      model_spheres[s*4 + 3] = r;
    }
    chn_sim_set_sph(&sim, chn, model_spheres, 3);
    chn_sim_run(&sim, dt);

    SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
    SDL_RenderClear(renderer);
    SDL_SetRenderDrawColor(renderer, 0, 255, 0, 255);  // Green for chain & spheres

    float root_x = sim.chn_pos_x[chn];  // Current root world x/y
    float root_y = sim.chn_pos_y[chn];

    // Draw chain segments
    for (int i = 0; i < CHAIN_LENGTH - 1; i++) {
      int idx0 = (i % 2 == 0 ? i / 2 : 8 + (i - 1) / 2);
      int idx1 = ((i + 1) % 2 == 0 ? (i + 1) / 2 : 8 + i / 2);

      float model_x0 = sim.ptc_pos_x[p_base + idx0], model_y0 = sim.ptc_pos_y[p_base + idx0];
      float model_x1 = sim.ptc_pos_x[p_base + idx1], model_y1 = sim.ptc_pos_y[p_base + idx1];
      float world_x0 = model_x0 + root_x;
      float world_y0 = model_y0 + root_y;
      float world_x1 = model_x1 + root_x;
      float world_y1 = model_y1 + root_y;

      int sx0, sy0, sx1, sy1;
      project(world_x0, world_y0, 0, &sx0, &sy0);
      project(world_x1, world_y1, 0, &sx1, &sy1);
      SDL_RenderDrawLine(renderer, sx0, sy0, sx1, sy1);
    }
    // Draw spheres (approx circle outline with 32 points)
    int p_base = chn * PTC_PER_CHN;
    int s_base = chn * SPH_PER_CHN;
    for (int s = 0; s < 3; ++s) {  // Only draw active spheres
      float sph_model_x = sim.sph_x[s_base + s];
      float sph_model_y = sim.sph_y[s_base + s];
      float sph_r = sim.sph_r[s_base + s];
      if (sph_r <= 0.0f) continue;

      float sph_world_x = sph_model_x + root_x;
      float sph_world_y = sph_model_y + root_y;
      int sph_sx, sph_sy;
      project(sph_world_x, sph_world_y, 0, &sph_sx, &sph_sy);

      // Parametric circle: 32 points
      const int num_points = 32;
      for (int pt = 0; pt < num_points; ++pt) {
        float angle0 = 2.0f * M_PI * pt / num_points;
        float angle1 = 2.0f * M_PI * (pt + 1) / num_points;
        float dx0 = sph_r * cosf(angle0), dy0 = sph_r * sinf(angle0);
        float dx1 = sph_r * cosf(angle1), dy1 = sph_r * sinf(angle1);

        int px0, py0, px1, py1;
        project(sph_world_x + dx0, sph_world_y + dy0, 0, &px0, &py0);
        project(sph_world_x + dx1, sph_world_y + dy1, 0, &px1, &py1);
        SDL_RenderDrawLine(renderer, px0, py0, px1, py1);
      }
    }
    SDL_RenderPresent(renderer);
    SDL_Delay(16);
  }
  SDL_DestroyRenderer(renderer);
  SDL_DestroyWindow(window);
  SDL_Quit();
  return 0;
}