/**
 * Copyright (c) 2022 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

/**
 * Lab 5
 *
 * Done on: pico-examples/pico_w/wifi/freertos/ping/picow_freertos_ping.c
 */

#include <stdio.h>
#include "pico/stdlib.h"

#include "FreeRTOS.h"
#include "task.h"
#include "message_buffer.h"

#include "hardware/adc.h"
#include <queue.h>

#define mbaTASK_MESSAGE_BUFFER_SIZE             ( 60 )

#ifndef PING_ADDR
#define PING_ADDR "142.251.35.196"
#endif
#ifndef RUN_FREERTOS_ON_CORE
#define RUN_FREERTOS_ON_CORE 0
#endif

#define BUFFER_SIZE                              ( 10 )  // 10 data points
#define QUEUE_SIZE                               ( 3  )  // 3 Tasks to print

#define TASK_ONE_PRIORITY                       ( tskIDLE_PRIORITY + 1UL )
#define TASK_TWO_PRIORITY                       ( tskIDLE_PRIORITY + 2UL )
#define TASK_THREE_PRIORITY                     ( tskIDLE_PRIORITY + 3UL )
#define TASK_FOUR_PRIORITY                      ( tskIDLE_PRIORITY + 4UL )

#define TEMP_READING_DELAY_MS                   ( 1000 ) // 1 second

static MessageBufferHandle_t TaskTwoMsgBuffer;  // Task 2 Buffer
static QueueHandle_t TaskThreeQueue;            // Task 3 Queue
static QueueHandle_t printQueue;                // Task 4 Queue

static float read_onboard_temperature () {

    /* 12-bit conversion, assume max value == ADC_VREF == 3.3 V */
    const float conversionFactor = 3.3f / (1 << 12);

    float adc = (float) adc_read() * conversionFactor;
    float tempC = 27.0f - (adc - 0.706f) / 0.001721f;

    return tempC;
}

/**
 * Task 1: Read temperature data from Sensor
 * @param params Pointer to the task parameters
 */
static void temp_sensor_task (__unused void *params) {
    const TickType_t xBlockTime = pdMS_TO_TICKS(TEMP_READING_DELAY_MS);

    static float temperature = 0.0f;
    static char text_to_print[mbaTASK_MESSAGE_BUFFER_SIZE];

    adc_init();
    adc_set_temp_sensor_enabled(true);
    adc_select_input(4);

    while (true)
    {
        vTaskDelay(xBlockTime);
        temperature = read_onboard_temperature();

        // Print the temperature first
        snprintf(text_to_print,
                 sizeof(text_to_print),
                 "Task 1: Onboard temperature = %.02f C\n",
                 temperature);

        xQueueSend(printQueue, &text_to_print, 0);

        // Send to Task 2
        xMessageBufferSend(TaskTwoMsgBuffer,
                           (void *) &temperature,
                           sizeof(temperature),
                           0);

        // Send to Task 3
        xQueueSend(TaskThreeQueue, &temperature, 0);


    }
}


/**
 * Task 2: Calculate moving average of 10 data points
 *
 * @param params  Pointer to the task parameters
 */
static void avg_task (__unused void *params) {
    static float fReceivedData;
    static float sum = 0.0f;
    static float data[BUFFER_SIZE] = {0};

    static int index = 0;
    static int count = 0;

    static char text_to_print[mbaTASK_MESSAGE_BUFFER_SIZE];

    while (true)
    {
        xMessageBufferReceive(TaskTwoMsgBuffer,
                              (void *) &fReceivedData,
                              sizeof(fReceivedData),
                              portMAX_DELAY);

        // Circular buffer

        sum -= data[index];
        data[index] = fReceivedData;
        sum += data[index];
        index = (index + 1) % BUFFER_SIZE;

        if (count < BUFFER_SIZE)
        {
            count ++;
        }

        snprintf(text_to_print,
                 sizeof(text_to_print),
                 "Task 2: Moving Average Temperature = %0.2f C\n",
                 sum / count);

        xQueueSend(printQueue, &text_to_print, 0);

    }
}

