/**
 * @brief Subscribe to an MQTT topic.
 *
 * Called to send a subscribe message to the broker requesting a subscription
 * to an MQTT topic.
 * This is the internal function which is called by the resubscribe API to perform the operation.
 * Not meant to be called directly as it doesn't do validations or client state changes
 * @note Call is blocking.  The call returns after the receipt of the SUBACK control packet.
 *
 * @param pClient Reference to the IoT Client
 *
 * @return An IoT Error Type defining successful/failed subscription
 */
static IoT_Error_t _aws_iot_mqtt_internal_resubscribe(AWS_IoT_Client *pClient) {
	uint16_t packetId;
	uint32_t len, count, existingSubCount, itr;
	IoT_Error_t rc;
	Timer timer;
	QoS grantedQoS[3] = {QOS0, QOS0, QOS0};

	FUNC_ENTRY;

	packetId = 0;
	len = 0;
	count = 0;
	existingSubCount = _aws_iot_mqtt_get_free_message_handler_index(pClient);

	for(itr = 0; itr < existingSubCount; itr++) {
		init_timer(&timer);
		countdown_ms(&timer, pClient->clientData.commandTimeoutMs);

		rc = _aws_iot_mqtt_serialize_subscribe(pClient->clientData.writeBuf, pClient->clientData.writeBufSize, 0,
											   aws_iot_mqtt_get_next_packet_id(pClient), 1,
											   &(pClient->clientData.messageHandlers[itr].topicName),
											   &(pClient->clientData.messageHandlers[itr].topicNameLen),
											   &(pClient->clientData.messageHandlers[itr].qos), &len);
		if(AWS_SUCCESS != rc) {
			FUNC_EXIT_RC(rc);
		}

		/* send the subscribe packet */
		rc = aws_iot_mqtt_internal_send_packet(pClient, len, &timer);
		if(AWS_SUCCESS != rc) {
			FUNC_EXIT_RC(rc);
		}

		/* wait for suback */
		rc = aws_iot_mqtt_internal_wait_for_read(pClient, SUBACK, &timer);
		if(AWS_SUCCESS != rc) {
			FUNC_EXIT_RC(rc);
		}

		/* Granted QoS can be 0, 1 or 2 */
		rc = _aws_iot_mqtt_deserialize_suback(&packetId, 1, &count, grantedQoS, pClient->clientData.readBuf,
											  pClient->clientData.readBufSize);
		if(AWS_SUCCESS != rc) {
			FUNC_EXIT_RC(rc);
		}
	}

	FUNC_EXIT_RC(AWS_SUCCESS);
}
/**
 * @brief MQTT Connection Function
 *
 * Called to establish an MQTT connection with the AWS IoT Service
 * This is the internal function which is called by the connect API to perform the operation.
 * Not meant to be called directly as it doesn't do validations or client state changes
 *
 * @param pClient Reference to the IoT Client
 * @param pConnectParams Pointer to MQTT connection parameters
 *
 * @return An IoT Error Type defining successful/failed connection
 */
