#include "global.h"


/* ------------------------------------------------------------------- */

/*
 * mqtt_msgCONNECT() Create CONNECT message
 *
 * @buf = Message buffer
 * @csConnID = ConnectID
 * @kasec = Keep alive timeout
 * @csUser = Username if !=NULL
 * @csPass = Password for Username, only if csUser is set
 * @csWillTopic = Will Topic if !=NULL Will Flags set into message
 * @csWillMessage = Will Message, may be NULL
 * @ClrSess = Clear Session subscriptions after disconnect
 * @WillQOS = Will QOS if csWillTopic is set
 * @WillRetain = Will Retain Will Message if csWillTopic is set
 * return: -1 error or >-1 message size for send
 */
int
mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const char *csConnID, 
		u_short kasec, const char *csUser, const char *csPass, 
		const char *csWillTopic, const char *csWillMessage, 
		u_char ClrSess, u_char WillQOS, u_char WillRetain)
{
	int siz = 0;
	struct mqtthdr *hdr;
	mqtthdr_var_t *var, *cid, *topic, *wmsg, *user, *pass;
	mqtthdr_protover_t *proto;
	mqtthdr_connflgs_t *flags;
	mqtt_v_t *ka;

	if (!buf || !csConnID)
		return -1;
	if (strlen(csConnID) > 23) {
		mqtt_SetErr(EINVAL, "Error:: invalid argument ConnID is too long (max 23 bytes)");
		return -1;
	}
	if (csUser && strlen(csUser) > 12) {
		mqtt_SetErr(EINVAL, "Error:: invalid argument Username is too long (max 12 bytes)");
		return -1;
	}
	if (csPass && strlen(csPass) > 12) {
		mqtt_SetErr(EINVAL, "Error:: invalid argument Password is too long (max 12 bytes)");
		return -1;
	}
	if (WillQOS > MQTT_QOS_EXACTLY) {
		mqtt_SetErr(EINVAL, "Error:: invalid argument WillQOS - unknown QOS value");
		return -1;
	}

	if (mqtt_msgRealloc(buf, BUFSIZ) == -1)
		return -1;
	else {
		hdr = (struct mqtthdr *) (buf->msg_base + siz);
		siz += sizeof(struct mqtthdr);
		var = (mqtthdr_var_t*) (buf->msg_base + siz);
		siz += 8;
		proto = buf->msg_base + siz;
		siz++;
		flags = (mqtthdr_connflgs_t*) (buf->msg_base + siz);
		siz++;
		ka = (mqtt_v_t*) (buf->msg_base + siz);
		siz += sizeof(mqtt_v_t);
	}

	/* fixed header */
	MQTTHDR_MSGINIT(hdr);
	hdr->mqtt_msg.type = MQTT_TYPE_CONNECT;
	*hdr->mqtt_len = 0;

	/* variable header */
	var->var_sb.sb.l = 6;
	memcpy(var->var_data, MQTT_CONN_STR, 6);

	*proto = MQTT_PROTO_VER;

	/* CONNECT header */
	flags->clean_sess = ClrSess ? 1 : 0;
	if (csUser && *csUser) {
		flags->username = 1;
		flags->password = csPass ? 1 : 0;
	} else {
		flags->username = 0;
		flags->password = 0;
	}
	if (csWillTopic && *csWillTopic) {
		flags->will_flg = 1;
		flags->will_qos = WillQOS;
		flags->will_retain = WillRetain ? 1 : 0;
	} else {
		flags->will_flg = 0;
		flags->will_qos = 0;
		flags->will_retain = 0;
	}

	ka->val = kasec ? htons(kasec) : htons(MQTT_KEEPALIVE);

	/* ConnID */
	cid = (mqtthdr_var_t*) (buf->msg_base + siz);
	cid->var_sb.val = htons(strlen(csConnID));
	siz += MQTTHDR_VAR_SIZEOF(cid);
	memcpy(cid->var_data, csConnID, ntohs(cid->var_sb.val));

	/* If Will Flags setup */
	if (csWillTopic && *csWillTopic) {
		topic = (mqtthdr_var_t*) (buf->msg_base + siz);
		topic->var_sb.val = htons(strlen(csWillTopic));
		memcpy(topic->var_data, csWillTopic, ntohs(topic->var_sb.val));
		siz += MQTTHDR_VAR_SIZEOF(topic);

		wmsg = (mqtthdr_var_t*) (buf->msg_base + siz);
		if (csWillMessage && *csWillMessage) {
			wmsg->var_sb.val = htons(strlen(csWillMessage));
			memcpy(wmsg->var_data, csWillMessage, ntohs(wmsg->var_sb.val));
		} else
			wmsg->var_sb.val = 0;
		siz += MQTTHDR_VAR_SIZEOF(wmsg);
	}

	/* If defined Username & Password */
	if (csUser && *csUser) {
		user = (mqtthdr_var_t*) (buf->msg_base + siz);
		user->var_sb.val = htons(strlen(csUser));
		memcpy(user->var_data, csUser, ntohs(user->var_sb.val));
		siz += MQTTHDR_VAR_SIZEOF(user);

		if (csPass && *csPass) {
			pass = (mqtthdr_var_t*) (buf->msg_base + siz);
			pass->var_sb.val = htons(strlen(csPass));
			memcpy(pass->var_data, csPass, ntohs(pass->var_sb.val));
			siz += MQTTHDR_VAR_SIZEOF(pass);
		}
	}

	*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
	mqtt_msgRealloc(buf, siz);
	return siz;
}

