#include "raylib.h"
#include "raymath.h"
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "shared.h"
#include "server.h"
#include "queue.h"

void PrintAddress(struct sockaddr_in *addr) {
    char ip[INET_ADDRSTRLEN];

    inet_ntop(AF_INET, &addr->sin_addr, ip, sizeof(ip));

    printf("IP: %s, Port: %d\n", ip, ntohs(addr->sin_port));
}

Vector3 ToRaylib(Vec3 v) {
    return (Vector3){ v.x, v.y, v.z };
}

Vec3 FromRaylib(Vector3 v) {
    return (Vec3){ v.x, v.y, v.z };
}

void NetworkInit() {
  net_init();

  networkFd = socket(AF_INET, SOCK_DGRAM, 0);

  if (networkFd == -1)
    err(EXIT_FAILURE, "socket failed");

  struct sockaddr_in address;
  address.sin_family = AF_INET;
  address.sin_port = htons(8080);
  inet_pton(AF_INET, "10.148.10.109", &address.sin_addr);

  if (bind(networkFd, (struct sockaddr *)&address, sizeof(address)) == -1) {
    close(networkFd);
    err(EXIT_FAILURE, "bind failed");
  }
}

void NetworkShutdown() {
  close(networkFd);
  net_cleanup();
}

void *RecvThread(void *arg) {
    uint8_t buffer[PACKET_MAX_SIZE];
    struct sockaddr_in from;
    socklen_t fromLen = sizeof(from);

    while (1) {
      int size = recvfrom(networkFd, (char *)buffer, sizeof(buffer), 0,
                          (struct sockaddr *)&from, &fromLen);

      if (size <= 0) continue;

      PacketType type = *(PacketType *)buffer;

      // handle ping immediately without queuing
      if (type == PACKET_PING) {
	PingPacket pong = {
	  .type     = PACKET_PONG,
	  .sentTime = ((PingPacket *)buffer)->sentTime
	};
	sendto(networkFd, (const char*)&pong, sizeof(pong), 0,
	       (struct sockaddr *)&from, sizeof(from));
	continue;  // don't queue
      }


      // figure out which player this came from
      int playerId = FindPlayerByAddress(&from);
      if (playerId == -1) {
	// new connection - handle handshake
	HandleNewConnection(&from, buffer, size);
      } else {
	// existing player - queue their packet
	QueuePush(&incomingQueue, buffer, size);
      }
    }
    return NULL;
}

void *SendThread(void *arg) {
  double previousTime = GetWallTime();
    double accumulator  = 0;

    while (1) {
        double currentTime = GetWallTime();
        double elapsed     = currentTime - previousTime;
        previousTime       = currentTime;
        accumulator       += elapsed;

        while (accumulator >= NETWORK_RATE) {
            // send snapshot to each connected client
            for (int i = 0; i < MAX_PLAYERS; i++) {
                if (!connections[i].connected) continue;

                // build custom snapshot for this player (nearby entities only)
                ServerSnapshot snapshot = BuildSnapshotForPlayer(i);
                snapshot.type = PACKET_SERVER_SNAPSHOT;

                sendto(networkFd, (const char *)&snapshot, sizeof(snapshot), 0,
                       (struct sockaddr *)&connections[i].address,
                       sizeof(connections[i].address));

            }
            accumulator -= NETWORK_RATE;
        }

	sleep_us(1000);
        /* // small sleep to avoid burning CPU */
        /* double sleepTime = NETWORK_RATE -  */
        /*     (GetWallTime() - previousTime); */
        /* if (sleepTime > 0) */
        /*     sleep_us((useconds_t)(sleepTime * 1000000)); */
    }
    return NULL;
}

void *GameThread(void *arg) {
    double previousTime = GetWallTime();
    double accumulator = 0;

    for (int i = 0; i < 2000; i++) {
      float x = (float)GetRandomValue(-500, 500);
      float z = (float)GetRandomValue(-500, 500);
      float y = GetTerrainHeight(x, z);

      npcs[i].active = true;
      npcs[i].model =       i % 47;
      npcs[i].position      = (Vec3){x, y, z};
      npcs[i].spawnPosition = (Vec3){x, y, z};
      npcs[i].target        = (Vec3){x, y, z};
      npcs[i].rotation      = (float)GetRandomValue(0, 360);
      npcs[i].scale         = (float)GetRandomValue(8, 15) / 10.0f;
      npcs[i].isMoving      = false;
      npcs[i].waitTimer  = (float)GetRandomValue(0, 300) / 100.0f;  // 0-3 sec initial wait
      npcs[i].moveTimer  = 0.0f;
    }

    while (1) {
        double currentTime = GetWallTime();
        double elapsed     = currentTime - previousTime;
        previousTime       = currentTime;
        accumulator       += elapsed;

        while (accumulator >= TICK_RATE) {
            ProcessIncoming();

            for (int i = 0; i < MAX_NPCS; i++) {
                if (npcs[i].active)
                    UpdateServerNPC(&npcs[i], TICK_RATE);
            }

            CheckHeartbeats();
            accumulator -= TICK_RATE;
        }

	sleep_us(1000);
    }
    return NULL;
}

