#include <string.h>
#include <stdlib.h>

int strlen(const char *str) {
	int i = 0;
	while (str[i++]);
	return i-1;
}

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

char *strcpy(char *dest, const char *src) {
	memcpy(dest, src, strlen(src) + 1);
	return (char*)src;
}

char *strdup(const char *src) {
	char* ret = (char*)malloc(strlen(src) + 1);
	if (ret == NULL) return ret;
	strcpy(ret, src);
	return ret;
}

char *strcat(char *dest, const char *src) {
	char *dest2 = dest;
	dest2 += strlen(dest) - 1;
	while (*src) {
		*dest2 = *src;
		src++;
		dest2++;
	}
	*dest2 = 0;
	return dest;
}

int strcmp(const char *s1, const char *s2) {
	while ((*s1) && (*s1 == *s2)) {
		s1++;
		s2++;
	}
	return (* (unsigned char*)s1 - *(unsigned char*)s2);
}

void *memcpy(void *vd, const void *vs, int count) {
	uint8_t *dest = (uint8_t*)vd, *src = (uint8_t*)vs;
	int f = count % 4, n = count / 4, i;
	const uint32_t* s = (uint32_t*)src;
	uint32_t* d = (uint32_t*)dest;
	for (i = 0; i < n; i++) {
		d[i] = s[i];
	}
	if (f != 0) {
		for (i = count - f; i < count; i++) {
			dest[i] = src[i];
		}
	}
	return vd;
}

void *memset(void *dest, int val, int count) {
	uint8_t *dest_c = (uint8_t*)dest;
	int i;
	for (i = 0; i < count; i++) {
		dest_c[i] = val;
	}
	return dest;
}

uint16_t *memsetw(uint16_t *dest, uint16_t val, int count) {
	int i;
	for (i = 0; i < count; i++) {
		dest[i] = val;
	}
	return dest;
}


// Formatting

char* format_int(char* buf, int number) {
	if (number == 0) {
		*(buf++) = '0';
		return buf;
	}
	if (number < 0) {
		*(buf++) = '-';
		number = 0 - number;
	}

	int order = 0, temp = number, i;
	char numbers[] = "0123456789";
	while (temp > 0) {
		order++;
		temp /= 10;
	}

	for (i = order; i > 0; i--) {
		buf[i - 1] = numbers[number % 10];
		number /= 10;
	}
	return buf + order;
}

char* format_hex(char *buf, unsigned v) {
	*(buf++) = '0';
	*(buf++) = 'x';

	int i;
	char hexdigits[] = "0123456789ABCDEF";
	for (i = 0; i < 8; i++) {
		*(buf++) = hexdigits[v >> 28];
		v = v << 4;
	}
	return buf;
}

int printf_str_len(const char *format, va_list ap) {
	int l = 0;
	while (*format) {
		if (*format == '%') {
			format++;
			if (*format == 'd' || *format == 'i') {
				l += 15;
				va_arg(ap, int);
			} else if (*format == 'p') {
				l += 11;
				va_arg(ap, void*);
			} else if (*format == 's') {
				l += strlen(va_arg(ap, const char*));
			}
		} else {
			l++;
		}
		format++;
	}
	return l;
}

int sprintf(char *buf, const char *fmt, ...) {
	va_list ap;
	va_start(ap, fmt);
	int ret = vsprintf(buf, fmt, ap);
	va_end(ap);
	return ret;
}

int vsprintf(char *buf, const char *format, va_list ap) {
	char *end = buf;

	while (*format) {
		if (*format == '%') {
			format++;
			if (*format == 'd' || *format == 'i') {
				end = format_int(end, va_arg(ap, int));
			} else if (*format == 'p') {
				end = format_hex(end, va_arg(ap, uint32_t));
			} else if (*format == 's') {
				const char *s = va_arg(ap, const char*);
				strcpy(end, s);
				end += strlen(s);
			}
			format++;
		} else {
			*(end++) = *(format++);
		}
	}
	*end = 0;
	return end - buf;
}