/*
 * mqtt_msgCONNACK() Create CONNACK message
 *
 * @buf = Message buffer
 * @retcode = Return code
 * return: -1 error or >-1 message size for send
 */
int
mqtt_msgCONNACK(mqtt_msg_t * __restrict buf, u_char retcode)
{
	int siz = 0;
	struct mqtthdr *hdr;
	mqtthdr_connack_t *ack;

	if (!buf)
		return -1;
	if (retcode > MQTT_RETCODE_DENIED) {
		mqtt_SetErr(EINVAL, "Error:: invalid retcode");
		return -1;
	}

	if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr) + sizeof(mqtthdr_connack_t)) == -1)
		return -1;
	else {
		hdr = (struct mqtthdr *) (buf->msg_base + siz);
		siz += sizeof(struct mqtthdr);
		ack = (mqtthdr_connack_t*) (buf->msg_base + siz);
		siz += sizeof(mqtthdr_connack_t);
	}

	/* fixed header */
	MQTTHDR_MSGINIT(hdr);
	hdr->mqtt_msg.type = MQTT_TYPE_CONNACK;
	*hdr->mqtt_len = sizeof(mqtthdr_connack_t);

	/* CONNACK header */
	ack->reserved = 0;
	ack->retcode = retcode;

	return siz;
}

static int
_mqtt_msgSIMPLE_(mqtt_msg_t * __restrict buf, u_char cmd)
{
	int siz = 0;
	struct mqtthdr *hdr;

	if (!buf)
		return -1;

	if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr)) == -1)
		return -1;
	else {
		hdr = (struct mqtthdr *) (buf->msg_base + siz);
		siz += sizeof(struct mqtthdr);
	}

	/* fixed header */
	MQTTHDR_MSGINIT(hdr);
	hdr->mqtt_msg.type = cmd;
	*hdr->mqtt_len = 0;

	return siz;
}

/*
 * mqtt_msgPINGREQ() Create PINGREQ message
 *
 * @buf = Message buffer
 * return: -1 error or >-1 message size for send
 */
int
mqtt_msgPINGREQ(mqtt_msg_t * __restrict buf)
{
	return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGREQ);
}

/*
 * mqtt_msgPINGRESP() Create PINGRESP message
 *
 * @buf = Message buffer
 * return: -1 error or >-1 message size for send
 */
int
mqtt_msgPINGRESP(mqtt_msg_t * __restrict buf)
{
	return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGRESP);
}

/*
 * mqtt_msgDISCONNECT() Create DISCONNECT message
 *
 * @buf = Message buffer
 * return: -1 error or >-1 message size for send
 */
int
mqtt_msgDISCONNECT(mqtt_msg_t * __restrict buf)
{
	return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_DISCONNECT);
}

/* ============= decode ============ */

