#include "libc.h"

void memcopy(u8 *dest, const u8 *src, u32 n) {
    size_t i;
    size_t words;
    size_t bytes;
    size_t unroll;
    size_t remainder;
    u32 *d32;
    const u32 *s32;
    u8 *d8;
    const u8 *s8;

    /* Fast path for small copies (common case) */
    if (n <= 8) {
        for (i = 0; i < n; i++) {
            dest[i] = src[i];
        }
        return;
    }

    /* Check for word alignment (assuming 32-bit words) */
    if ((((size_t)dest) & 0x3) == 0 && (((size_t)src) & 0x3) == 0) {
        /* Both pointers are 4-byte aligned - copy by words */
        d32 = (u32 *)dest;
        s32 = (const u32 *)src;
        words = n / 4;
        bytes = n % 4;

        /* Loop unrolling - 4x unroll for better performance */
        unroll = words / 4;
        remainder = words % 4;

        for (i = 0; i < unroll; i++) {
            d32[0] = s32[0];
            d32[1] = s32[1];
            d32[2] = s32[2];
            d32[3] = s32[3];
            d32 += 4;
            s32 += 4;
        }

        /* Handle remaining words */
        for (i = 0; i < remainder; i++) {
            *d32++ = *s32++;
        }

        /* Handle trailing bytes */
        d8 = (u8 *)d32;
        s8 = (const u8 *)s32;
        for (i = 0; i < bytes; i++) {
            d8[i] = s8[i];
        }
    } else {
        /* Unaligned copy - byte by byte but with loop unrolling */
        unroll = n / 4;
        remainder = n % 4;

        for (i = 0; i < unroll; i++) {
            dest[0] = src[0];
            dest[1] = src[1];
            dest[2] = src[2];
            dest[3] = src[3];
            dest += 4;
            src += 4;
        }

        for (i = 0; i < remainder; i++) {
            dest[i] = src[i];
        }
    }
}

i32 strcopy(char *to, const char *from, u32 length) {
    u32 i;
    if (to == nil || from == nil) return -1;
    if (length == 0) {return 0;}
    for (i = 0; i < length - 1 && from[i] != '\0'; i++) {
        to[i] = from[i];
    }
    to[i] = '\0';
    return 0;
}

bool streq(const char *s1, const char *s2) {
    if (s1 == nil && s2 == nil) return true;
    if (s1 == nil || s2 == nil) return false;

    while (*s1 && *s2) {
        if (*s1 != *s2) return false;
        s1++;
        s2++;
    }

    return (*s1 == '\0' && *s2 == '\0');
}

bool strleq(const char *s1, const char *s2, u32 length) {
    u32 i;
    if (s1 == nil && s2 == nil) return true;
    if (s1 == nil || s2 == nil) return false;

    i = 0;
    while (i < length && *s1 && *s2) {
        if (*s1 != *s2) return false;
        s1++;
        s2++;
        i++;
    }
    if (i == length) return true;
    return (*s1 == '\0' && *s2 == '\0');
}

u32 strlength(const char *str) {
    u32 i;
    if (str == nil) {return 0;}
    for (i = 0; str[i] != '\0'; i++) {
        ; /* twiddle thumbs, 'i' is doing all the work*/
    }
    return i;
}

u32 strnlength(const char *str, u32 max_len) {
    u32 i;
    if (str == nil) {return 0;}
    for (i = 0; i < max_len && str[i] != '\0'; i++) {
        ; /* twiddle thumbs, 'i' is doing all the work*/
    }
    return i;
}


/* Static digit lookup table (0-9) */
const char digits[10] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9'};

/* Writes decimal digits of 'value' backwards into 'buf_end' (inclusive),
   stopping at 'buf_start'. Returns pointer to first written digit. */
char *write_digits_backwards(u32 value, char *buf_end, char *buf_start) {
  char *p = buf_end;
  if (value == 0) {
    *--p = '0';
  } else {
    u32 num = value;
    while (num && p > buf_start) {
      *--p = digits[num % 10];
      num /= 10;
    }
  }
  return p;
}

void nat_to_string(u32 value, char *buffer) {
  char temp[16];
  char *start;
  char *end = temp + sizeof(temp) - 1;
  *end = '\0';
  start = write_digits_backwards(value, end, temp);
  strcopy(buffer, start, end - start + 1); /* +1 for null terminator */
}

void int_to_string(i32 value, char *buffer) {
  char temp[17]; /* Extra space for '-' */
  i32 negative = 0;
  u32 abs_value;
  char *end = temp + sizeof(temp) - 1;
  *end = '\0';

  if (value == (-2147483647 - 1)) { /* INT32_MIN */
    strcopy(buffer, "-2147483648", 12);
    return;
  }

  if (value == 0) {
    *--end = '0';
  } else {
    if (value < 0) {
      negative = 1;
      abs_value = (u32)(-value);
    } else {
      abs_value = (u32)value;
    }

    end = write_digits_backwards(abs_value, end, temp);

    if (negative) {
      *--end = '-';
    }
  }

  strcopy(buffer, end, temp + sizeof(temp) - end);
}

void fixed_to_string(i32 value, char *buffer) {
  char temp[32];
  i32 negative;
  u32 int_part;
  u32 frac_part, frac_digits, original_frac_digits;
  char *end = temp + sizeof(temp) - 1;
  char *frac_start;
  *end = '\0';

  negative = 0;
  if (value < 0) {
    negative = 1;
    value = -value;
  }

  int_part = AS_NAT(value >> 16);
  frac_part = AS_NAT(value & 0xFFFF);

  /* Convert fractional part to 5 decimal digits */
  original_frac_digits = (frac_part * 100000U) / 65536U;
  frac_digits = original_frac_digits;

  if (frac_digits > 0) {
    /* Write fractional digits backwards */
    frac_start = write_digits_backwards(frac_digits, end, temp);
    
    /* Remove trailing zeros by moving the start pointer */
    while (*(end - 1) == '0' && end > frac_start) {
      end--;
    }
    
    /* If all fractional digits were zeros after removing trailing zeros,
       we need to add back one zero to represent the .0 */
    if (end == frac_start) {
      *--end = '0';
    }
    
    /* Add decimal point */
    *--end = '.';
  } else if (frac_part > 0) {
    /* Handle case where original_frac_digits was rounded to 0 but frac_part was not 0 */
    /* This means we have a very small fractional part that should be represented as .0 */
    *--end = '0';
    *--end = '.';
  } else if (frac_part == 0) {
    /* No fractional part - just add .0 if we have an integer part */
    if (int_part != 0) {
      *--end = '0';
      *--end = '.';
    }
  }

  if (int_part == 0 && frac_digits == 0 && frac_part == 0) {
    *--end = '0';
  } else {
    end = write_digits_backwards(int_part, end, temp);
  }

  if (negative) {
    *--end = '-';
  }

  strcopy(buffer, end, temp + sizeof(temp) - end);
}