/*
 * Copyright (c) 2016-2017 Satlab Aps <satlab@satlab.com>
 * All rights reserved. Do not distribute without permission.
 */

#include <satlab/polaris_sample.h>

#include <errno.h>

#include <csp/csp.h>
#include <csp/csp_endian.h>

#include <satlab/polaris.h>

int sl_polaris_sample_read(uint8_t node, uint32_t timeout, uint8_t source, uint32_t *out, size_t outlen, bool wait_pps)
{
	int expected, i;

	sl_polaris_samplebuf_read_req_t *request;
	sl_polaris_samplebuf_read_rep_t *reply;
	sl_polaris_samplebuf_read_chunk_t *chunk;
	csp_packet_t *packet;
	csp_conn_t *conn;

	packet = csp_buffer_get(sizeof(*request));
	if (!packet)
		return -ENOMEM;

	request = (void *)packet->data;
	request->type = SL_POLARIS_SAMPLEBUF_READ_REQ;
	request->source = source;
	request->dest = SL_POLARIS_SAMPLEBUF_DEST_CSP;
	request->flags = wait_pps ? SL_POLARIS_SAMPLEBUF_FL_SYNC_PPS : 0;
	request->id = csp_hton32(0);
	request->samples = csp_hton32(outlen / sizeof(uint32_t));
	packet->length = sizeof(*request);

	conn = csp_connect(CSP_PRIO_NORM, node, SL_POLARIS_PORT_SAMPLEBUF, timeout, CSP_O_NONE);
	if (!conn) {
		csp_buffer_free(packet);
		return -ECONNREFUSED;
	}

	if (!csp_send(conn, packet, timeout)) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -EIO;
	}

	/* Wait for reply packet */
	packet = csp_read(conn, timeout);
	if (!packet) {
		csp_close(conn);
		return -ETIMEDOUT;
	}

	reply = (void *)packet->data;
	if (reply->type != SL_POLARIS_SAMPLEBUF_READ_REP) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -EINVAL;
	}

	if (reply->error != SL_POLARIS_ERR_NONE) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -reply->error;
	}

	csp_buffer_free(packet);
	reply = NULL;

	/* Expected number of frames */
	expected = outlen / sizeof(uint32_t) / SL_POLARIS_SAMPLEBUF_READ_CHUNK_SAMPLES;

	for (i = 0; i < expected; i++) {
		packet = csp_read(conn, timeout);
		if (!packet) {
			csp_close(conn);
			return -ETIMEDOUT;
		}

		chunk = (void *)packet->data;
		if (chunk->type != SL_POLARIS_SAMPLEBUF_READ_CHUNK) {
			csp_buffer_free(packet);
			csp_close(conn);
			return -EINVAL;
		}

		chunk->seq = csp_ntoh16(chunk->seq);
		chunk->total = csp_ntoh16(chunk->total);
		if (chunk->total != expected || chunk->seq != i) {
			csp_buffer_free(packet);
			csp_close(conn);
			return -EINVAL; 
		}

		memcpy(out, chunk->data, sizeof(chunk->data));
		out += sizeof(chunk->data) / sizeof(uint32_t);
		
		csp_buffer_free(packet);
	}

	csp_close(conn);

	return 0;
}

int sl_polaris_sample_store(uint8_t node, uint32_t timeout, uint8_t source, uint8_t dest, size_t samples, uint32_t id, bool wait_pps)
{
	int ret;
	sl_polaris_samplebuf_read_req_t request;
	sl_polaris_samplebuf_read_rep_t reply;

	request.type = SL_POLARIS_SAMPLEBUF_READ_REQ;
	request.source = source;
	request.dest = dest;
	request.flags = wait_pps ? SL_POLARIS_SAMPLEBUF_FL_SYNC_PPS : 0;
	request.id = csp_hton32(id);
	request.samples = csp_hton32(samples);

	ret = csp_transaction(CSP_PRIO_NORM, node, SL_POLARIS_PORT_SAMPLEBUF, timeout,
			      &request, sizeof(request), &reply, sizeof(reply));
	if (ret != sizeof(reply))
		return -ECONNREFUSED;

	if (reply.type != SL_POLARIS_SAMPLEBUF_READ_REP)
		return -EINVAL;

	if (reply.error != SL_POLARIS_ERR_NONE)
		return -reply.error;

	return 0;
}
