/*
 * meas_tasks.c
 *
 *  Created on: Sep 5, 2024
 *      Author: jakubski
 */

#include "meas_tasks.h"
#include "adc_buffers.h"
#include "measurements.h"
#include "node-red-config.h"
#include "peripherial.h"

#include "cmsis_os.h"
#include "main.h"

#ifdef PV_BOARD
#define VOLTAGES_COUNT 1
#define CURRENTS_COUNT 1
#else
#define VOLTAGES_COUNT 3
#define CURRENTS_COUNT 3
#endif

#define CIRC_BUFF_LEN 10

osThreadId_t adc1MeasTaskHandle      = NULL;
osThreadId_t adc2MeasTaskHandle      = NULL;
osThreadId_t adc3MeasTaskHandle      = NULL;
osThreadId_t limiterSwitchTaskHandle = NULL;
osThreadId_t encoderTaskHandle      = NULL;

osMessageQueueId_t adc1MeasDataQueue      = NULL;
osMessageQueueId_t adc2MeasDataQueue      = NULL;
osMessageQueueId_t adc3MeasDataQueue      = NULL;
osMessageQueueId_t limiterSwitchDataQueue = NULL;
osMessageQueueId_t encoderDataQueue      = NULL;

osMutexId_t vRefmVMutex;
osMutexId_t resMeasurementsMutex;
osMutexId_t sensorsInfoMutex;
osMutexId_t ILxRefMutex;

volatile uint32_t vRefmV = 3000;

RESMeasurements resMeasurements = { 0 };
SesnorsInfo sensorsInfo         = { 0 };

uint16_t ILxRef[CURRENTS_COUNT] = { 0 };

extern TIM_HandleTypeDef htim3;
extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
extern osTimerId_t motorXTimerHandle;
extern osTimerId_t motorYTimerHandle;


void MeasTasksInit (void) {
    vRefmVMutex          = osMutexNew (NULL);
    resMeasurementsMutex = osMutexNew (NULL);
    sensorsInfoMutex     = osMutexNew (NULL);
    ILxRefMutex          = osMutexNew (NULL);
    adc1MeasDataQueue    = osMessageQueueNew (8, sizeof (ADC1_Data), NULL);
    adc2MeasDataQueue    = osMessageQueueNew (8, sizeof (ADC2_Data), NULL);
    adc3MeasDataQueue    = osMessageQueueNew (8, sizeof (ADC3_Data), NULL);

    osThreadAttr_t osThreadAttradc1MeasTask = { 0 };
    osThreadAttr_t osThreadAttradc2MeasTask = { 0 };
    osThreadAttr_t osThreadAttradc3MeasTask = { 0 };

    osThreadAttradc1MeasTask.stack_size = configMINIMAL_STACK_SIZE * 2;
    osThreadAttradc1MeasTask.priority   = (osPriority_t)osPriorityRealtime;

    osThreadAttradc2MeasTask.stack_size = configMINIMAL_STACK_SIZE * 2;
    osThreadAttradc2MeasTask.priority   = (osPriority_t)osPriorityRealtime;

    osThreadAttradc3MeasTask.stack_size = configMINIMAL_STACK_SIZE * 2;
    osThreadAttradc3MeasTask.priority   = (osPriority_t)osPriorityNormal;

    adc1MeasTaskHandle = osThreadNew (ADC1MeasTask, NULL, &osThreadAttradc1MeasTask);
    adc2MeasTaskHandle = osThreadNew (ADC2MeasTask, NULL, &osThreadAttradc2MeasTask);
    adc3MeasTaskHandle = osThreadNew (ADC3MeasTask, NULL, &osThreadAttradc3MeasTask);

    limiterSwitchDataQueue = osMessageQueueNew (8, sizeof (LimiterSwitchData), NULL);

    osThreadAttr_t osThreadAttradc1LimiterSwitchTask = { 0 };
    osThreadAttradc1LimiterSwitchTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
    osThreadAttradc1LimiterSwitchTask.priority       = (osPriority_t)osPriorityNormal;
    limiterSwitchTaskHandle                          = osThreadNew (LimiterSwitchTask, NULL, &osThreadAttradc1LimiterSwitchTask);

    encoderDataQueue = osMessageQueueNew (16, sizeof (EncoderData), NULL);

    osThreadAttr_t osThreadAttrEncoderTask = { 0 };
    osThreadAttrEncoderTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrEncoderTask.priority       = (osPriority_t)osPriorityNormal;
    encoderTaskHandle                      = osThreadNew (EncoderTask, encoderDataQueue, &osThreadAttrEncoderTask);
}

