#include "compute_distance.h"

void read_conn_rssi(uint16_t handle, int8_t *rssi)
{
	struct net_buf *buf, *rsp = NULL;
	struct bt_hci_cp_read_rssi *cp;
	struct bt_hci_rp_read_rssi *rp;

	int err;

	buf = bt_hci_cmd_create(BT_HCI_OP_READ_RSSI, sizeof(*cp));
	if (!buf) {
		printk("Unable to allocate command buffer\n");
		return;
	}

	cp = net_buf_add(buf, sizeof(*cp));
	cp->handle = sys_cpu_to_le16(handle);

	err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
	if (err) {
		uint8_t reason = rsp ?
			((struct bt_hci_rp_read_rssi *)rsp->data)->status : 0;
		printk("Read RSSI err: %d reason 0x%02x\n", err, reason);
		return;
	}

	rp = (void *)rsp->data;
	*rssi = rp->rssi;

	net_buf_unref(rsp);
}

double compute_distance(int rssi)	{
	double MeasPower = -55, N = 3; //N = 3 for a semi-noisy environment 

	return pow(10, ((MeasPower-rssi)/(10*N)));
}

float calculateMean(float arr[], int size)  {
    float sum = 0;
    for (int i = 0; i < size; i++) {
        sum += arr[i];
    }
    return sum / size;
}

double get_distance_rssi(struct bt_conn *conn_snapshot)
{
	int8_t rssi = 0xFF;
	uint16_t conn_handle;
	float dist[NUM_MEASUREMENTS], dist_mean;
	int valid_measurements = 0;

	if ((!conn_snapshot)) {
        printk("No active connection\n");
        return -1.0;
    }

	printk("Check RSSI\n");

	int rc = bt_hci_get_conn_handle(conn_snapshot, &conn_handle);
	if(rc)
	{
		printk("fail getting conn handle");
	}

	k_msleep(200);  // Delay before the 1st measure

	for (int i=0; i<NUM_MEASUREMENTS; i++)	{
		read_conn_rssi(conn_handle, &rssi);

		printk("RSSI in connection: %i\n", rssi);

		if(rssi > 0)	{
			valid_measurements = 0;
			break;
		}
		else	{
			dist[valid_measurements++] = compute_distance(rssi);
			printk("Measured distance %d : %.2f m \n", i+1, (double)dist[i]);
		}

		k_msleep(200);	// Delay between each measure
	}
	
	if (valid_measurements == 0)	{
		printk("Distance measurement failed\n");
		dist_mean = 0;
	}
	else	{
		dist_mean = calculateMean(dist, valid_measurements);
		printk("Mean distance from %d samples : %.2f m \n", valid_measurements, (double)dist_mean);
		valid_measurements = 0;
	}
	return dist_mean;	
}

bool is_device_nearby(struct bt_conn *conn_snapshot)
{
    double distance = get_distance_rssi(conn_snapshot);
    return (distance > 0 && distance < SEUIL_DISTANCE);
}