diff --git a/src/simple-printf.c b/src/simple-printf.c
index 98e2990..dbce09d 100644
--- a/src/simple-printf.c
+++ b/src/simple-printf.c
@@ -8,7 +8,7 @@ enum format_specifier_type {
 	FMT_UNSIGNED,
 	FMT_STRING,
 	FMT_HEXUINT_CAPITALIZED,
-	FMT_HEXUNIT,
+	FMT_HEXUINT,
 	FMT_CHAR,
 	FMT_SIZE,
 	FMT_PERCENT,
@@ -21,14 +21,84 @@ struct format_specifier {
 	size_t length_of_provided_specifier;
 };
 
+enum format_parser_state {
+	PARSER_PERCENT = 0,
+	PARSER_SECOND,
+	PARSER_NUM,
+	PARSER_SPECIFIER,
+};
+
+static int parse_number(const char *str, size_t len)
+{
+	int ret = 0;
+	int base;
+	size_t idx;
+	char c;
+
+	if (!len)
+		return 0;
+
+	for (idx = 0, base = 1; idx < len; idx++) {
+		c = str[len - idx - 1];
+		if (c > '9' || c < '0')
+			return -1;
+		ret += ((int)(c-'0'))* base;
+		base *= 10;
+	}
+
+	return ret;
+}
+
+static enum format_specifier_type resolve_specifier(char c)
+{
+	enum format_specifier_type ret;
+
+	switch (c) {
+	case 's':
+		ret = FMT_STRING;
+		break;
+	case 'z':
+		ret = FMT_SIZE;
+		break;
+	case 'u':
+		ret = FMT_UNSIGNED;
+		break;
+	case 'i': /* Fallthru */
+	case 'd':
+		ret = FMT_INTEGER;
+		break;
+	case 'x':
+		ret = FMT_HEXUINT;
+		break;
+	case 'X':
+		ret = FMT_HEXUINT_CAPITALIZED;
+		break;
+	case 'c':
+		ret = FMT_CHAR;
+		break;
+	case '%':
+		ret = FMT_PERCENT;
+		break;
+	default:
+		ret = FMT_INVALID;
+		break;
+	}
+
+	return ret;
+}
+
 static void parse_format_specifier(const char *start, struct format_specifier *result)
 {
 	size_t fmt_len = 1ull;
 	bool keep_parsing = true;
-	bool is_in_number = false;
-	const char *number;
-	size_t num_len = 0;
-	
+	int num;
+	const char *current_token;
+	size_t token_length = 0;
+	enum format_parser_state state = PARSER_PERCENT;
+	enum format_parser_state next_state;
+	const char *ptr;
+	bool advance_ptr;
+
 	result->specifier = FMT_INVALID;
 	result->zero_pad = 0;
 	result->length_of_provided_specifier = 0;
@@ -36,99 +106,99 @@ static void parse_format_specifier(const char *start, struct format_specifier *r
 	if (!start)
 		return;
 
-	/* Check if specifier starts with % */
-	if (*start != '%')
-		return;
-
-	start++;
+	ptr = start;
 	while (keep_parsing) {
-		/* Parse the format specifier*/
-		switch (*start) {
-		case '0':
-			if (fmt_len == 1) {
-				result->zero_pad = 1;
-				break;
-			} 
-			/* Fallthru */		
-		case '1':
-		case '2':
-		case '3':
-		case '4':
-		case '5':
-		case '6':
-		case '7':
-		case '8':
-		case '9':
-			/* Number */
-			if (!is_in_number) {
-				/* Start of number */
-				is_in_number = true;
-				number = start;
-				num_len = 0;
+
+		advance_ptr = false;
+		next_state = state;
+
+		switch (state) {
+		case PARSER_PERCENT:
+			if (*ptr == '%') {
+				advance_ptr++;
+				next_state = PARSER_SECOND;
+			}
+			break;
+		case PARSER_SECOND:
+			if (*ptr == '0') {
+				result->zero_pad = true;
+				advance_ptr++;
+				next_state = PARSER_NUM;
+			} else if (*ptr > '9' || *ptr < '0') {
+				/* Specifier */
+				next_state = PARSER_SPECIFIER;
 			} else {
-				/* Continue parsing number */
-				num_len++;
+				next_state = PARSER_NUM;
+				current_token = ptr;
+				token_length = 0;
 			}
 			break;
-		case 'c':
-			result->specifier = FMT_CHAR;
-			keep_parsing = false;
-			break;
-		case 'z':
-			result->specifier = FMT_SIZE;
-			keep_parsing = false;
-			break;
-		case 'x':
-			result->specifier = FMT_HEXUNIT;
-			keep_parsing = false;
-			break;
-		case 'X':
-			result->specifier = FMT_HEXUINT_CAPITALIZED;
-			keep_parsing = false;
-			break;
-		case 's':
-			result->specifier = FMT_STRING;
-			keep_parsing = false;
-			break;
-		case 'u':
-			result->specifier = FMT_UNSIGNED;
-			keep_parsing = false;
-			break;
-		case 'i': /* Falltrhu */
-		case 'd':
-			result->specifier = FMT_INTEGER;
-			keep_parsing = false;
-			break;
-		case '%':
-			if (fmt_len == 1) {
-				result->specifier = FMT_PERCENT;
+		case PARSER_NUM:
+			if (*ptr >= '0' && *ptr <= '9') {
+				/* Number */
+				advance_ptr = true;
+				next_state = PARSER_NUM;
+				token_length++;
+			} else {
+				num = parse_number(current_token, token_length);
+				if (num < 0) {
+					keep_parsing = false;
+				}
+				next_state = PARSER_SPECIFIER;
+				result->length = (size_t)num;
 			}
-			keep_parsing = false;
-		default:
-			/* Unrecognized stuff. Abort */
+			break;
+		case PARSER_SPECIFIER:
+			result->specifier = resolve_specifier(*ptr);
+			if (result->specifier == FMT_PERCENT) {
+				if (result->length != 0 || result->zero_pad != 0) {
+					result->specifier = FMT_INVALID;
+				}
+			}
+			result->length_of_provided_specifier = ptr - start + 1;
 			keep_parsing = false;
 			break;
 		}
 
-		if (keep_parsing == false) {
-			/* End reached. Check the number */
-			
+		if (advance_ptr) {
+			ptr++;
 		}
-
-		start++;
-		fmt_len++;
+		state = next_state;
 	}
 }
 
 int simple_snprintf(char *dest, size_t dest_size, const char *fmt, ...)
 {
 	int ret = -1;
-	
+	int written = 0;
+	size_t dest_idx;
+	const char *fmt_iter;
+	struct format_specifier specifier;
+
+
 	if (!dest || !dest_size || !fmt) {
 		/* Parameter error */
 		return -1000;
 	}
 
+	dest_idx = 0;
+	fmt_iter = fmt;
+	while (*fmt_iter) {
+		parse_format_specifier(fmt_iter, &specifier);
+		if (specifier.specifier == FMT_INVALID) {
+			if (dest_idx < dest_size - 1) {
+				written++;
+				dest[dest_idx] = *fmt_iter;
+			} else {
+				/* Full. set terminator */
+				dest[dest_size-1] = 0;
+				break;
+			}
+		} else {
+			/* Do handling of stuff */
+		}
+	}
+
 	return ret;
 }