void ADC1MeasTask (void* arg) {
    float circBuffer[VOLTAGES_COUNT][CIRC_BUFF_LEN] = { 0 };
    float rms[VOLTAGES_COUNT]                       = { 0 };
    ;
    ADC1_Data adcData    = { 0 };
    uint32_t circBuffPos = 0;
    float gainCorrection = 1.0;
    while (pdTRUE) {
        osMessageQueueGet (adc1MeasDataQueue, &adcData, 0, osWaitForever);
#ifdef GAIN_AUTO_CORRECTION
        if (osMutexAcquire (vRefmVMutex, osWaitForever) == osOK) {
            gainCorrection = (float)vRefmV;
            osMutexRelease (vRefmVMutex);
        }
        gainCorrection = gainCorrection / EXT_VREF_mV;
#endif
        for (uint8_t i = 0; i < VOLTAGES_COUNT; i++) {
            float val                  = adcData.adcDataBuffer[i] * deltaADC * U_CHANNEL_CONST * gainCorrection * U_MeasCorrectionData[i].gain + U_MeasCorrectionData[i].offset;
            circBuffer[i][circBuffPos] = val;
            rms[i]                     = 0.0;
            for (uint8_t c = 0; c < CIRC_BUFF_LEN; c++) {
                rms[i] += circBuffer[i][c];
            }
            rms[i] = rms[i] / CIRC_BUFF_LEN;
            if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
                if (fabs (resMeasurements.voltagePeak[i]) < fabs (val)) {
                    resMeasurements.voltagePeak[i] = val;
                }
                resMeasurements.voltageRMS[i] = rms[i];
                resMeasurements.power[i]      = resMeasurements.voltageRMS[i] * resMeasurements.currentRMS[i];
                osMutexRelease (resMeasurementsMutex);
            }
        }
        ++circBuffPos;
        circBuffPos = circBuffPos % CIRC_BUFF_LEN;

        if (osMutexAcquire (ILxRefMutex, osWaitForever) == osOK) {
            uint8_t refIdx = 0;
            for (uint8_t i = (uint8_t)IL1Ref; i <= (uint8_t)IL3Ref; i++) {
                ILxRef[refIdx++] = adcData.adcDataBuffer[i];
            }
            osMutexRelease (ILxRefMutex);
        }

        float fanFBVoltage = adcData.adcDataBuffer[FanFB] * deltaADC * -4.35 + 12;
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            sensorsInfo.fanVoltage = fanFBVoltage;
            osMutexRelease (sensorsInfoMutex);
        }
    }
}

