// game/server.c - SMOOTH TERRAIN VERSION
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <time.h>

#include <ode/ode.h>

#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib")
typedef int socklen_t;
#define CLOSESOCK closesocket
#else
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <unistd.h>
#define CLOSESOCK close
#endif

#define PROTOCOL_VERSION 67
#define SERVER_PORT 27015
#define MAX_PLAYERS 16
#define USERNAME_MAX 16

#define TICK_HZ 30
#define DT (1.0f / (float)TICK_HZ)

#define WEAPON_PISTOL 0
#define WEAPON_RIFLE 1

#define MAX_ITEMS 64

#define ITEM_NONE 0
#define ITEM_MEDKIT 1
#define ITEM_AMMO_PISTOL 2
#define ITEM_AMMO_RIFLE 3

#define BTN_RELOAD (1u << 0)
#define BTN_SWITCH_PISTOL (1u << 1)
#define BTN_SWITCH_RIFLE (1u << 2)
#define BTN_PICK (1u << 3)
#define BTN_USE_MEDKIT (1u << 4)
#define BTN_JUMP (1u << 5)

#define CAT_WORLD (1u << 0)
#define CAT_PLAYER (1u << 1)
#define CAT_ITEM (1u << 2)

// ============== SIMPLEX NOISE ==============
static const uint8_t perm[512] = {
    151, 160, 137, 91,  90,  15,  131, 13,  201, 95,  96,  53,  194, 233, 7,
    225, 140, 36,  103, 30,  69,  142, 8,   99,  37,  240, 21,  10,  23,  190,
    6,   148, 247, 120, 234, 75,  0,   26,  197, 62,  94,  252, 219, 203, 117,
    35,  11,  32,  57,  177, 33,  88,  237, 149, 56,  87,  174, 20,  125, 136,
    171, 168, 68,  175, 74,  165, 71,  134, 139, 48,  27,  166, 77,  146, 158,
    231, 83,  111, 229, 122, 60,  211, 133, 230, 220, 105, 92,  41,  55,  46,
    245, 40,  244, 102, 143, 54,  65,  25,  63,  161, 1,   216, 80,  73,  209,
    76,  132, 187, 208, 89,  18,  169, 200, 196, 135, 130, 116, 188, 159, 86,
    164, 100, 109, 198, 173, 186, 3,   64,  52,  217, 226, 250, 124, 123, 5,
    202, 38,  147, 118, 126, 255, 82,  85,  212, 207, 206, 59,  227, 47,  16,
    58,  17,  182, 189, 28,  42,  223, 183, 170, 213, 119, 248, 152, 2,   44,
    154, 163, 70,  221, 153, 101, 155, 167, 43,  172, 9,   129, 22,  39,  253,
    19,  98,  108, 110, 79,  113, 224, 232, 178, 185, 112, 104, 218, 246, 97,
    228, 251, 34,  242, 193, 238, 210, 144, 12,  191, 179, 162, 241, 81,  51,
    145, 235, 249, 14,  239, 107, 49,  192, 214, 31,  181, 199, 106, 157, 184,
    84,  204, 176, 115, 121, 50,  45,  127, 4,   150, 254, 138, 236, 205, 93,
    222, 114, 67,  29,  24,  72,  243, 141, 128, 195, 78,  66,  215, 61,  156,
    180, 151, 160, 137, 91,  90,  15,  131, 13,  201, 95,  96,  53,  194, 233,
    7,   225, 140, 36,  103, 30,  69,  142, 8,   99,  37,  240, 21,  10,  23,
    190, 6,   148, 247, 120, 234, 75,  0,   26,  197, 62,  94,  252, 219, 203,
    117, 35,  11,  32,  57,  177, 33,  88,  237, 149, 56,  87,  174, 20,  125,
    136, 171, 168, 68,  175, 74,  165, 71,  134, 139, 48,  27,  166, 77,  146,
    158, 231, 83,  111, 229, 122, 60,  211, 133, 230, 220, 105, 92,  41,  55,
    46,  245, 40,  244, 102, 143, 54,  65,  25,  63,  161, 1,   216, 80,  73,
    209, 76,  132, 187, 208, 89,  18,  169, 200, 196, 135, 130, 116, 188, 159,
    86,  164, 100, 109, 198, 173, 186, 3,   64,  52,  217, 226, 250, 124, 123,
    5,   202, 38,  147, 118, 126, 255, 82,  85,  212, 207, 206, 59,  227, 47,
    16,  58,  17,  182, 189, 28,  42,  223, 183, 170, 213, 119, 248, 152, 2,
    44,  154, 163, 70,  221, 153, 101, 155, 167, 43,  172, 9,   129, 22,  39,
    253, 19,  98,  108, 110, 79,  113, 224, 232, 178, 185, 112, 104, 218, 246,
    97,  228, 251, 34,  242, 193, 238, 210, 144, 12,  191, 179, 162, 241, 81,
    51,  145, 235, 249, 14,  239, 107, 49,  192, 214, 31,  181, 199, 106, 157,
    184, 84,  204, 176, 115, 121, 50,  45,  127, 4,   150, 254, 138, 236, 205,
    93,  222, 114, 67,  29,  24,  72,  243, 141, 128, 195, 78,  66,  215, 61,
    156, 180};