static IoT_Error_t _aws_iot_mqtt_internal_connect(AWS_IoT_Client *pClient, IoT_Client_Connect_Params *pConnectParams) {
	Timer connect_timer;
	IoT_Error_t connack_rc = FAILURE;
	char sessionPresent = 0;
	size_t len = 0;
	IoT_Error_t rc = FAILURE;

	FUNC_ENTRY;

	if(NULL != pConnectParams) {
		/* override default options if new options were supplied */
		rc = aws_iot_mqtt_set_connect_params(pClient, pConnectParams);
		if(SUCCESS != rc) {
			FUNC_EXIT_RC(MQTT_CONNECTION_ERROR);
		}
	}

	rc = pClient->networkStack.connect(&(pClient->networkStack), NULL);
	if(SUCCESS != rc) {
		/* TLS Connect failed, return error */
		FUNC_EXIT_RC(rc);
	}

	init_timer(&connect_timer);
	countdown_ms(&connect_timer, pClient->clientData.commandTimeoutMs);

	pClient->clientData.keepAliveInterval = pClient->clientData.options.keepAliveIntervalInSec;
	rc = _aws_iot_mqtt_serialize_connect(pClient->clientData.writeBuf, pClient->clientData.writeBufSize,
										 &(pClient->clientData.options), &len);
	if(SUCCESS != rc || 0 >= len) {
		FUNC_EXIT_RC(rc);
	}

	/* send the connect packet */
	rc = aws_iot_mqtt_internal_send_packet(pClient, len, &connect_timer);
	if(SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	/* this will be a blocking call, wait for the CONNACK */
	rc = aws_iot_mqtt_internal_wait_for_read(pClient, CONNACK, &connect_timer);
	if(SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	/* Received CONNACK, check the return code */
	rc = _aws_iot_mqtt_deserialize_connack((unsigned char *) &sessionPresent, &connack_rc, pClient->clientData.readBuf,
										   pClient->clientData.readBufSize);
	if(SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	if(MQTT_CONNACK_CONNECTION_ACCEPTED != connack_rc) {
		FUNC_EXIT_RC(connack_rc);
	}

	pClient->clientStatus.isPingOutstanding = false;
	countdown_sec(&pClient->pingTimer, pClient->clientData.keepAliveInterval);

	FUNC_EXIT_RC(SUCCESS);
}
/**
 * @brief Subscribe to an MQTT topic.
 *
 * Called to send a subscribe message to the broker requesting a subscription
 * to an MQTT topic. This is the internal function which is called by the
 * subscribe API to perform the operation. Not meant to be called directly as
 * it doesn't do validations or client state changes
 * @note Call is blocking.  The call returns after the receipt of the SUBACK control packet.
 *
 * @param pClient Reference to the IoT Client
 * @param pTopicName Topic Name to publish to
 * @param topicNameLen Length of the topic name
 * @param pApplicationHandler_t Reference to the handler function for this subscription
 *
 * @return An IoT Error Type defining successful/failed subscription
 */
static IoT_Error_t _aws_iot_mqtt_internal_subscribe(AWS_IoT_Client *pClient, const char *pTopicName,
													uint16_t topicNameLen, QoS qos,
													pApplicationHandler_t pApplicationHandler,
													void *pApplicationHandlerData) {
	uint16_t txPacketId, rxPacketId;
	uint32_t serializedLen, indexOfFreeMessageHandler, count;
	IoT_Error_t rc;
	Timer timer;
	QoS grantedQoS[3] = {QOS0, QOS0, QOS0};

	FUNC_ENTRY;
	init_timer(&timer);
	countdown_ms(&timer, pClient->clientData.commandTimeoutMs);

	serializedLen = 0;
	count = 0;
	txPacketId = aws_iot_mqtt_get_next_packet_id(pClient);
	rxPacketId = 0;

	rc = _aws_iot_mqtt_serialize_subscribe(pClient->clientData.writeBuf, pClient->clientData.writeBufSize, 0,
										   txPacketId, 1, &pTopicName, &topicNameLen, &qos, &serializedLen);
	if(AWS_SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	indexOfFreeMessageHandler = _aws_iot_mqtt_get_free_message_handler_index(pClient);
	if(AWS_IOT_MQTT_NUM_SUBSCRIBE_HANDLERS <= indexOfFreeMessageHandler) {
		FUNC_EXIT_RC(MQTT_MAX_SUBSCRIPTIONS_REACHED_ERROR);
	}

	/* send the subscribe packet */
	rc = aws_iot_mqtt_internal_send_packet(pClient, serializedLen, &timer);
	if(AWS_SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	/* wait for suback */
	rc = aws_iot_mqtt_internal_wait_for_read(pClient, SUBACK, &timer);
	if(AWS_SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	/* Granted QoS can be 0, 1 or 2 */
	rc = _aws_iot_mqtt_deserialize_suback(&rxPacketId, 1, &count, grantedQoS, pClient->clientData.readBuf,
										  pClient->clientData.readBufSize);
	if(AWS_SUCCESS != rc) {
		FUNC_EXIT_RC(rc);
	}

	/* TODO : Figure out how to test this before activating this check */
	//if(txPacketId != rxPacketId) {
	/* Different SUBACK received than expected. Return error
	 * This can cause issues if the request timeout value is too small */
	//	return RX_MESSAGE_INVALID_ERROR;
	//}

	pClient->clientData.messageHandlers[indexOfFreeMessageHandler].topicName =
			pTopicName;
	pClient->clientData.messageHandlers[indexOfFreeMessageHandler].topicNameLen =
			topicNameLen;
	pClient->clientData.messageHandlers[indexOfFreeMessageHandler].pApplicationHandler =
			pApplicationHandler;
	pClient->clientData.messageHandlers[indexOfFreeMessageHandler].pApplicationHandlerData =
			pApplicationHandlerData;
	pClient->clientData.messageHandlers[indexOfFreeMessageHandler].qos = qos;

	FUNC_EXIT_RC(AWS_SUCCESS);
}