/**
 * Copyright (c) 2025 Dominic Masters
 * 
 * This software is released under the MIT License.
 * https://opensource.org/licenses/MIT
 */

#include "worldunit.h"
#include "assert/assert.h"

void worldChunkPosAdd(worldchunkpos_t *pos, const worldsubtile_t amt) {
  assertNotNull(pos, "Position pointer cannot be NULL");

  /*
    int32_t sum     = (int32_t)pos->subtile + (int32_t)amt;
    int32_t shifted = sum + 128;                    // may be negative or large
    int32_t carry   = div_floor(shifted, 256);      // tiles to add (can be neg)
    int32_t rem     = mod_floor(shifted, 256);      // 0..255
    int32_t new_sub = rem - 128;                    // back to [-128,127]

    pos->subtile = (int8_t)new_sub;
    pos->tile = (uint8_t)((int32_t)pos->tile + carry); // wrap mod 256
  */
  
  int32_t shiftedTotal = (int32_t)pos->subtile + (int32_t)amt + 128;
  int32_t tileCarry = shiftedTotal >> 8; // divide by 256
  int32_t wrappedSubtile = shiftedTotal - (tileCarry << 8);
  pos->subtile = (int8_t)(wrappedSubtile - 128);
  pos->tile = (uint8_t)(pos->tile + (uint8_t)tileCarry);
}

void worldPosAddSubtile(worldpos_t *pos, const worldsubtile_t amt) {
  assertNotNull(pos, "Position pointer cannot be NULL");

  // Same as worldChunkPosAdd but with chunk handling.
  int32_t shiftedTotal = (int32_t)pos->subtile + (int32_t)amt + 128;
  int32_t tileCarry = shiftedTotal >> 8; // divide by 256
  int32_t wrappedSubtile = shiftedTotal - (tileCarry << 8);
  pos->subtile = (int8_t)(wrappedSubtile - 128);
  int32_t newTile = (int32_t)pos->tile + (int32_t)tileCarry;
  int32_t chunkCarry = newTile / WORLD_CHUNK_SIZE;
  pos->tile = (uint8_t)(newTile % WORLD_CHUNK_SIZE);
  pos->chunk = (uint8_t)(pos->chunk + (uint8_t)chunkCarry);
}

float_t worldChunkPosToF32(const worldchunkpos_t pos, const uint8_t tileSize) {
  const float_t scaleFactor  = (float_t)tileSize * (1.0f / 256.0f);
  return (
    (float_t)pos.tile * (float_t)tileSize + ((float_t)pos.subtile + 128.0f) *
    scaleFactor
  );
}

float_t worldPosToF32(const worldpos_t pos, const uint8_t tileSize) {
  const float_t scaleFactor  = (float_t)tileSize * (1.0f / 256.0f);
  const float_t chunkFactor  = WORLD_CHUNK_SIZE * (float_t)tileSize;
  return (
    (float_t)pos.chunk * chunkFactor +
    (float_t)pos.tile * (float_t)tileSize +
    ((float_t)pos.subtile + 128.0f) * scaleFactor
  );
}