/**
 * Task 3: Calculate simple average of ALL data points
 *
 * @param params  Pointer to the task parameters
 */
static void avg_task_simple (__unused void *params) {
    static float fReceivedData;
    static float sum = 0.0f;

    static float count = 0.0f;

    static char text_to_print[mbaTASK_MESSAGE_BUFFER_SIZE];


    while (true)
    {
        xQueueReceive(TaskThreeQueue,
                      &fReceivedData,
                      portMAX_DELAY);

        sum += fReceivedData;
        count ++;

        snprintf(text_to_print,
                 sizeof(text_to_print),
                 "Task 3: Simple Average Temperature = %0.2f C\n",
                 sum / count);

        xQueueSend(printQueue, &text_to_print, 0);
    }
}

/**
 * Task 4: To print all the printf statements from other tasks
 *
 * @param params  Pointer to the task parameters
 */
static void print_task (__unused void *params) {
    static char text_to_print[mbaTASK_MESSAGE_BUFFER_SIZE];

    while (true)
    {
        // Receive from Queue
        xQueueReceive(printQueue,
                      &text_to_print,
                      portMAX_DELAY);

        printf("%s", text_to_print);
    }
}


void vLaunch (void) {

    // Task 1: Read temperature data from Sensor
    TaskHandle_t task_1;
    xTaskCreate(
            temp_sensor_task,
            "TempSensorTask",
            configMINIMAL_STACK_SIZE,
            NULL,
            TASK_ONE_PRIORITY,
            &task_1
    );

    // Task 2: Calculate moving average
    TaskHandle_t task_2;
    xTaskCreate(
            avg_task,
            "AvgTask",
            configMINIMAL_STACK_SIZE,
            NULL,
            TASK_TWO_PRIORITY,
            &task_2
    );

    // Task 3: Calculate simple average
    TaskHandle_t task_3;
    xTaskCreate(
            avg_task_simple,
            "AvgTaskSimple",
            configMINIMAL_STACK_SIZE,
            NULL,
            TASK_THREE_PRIORITY,
            &task_3
    );

    // Task 4: Print all the printf statements from other tasks
    TaskHandle_t task_4;
    xTaskCreate(
            print_task,
            "PrintTask",
            configMINIMAL_STACK_SIZE,
            NULL,
            TASK_FOUR_PRIORITY,
            &task_4
    );

    TaskTwoMsgBuffer = xMessageBufferCreate(mbaTASK_MESSAGE_BUFFER_SIZE);
    TaskThreeQueue = xQueueCreate(10, sizeof(float));
    printQueue = xQueueCreate(QUEUE_SIZE, sizeof(char[60]));


#if NO_SYS && configUSE_CORE_AFFINITY && configNUM_CORES > 1
    // we must bind the main task to one core (well at least while the init is called)
    // (note we only do this in NO_SYS mode, because cyw43_arch_freertos
    // takes care of it otherwise)
    vTaskCoreAffinitySet(task, 1);
#endif

    /* Start the tasks and timer running. */
    vTaskStartScheduler();
}

int main (void) {
    stdio_init_all();

    /* Configure the hardware ready to run the demo. */
    const char *rtos_name;
#if (portSUPPORT_SMP == 1)
    rtos_name = "FreeRTOS SMP";
#else
    rtos_name = "FreeRTOS";
#endif

#if (portSUPPORT_SMP == 1) && (configNUM_CORES == 2)
    printf("Starting %s on both cores:\n", rtos_name);
    vLaunch();
#elif (RUN_FREERTOS_ON_CORE == 1)
    printf("Starting %s on core 1:\n", rtos_name);
    multicore_launch_core1(vLaunch);
    while (true);
#else
    printf("Starting %s on core 0:\n", rtos_name);
    vLaunch();
#endif
    return 0;
}