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

#include <math.h>
#include <stdio.h>

#include "adc_buffers.h"
#include "meas_tasks.h"
#include "measurements.h"
#include "node-red-config.h"
#include "peripherial.h"
#include "arm_math.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

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

#ifdef PV_BOARD
#define CHANNELS_COUNT 1
#else
#define CHANNELS_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 encoderXTaskHandle      = NULL;
osThreadId_t encoderYTaskHandle      = NULL;

osMessageQueueId_t adc1MeasDataQueue = NULL;
osMessageQueueId_t adc2MeasDataQueue = NULL;
osMessageQueueId_t adc3MeasDataQueue = NULL;

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

volatile uint32_t vRefmV = 3000;

#ifdef MOCK_VOLTAGES_AND_CURRENS
#define SAMPLE_BUFFER_LENGTH        1024
#define SAMPLE_BUFFER_LENGTH_HALF   (SAMPLE_BUFFER_LENGTH/2)
#define FFT_Length  SAMPLE_BUFFER_LENGTH
float32_t voltageWave[3][SAMPLE_BUFFER_LENGTH] = { 0 };
float32_t currentWave[3][SAMPLE_BUFFER_LENGTH] = { 0 };
float fft_output[SAMPLE_BUFFER_LENGTH] = { 0 };
float fft_power_scaled[SAMPLE_BUFFER_LENGTH_HALF] = { 0 };
float fft_power[SAMPLE_BUFFER_LENGTH_HALF] = { 0 };
uint8_t     ifftFlag = 0;
#endif

RESMeasurements resMeasurements __attribute__ ((aligned (32))) = { 0 };
SesnorsInfo sensorsInfo __attribute__ ((aligned (32)))         = { 0 };

//uint16_t ILxRef[CURRENTS_COUNT] __attribute__ ((aligned (32))) = { 0 };
EncoderTaskArg encoderXTaskArg __attribute__ ((aligned (32)))  = { 0 };
EncoderTaskArg encoderYTaskArg __attribute__ ((aligned (32)))  = { 0 };

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

void GenenarateWaveSamples(float32_t amplitude, float32_t phase, uint16_t samplesPerPeriod, uint16_t periods, float32_t* outBuff)
{
	float32_t arg = 0;
	float32_t delta = 2*PI/samplesPerPeriod;
	uint32_t samples = samplesPerPeriod * periods;
	for(uint32_t i = 0; i < samples; i++)
	{
		arg = delta*i + phase;
		outBuff[i] = amplitude * arm_sin_f32(arg);
	}
}

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 osThreadAttradc3MeasTask = { 0 };

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

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

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

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

    encoderXTaskArg.dbgLed                   = DBG_LED2;
    encoderXTaskArg.pvEncoder                = &(sensorsInfo.pvEncoderX);
    encoderXTaskArg.currentPosition          = &(sensorsInfo.currentXPosition);
    osMessageQueueAttr_t encoderMsgQueueAttr = { 0 };
    encoderXTaskArg.dataQueue                = osMessageQueueNew (16, sizeof (uint32_t), &encoderMsgQueueAttr);
    encoderXTaskArg.initPinStates = ((HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_15) << 1) | HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_14)) & 0x3;

    encoderYTaskArg.dbgLed          = DBG_LED3;
    encoderYTaskArg.pvEncoder       = &(sensorsInfo.pvEncoderY);
    encoderYTaskArg.currentPosition = &(sensorsInfo.currentYPosition);
    encoderYTaskArg.dataQueue       = osMessageQueueNew (16, sizeof (uint32_t), &encoderMsgQueueAttr);
    encoderYTaskArg.initPinStates = ((HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) << 1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_10)) & 0x3;

    osThreadAttr_t osThreadAttrEncoderTask = { 0 };
    osThreadAttrEncoderTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrEncoderTask.priority       = (osPriority_t)osPriorityRealtime;
    encoderXTaskHandle                     = osThreadNew (EncoderTask, &encoderXTaskArg, &osThreadAttrEncoderTask);
    encoderYTaskHandle                     = osThreadNew (EncoderTask, &encoderYTaskArg, &osThreadAttrEncoderTask);
#ifdef MOCK_VOLTAGES_AND_CURRENS
    GenenarateWaveSamples(325.269, 0, 128, 8, voltageWave[0]);
    GenenarateWaveSamples(324.269, 0, 128, 8, voltageWave[1]);
    GenenarateWaveSamples(323.269, 0, 128, 8, voltageWave[2]);
    GenenarateWaveSamples(1.414213562, 0, 128, 8, currentWave[0]);
    GenenarateWaveSamples(1.314213562, 0, 128, 8, currentWave[1]);
    GenenarateWaveSamples(1.214213562, 0, 128, 8, currentWave[2]);