static float grad2(int hash, float x, float y) {
  int h = hash & 7;
  float u = h < 4 ? x : y;
  float v = h < 4 ? y : x;
  return ((h & 1) ? -u : u) + ((h & 2) ? -2.0f * v : 2.0f * v);
}

static float simplex_noise_2d(float x, float y) {
  const float F2 = 0.366025403f;
  const float G2 = 0.211324865f;
  float s = (x + y) * F2;
  int i = (int)floorf(x + s);
  int j = (int)floorf(y + s);
  float t = (i + j) * G2;
  float X0 = i - t, Y0 = j - t;
  float x0 = x - X0, y0 = y - Y0;
  int i1 = (x0 > y0) ? 1 : 0;
  int j1 = (x0 > y0) ? 0 : 1;
  float x1 = x0 - i1 + G2, y1 = y0 - j1 + G2;
  float x2 = x0 - 1.0f + 2.0f * G2, y2 = y0 - 1.0f + 2.0f * G2;
  int ii = i & 255, jj = j & 255;
  float n0 = 0.0f, n1 = 0.0f, n2 = 0.0f;
  float t0 = 0.5f - x0 * x0 - y0 * y0;
  if (t0 >= 0.0f) {
    t0 *= t0;
    n0 = t0 * t0 * grad2(perm[ii + perm[jj]], x0, y0);
  }
  float t1 = 0.5f - x1 * x1 - y1 * y1;
  if (t1 >= 0.0f) {
    t1 *= t1;
    n1 = t1 * t1 * grad2(perm[ii + i1 + perm[jj + j1]], x1, y1);
  }
  float t2 = 0.5f - x2 * x2 - y2 * y2;
  if (t2 >= 0.0f) {
    t2 *= t2;
    n2 = t2 * t2 * grad2(perm[ii + 1 + perm[jj + 1]], x2, y2);
  }
  return 45.0f * (n0 + n1 + n2);
}

static float fbm_noise(float x, float y, int octaves) {
  float value = 0.0f, amplitude = 1.0f, frequency = 1.0f, max_value = 0.0f;
  for (int i = 0; i < octaves; i++) {
    value += simplex_noise_2d(x * frequency, y * frequency) * amplitude;
    max_value += amplitude;
    amplitude *= 0.5f;
    frequency *= 2.0f;
  }
  return value / max_value;
}

float get_terrain_height(float x, float z) {
  float height = fbm_noise(x * 0.03f, z * 0.03f, 4);
  height += fbm_noise(x * 0.01f, z * 0.01f, 2) * 1.5f;
  return height * 5.0f + 2.0f;
}

#define TERRAIN_SIZE 256
#define TERRAIN_SCALE 1.0f
static dReal *gHeightData = NULL;
static dGeomID gTerrainGeom = NULL;

static void generate_smooth_terrain(void) {
  int size = TERRAIN_SIZE;
  gHeightData = (dReal *)malloc(size * size * sizeof(dReal));

  for (int z = 0; z < size; z++) {
    for (int x = 0; x < size; x++) {
      float wx = ((float)x - size / 2.0f) * TERRAIN_SCALE;
      float wz = ((float)z - size / 2.0f) * TERRAIN_SCALE;
      gHeightData[z * size + x] = (dReal)get_terrain_height(wx, wz);
    }
  }

  printf("Generated smooth heightmap terrain %dx%d\n", size, size);
}
// ============== END SIMPLEX NOISE ==============

