#include <stdlib.h>
#include <vector_extentions/sse.h>

#include <string.h>


extern int16_t sse_initialized;

size_t strlen(const char* str)
{
    size_t len = 0;
    while (str[len]) len++;
    return len;
}

size_t strcmp(const char *s1, const char *s2)
{
  int32_t i = 0;

  while ((s1[i] == s2[i]))
  {
      if (s2[i++] == 0)
      {
        return 0;
      }
  }
  return 1;
}

int32_t strncmp(const char *s1, const char *s2, size_t n)
{
  while (n--)
  {
    unsigned char c1 = (unsigned char)*s1++;
    unsigned char c2 = (unsigned char)*s2++;
    if (c1 != c2)
    {
      return c1 - c2;
    }
    if (c1 == '\0')
    {
      break;
    }
  }
  return 0;
}


size_t strcpy(char *dst, const char *src)
{
    int32_t i = 0;
    while ((*dst++ = *src++) != 0)
        i++;
    return i;
}

char *strncpy(char *dest, const char *src, uint32_t n)
{
  if (sse_initialized > 0)
  {
    return sse2_strncpy(dest, src, n);
  }

  uint32_t i = 0;
  for (; i < n && src[i]; ++i)
    dest[i] = src[i];

  for (; i < n; ++i)
    dest[i] = '\0';

  return dest;
}

void strcat(char *dest, const char *src)
{
    char *end = (char *)dest + strlen(dest);
    memcpy((void *)end, (void *)src, strlen(src));
    end = end + strlen(src);
    *end = '\0';
}

char* strdup(const char* s)
{
  if (!s)
  {
    return NULL;
  }

  size_t len = strlen(s);
  char* copy = (char*)malloc(len + 1);

  if (!copy)
  {
    return NULL;
  }

  memcpy(copy, s, len);
  copy[len] = '\0';

  return copy;
}

char* strtok(char* str, const char* delim)
{
  static char* next_token = NULL;

  if (str != NULL)
  {
    next_token = str;
  }
  else if (next_token == NULL)
  {
    return NULL;
  }

  while (*next_token && strchr(delim, *next_token))
  {
    next_token++;
  }

  if (*next_token == '\0')
  {
    return NULL;
  }

  char* token_start = next_token;

  while (*next_token && !strchr(delim, *next_token))
  {
    next_token++;
  }

  if (*next_token)
  {
    *next_token = '\0';
    next_token++;
  }

  return token_start;
}

char* strchr(const char* s, int c)
{
  while (*s)
  {
      if (*s == (char)c)
      {
        return (char*)s;
      }
      s++;
  }

  return NULL;
}



void *memset(void *dst, char c, uint32_t n)
{
  char *temp = dst;
  for (; n != 0; n--) *temp++ = c;
  return dst;
}

void *memcpy(void *dst, const void *src, uint32_t n)
{
  if (sse_initialized > 0)
  {
    return sse2_memcpy(dst, src, n);
  }
    
  char *ret = dst;
  char *p = dst;
  const char *q = src;
  while (n--)
    *p++ = *q++;
  return ret;
}

int32_t memcmp(uint8_t *s1, uint8_t *s2, uint32_t n)
{
    while (n--)
    {
        if (*s1 != *s2)
            return 0;
        s1++;
        s2++;
    }
    return 1;
}

void* memclr(const void * const m_start, const size_t m_count)
{
  if (sse_initialized > 0)
  {
    return memclr_sse2(m_start, m_count);
  }
  
  return memset(m_start, '\0', (uint32_t)m_count);
}

int32_t isspace(char c)
{
  return c == ' ' || c == '\t' || c == '\n' || c == '\v' || c == '\f' || c == '\r';
}

int32_t isalpha(char c)
{
  return (((c >= 'A') && (c <= 'Z')) || ((c >= 'a') && (c <= 'z')));
}

int32_t ischar(int32_t c)
{
  return ((c >= 32) && (c <= 126));
}

char upper(char c)
{
    if ((c >= 'a') && (c <= 'z'))
        return (c - 32);
    return c;
}

char toupper(char c)
{
  if ((c >= 'a') && (c <= 'z'))
  {
    return (c - 32);
  }
  return c;
}

char lower(char c)
{
    if ((c >= 'A') && (c <= 'Z'))
        return (c + 32);
    return c;
}