#include <socketcan_interface/string.h>
#include <iomanip>

namespace can {

bool hex2dec(uint8_t& d, const char& h) {
	if ('0' <= h && h <= '9')
		d = h - '0';
	else if ('a' <= h && h <= 'f')
		d = h - 'a' + 10;
	else if ('A' <= h && h <= 'F')
		d = h - 'A' + 10;
	else
		return false;

	return true;
}

bool hex2buffer(std::string& out, const std::string& in_raw, bool pad) {
	std::string in(in_raw);
	if ((in.size() % 2) != 0) {
		if (pad)
			in.insert(0, "0");
		else
			return false;
	}
	out.resize(in.size() >> 1);
	for (size_t i = 0; i < out.size(); ++i) {
		uint8_t hi, lo;
		if (!hex2dec(hi, in[i << 1]) || !hex2dec(lo, in[(i << 1) + 1]))
			return false;

		out[i] = (hi << 4) | lo;
	}
	return true;
}

bool dec2hex(char& h, const uint8_t& d, bool lc) {
	if (d < 10) {
		h = '0' + d;
	} else if (d < 16 && lc) {
		h = 'a' + (d - 10);
	} else if (d < 16 && !lc) {
		h = 'A' + (d - 10);
	} else {
		h='?';
		return false;
	}
	return true;
}

std::string byte2hex(const uint8_t& d, bool pad, bool lc) {
	uint8_t hi = d >> 4;
	char c=0;
	std::string s;
	if (hi || pad) {
		dec2hex(c, hi, lc);
		s += c;
	}
	dec2hex(c, d & 0xf, lc);
	s += c;
	return s;
}

std::string buffer2hex(const std::string& in, bool lc) {
	std::string s;
	s.reserve(in.size() * 2);
	for (size_t i = 0; i < in.size(); ++i) {
		std::string b = byte2hex(in[i], true, lc);
		if (b.empty())
			return b;

		s += b;
	}
	return s;
}

std::string tostring(const Header& h, bool lc) {
	std::string s;
	s.reserve(8);
	std::stringstream buf;
	buf << std::hex;
	if (lc)
		buf << std::nouppercase;
	else
		buf << std::uppercase;

	if (h.is_extended)
		buf << std::setfill('0') << std::setw(8);

	buf << (h.fullid() & ~Header::EXTENDED_MASK);
	return buf.str();
}

uint32_t tohex(const std::string& s) {
		unsigned int h = 0;
		std::stringstream stream;
		stream << std::hex << s;
		stream >> h;
		return h;
}

Header toheader(const std::string& s) {
	unsigned int h = tohex(s);
	unsigned int id = h & Header::ID_MASK;
	return Header(id, h & Header::EXTENDED_MASK || (s.size() == 8 && id >= (1<<11)),
			h & Header::RTR_MASK, h & Header::ERROR_MASK);
}

std::string tostring(const Frame& f, bool lc) {
	std::string s;
	s.resize(f.dlc);
	for (uint8_t i = 0; i < f.dlc; ++i) {
		s[i] = f.data[i];
	}
	return tostring((const Header&) (f), lc) + '#' + buffer2hex(s, lc);
}

Frame toframe(const std::string& s) {
	size_t sep = s.find('#');
	if (sep == std::string::npos)
		return MsgHeader(0xfff);

	Header header = toheader(s.substr(0, sep));
	Frame frame(header);
	std::string buffer;
	if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) {
		if (buffer.size() > 8)
			return MsgHeader(0xfff);

		for (size_t i = 0; i < buffer.size(); ++i) {
			frame.data[i] = buffer[i];
		}
		frame.dlc = buffer.size();
	}
	return frame;
}

template<> FrameFilterSharedPtr tofilter(const std::string  &s){
	  FrameFilter * filter = 0;
		size_t delim = s.find_first_of(":~-_");

		uint32_t second = FrameMaskFilter::MASK_RELAXED;
		bool invert = false;
		char type = ':';

		if(delim != std::string::npos) {
			type = s.at(delim);
			second = tohex(s.substr(delim +1));
		}
		uint32_t first = toheader(s.substr(0, delim)).fullid();
		switch (type) {
			case '~':
				invert = true;
			case ':':
				filter = new FrameMaskFilter(first, second, invert);
				break;
			case '_':
				invert = true;
			case '-':
				filter = new FrameRangeFilter(first, second, invert);
				break;
		}
		return FrameFilterSharedPtr(filter);
}
template<> FrameFilterSharedPtr tofilter(const uint32_t &id){
		return FrameFilterSharedPtr(new FrameMaskFilter(id));
}

FrameFilterSharedPtr tofilter(const char* s){
		return tofilter<std::string>(s);
}

std::ostream& operator <<(std::ostream& stream, const Header& h) {
	return stream << can::tostring(h, true);
}

std::ostream& operator <<(std::ostream& stream, const Frame& f) {
	return stream << can::tostring(f, true);
}

}