static double now_seconds(void) {
#ifdef _WIN32
  static LARGE_INTEGER freq;
  static int init = 0;
  LARGE_INTEGER t;
  if (!init) {
    QueryPerformanceFrequency(&freq);
    init = 1;
  }
  QueryPerformanceCounter(&t);
  return (double)t.QuadPart / (double)freq.QuadPart;
#else
  struct timespec ts;
  clock_gettime(CLOCK_MONOTONIC, &ts);
  return (double)ts.tv_sec + (double)ts.tv_nsec / 1e9;
#endif
}

static void set_nonblocking(int sock) {
#ifdef _WIN32
  u_long mode = 1;
  ioctlsocket(sock, FIONBIO, &mode);
#else
  int flags = fcntl(sock, F_GETFL, 0);
  fcntl(sock, F_SETFL, flags | O_NONBLOCK);
#endif
}

#pragma pack(push, 1)
typedef enum MsgType : uint8_t {
  MSG_HELLO = 1,
  MSG_WELCOME = 2,
  MSG_INPUT = 3,
  MSG_SNAPSHOT = 4,
  MSG_SHOOT = 5,
  MSG_ITEMS = 6
} MsgType;

typedef struct MsgHello {
  uint8_t type;
  uint32_t protocol;
  char username[USERNAME_MAX];
} MsgHello;

typedef struct MsgWelcome {
  uint8_t type;
  uint8_t playerId;
  uint32_t serverTick;
} MsgWelcome;

typedef struct MsgInput {
  uint8_t type;
  uint8_t playerId;
  uint32_t clientTick;
  float moveX, moveZ, yaw, pitch;
  uint8_t buttons;
} MsgInput;

typedef struct MsgShoot {
  uint8_t type;
  uint8_t playerId;
  uint32_t clientTick;
} MsgShoot;

typedef struct PlayerStateNet {
  uint8_t id, alive;
  int16_t hp;
  float x, y, z, yaw, pitch;
  uint8_t weapon;
  int16_t pistolMag, rifleMag, pistolAmmo, rifleAmmo, medkits;
  int16_t reloadTimeLeft;
  char username[USERNAME_MAX];
} PlayerStateNet;

typedef struct MsgSnapshot {
  uint8_t type;
  uint32_t serverTick;
  uint8_t count;
  PlayerStateNet p[MAX_PLAYERS];
} MsgSnapshot;

typedef struct ItemNet {
  uint16_t id;
  uint8_t type;
  int16_t qty;
  float x, y, z;
} ItemNet;

typedef struct MsgItems {
  uint8_t type;
  uint32_t serverTick;
  uint8_t count;
  ItemNet items[MAX_ITEMS];
} MsgItems;
#pragma pack(pop)

typedef struct Item {
  int active;
  uint16_t id;
  uint8_t type;
  int qty;
  float x, y, z;
  dGeomID geom;
} Item;

typedef struct Player {
  int inUse;
  struct sockaddr_in addr;
  double lastHeard;
  char username[USERNAME_MAX];
  float moveX, moveZ, yaw, pitch;
  int alive, hp, weapon;
  int pistolMag, rifleMag, pistolAmmo, rifleAmmo, medkits;
  double lastShotTime;
  int shotsInBurst;
  uint32_t rng;
  float reloadTimer;
  int reloadWeapon;
  dBodyID body;
  dGeomID geom;
} Player;

static int addr_equal(const struct sockaddr_in *a,
                      const struct sockaddr_in *b) {
  return (a->sin_family == b->sin_family) && (a->sin_port == b->sin_port) &&
         (a->sin_addr.s_addr == b->sin_addr.s_addr);
}

static float clampf(float v, float lo, float hi) {
  return (v < lo) ? lo : (v > hi) ? hi : v;
}

static void sendto_player(int sock, const Player *pl, const void *data,
                          int len) {
  sendto(sock, (const char *)data, len, 0, (const struct sockaddr *)&pl->addr,
         sizeof(pl->addr));
}

static void broadcast(int sock, const Player players[MAX_PLAYERS],
                      const void *data, int len) {
  for (int i = 0; i < MAX_PLAYERS; i++)
    if (players[i].inUse)
      sendto_player(sock, &players[i], data, len);
}

static dWorldID gWorld;
static dSpaceID gSpace;
static dJointGroupID gContactGroup;
static Item gItems[MAX_ITEMS];
static uint16_t gNextItemId = 1;

static void item_destroy(Item *it) {
  if (it->geom) {
    dGeomDestroy(it->geom);
    it->geom = 0;
  }
  memset(it, 0, sizeof(*it));
}

