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

#include <satlab/polaris_adsb_store.h>

#include <errno.h>

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

#include <satlab/polaris_adsb.h>

static inline size_t array_weight(const uint8_t *array, size_t length)
{
	size_t i, c;
	uint8_t v;
	int weight = 0;

	for (i = 0; i < length; i++) {
		v = array[i];
		for (c = 0; v; c++)
			v &= v - 1;
		weight += c;
	}

	return weight;
}

int sl_polaris_adsb_store_read(uint8_t node, uint32_t timeout, uint32_t start, uint8_t *bitmap, sl_polaris_adsb_frame_cb_t cb, void *arg)
{
	int i, count;
	sl_polaris_adsb_store_read_req_t *request;
	sl_polaris_adsb_store_frame_t *frame;
	csp_packet_t *packet;
	csp_conn_t *conn;

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

	request = (void *) packet->data;
	request->type = SL_POLARIS_ADSB_STORE_READ_REQ;

	request->start = csp_hton32(start);
	memcpy(request->bitmap, bitmap, sizeof(request->bitmap));

	packet->length = sizeof(*request);

	/* Calculated expected reply frames */
	count = array_weight(bitmap, sizeof(request->bitmap));

	/* Send request */
	conn = csp_connect(CSP_PRIO_NORM, node, SL_POLARIS_ADSB_PORT_STORE, 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 frames */
	for (i = 0; i < count; i++) {
		packet = csp_read(conn, timeout);
		if (!packet) {
			csp_close(conn);
			return -ETIMEDOUT;
		}

		frame = (void *) packet->data;

		if (packet->length != sizeof(*frame) ||
		    frame->type != SL_POLARIS_ADSB_STORE_FRAME ||
		    frame->error != SL_POLARIS_ADSB_ERR_NONE) {
			csp_buffer_free(packet);
			csp_close(conn);
			return -EINVAL;
		}

		frame->seq = csp_ntoh32(frame->seq);
		frame->time = csp_ntoh64(frame->time);
		frame->rssi = csp_ntoh16(frame->rssi);
		frame->flags = csp_ntoh16(frame->flags);

		cb(frame, arg);
		csp_buffer_free(packet);
	}

	csp_close(conn);

	return 0;
}
