/**
 * @file mcdmex.mex.cpp
 * @author Peter Fiala
 * @brief A mex interface for the mcd device
 * @version 0.1
 * @date 2024-02-13
 */

#include "mex.h"
#include "matrix.h"
#include "fifo.hpp"
#include "mcd_device.h"

#include <cstring>

boost::asio::io_service io_service;
mcd_device *p_mcd_device = nullptr;
fifo<sample_t> data_fifo;

/**
 * @brief Connect to the MCDrec device and open a session
 * 
 * @param host_ip IP address of the MCD device (10.0.0.4)
 * @param input_indices vector of 0-based indices of microphone channels
 * @param fifo_depth depth of the internal buffer
 */
void open(char const *host_ip, std::vector<size_t> const &input_indices, size_t fifo_depth, mcd_control_packet::chunk_size chunk_size)
{
	size_t num_channels = input_indices.size();

	if (p_mcd_device != nullptr)
		delete p_mcd_device;

	// initialize fifo
	data_fifo.resize(fifo_depth, num_channels);

	// initialize mcd device
	p_mcd_device = new mcd_device(io_service, host_ip, &data_fifo);
	p_mcd_device->set_input_indices(input_indices);
	p_mcd_device->set_chunk_size(chunk_size);
	p_mcd_device->connect();
	p_mcd_device->open_stream();
}

/**
 * @brief Start data acquisition
 * 
 */
void start()
{
	if (p_mcd_device == nullptr)
		throw std::runtime_error("You tried to start the MCD device without opening");
	p_mcd_device->start();
}

/**
 * @brief Stop data acquisition
 * 
 */
void stop()
{
	if (p_mcd_device == nullptr)
		throw std::runtime_error("You tried to stop the MCD device without opening and starting");
	p_mcd_device->stop();
}

/**
 * @brief Close connection with the MCD device
 * 
 */
void close()
{
	if (p_mcd_device == nullptr)
		throw std::runtime_error("You tried to close the MCD device without opening");

	p_mcd_device->close_stream();
	p_mcd_device->disconnect();
	delete p_mcd_device;
	p_mcd_device = nullptr;
}

/**
 * @brief Read data from the device buffer
 * 
 * @param num_frames number of frames to read
 * @return mxArray* pointer to the newly allocated Matlab matrix containing the data
 */
mxArray *read(size_t num_frames)
{
	size_t num_channels = data_fifo.get_num_channels();
	mxArray *matrix = mxCreateNumericMatrix(num_channels, num_frames, mxSINGLE_CLASS, mxREAL);
	// wait until enough data in FIFO
	while (data_fifo.get_num_used_frames() < num_frames)
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	// read and consume
	data_fifo.reserve();
	data_fifo.read(num_frames, mxGetSingles(matrix));
	data_fifo.pop(num_frames);
	data_fifo.release();

	return matrix;
}

/**
 * @brief The Matlab mex gateway function
 * 
 * @param nlhs number of lhs arguments
 * @param plhs pointer to lhs arguments
 * @param nrhs number of rhs arguments
 * @param prhs pointer to rhs arguments
 */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, mxArray const *prhs[])
{
	char const *usage_string = "Usage: mcdmex(command, ...),\n where command is 'open', 'close', 'start', 'stop', 'read'\n";
	if (nrhs < 1 || !mxIsChar(prhs[0]))
	{
		mexPrintf(usage_string);
		return;
	}

	// read first argument
	char command[100];
	mxGetString(prhs[0], command, 100);
	if (std::strcmp(command, "open") == 0)
	{
		if (nrhs < 5)
		{
			mexPrintf("Usage: mcdmex('open', ipaddr, input_indices, fifo_depth, chunk_size)\n");
			return;
		}
		char host_ip[100];
		mxGetString(prhs[1], host_ip, 100);
		std::vector<size_t> input_indices(mxGetNumberOfElements(prhs[2]));
		for (size_t i = 0; i < input_indices.size(); ++i)
			input_indices[i] = mxGetPr(prhs[2])[i];
		size_t fifo_depth = mxGetScalar(prhs[3]);
		unsigned cs = mxGetScalar(prhs[4]);
		mcd_control_packet::chunk_size chunk_size = mcd_control_packet::CHUNK_128MS;
		if (cs == 128)
			chunk_size = mcd_control_packet::CHUNK_128MS;
		else if (cs == 64)
			chunk_size = mcd_control_packet::CHUNK_64MS;
		else if (cs == 32)
			chunk_size = mcd_control_packet::CHUNK_32MS;
		else if (cs == 16)
			chunk_size = mcd_control_packet::CHUNK_16MS;
		else if (cs == 8)
			chunk_size = mcd_control_packet::CHUNK_8MS;
		else
		{
			mexPrintf("Invalid chunk_size: %d. Possible values are 128, 64, 32, 16, 8\n");
			return;
		}
		open(host_ip, input_indices, fifo_depth, chunk_size);
	}
	else if (std::strcmp(command, "start") == 0)
		start();
	else if (std::strcmp(command, "stop") == 0)
		stop();
	else if (std::strcmp(command, "close") == 0)
		close();
	else if (std::strcmp(command, "read") == 0)
	{
		if (nlhs < 1 || nrhs < 2)
		{
			mexPrintf("Usage: data = mcdmex('read', num_frames)\n");
			return;
		}
		size_t num_frames = mxGetScalar(prhs[1]);
		plhs[0] = read(num_frames);
	}
	else if (std::strcmp(command, "status") == 0)
	{
		if (nlhs < 1)
		{
			mexPrintf("Usage: status = mcdmex('status')\n");
			return;
		}

		plhs[0] = mxCreateDoubleMatrix(2, 1, mxREAL);
		mxGetPr(plhs[0])[0] = data_fifo.get_num_used_frames();
		mxGetPr(plhs[0])[1] = data_fifo.get_num_free_frames();
	}
	else
		mexPrintf(usage_string);
}