static int item_spawn(uint8_t type, int qty, float x, float z) {
  float y = get_terrain_height(x, z) + 0.5f;
  for (int i = 0; i < MAX_ITEMS; i++) {
    if (!gItems[i].active) {
      Item *it = &gItems[i];
      it->active = 1;
      it->id = gNextItemId++;
      it->type = type;
      it->qty = qty;
      it->x = x;
      it->y = y;
      it->z = z;
      it->geom = dCreateSphere(gSpace, 0.30);
      dGeomSetPosition(it->geom, x, y, z);
      dGeomSetCategoryBits(it->geom, CAT_ITEM);
      dGeomSetCollideBits(it->geom, 0);
      dGeomSetData(it->geom, it);
      return i;
    }
  }
  return -1;
}

static void nearCallback(void *user, dGeomID o1, dGeomID o2) {
  (void)user;
  if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
    dSpaceCollide2(o1, o2, user, &nearCallback);
    return;
  }
  unsigned long c1 = dGeomGetCategoryBits(o1);
  unsigned long c2 = dGeomGetCategoryBits(o2);
  if ((c1 & CAT_ITEM) || (c2 & CAT_ITEM))
    return;

  const int MAXC = 8;
  dContact c[MAXC];
  int n = dCollide(o1, o2, MAXC, &c[0].geom, sizeof(dContact));
  for (int i = 0; i < n; i++) {
    c[i].surface.mode = dContactApprox1;
    c[i].surface.mu = 1.5;
    c[i].surface.bounce = 0.0;
    dJointID j = dJointCreateContact(gWorld, gContactGroup, &c[i]);
    dJointAttach(j, dGeomGetBody(o1), dGeomGetBody(o2));
  }
}

static void ode_init_world(void) {
  dInitODE();
  gWorld = dWorldCreate();
  gSpace = dHashSpaceCreate(0);
  gContactGroup = dJointGroupCreate(0);
  dWorldSetGravity(gWorld, 0, -20.0, 0);
  dWorldSetERP(gWorld, 0.2);
  dWorldSetCFM(gWorld, 1e-5);

  generate_smooth_terrain();

  dHeightfieldDataID hfData = dGeomHeightfieldDataCreate();
  dGeomHeightfieldDataBuildDouble(hfData, gHeightData, 0,
                                  TERRAIN_SCALE * TERRAIN_SIZE,
                                  TERRAIN_SCALE * TERRAIN_SIZE, TERRAIN_SIZE,
                                  TERRAIN_SIZE, 1.0, 0.0, 0.0, 0);
  dGeomHeightfieldDataSetBounds(hfData, -10.0, 20.0);

  gTerrainGeom = dCreateHeightfield(gSpace, hfData, 1);
  dGeomSetPosition(gTerrainGeom, 0, 0, 0);
  dGeomSetCategoryBits(gTerrainGeom, CAT_WORLD);
  dGeomSetCollideBits(gTerrainGeom, CAT_PLAYER);

  item_spawn(ITEM_MEDKIT, 1, 10.0f, 10.0f);
  item_spawn(ITEM_MEDKIT, 1, -15.0f, 20.0f);
  item_spawn(ITEM_AMMO_RIFLE, 30, 5.0f, -10.0f);
  item_spawn(ITEM_AMMO_PISTOL, 12, -10.0f, -15.0f);
}

static void ode_shutdown_world(void) {
  if (gHeightData) {
    free(gHeightData);
    gHeightData = NULL;
  }
  dJointGroupDestroy(gContactGroup);
  dSpaceDestroy(gSpace);
  dWorldDestroy(gWorld);
  dCloseODE();
}

static void player_ode_create(Player *p, int spawnIndex) {
  p->body = dBodyCreate(gWorld);
  dMass m;
  dMassSetCapsuleTotal(&m, 80.0, 3, 0.35, 1.0);
  dBodySetMass(p->body, &m);
  dBodySetLinearDamping(p->body, 0.05);
  dBodySetAngularDamping(p->body, 0.99);

  float sx = (float)(spawnIndex * 4 - 8);
  float sz = (float)(spawnIndex * 3 - 6);
  float sy = get_terrain_height(sx, sz) + 3.0f;
  dBodySetPosition(p->body, sx, sy, sz);

  p->geom = dCreateCapsule(gSpace, 0.35, 1.0);
  dGeomSetBody(p->geom, p->body);
  dGeomSetCategoryBits(p->geom, CAT_PLAYER);
  dGeomSetCollideBits(p->geom, CAT_WORLD | CAT_PLAYER);
}

