#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>
#include "max86150.h"
#include "epicardium.h"
#include "modules.h"
#include "modules/log.h"
#include "modules/stream.h"
#include "gpio.h"
#include "pmic.h"

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

#include "api/interrupt-sender.h"
#include "modules/modules.h"

static const gpio_cfg_t max86150_interrupt_pin = {
	PORT_1, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP
};

/* MAX86150 Task ID */
static TaskHandle_t max86150_task_id = NULL;

/* MAX86150 Mutex */
static struct mutex max86150_mutex = { 0 };

/* Stream */
static struct stream_info max86150_stream;

/* Active */
static bool max86150_sensor_active = false;

int epic_max86150_enable_sensor(
	struct max86150_sensor_config *config, size_t config_size
) {
	int result = 0;

	// if (sizeof(struct max86150_sensor_config) != config_size) {
	// 	return -EINVAL;
	// }

	mutex_lock(&max86150_mutex);
	hwlock_acquire(HWLOCK_I2C);

	struct stream_info *stream = &max86150_stream;
	stream->item_size          = sizeof(struct max86150_sensor_data);
	stream->queue =
		xQueueCreate(config->sample_buffer_len, stream->item_size);
	if (stream->queue == NULL) {
		result = -ENOMEM;
		goto out_free;
	}

	uint8_t ppg_sample_rate;

	if (config->ppg_sample_rate == 10) {
		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_10;
	} else if (config->ppg_sample_rate == 20) {
		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_20;
	} else if (config->ppg_sample_rate == 50) {
		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_50;
	} else if (config->ppg_sample_rate == 84) {
		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_84;
	} else if (config->ppg_sample_rate == 100) {
		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_100;
	} else if (config->ppg_sample_rate == 200) {
		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_200;
	} else {
		result = -EINVAL;
		goto out_free;
	}

	result = stream_register(SD_MAX86150, stream);
	if (result < 0) {
		vQueueDelete(stream->queue);
		goto out_free;
	}

	bool begin_result = max86150_begin();
	if (!begin_result) {
		result = -ENODEV;
		vQueueDelete(stream->queue);
		goto out_free;
	}

	max86150_setup(ppg_sample_rate);
	max86150_get_int1();
	max86150_get_int2();

	max86150_sensor_active = true;
	result                 = SD_MAX86150;

out_free:
	hwlock_release(HWLOCK_I2C);
	mutex_unlock(&max86150_mutex);
	return result;
}

int epic_max86150_disable_sensor(void)
{
	int result = 0;

	mutex_lock(&max86150_mutex);
	hwlock_acquire(HWLOCK_I2C);

	struct stream_info *stream = &max86150_stream;
	result                     = stream_deregister(SD_MAX86150, stream);
	if (result < 0) {
		goto out_free;
	}

	vQueueDelete(stream->queue);
	stream->queue = NULL;

	// disable max86150 leds
	max86150_set_led_red_amplitude(0);
	max86150_set_led_ir_amplitude(0);

	max86150_sensor_active = false;

	result = 0;
out_free:
	hwlock_release(HWLOCK_I2C);
	mutex_unlock(&max86150_mutex);
	return result;
}

static int max86150_handle_sample(struct max86150_sensor_data *data)
{
	//LOG_INFO("max86150", "Sample! %ld, %ld, %ld", data->red, data->ir, data->ecg);

	if (max86150_stream.queue == NULL) {
		return -ESRCH;
	}

	/* Discard overflow.  See discussion in !316. */
	if (xQueueSend(max86150_stream.queue, data, 0) != pdTRUE) {
		LOG_WARN("max86150", "queue full");
		return -EIO;
	}
	return api_interrupt_trigger(EPIC_INT_MAX86150);
}

static int max86150_fetch_fifo(void)
{
	int result = 0;

	mutex_lock(&max86150_mutex);
	hwlock_acquire(HWLOCK_I2C);

	struct max86150_sensor_data sample;

	// There is a recommendation from Maxim not to read the entire FIFO, but rather a fixed number of samples.
	// See https://os.mbed.com/users/laserdad/code/MAX86150_ECG_PPG//file/3c728f3d1f10/main.cpp/
	// So we should not use max86150_check() but max86150_get_sample().
	while (max86150_get_sample(&sample.red, &sample.ir, &sample.ecg) > 0) {
		result = max86150_handle_sample(&sample);
		// stop in case of errors
		if (result < 0) {
			break;
		}
	}
	hwlock_release(HWLOCK_I2C);
	mutex_unlock(&max86150_mutex);
	return result;
}

/*
 * Callback for the MAX86150 interrupt pin.  This callback is called from the
 * SDK's GPIO interrupt driver, in interrupt context.
 */
static void max86150_interrupt_callback(void *_)
{
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;

	if (max86150_task_id != NULL) {
		vTaskNotifyGiveFromISR(
			max86150_task_id, &xHigherPriorityTaskWoken
		);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
}
/* }}} */

void max86150_mutex_init(void)
{
	mutex_create(&max86150_mutex);
}

void vMAX86150Task(void *pvParameters)
{
	max86150_task_id = xTaskGetCurrentTaskHandle();

	mutex_lock(&max86150_mutex);
	hwlock_acquire(HWLOCK_I2C);

	/* Install interrupt callback */
	GPIO_Config(&max86150_interrupt_pin);
	GPIO_RegisterCallback(
		&max86150_interrupt_pin, max86150_interrupt_callback, NULL
	);
	GPIO_IntConfig(
		&max86150_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_FALLING
	);
	GPIO_IntEnable(&max86150_interrupt_pin);
	NVIC_SetPriority(
		(IRQn_Type)MXC_GPIO_GET_IRQ(max86150_interrupt_pin.port), 2
	);
	NVIC_EnableIRQ(
		(IRQn_Type)MXC_GPIO_GET_IRQ(max86150_interrupt_pin.port)
	);

	hwlock_release(HWLOCK_I2C);
	mutex_unlock(&max86150_mutex);

	/* ----------------------------------------- */

	while (1) {
		if (max86150_sensor_active) {
			//LOG_INFO("max86150", "Interrupt!");
			int ret = max86150_fetch_fifo();
			if (ret < 0) {
				LOG_ERR("max86150", "Unknown error: %d", -ret);
			}
		}
		/*
		 * Wait for interrupt.  After two seconds, fetch FIFO anyway
		 *
		 * In the future, reads using epic_stream_read() might also
		 * trigger a FIFO fetch, from outside this task.
		 */
		ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(2000));
	}
}