void ADC2MeasTask (void* arg) {
    float circBuffer[CURRENTS_COUNT][CIRC_BUFF_LEN] = { 0 };
    float rms[CURRENTS_COUNT]                       = { 0 };
    ADC2_Data adcData                               = { 0 };
    uint32_t circBuffPos                            = 0;
    float gainCorrection                            = 1.0;
    while (pdTRUE) {
        osMessageQueueGet (adc2MeasDataQueue, &adcData, 0, osWaitForever);
        if (osMutexAcquire (vRefmVMutex, osWaitForever) == osOK) {
            gainCorrection = (float)vRefmV;
            osMutexRelease (vRefmVMutex);
        }
        gainCorrection            = gainCorrection / EXT_VREF_mV;
        float ref[CURRENTS_COUNT] = { 0 };
        if (osMutexAcquire (ILxRefMutex, osWaitForever) == osOK) {
            for (uint8_t i = 0; i < CURRENTS_COUNT; i++) {
                ref[i] = (float)ILxRef[i];
            }
            osMutexRelease (ILxRefMutex);
        }
        for (uint8_t i = 0; i < CURRENTS_COUNT; i++) {
            float adcVal               = (float)adcData.adcDataBuffer[i];
            float val                  = (adcVal - ref[i]) * deltaADC * I_CHANNEL_CONST * gainCorrection * I_MeasCorrectionData[i].gain + I_MeasCorrectionData[i].offset;
            circBuffer[i][circBuffPos] = val;
            rms[i]                     = 0.0;
            for (uint8_t c = 0; c < CIRC_BUFF_LEN; c++) {
                rms[i] += circBuffer[i][c];
            }
            rms[i] = rms[i] / CIRC_BUFF_LEN;
            if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
                if (resMeasurements.currentPeak[i] < val) {
                    resMeasurements.currentPeak[i] = val;
                }
                resMeasurements.currentRMS[i] = rms[i];
                osMutexRelease (resMeasurementsMutex);
            }
        }
        ++circBuffPos;
        circBuffPos = circBuffPos % CIRC_BUFF_LEN;
    }
}

void ADC3MeasTask (void* arg) {
    float motorXSensCircBuffer[CIRC_BUFF_LEN] = { 0 };
    float motorYSensCircBuffer[CIRC_BUFF_LEN] = { 0 };
    float pvT1CircBuffer[CIRC_BUFF_LEN]       = { 0 };
    float pvT2CircBuffer[CIRC_BUFF_LEN]       = { 0 };
    uint32_t circBuffPos                      = 0;
    ADC3_Data adcData                         = { 0 };
    while (pdTRUE) {
        osMessageQueueGet (adc3MeasDataQueue, &adcData, 0, osWaitForever);
        uint32_t vRef = __LL_ADC_CALC_VREFANALOG_VOLTAGE (adcData.adcDataBuffer[VrefInt], LL_ADC_RESOLUTION_16B);
        if (osMutexAcquire (vRefmVMutex, osWaitForever) == osOK) {
            vRefmV = vRef;
            osMutexRelease (vRefmVMutex);
        }
        float motorXCurrentSense          = adcData.adcDataBuffer[motorXSense] * deltaADC * 10 / 8.33333;
        float motorYCurrentSense          = adcData.adcDataBuffer[motorYSense] * deltaADC * 10 / 8.33333;
        motorXSensCircBuffer[circBuffPos] = motorXCurrentSense;
        motorYSensCircBuffer[circBuffPos] = motorYCurrentSense;
        pvT1CircBuffer[circBuffPos]       = adcData.adcDataBuffer[pvTemp1] * deltaADC * 45.33333333 - 63;
        pvT2CircBuffer[circBuffPos]       = adcData.adcDataBuffer[pvTemp2] * deltaADC * 45.33333333 - 63;
        float motorXAveCurrent            = 0;
        float motorYAveCurrent            = 0;
        float pvT1AveTemp                 = 0;
        float pvT2AveTemp                 = 0;
        for (uint8_t i = 0; i < CIRC_BUFF_LEN; i++) {
            motorXAveCurrent += motorXSensCircBuffer[i];
            motorYAveCurrent += motorYSensCircBuffer[i];
#ifdef PV_BOARD
            pvT1AveTemp += pvT1CircBuffer[i];
            pvT2AveTemp += pvT2CircBuffer[i];
#endif
        }
        motorXAveCurrent /= CIRC_BUFF_LEN;
        motorYAveCurrent /= CIRC_BUFF_LEN;
        pvT1AveTemp /= CIRC_BUFF_LEN;
        pvT2AveTemp /= CIRC_BUFF_LEN;
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            if (sensorsInfo.motorXStatus == 1) {
                sensorsInfo.motorXAveCurrent = motorXAveCurrent;
                if (sensorsInfo.motorXPeakCurrent < motorXCurrentSense) {
                    sensorsInfo.motorXPeakCurrent = motorXCurrentSense;
                }
            }
            if (sensorsInfo.motorYStatus == 1) {
                sensorsInfo.motorYAveCurrent = motorYAveCurrent;
                if (sensorsInfo.motorYPeakCurrent < motorYCurrentSense) {
                    sensorsInfo.motorYPeakCurrent = motorYCurrentSense;
                }
            }
            sensorsInfo.pvTemperature[0] = pvT1AveTemp;
            sensorsInfo.pvTemperature[1] = pvT2AveTemp;
            osMutexRelease (sensorsInfoMutex);
        }
        ++circBuffPos;
        circBuffPos = circBuffPos % CIRC_BUFF_LEN;
    }
}