static void player_ode_destroy(Player *p) {
  if (p->geom) {
    dGeomDestroy(p->geom);
    p->geom = 0;
  }
  if (p->body) {
    dBodyDestroy(p->body);
    p->body = 0;
  }
}

typedef struct RaycastCtx {
  dGeomID ray, hitGeom;
  dContactGeom hit;
  int hasHit;
} RaycastCtx;

static void rayCallback(void *data, dGeomID o1, dGeomID o2) {
  RaycastCtx *rc = (RaycastCtx *)data;
  dGeomID ray = rc->ray;
  dGeomID other = (o1 == ray) ? o2 : o1;
  if (other == ray)
    return;
  dContact c[4];
  int n = dCollide(ray, other, 4, &c[0].geom, sizeof(dContact));
  for (int i = 0; i < n; i++) {
    if (!rc->hasHit || c[i].geom.depth < rc->hit.depth) {
      rc->hasHit = 1;
      rc->hit = c[i].geom;
      rc->hitGeom = other;
    }
  }
}

static int raycast_space(float ox, float oy, float oz, float dx, float dy,
                         float dz, float maxDist, dContactGeom *outHit,
                         dGeomID *outGeom) {
  dGeomID ray = dCreateRay(0, maxDist);
  dGeomRaySet(ray, ox, oy, oz, dx, dy, dz);
  dGeomSetCategoryBits(ray, 0xFFFFFFFFu);
  dGeomSetCollideBits(ray, CAT_WORLD | CAT_PLAYER);
  RaycastCtx rc = {0};
  rc.ray = ray;
  dSpaceCollide2(ray, (dGeomID)gSpace, &rc, &rayCallback);
  dGeomDestroy(ray);
  if (!rc.hasHit)
    return 0;
  if (outHit)
    *outHit = rc.hit;
  if (outGeom)
    *outGeom = rc.hitGeom;
  return 1;
}

static uint32_t xorshift32(uint32_t *s) {
  uint32_t x = *s;
  x ^= x << 13;
  x ^= x >> 17;
  x ^= x << 5;
  *s = x;
  return x;
}

static float frand_signed(uint32_t *s) {
  return ((xorshift32(s) & 0xFFFFFF) / (float)0x7FFFFF) - 1.0f;
}

static void dir_from_yaw_pitch(float yaw, float pitch, float *dx, float *dy,
                               float *dz) {
  *dx = sinf(yaw) * cosf(pitch);
  *dy = sinf(pitch);
  *dz = cosf(yaw) * cosf(pitch);
}

static void v3_cross(float ax, float ay, float az, float bx, float by, float bz,
                     float *ox, float *oy, float *oz) {
  *ox = ay * bz - az * by;
  *oy = az * bx - ax * bz;
  *oz = ax * by - ay * bx;
}

static void v3_normf(float *x, float *y, float *z) {
  float l = sqrtf((*x) * (*x) + (*y) * (*y) + (*z) * (*z));
  if (l < 1e-6f) {
    *x = 0;
    *y = 0;
    *z = 1;
    return;
  }
  *x /= l;
  *y /= l;
  *z /= l;
}

static int find_nearest_item(float x, float y, float z, float maxDist) {
  int best = -1;
  float bestD2 = maxDist * maxDist;
  for (int i = 0; i < MAX_ITEMS; i++) {
    if (!gItems[i].active)
      continue;
    float dx = gItems[i].x - x;
    float dy = (gItems[i].y) - y;
    float dz = gItems[i].z - z;
    float d2 = dx * dx + dy * dy + dz * dz;
    if (d2 < bestD2) {
      bestD2 = d2;
      best = i;
    }
  }
  return best;
}

