#include "codecvt.h"

using namespace std;

ucs2_conversion::result
ucs2_conversion::do_in(mbstate_t&,
					   const char*  from, const char* from_end, const char*& from_next,
					   wchar_t* to, wchar_t* to_limit, wchar_t*& to_next) const
{
	size_t max_input = (from_end - from) & ~1;
	size_t max_output = (to_limit - to);
	size_t count = min(max_input/2, max_output);

	result res = ok;

	from_next = from;
	to_next = to;
	for (;count--; from_next += 2, ++to_next) {
		unsigned char c1 = *from_next, c2 = *(from_next + 1);
		*to_next = c1 | c2 << 8;
	}
	if (to_next == to && from_next == from_end - 1) res = partial;
	return res; 
}

ucs2_conversion::result
ucs2_conversion::do_out(mbstate_t&,
				  const wchar_t* from, const wchar_t* from_end, const wchar_t*& from_next,
				  char* to, char* to_limit, char*& to_next) const
{
	size_t max_input = (from_end - from);
	size_t max_output = (to_limit - to) & ~1;
	size_t count = min(max_input, max_output/2);

	from_next = from;
	to_next = to;
	for (;count--; ++from_next, to_next += 2) {
		*(to_next + 0) = (char)(*from_next & 0xFF);
		*(to_next + 1) = (char)(*from_next >> 8 & 0xFF);
	}
	return ok;
}

typedef unsigned char uchar;

inline unsigned char 
take_6_bits(unsigned value, size_t right_position)
{
	return uchar((value >> right_position) & 63);
}



inline size_t
most_signifant_bit_position(unsigned value)
{
	size_t result(0);

	size_t half = 16;
	for(; half > 0; half >>= 1) {
		if (1u << (result + half) <= value ) result += half;
	}	
	return result + 1;
	//return *lower_bound(range(0u, 31u), \x -> (1 << x <= value));
}



utf8_conversion::result
utf8_conversion::do_in(mbstate_t&,
					   const char*  from, const char* from_end, const char*& from_next,
					   wchar_t* to, wchar_t* to_limit, wchar_t*& to_next) const
{
	from_next = from;
	to_next = to;

	for(; to_next < to_limit && from_next < from_end; ++to_next) {

		if (uchar(*from_next) < 0x80) *to_next = uchar(*from_next++);
		else {

			// 111zxxxx : z = 0 xxxx are data bits
			size_t zero_bit_pos = most_signifant_bit_position(~*from_next);
			size_t extra_bytes  = 7 - zero_bit_pos;

			if (size_t(from_end - from_next) < extra_bytes + 1) 
				return partial;

			*to_next = uchar(*from_next++) & (wchar_t)((1 << (zero_bit_pos - 1)) - 1);

			for (;extra_bytes--; ++from_next) {
				*to_next = *to_next << 6  |  uchar(*from_next) & 63;
			}
		}
	}	

	return ok;
}


utf8_conversion::result
utf8_conversion::do_out(mbstate_t&,
				  const wchar_t* from, const wchar_t* from_end, const wchar_t*& from_next,
				  char* to, char* to_limit, char*& to_next) const
{
		from_next = from;
		to_next = to;

		for (;from_next < from_end; ++from_next) {

			unsigned symbol = *from_next;

			if (symbol < 0x7F) {
				if (to_next < to_limit)
					*to_next++ = (unsigned char)symbol;
				else
					return ok;
			} else {

				size_t msb_pos = most_signifant_bit_position(symbol);
				size_t extra_bytes = msb_pos / 6;

				if (size_t(to_limit - to_next) >= extra_bytes + 1) {

					*to_next = uchar(0xFF80 >> extra_bytes);
					*to_next++ |= take_6_bits(symbol, extra_bytes*6);

					for(;extra_bytes--;)
						*to_next++ = 0x80 | take_6_bits(symbol, extra_bytes*6);
				}
				else
					return ok;
			}
		}
		return ok;
}

