#include "system.h"
#include "esp_log.h"

static SystemState_t _systemState;

static EventGroupHandle_t _systemEvent;
static EventManager_t _eventManager;

void system_init(void)
{
    _systemState.zeroAngle = 0.0f;
    _systemState.primaryAxis = PRIMARY_AXIS;

	EventGroupHandle_t evt = xEventGroupCreate();

	_eventManager.count = 0;
    _eventManager.mutex = xSemaphoreCreateMutex();
}

int system_getPrimaryAxis(void)
{
    int axis;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    axis = _systemState.primaryAxis;
    xSemaphoreGive(_eventManager.mutex);

    return axis;
}

float system_getAngle(void)
{
    float angle;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

    
    angle = _systemState.imu.raw[_systemState.primaryAxis] - _systemState.zeroAngle;


	xSemaphoreGive(_eventManager.mutex);

    return angle;    
}

void system_setZeroAngle(void)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

    
	_systemState.zeroAngle = _systemState.imu.raw[_systemState.primaryAxis];

    ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);

	xSemaphoreGive(_eventManager.mutex);
}

void system_clearZeroAngle(void)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

	_systemState.zeroAngle = 0.0f;

    ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);

	xSemaphoreGive(_eventManager.mutex);
}

float system_getZeroAngle(void)
{
    float angle;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

	angle = _systemState.zeroAngle;

	xSemaphoreGive(_eventManager.mutex);

    return angle;
}

void system_setImuData(ImuData_t imu)
{
	xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

	_systemState.imu = imu;

	xSemaphoreGive(_eventManager.mutex);

    //ESP_LOGI("g", "New IMU Data: %.2f", xy);

	system_notifyAll(EM_EVENT_NEW_DATA);
}

ImuData_t system_getImuData(void)
{
	ImuData_t imu;
	xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

    imu = _systemState.imu;

	xSemaphoreGive(_eventManager.mutex);

	return imu;
}

void system_waitForEvent(void)
{
}

SystemState_t *system_getState(void)
{
	return &_systemState;
}


BaseType_t system_subscribe(TaskHandle_t task) {

	EventManager_t *em = &_eventManager;

    ESP_LOGI("g", "Subscribe: %p", task);
    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdFALSE) {
        return pdFAIL;
    }
    if (em->count < EM_MAX_SUBSCRIBERS) {
        // avoid duplicates?
        ESP_LOGI("g", "PASS");
        em->subscribers[em->count++] = task;
        xSemaphoreGive(em->mutex);
        return pdPASS;
    }
    xSemaphoreGive(em->mutex);
    return pdFAIL; // full
}

BaseType_t system_unsubscribe(TaskHandle_t task) {

	EventManager_t *em = &_eventManager;

    BaseType_t removed = pdFAIL;
    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
        for (size_t i = 0; i < em->count; ++i) {
            if (em->subscribers[i] == task) {
                // shift down
                for (size_t j = i; j + 1 < em->count; ++j)
                    em->subscribers[j] = em->subscribers[j+1];
                em->count--;
                removed = pdPASS;
                break;
            }
        }
        xSemaphoreGive(em->mutex);
    }
    return removed;
}

void system_notifyAll(uint32_t eventBits) {
	EventManager_t *em = &_eventManager;
    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
        for (size_t i = 0; i < em->count; ++i) {
            // set the bits in each task's notification value
            //ESP_LOGI("g", "Notify: %p", em->subscribers[i]);
            xTaskNotify(em->subscribers[i],
                        eventBits,
                        eSetBits);
        }
        xSemaphoreGive(em->mutex);
    }
}