/*
 * mqtt_readCONNECT() Read elements from CONNECT message
 *
 * @buf = Message buffer
 * @kasec = Keep Alive in seconds for current connection
 * @psConnID = ConnectID
 * @connLen = ConnectID length
 * @psUser = Username if !=NULL
 * @userLen = Username length
 * @psPass = Password for Username, only if csUser is set
 * @passLen = Password length
 * @psWillTopic = Will Topic if !=NULL Will Flags set into message and must be free()
 * @psWillMessage = Will Message, may be NULL if !NULL must be free() after use!
 * return: .reserved == 1 is error or == 0 connection flags & msg ok
 */
mqtthdr_connack_t
mqtt_readCONNECT(mqtt_msg_t * __restrict buf, u_short *kasec, char * __restrict psConnID, int connLen, 
		char * __restrict psUser, int userLen, char * __restrict psPass, int passLen,  
		char ** __restrict psWillTopic, char ** __restrict psWillMessage)
{
	mqtthdr_connflgs_t flg = { MQTT_CONNFLGS_INIT };
	mqtthdr_connack_t cack = { 1, MQTT_RETCODE_DENIED };
	struct mqtthdr *hdr;
	mqtthdr_var_t *var;
	mqtt_v_t *ka;
	mqtthdr_protover_t *proto;
	int len, ret;
	caddr_t pos;

	if (!buf || !buf->msg_base || !buf->msg_len || !psConnID || !connLen)
		return cack;

	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNECT, &ret, &len);
	if (!hdr)
		return cack;
	if (len < 12) {
		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
		return cack;
	} else {
		pos = buf->msg_base + ret + 1;
		var = (mqtthdr_var_t*) pos;
	}
	/* check init string & protocol */
	if (var->var_sb.sb.l != 6 || strncmp((char*) var->var_data, MQTT_CONN_STR, 6)) {
		mqtt_SetErr(EINVAL, "Error:: invalid init string %.6s(%d)", 
				var->var_data, var->var_sb.sb.l);
		cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
		return cack;
	} else {
		pos += var->var_sb.sb.l + sizeof(mqtt_v_t);
		proto = (mqtthdr_protover_t*) pos;
	}
	if (*proto != MQTT_PROTO_VER) {
		mqtt_SetErr(EINVAL, "Error:: invalid protocol version %d", *pos);
		cack.retcode = MQTT_RETCODE_REFUSE_VER;
		return cack;
	} else
		pos++;
	flg = *(mqtthdr_connflgs_t*) pos;
	pos++;
	ka = (mqtt_v_t*) pos;
	*kasec = ntohs(ka->val);
	pos += sizeof(mqtt_v_t);

	len -= pos - (caddr_t) var;

	/* get ConnID */
	var = (mqtthdr_var_t*) pos;
	len -= MQTTHDR_VAR_SIZEOF(var);
	if (len < 0 || var->var_sb.sb.l > 23) {
		mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Connection ID %d", len);
		cack.retcode = MQTT_RETCODE_REFUSE_ID;
		return cack;
	} else {
		memset(psConnID, 0, connLen--);
		memcpy(psConnID, var->var_data, ntohs(var->var_sb.val) > connLen ? connLen : ntohs(var->var_sb.val));
		pos += MQTTHDR_VAR_SIZEOF(var);
	}

	/* get Willz */
	if (flg.will_flg) {
		var = (mqtthdr_var_t*) pos;
		len -= MQTTHDR_VAR_SIZEOF(var);
		if (len < 0) {
			mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Will Topic %d", len);
			cack.retcode = MQTT_RETCODE_REFUSE_ID;
			return cack;
		} else {
			if (psWillTopic) {
				*psWillTopic = malloc(ntohs(var->var_sb.val) + 1);
				if (!*psWillTopic) {
					LOGERR;
					cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
					return cack;
				} else
					memset(*psWillTopic, 0, ntohs(var->var_sb.val) + 1);
				memcpy(*psWillTopic, var->var_data, ntohs(var->var_sb.val));
			}
			pos += MQTTHDR_VAR_SIZEOF(var);
		}

		var = (mqtthdr_var_t*) pos;
		len -= MQTTHDR_VAR_SIZEOF(var);
		if (len < 0) {
			mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Will Message %d", len);
			cack.retcode = MQTT_RETCODE_REFUSE_ID;
			return cack;
		} else {
			if (psWillMessage) {
				*psWillMessage = malloc(ntohs(var->var_sb.val) + 1);
				if (!*psWillMessage) {
					LOGERR;
					cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
					return cack;
				} else
					memset(*psWillMessage, 0, ntohs(var->var_sb.val) + 1);
				memcpy(*psWillMessage, var->var_data, ntohs(var->var_sb.val));
			}
			pos += MQTTHDR_VAR_SIZEOF(var);
		}
	}

	/* get User/Pass */
	if (flg.username) {
		var = (mqtthdr_var_t*) pos;
		len -= MQTTHDR_VAR_SIZEOF(var);
		if (len < 0 || var->var_sb.sb.l > 12) {
			mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Username %d", len);
			cack.retcode = MQTT_RETCODE_REFUSE_USERPASS;
			return cack;
		} else {
			if (psUser && userLen) {
				memset(psUser, 0, userLen--);
				memcpy(psUser, var->var_data, 
						ntohs(var->var_sb.val) > userLen ? userLen : ntohs(var->var_sb.val));
			}
			pos += MQTTHDR_VAR_SIZEOF(var);
		}
	}
	if (flg.password) {
		var = (mqtthdr_var_t*) pos;
		len -= MQTTHDR_VAR_SIZEOF(var);
		if (len < 0 || var->var_sb.sb.l > 12) {
			mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Password %d", len);
			cack.retcode = MQTT_RETCODE_REFUSE_USERPASS;
			return cack;
		} else {
			if (psPass && passLen) {
				memset(psPass, 0, passLen--);
				memcpy(psPass, var->var_data, 
						ntohs(var->var_sb.val) > passLen ? passLen : ntohs(var->var_sb.val));
			}
			pos += MQTTHDR_VAR_SIZEOF(var);
		}
	}

	flg.reserved = 0;
	cack.reserved = flg.flags;
	cack.retcode = MQTT_RETCODE_ACCEPTED;
	return cack;
}