void ProcessIncoming() {
    uint8_t buffer[PACKET_MAX_SIZE];
    int size;

    while (QueuePop(&incomingQueue, buffer, &size)) {
        PacketType type = *(PacketType *)buffer;

        switch (type) {
        case PACKET_CLIENT_INPUT:
	  HandleClientInput((ClientPacket *)buffer);
	  break;
            /* case PACKET_HANDSHAKE: */
            /*     HandleHandshake((HandshakePacket *)buffer); */
            /*     break; */
        }
    }
}

void HandleClientInput(ClientPacket *packet) {
  int id = packet->playerId;
  if (!players[id].connected) return;

    // update last seen time
  connections[id].lastHeartbeat = GetWallTime();

    // simulate player movement with their inputs
    UpdateServerPlayer(&players[id], packet->inputs, packet->yaw, (float)TICK_RATE,
                       terrainMesh, terrainTransform);
}

int CompareNPCDist(const void *a, const void *b) {
    float da = ((NPCDist*)a)->dist;
    float db = ((NPCDist*)b)->dist;
    return (da > db) - (da < db);
}

ServerSnapshot BuildSnapshotForPlayer(int playerId) {
    ServerSnapshot snapshot = {0};
    snapshot.type = PACKET_SERVER_SNAPSHOT;
    Vec3 playerPos = players[playerId].position;

    // always add current player first - server is authoritative for their position
    snapshot.players[0].id       = players[playerId].id;
    snapshot.players[0].position = players[playerId].position;
    snapshot.players[0].yaw = players[playerId].yaw;
    snapshot.players[0].velocity.x = players[playerId].velocity.x;
    snapshot.players[0].velocity.y = players[playerId].velocity.y;
    snapshot.players[0].velocity.z = players[playerId].velocity.z;
    snapshot.players[0].onGround = players[playerId].onGround;
    snapshot.playerCount = 1;


    // nearby OTHER players
    for (int i = 0; i < MAX_PLAYERS; i++) {
        if (i == playerId) continue;  // skip self, already added
        if (!players[i].connected) continue;
        if (snapshot.playerCount >= 100) break;

        float dist = Vec3Distance(players[i].position, playerPos);
        if (dist < VISIBILITY_RANGE) {
            snapshot.players[snapshot.playerCount].id       = players[i].id;
            snapshot.players[snapshot.playerCount].position = players[i].position;
            snapshot.players[snapshot.playerCount].yaw = players[i].yaw;
	    snapshot.players[snapshot.playerCount].velocity.x = players[i].velocity.x;
	    snapshot.players[snapshot.playerCount].velocity.y = players[i].velocity.y;
            snapshot.players[snapshot.playerCount].velocity.z = players[i].velocity.z;
	    snapshot.players[snapshot.playerCount].onGround = players[i].onGround;
            snapshot.playerCount++;
        }
    }

    // nearby npcs
    NPCDist visible[MAX_NPCS];
    int visibleCount = 0;
    float cosFov = cosf((180.0f * 0.5f) * DEG2RAD); // 180° FOV

    Vector3 forward = {
      sinf(players[playerId].yaw),
      0.0f,
      cosf(players[playerId].yaw)
    };

    for (int i = 0; i < MAX_NPCS; i++) {
      if (!npcs[i].active) continue;

      Vector3 toNPCVec =
          Vector3Subtract(ToRaylib(npcs[i].position), ToRaylib(playerPos));

      float dist = Vector3Length(toNPCVec);
      if (dist > VISIBILITY_RANGE)
        continue;

      Vector3 toNPC = Vector3Scale(toNPCVec, 1.0f / dist); // normalize

      float dot = Vector3DotProduct(forward, toNPC);

      // Only keep NPCs in front of player
      if (dot >= cosFov) {
        visible[visibleCount].index = i;
        visible[visibleCount].dist = dist;
        visibleCount++;
      }
    }

    qsort(visible, visibleCount, sizeof(NPCDist), CompareNPCDist);

    snapshot.npcCount = 0;

    for (int i = 0; i < visibleCount && i < MAX_VIEW_NPCS; i++) {
      int idx = visible[i].index;

      snapshot.npcs[snapshot.npcCount].id       = npcs[idx].id;
      snapshot.npcs[snapshot.npcCount].position = npcs[idx].position;
      snapshot.npcs[snapshot.npcCount].rotation = npcs[idx].rotation;
      snapshot.npcs[snapshot.npcCount].health   = npcs[idx].health;
      snapshot.npcs[snapshot.npcCount].state    = npcs[idx].state;
      snapshot.npcs[snapshot.npcCount].scale    = npcs[idx].scale;
      snapshot.npcs[snapshot.npcCount].model    = npcs[idx].model;
      snapshot.npcCount++;
    }

    return snapshot;
}