int main(void) {
#ifdef _WIN32
  WSADATA wsa;
  WSAStartup(MAKEWORD(2, 2), &wsa);
#endif

  ode_init_world();

  int sock = (int)socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (sock < 0) {
    perror("socket");
    return 1;
  }
  set_nonblocking(sock);

  struct sockaddr_in srv = {0};
  srv.sin_family = AF_INET;
  srv.sin_addr.s_addr = htonl(INADDR_ANY);
  srv.sin_port = htons(SERVER_PORT);

  if (bind(sock, (struct sockaddr *)&srv, sizeof(srv)) < 0) {
    perror("bind");
    CLOSESOCK(sock);
    return 1;
  }

  Player players[MAX_PLAYERS] = {0};
  uint32_t serverTick = 0, itemsTickDiv = 0;

  printf("Server listening UDP %d\n", SERVER_PORT);
  double nextTick = now_seconds();

  for (;;) {
    for (;;) {
      uint8_t buf[1400];
      struct sockaddr_in from = {0};
      socklen_t fromLen = sizeof(from);
      int n = (int)recvfrom(sock, (char *)buf, (int)sizeof(buf), 0,
                            (struct sockaddr *)&from, &fromLen);
      if (n <= 0) {
#ifdef _WIN32
        if (WSAGetLastError() == WSAEWOULDBLOCK)
          break;
#else
        if (errno == EWOULDBLOCK || errno == EAGAIN)
          break;
#endif
        break;
      }

      uint8_t type = buf[0];
      int pid = -1;
      for (int i = 0; i < MAX_PLAYERS; i++) {
        if (players[i].inUse && addr_equal(&players[i].addr, &from)) {
          pid = i;
          break;
        }
      }

      if (type == MSG_HELLO && n >= (int)sizeof(MsgHello)) {
        const MsgHello *m = (const MsgHello *)buf;
        if (m->protocol != PROTOCOL_VERSION)
          continue;

        if (pid < 0) {
          for (int i = 0; i < MAX_PLAYERS; i++)
            if (!players[i].inUse) {
              pid = i;
              Player *p = &players[i];
              memset(p, 0, sizeof(*p));
              p->inUse = 1;
              p->addr = from;
              p->lastHeard = now_seconds();
              strncpy(p->username, m->username, USERNAME_MAX - 1);
              p->alive = 1;
              p->hp = 100;
              p->weapon = WEAPON_RIFLE;
              p->pistolMag = 12;
              p->rifleMag = 30;
              p->pistolAmmo = 48;
              p->rifleAmmo = 90;
              p->medkits = 2;
              p->rng = 0xA341316Cu ^ (uint32_t)(i * 2654435761u);
              player_ode_create(p, i);
              printf("Player %d joined as '%s'\n", pid, p->username);
              break;
            }
        } else {
          players[pid].lastHeard = now_seconds();
        }

        if (pid >= 0) {
          MsgWelcome w = {0};
          w.type = MSG_WELCOME;
          w.playerId = (uint8_t)pid;
          w.serverTick = serverTick;
          sendto(sock, (const char *)&w, (int)sizeof(w), 0,
                 (struct sockaddr *)&from, sizeof(from));
        }
      } else if (type == MSG_INPUT && n >= (int)sizeof(MsgInput)) {
        const MsgInput *m = (const MsgInput *)buf;
        if (pid >= 0 && m->playerId == (uint8_t)pid) {
          Player *p = &players[pid];
          p->lastHeard = now_seconds();
          p->moveX = clampf(m->moveX, -1.0f, 1.0f);
          p->moveZ = clampf(m->moveZ, -1.0f, 1.0f);
          p->yaw = m->yaw;
          p->pitch = clampf(m->pitch, -1.5f, 1.5f);

          if (m->buttons & BTN_SWITCH_PISTOL)
            p->weapon = WEAPON_PISTOL;
          if (m->buttons & BTN_SWITCH_RIFLE)
            p->weapon = WEAPON_RIFLE;

          if (m->buttons & BTN_RELOAD) {
            if (p->reloadTimer <= 0.0f) {
              p->reloadTimer = 2.0f; // 2 second reload time
              p->reloadWeapon = p->weapon;
            }
          }

          if (m->buttons & BTN_PICK) {
            const dReal *pos = dBodyGetPosition(p->body);
            int itIdx = find_nearest_item((float)pos[0], (float)pos[1],
                                          (float)pos[2], 2.0f);
            if (itIdx >= 0) {
              Item *it = &gItems[itIdx];
              int taken = 0;
              if (it->type == ITEM_AMMO_PISTOL) {
                p->pistolAmmo += it->qty;
                if (p->pistolAmmo > 120)
                  p->pistolAmmo = 120;
                taken = 1;
              } else if (it->type == ITEM_AMMO_RIFLE) {
                p->rifleAmmo += it->qty;
                if (p->rifleAmmo > 300)
                  p->rifleAmmo = 300;
                taken = 1;
              } else if (it->type == ITEM_MEDKIT) {
                p->medkits += it->qty;
                if (p->medkits > 10)
                  p->medkits = 10;
                taken = 1;
              }
              if (taken)
                item_destroy(it);
            }
          }

          if (m->buttons & BTN_USE_MEDKIT) {
            if (p->medkits > 0 && p->hp < 100) {
              p->medkits--;
              p->hp += 25;
              if (p->hp > 100)
                p->hp = 100;
            }
          }

          if (m->buttons & BTN_JUMP) {
            const dReal *lvel = dBodyGetLinearVel(p->body);
            dBodySetLinearVel(p->body, lvel[0], 12.0f,
                              lvel[2]); // Set upward velocity
            printf("Player %d jumped\n", pid);
          }
        }
      } else if (type == MSG_SHOOT && n >= (int)sizeof(MsgShoot)) {
        const MsgShoot *m = (const MsgShoot *)buf;
        if (pid >= 0 && m->playerId == (uint8_t)pid && players[pid].inUse &&
            players[pid].alive) {
          Player *sh = &players[pid];
          const double now = now_seconds();
          double fireInterval =
              (sh->weapon == WEAPON_PISTOL) ? (1.0 / 4.0) : (1.0 / 12.0);
          if (now - sh->lastShotTime < fireInterval)
            continue;
          if (now - sh->lastShotTime > 0.18)
            sh->shotsInBurst = 0;

          if (sh->reloadTimer > 0.0f)
            continue;

          int *mag =
              (sh->weapon == WEAPON_PISTOL) ? &sh->pistolMag : &sh->rifleMag;
          if (*mag <= 0)
            continue;

          (*mag)--;
          sh->shotsInBurst++;
          sh->lastShotTime = now;

          const dReal *pos = dBodyGetPosition(sh->body);
          float ox = (float)pos[0], oy = (float)pos[1] + 0.7f,
                oz = (float)pos[2];
          float fx, fy, fz;
          dir_from_yaw_pitch(sh->yaw, sh->pitch, &fx, &fy, &fz);
          v3_normf(&fx, &fy, &fz);

          float base = (sh->weapon == WEAPON_PISTOL) ? 0.0035f : 0.0080f;
          float grow = (sh->weapon == WEAPON_PISTOL) ? 0.0010f : 0.0025f;
          float spread = base + grow * (float)sh->shotsInBurst;
          if (spread > 0.08f)
            spread = 0.08f;

          float upx = 0, upy = 1, upz = 0;
          float rx, ry, rz;
          v3_cross(fx, fy, fz, upx, upy, upz, &rx, &ry, &rz);
          v3_normf(&rx, &ry, &rz);
          float ux, uy, uz;
          v3_cross(rx, ry, rz, fx, fy, fz, &ux, &uy, &uz);
          v3_normf(&ux, &uy, &uz);

          float ax = frand_signed(&sh->rng) * spread;
          float ay = frand_signed(&sh->rng) * spread;
          float dx = fx + rx * ax + ux * ay;
          float dy = fy + ry * ax + uy * ay;
          float dz = fz + rz * ax + uz * ay;
          v3_normf(&dx, &dy, &dz);

          dContactGeom hit;
          dGeomID hitGeom = 0;
          if (raycast_space(ox, oy, oz, dx, dy, dz, 100.0f, &hit, &hitGeom)) {
            int victim = -1;
            for (int i = 0; i < MAX_PLAYERS; i++) {
              if (!players[i].inUse)
                continue;
              if (players[i].geom == hitGeom) {
                victim = i;
                break;
              }
            }
            if (victim >= 0 && victim != pid && players[victim].alive) {
              int dmg = 25;
              players[victim].hp -= dmg;
              if (players[victim].hp <= 0) {
                players[victim].hp = 0;
                players[victim].alive = 0;
              }
            }
          }
        }
      }
    }

    double t = now_seconds();
    if (t >= nextTick) {
      nextTick += DT;
      serverTick++;

      dSpaceCollide(gSpace, 0, &nearCallback);
      dWorldQuickStep(gWorld, DT);
      dJointGroupEmpty(gContactGroup);

      // Handle reload timers
      for (int i = 0; i < MAX_PLAYERS; i++) {
        Player *p = &players[i];
        if (!p->inUse || !p->alive)
          continue;
        if (p->reloadTimer > 0.0f) {
          p->reloadTimer -= DT;
          if (p->reloadTimer <= 0.0f) {
            p->reloadTimer = 0.0f;
            // Perform reload
            if (p->reloadWeapon == WEAPON_PISTOL) {
              int need = 12 - p->pistolMag;
              if (need > 0 && p->pistolAmmo > 0) {
                int take = (p->pistolAmmo < need) ? p->pistolAmmo : need;
                p->pistolAmmo -= take;
                p->pistolMag += take;
              }
            } else {
              int need = 30 - p->rifleMag;
              if (need > 0 && p->rifleAmmo > 0) {
                int take = (p->rifleAmmo < need) ? p->rifleAmmo : need;
                p->rifleAmmo -= take;
                p->rifleMag += take;
              }
            }
          }
        }
      }

      for (int i = 0; i < MAX_PLAYERS; i++) {
        Player *p = &players[i];
        if (!p->inUse || !p->alive)
          continue;

        const dReal *lvel = dBodyGetLinearVel(p->body);
        const dReal *avel = dBodyGetAngularVel(p->body);

        float dx = p->moveX, dz = p->moveZ;
        float len = sqrtf(dx * dx + dz * dz);
        if (len > 1e-6f) {
          dx /= len;
          dz /= len;
        }

        float speed = 10.0f;
        float fx = sinf(p->yaw) * dz - cosf(p->yaw) * dx;
        float fz = cosf(p->yaw) * dz + sinf(p->yaw) * dx;
        dBodySetLinearVel(p->body, fx * speed, lvel[1], fz * speed);
        dBodySetAngularVel(p->body, avel[0], 0, avel[2]);
      }

      MsgSnapshot snap = {0};
      snap.type = MSG_SNAPSHOT;
      snap.serverTick = serverTick;
      snap.count = 0;

      for (int i = 0; i < MAX_PLAYERS; i++) {
        if (!players[i].inUse)
          continue;
        const dReal *pos = dBodyGetPosition(players[i].body);
        PlayerStateNet *ps = &snap.p[snap.count++];
        ps->id = (uint8_t)i;
        ps->alive = players[i].alive;
        ps->hp = (int16_t)players[i].hp;
        ps->x = (float)pos[0];
        ps->y = (float)pos[1];
        ps->z = (float)pos[2];
        ps->yaw = players[i].yaw;
        ps->pitch = players[i].pitch;
        ps->weapon = players[i].weapon;
        ps->pistolMag = (int16_t)players[i].pistolMag;
        ps->rifleMag = (int16_t)players[i].rifleMag;
        ps->pistolAmmo = (int16_t)players[i].pistolAmmo;
        ps->rifleAmmo = (int16_t)players[i].rifleAmmo;
        ps->medkits = (int16_t)players[i].medkits;
        ps->reloadTimeLeft = (int16_t)(players[i].reloadTimer * 1000.0f);
        strncpy(ps->username, players[i].username, USERNAME_MAX - 1);
      }
      broadcast(sock, players, &snap, (int)sizeof(snap));

      itemsTickDiv++;
      if (itemsTickDiv >= 6) {
        itemsTickDiv = 0;
        MsgItems im = {0};
        im.type = MSG_ITEMS;
        im.serverTick = serverTick;
        im.count = 0;
        for (int i = 0; i < MAX_ITEMS; i++) {
          if (!gItems[i].active)
            continue;
          ItemNet *it = &im.items[im.count++];
          it->id = gItems[i].id;
          it->type = gItems[i].type;
          it->qty = (int16_t)gItems[i].qty;
          it->x = gItems[i].x;
          it->y = gItems[i].y;
          it->z = gItems[i].z;
        }
        broadcast(sock, players, &im, (int)sizeof(im));
      }
    }

    double now = now_seconds();
    for (int i = 0; i < MAX_PLAYERS; i++) {
      if (players[i].inUse && (now - players[i].lastHeard) > 10.0) {
        printf("Player %d timed out\n", i);
        player_ode_destroy(&players[i]);
        memset(&players[i], 0, sizeof(Player));
      }
    }

    double wait = nextTick - now_seconds();
    if (wait > 0.0) {
#ifdef _WIN32
      Sleep((DWORD)(wait * 1000.0));
#else
      usleep((useconds_t)(wait * 1e6));
#endif
    }
  }

  CLOSESOCK(sock);
  ode_shutdown_world();
#ifdef _WIN32
  WSACleanup();
#endif
  return 0;
}