/*
 * mqtt_readCONNACK() Read CONNACK message
 *
 * @buf = Message buffer
 * return: -1 error or >-1 CONNECT message return code
 */
u_char
mqtt_readCONNACK(mqtt_msg_t * __restrict buf)
{
	int len, ret;
	struct mqtthdr *hdr;
	mqtthdr_connack_t *ack;
	caddr_t pos;

	if (!buf || !buf->msg_base || !buf->msg_len)
		return (u_char) -1;

	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNACK, &ret, &len);
	if (!hdr)
		return (u_char) -1;
	if (len < sizeof(mqtthdr_connack_t)) {
		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
		return (u_char) -1;
	} else {
		pos = buf->msg_base + ret + 1;
		ack = (mqtthdr_connack_t*) pos;
	}

	if (ack->retcode > MQTT_RETCODE_DENIED) {
		mqtt_SetErr(EINVAL, "Error:: invalid retcode %u", ack->retcode);
		return (u_char) -1;
	}

	return ack->retcode;
}

/*
 * mqtt_readDISCONNECT() Read DISCONNECT message
 *
 * @buf = Message buffer
 * return: -1 error, 0 ok, >0 undefined result
 */
int
mqtt_readDISCONNECT(mqtt_msg_t * __restrict buf)
{
	int len, ret;
	struct mqtthdr *hdr;

	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_DISCONNECT, &ret, &len);
	if (!hdr || ret != 1)
		return -1;

	return len;
}

/*
 * mqtt_readPINGREQ() Read PINGREQ message
 *
 * @buf = Message buffer
 * return: -1 error, 0 ok, >0 undefined result
 */
int
mqtt_readPINGREQ(mqtt_msg_t * __restrict buf)
{
	int len, ret;
	struct mqtthdr *hdr;

	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PINGREQ, &ret, &len);
	if (!hdr || ret != 1)
		return -1;

	return len;
}

/*
 * mqtt_readPINGRESP() Read PINGRESP message
 *
 * @buf = Message buffer
 * return: -1 error, 0 ok, >0 undefined result
 */
int
mqtt_readPINGRESP(mqtt_msg_t * __restrict buf)
{
	int len, ret;
	struct mqtthdr *hdr;

	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PINGRESP, &ret, &len);
	if (!hdr || ret != 1)
		return -1;

	return len;
}