void LimiterSwitchTask (void* arg) {
    LimiterSwitchData limiterSwitchData = { 0 };
    limiterSwitchData.gpioPin           = GPIO_PIN_8;
    for (uint8_t i = 0; i < 6; i++) {
        limiterSwitchData.pinState = HAL_GPIO_ReadPin (GPIOD, limiterSwitchData.gpioPin);
        osMessageQueuePut (limiterSwitchDataQueue, &limiterSwitchData, 0, 0);
        limiterSwitchData.gpioPin = limiterSwitchData.gpioPin << 1;
    }
    while (pdTRUE) {
        osMessageQueueGet (limiterSwitchDataQueue, &limiterSwitchData, 0, osWaitForever);
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            switch (limiterSwitchData.gpioPin) {
            case GPIO_PIN_8: sensorsInfo.limitYSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 0 : 1; break;
            case GPIO_PIN_9: sensorsInfo.limitYSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 0 : 1; break;
            case GPIO_PIN_10: sensorsInfo.limitXSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 0 : 1; break;
            case GPIO_PIN_11: sensorsInfo.limitYSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 0 : 1; break;
            case GPIO_PIN_12: sensorsInfo.limitXSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 0 : 1; break;
            case GPIO_PIN_13: sensorsInfo.limitXSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 0 : 1; break;
            default: break;
            }
            if ((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) {
                sensorsInfo.motorXStatus = motorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, 0, 0, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
            }
            if ((sensorsInfo.limitYSwitchDown == 1) || (sensorsInfo.limitYSwitchUp == 1)) {
                sensorsInfo.motorYStatus = motorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, 0, 0, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
            }
            osMutexRelease (sensorsInfoMutex);
        }
    }
}

void EncoderTask (void* arg) {
    EncoderData encoderData         = { 0 };
    osMessageQueueId_t encoderQueue = (osMessageQueueId_t)arg;
    while (pdTRUE) {
        osMessageQueueGet (encoderQueue, &encoderData, 0, osWaitForever);
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            if (encoderData.axe == encoderAxeX) {
                if (encoderData.direction == encoderCW) {
                    sensorsInfo.pvEncoderX += 360.0 / ENCODER_X_IMP_PER_TURN;
                } else {
                    sensorsInfo.pvEncoderX -= 360.0 / ENCODER_X_IMP_PER_TURN;
                }
                DbgLEDToggle(DBG_LED2);
            } else {
                if (encoderData.direction == encoderCW) {
                    sensorsInfo.pvEncoderY += 360.0 / ENCODER_Y_IMP_PER_TURN;
                } else {
                    sensorsInfo.pvEncoderY -= 360.0 / ENCODER_Y_IMP_PER_TURN;
                }
                DbgLEDToggle(DBG_LED3);
            }
            osMutexRelease (sensorsInfoMutex);
        }
    }
}