#endif
//    arm_hanning_f32
//    arm_rfft_fast_instance_f32 fft;
//    arm_rfft_fast_init_f32(&fft, FFT_Length);
//    arm_rfft_fast_f32(&fft, waveOne, fft_output, ifftFlag);
//    arm_cmplx_mag_f32(fft_output, fft_power, SAMPLE_BUFFER_LENGTH_HALF);
//    float32_t scale = 2.0f/FFT_Length;
//    arm_scale_f32(fft_power, scale, fft_power_scaled, SAMPLE_BUFFER_LENGTH_HALF);
//    float32_t   maxValue;
//    uint32_t    maxIndex;
//    arm_max_f32(fft_power_scaled, SAMPLE_BUFFER_LENGTH_HALF, &maxValue, &maxIndex);
//    printf("maxValue %f, index %ld\n", maxValue, maxIndex);
}

void ADC1MeasTask (void* arg) {
	float voltageAcc[CHANNELS_COUNT] = { 0 };
	float currentAcc[CHANNELS_COUNT] = { 0 };
	float powerAcc[CHANNELS_COUNT] = { 0 };
	uint32_t samplesCounter = 0;
    ADC1_Data adcData    = { 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
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
			for (uint8_t i = 0; i < CHANNELS_COUNT; i++) {
#ifdef MOCK_VOLTAGES_AND_CURRENS
				float voltage = voltageWave[i][samplesCounter % SAMPLE_BUFFER_LENGTH];
				float current = currentWave[i][samplesCounter % SAMPLE_BUFFER_LENGTH];
#else
				float voltage = (adcData.adcDataBuffer[UL1 + i] & 0xFFFF) * deltaADC * U_CHANNEL_CONST * gainCorrection * U_MeasCorrectionData[i].gain + U_MeasCorrectionData[i].offset;
				float ref = (float)(adcData.adcDataBuffer[IL1Ref + i] & 0xFFFF);
				float adcVal = (float)(adcData.adcDataBuffer[UL1 + i] >> 16);
				float current = (adcVal - ref) * deltaADC * I_CHANNEL_CONST * gainCorrection * I_MeasCorrectionData[i].gain + I_MeasCorrectionData[i].offset;
#endif
				voltageAcc[i] += voltage * voltage;
				currentAcc[i] += current * current;
				powerAcc[i] += voltage * current;
				if (fabs (resMeasurements.voltagePeak[i]) < fabs (voltage)) {
					resMeasurements.voltagePeak[i] = voltage;
				}
				if (fabs (resMeasurements.currentPeak[i]) < fabs (current)) {
					resMeasurements.currentPeak[i] = current;
				}
			}
			samplesCounter += 1;
			if (samplesCounter > 33332)
			{
				for (uint8_t i = 0; i < CHANNELS_COUNT; i++) {
					resMeasurements.voltageRMS[i] = sqrtf(voltageAcc[i] / samplesCounter);
					resMeasurements.currentRMS[i] = sqrtf(currentAcc[i] / samplesCounter);
					resMeasurements.power[i] = powerAcc[i] / samplesCounter;
					voltageAcc[i] = 0;
					currentAcc[i] = 0;
					powerAcc[i] = 0;
				}
				samplesCounter = 0;
				DbgLEDToggle(DBG_LED3);
			}
			float fanFBVoltage = (adcData.adcDataBuffer[FanFB] & 0xFFFF) * deltaADC * -4.35 + 12;
            sensorsInfo.fanVoltage = fanFBVoltage;

			osMutexRelease (resMeasurementsMutex);
		}
    }
}