void CheckHeartbeats() {
  double now = GetWallTime();

    for (int i = 0; i < MAX_PLAYERS; i++) {
        if (!connections[i].connected) continue;

        if (now - connections[i].lastHeartbeat > TIMEOUT) {
            // player timed out
            connections[i].connected = false;
            players[i].connected     = false;

            // tell all other clients they left
            PlayerJoinedPacket packet = {
                .type     = PACKET_PLAYER_LEFT,
                .playerId = i,
            };
            /* BroadcastToAllClients(&packet); */

            TraceLog(LOG_INFO, "Player %d timed out", i);
        }
    }
}

void UpdateServerPlayer(ServerPlayer *p, int inputs, float yaw, float dt,
                        Mesh terrainMesh, Matrix terrainTransform) {
    p->yaw = yaw;
    Vector3 forward = { sinf(p->yaw), 0, cosf(p->yaw) };
    Vector3 right   = { cosf(p->yaw), 0, -sinf(p->yaw) };

    if (inputs & INPUT_JUMP && p->onGround) {
        p->velocity.y = 500.0f;
        p->onGround = false;
    }

    Vector3 moveDir = {0};
    if (inputs & INPUT_FORWARD) moveDir = Vector3Add(moveDir, forward);
    if (inputs & INPUT_BACK)    moveDir = Vector3Subtract(moveDir, forward);
    if (inputs & INPUT_RIGHT)   moveDir = Vector3Subtract(moveDir, right);
    if (inputs & INPUT_LEFT)    moveDir = Vector3Add(moveDir, right);

    if (Vector3Length(moveDir) > 0)
        moveDir = Vector3Normalize(moveDir);

    if (inputs & INPUT_SPRINT) {
      p->sprint = p->onGround ? 5 : 10;  // match client exactly
    } else {
      p->sprint = 1;
    }
    p->position.x += moveDir.x * p->speed * p->sprint * dt;
    p->position.z += moveDir.z * p->speed * p->sprint * dt;

    if (!p->onGround) p->velocity.y -= 300.0f * dt;
    p->position.y += p->velocity.y * dt;

    // terrain collision
    Vector3 rayOrigin = { p->position.x, p->position.y + 2000.0f, p->position.z };
    Ray downRay = { rayOrigin, (Vector3){0, -1, 0} };
    RayCollision hit = GetRayCollisionMesh(downRay, terrainMesh, terrainTransform);
    if (hit.hit && p->position.y <= hit.point.y + 0.5f) {
        p->position.y = hit.point.y;
        p->velocity.y = 0;
        p->onGround = true;
    } else {
        p->onGround = false;
    }
}

float GetTerrainHeight(float x, float z) {
    Vector3 rayOrigin = { x, 99999.0f, z };
    Ray ray = { rayOrigin, (Vector3){0, -1, 0} };
    RayCollision hit = GetRayCollisionMesh(ray, terrainMesh, terrainTransform);
    if (hit.hit) return hit.point.y;
    return 0.0f; // fallback if miss
}

void UpdateServerNPC(ServerNPC *npc, float dt) {
    if (npc->isMoving) {
        // walk toward target
      Vector3 dir = Vector3Subtract(ToRaylib(npc->target), ToRaylib(npc->position));
        dir.y = 0;
        float dist = Vector3Length(dir);

        if (dist > 1.0f) {
            dir = Vector3Normalize(dir);
            npc->position.x += dir.x * 20.0f * dt;  // npc walk speed
            npc->position.z += dir.z * 20.0f * dt;

            // snap to terrain height
            npc->position.y = GetTerrainHeight(
                npc->position.x, npc->position.z);

            // face direction of travel
            npc->rotation = atan2f(dir.x, dir.z) * RAD2DEG;
        }

        npc->moveTimer -= dt;
        if (npc->moveTimer <= 0.0f || dist <= 1.0f) {
            // stop walking, start waiting
            npc->isMoving  = false;
            npc->waitTimer = (float)GetRandomValue(200, 500) / 100.0f;  // wait 2-5 sec
        }

    } else {
        // waiting
        npc->waitTimer -= dt;
        if (npc->waitTimer <= 0.0f) {
            // pick a new random target near spawn
            float range = 100.0f;  // how far they wander from spawn
            float tx = npc->spawnPosition.x + (float)GetRandomValue(-range, range);
            float tz = npc->spawnPosition.z + (float)GetRandomValue(-range, range);
            npc->target    = (Vec3){tx, 0, tz};
            npc->isMoving = true;
            npc->moveTimer = (float)GetRandomValue(300, 800) / 100.0f;  // walk 3-8 sec
        }
    }
}

int FindPlayerByAddress(struct sockaddr_in *addr) {
    for (int i = 0; i < MAX_PLAYERS; i++) {
        if (!connections[i].connected) continue;

        if (connections[i].address.sin_addr.s_addr == addr->sin_addr.s_addr &&
            connections[i].address.sin_port        == addr->sin_port) {
            return i;
        }
    }
    return -1;  // not found
}

void HandleNewConnection(struct sockaddr_in *addr, uint8_t *buffer, int size) {
    PacketType type = *(PacketType *)buffer;
    if (type != PACKET_HANDSHAKE)
      return;

    ClientPacket *handshake = (ClientPacket *)buffer;

    // find empty slot
    int slot = -1;
    for (int i = 0; i < MAX_PLAYERS; i++) {
        if (!connections[i].connected) { slot = i; break; }
    }
    if (slot == -1) { printf("Server full\n"); return; }

    // register connection
    connections[slot].connected     = true;
    connections[slot].address       = *addr;
    connections[slot].lastHeartbeat = GetWallTime();

    // init player
    players[slot].id        = slot;
    players[slot].connected = true;
    players[slot].speed     = 30.0f;
    players[slot].sprint    = 1;
    players[slot].onGround  = false;
    players[slot].yaw       = 0;
    players[slot].velocity = (Vec3){0, 0, 0};
    players[slot].characterClass = handshake->characterClass;
    /* strncpy(players[slot].name, handshake->name, 32); */

    printf("character class %d", players[slot].characterClass);

    float spawnY = GetTerrainHeight(600, -1200);
    players[slot].position = (Vec3){600, spawnY, -1200};
    printf("Player %d connected: %s\n", slot, players[slot].name);

    // 1. send handshake response to new player
    HandshakeResponsePacket response = {
        .type     = PACKET_HANDSHAKE_RESPONSE,
        .playerId = slot,
        .spawnX   = players[slot].position.x,
        .spawnY   = players[slot].position.y,
        .spawnZ   = players[slot].position.z,
    };
    sendto(networkFd, (const char *)&response, sizeof(response), 0,
           (struct sockaddr *)addr, sizeof(*addr));

    // 2. tell new player about ALL already connected players
    for (int i = 0; i < MAX_PLAYERS; i++) {
        if (i == slot) continue;
        if (!players[i].connected) continue;

        PlayerJoinedPacket existing = {
          .type = PACKET_PLAYER_JOINED,
          .playerId = players[i].id,
          .position = players[i].position,
	  .characterClass = players[i].characterClass
        };
        strncpy(existing.name, players[i].name, 32);

        sendto(networkFd, (const char *)&existing, sizeof(existing), 0,
               (struct sockaddr *)addr, sizeof(*addr));  // send to new player (addr not from)
        printf("told new player %d about existing player %d\n", slot, i);
    }

    // 3. tell ALL existing players about the new player
    PlayerJoinedPacket joinPacket = {
      .type = PACKET_PLAYER_JOINED,
      .playerId = slot,
      .position = players[slot].position,
      .characterClass = players[slot].characterClass
    };
    strncpy(joinPacket.name, players[slot].name, 32);

    for (int i = 0; i < MAX_PLAYERS; i++) {
        if (i == slot) continue;
        if (!connections[i].connected) continue;
        sendto(networkFd, (const char *) &joinPacket, sizeof(joinPacket), 0,
               (struct sockaddr *)&connections[i].address,
               sizeof(connections[i].address));
    }
}