void ADC3MeasTask (void* arg) {
    float motorXSensCircBuffer[CIRC_BUFF_LEN] = { 0 };
    float motorYSensCircBuffer[CIRC_BUFF_LEN] = { 0 };
#ifdef PV_BOARD
    float pvT1CircBuffer[CIRC_BUFF_LEN]       = { 0 };
    float pvT2CircBuffer[CIRC_BUFF_LEN]       = { 0 };
#endif
    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;
#ifdef PV_BOARD
        pvT1CircBuffer[circBuffPos]       = adcData.adcDataBuffer[pvTemp1] * deltaADC * 45.33333333 - 63;
        pvT2CircBuffer[circBuffPos]       = adcData.adcDataBuffer[pvTemp2] * deltaADC * 45.33333333 - 63;
#endif
        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) {
    uint8_t limitXSwitchDownPrevState   = 0;
    uint8_t limitXSwitchCenterPrevState = 0;
    uint8_t limitXSwitchUpPrevState     = 0;
    uint8_t limitYSwitchDownPrevState   = 0;
    uint8_t limitYSwitchCenterPrevState = 0;
    uint8_t limitYSwitchUpPrevState     = 0;
    uint8_t pinStates                   = 0;
    uint8_t limiterXTriggered           = 0;
    uint8_t limiterYTriggered           = 0;
    if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
        sensorsInfo.positionXWeak = 1;
        sensorsInfo.positionYWeak = 1;
        osMutexRelease (sensorsInfoMutex);
    }
    while (pdTRUE) {
        osDelay (pdMS_TO_TICKS (100));
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            sensorsInfo.limitXSwitchDown = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_12);
            pinStates                    = (limitXSwitchDownPrevState << 1) | sensorsInfo.limitXSwitchDown;
            if ((pinStates & 0x3) == 0x1) {
                limiterXTriggered            = 1;
                sensorsInfo.currentXPosition = 0;
                sensorsInfo.positionXWeak    = 0;
            }
            limitXSwitchDownPrevState = sensorsInfo.limitXSwitchDown;

            sensorsInfo.limitXSwitchUp = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_13);
            pinStates                  = (limitXSwitchUpPrevState << 1) | sensorsInfo.limitXSwitchUp;
            if ((pinStates & 0x3) == 0x1) {
                limiterXTriggered            = 1;
                sensorsInfo.currentXPosition = 100;
                sensorsInfo.positionXWeak    = 0;
            }
            limitXSwitchUpPrevState = sensorsInfo.limitXSwitchUp;

            sensorsInfo.limitXSwitchCenter = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_10);
            pinStates                      = (limitXSwitchCenterPrevState << 1) | sensorsInfo.limitXSwitchCenter;
            if ((pinStates & 0x3) == 0x1) {
                sensorsInfo.currentXPosition = AXE_X_MIDDLE_VALUE;
                sensorsInfo.positionXWeak    = 0;
            }
            limitXSwitchCenterPrevState = sensorsInfo.limitXSwitchCenter;

            sensorsInfo.limitYSwitchDown = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_11);
            pinStates                    = (limitYSwitchDownPrevState << 1) | sensorsInfo.limitYSwitchDown;
            if ((pinStates & 0x3) == 0x1) {
                limiterYTriggered            = 1;
                sensorsInfo.currentYPosition = 0;
                sensorsInfo.positionYWeak    = 0;
            }
            limitYSwitchDownPrevState = sensorsInfo.limitYSwitchDown;

            sensorsInfo.limitYSwitchUp = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_9);
            pinStates                  = (limitYSwitchUpPrevState << 1) | sensorsInfo.limitYSwitchUp;
            if ((pinStates & 0x3) == 0x1) {
                limiterYTriggered            = 1;
                sensorsInfo.currentYPosition = 100;
                sensorsInfo.positionYWeak    = 0;
            }
            limitYSwitchUpPrevState = sensorsInfo.limitYSwitchUp;

            sensorsInfo.limitYSwitchCenter = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_8);
            pinStates                      = (limitYSwitchCenterPrevState << 1) | sensorsInfo.limitYSwitchCenter;
            if ((pinStates & 0x3) == 0x1) {
                sensorsInfo.currentYPosition = AXE_Y_MIDDLE_VALUE;
                sensorsInfo.positionYWeak    = 0;
            }
            limitYSwitchCenterPrevState = sensorsInfo.limitYSwitchCenter;
            if (((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) && (limiterXTriggered == 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)) && (limiterYTriggered == 1)) {
                sensorsInfo.motorYStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, 0, 0, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
            }
            limiterXTriggered = 0;
            limiterYTriggered = 0;
            osMutexRelease (sensorsInfoMutex);
        }
    }
}

void EncoderTask (void* arg) {
	// 01 11 10 00
    const uint32_t encoderStates[4] = { 0x00, 0x01, 0x03, 0x02 };
    uint8_t step                    = 0;
    EncoderTaskArg* encoderTaskArg  = (EncoderTaskArg*)arg;
    uint32_t pinStates              = encoderTaskArg->initPinStates;
    for (uint8_t i = 0; i < 4; i++) {
        if (pinStates == encoderStates[i]) {
            step = i;
            break;
        }
    }
    while (pdTRUE) {
        osMessageQueueGet (encoderTaskArg->dataQueue, &pinStates, 0, osWaitForever);
        float encoderValue = *encoderTaskArg->pvEncoder;
		if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
			if (encoderStates[(step + 1) % 4] == pinStates) {
				step++;
				encoderValue++;
//				encoderValue += 360.0 / ENCODER_X_IMP_PER_TURN;
//				printf ("Forward\n");
			} else if (encoderStates[(step - 1) % 4] == pinStates) {
				encoderValue--;
//				encoderValue -= 360.0 / ENCODER_X_IMP_PER_TURN;
//				if (encoderValue < 0) {
//					encoderValue = 360.0 + encoderValue;
//				}
//				printf ("Reverse\n");
				step--;
			} else {
				printf ("Forbidden\n");
			}
			step                             = step % 4;
//			*encoderTaskArg->pvEncoder       = fmodf (encoderValue, 360.0);
			*encoderTaskArg->pvEncoder       = encoderValue;
			*encoderTaskArg->currentPosition = 100 * (*encoderTaskArg->pvEncoder) / MAX_X_AXE_ANGLE;
			osMutexRelease (sensorsInfoMutex);
		}
        DbgLEDToggle (encoderTaskArg->dbgLed);
    }
}