/*
 * position_task.c
 *
 *  Created on: Nov 6, 2024
 *      Author: jakubski
 */

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

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

float positionXSetting __attribute__ ((aligned (32))) = 0.0;
float positionYSetting __attribute__ ((aligned (32))) = 0.0;

//osMutexId_t positionSettingMutex;

osThreadId_t positionXControlTaskHandle = NULL;
osThreadId_t positionYControlTaskHandle = NULL;

PositionControlTaskInitArg positionXControlTaskInitArg __attribute__ ((aligned (32))) = { 0 };
PositionControlTaskInitArg positionYControlTaskInitArg __attribute__ ((aligned (32))) = { 0 };

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

void PositionControlTaskInit (void) {
//    positionSettingMutex                           = osMutexNew (NULL);
    osThreadAttr_t osThreadAttrPositionControlTask = { 0 };
    osThreadAttrPositionControlTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrPositionControlTask.priority       = (osPriority_t)osPriorityNormal;

    positionXControlTaskInitArg.channel1                = TIM_CHANNEL_1;
    positionXControlTaskInitArg.channel2                = TIM_CHANNEL_2;
    positionXControlTaskInitArg.htim                    = &htim3;
    positionXControlTaskInitArg.motorTimerConfigOC      = &motorXYTimerConfigOC;
    positionXControlTaskInitArg.motorTimerHandle        = motorXTimerHandle;
    positionXControlTaskInitArg.positionSettingQueue    = osMessageQueueNew (16, sizeof (PositionControlTaskData), NULL);
    positionXControlTaskInitArg.switchLimiterCenterStat = &(sensorsInfo.limitXSwitchCenter);
    positionXControlTaskInitArg.switchLimiterUpStat     = &(sensorsInfo.limitXSwitchUp);
    positionXControlTaskInitArg.switchLimiterDownStat   = &(sensorsInfo.limitXSwitchDown);
    positionXControlTaskInitArg.currentPosition         = &(sensorsInfo.currentXPosition);
    positionXControlTaskInitArg.positionOffset			= &(sensorsInfo.positionXOffset);
    positionXControlTaskInitArg.motorStatus             = &(sensorsInfo.motorXStatus);
    positionXControlTaskInitArg.motorPeakCurrent        = &(sensorsInfo.motorXPeakCurrent);
    positionXControlTaskInitArg.positionSetting         = &positionXSetting;
    positionXControlTaskInitArg.axe                     = 'X';

    positionYControlTaskInitArg.channel1                = TIM_CHANNEL_3;
    positionYControlTaskInitArg.channel2                = TIM_CHANNEL_4;
    positionYControlTaskInitArg.htim                    = &htim3;
    positionYControlTaskInitArg.motorTimerConfigOC      = &motorXYTimerConfigOC;
    positionYControlTaskInitArg.motorTimerHandle        = motorYTimerHandle;
    positionYControlTaskInitArg.positionSettingQueue    = osMessageQueueNew (16, sizeof (PositionControlTaskData), NULL);
    positionYControlTaskInitArg.switchLimiterCenterStat = &(sensorsInfo.limitYSwitchCenter);
    positionYControlTaskInitArg.switchLimiterUpStat     = &(sensorsInfo.limitYSwitchUp);
    positionYControlTaskInitArg.switchLimiterDownStat   = &(sensorsInfo.limitYSwitchDown);
    positionYControlTaskInitArg.currentPosition         = &(sensorsInfo.currentYPosition);
    positionYControlTaskInitArg.positionOffset			= &(sensorsInfo.positionYOffset);
    positionYControlTaskInitArg.motorStatus             = &(sensorsInfo.motorYStatus);
    positionYControlTaskInitArg.motorPeakCurrent        = &(sensorsInfo.motorYPeakCurrent);
    positionXControlTaskInitArg.positionSetting         = &positionYSetting;
    positionYControlTaskInitArg.axe                     = 'Y';

    positionXControlTaskHandle = osThreadNew (PositionControlTask, &positionXControlTaskInitArg, &osThreadAttrPositionControlTask);
    positionYControlTaskHandle = osThreadNew (PositionControlTask, &positionYControlTaskInitArg, &osThreadAttrPositionControlTask);
}

void PositionControlTask (void* argument) {
    const int32_t PositionControlTaskTimeOut   = 100;
    PositionControlTaskInitArg* posCtrlTaskArg = (PositionControlTaskInitArg*)argument;
    PositionControlTaskData posCtrlData  __attribute__ ((aligned (32))) = { 0 };
    uint32_t motorStatus                       = 0;
    osStatus_t queueSatus;
    int32_t pwmValue              = MOTOR_START_STOP_PWM_VALUE;
    int32_t sign                  = 0;
    MovementPhases movementPhase  = idlePhase;
    float startPosition           = 0;
    float prevPosition            = 0;
    int32_t timeLeftMS            = 0;
    int32_t moveCmdTimeoutCounter = 0;
    while (pdTRUE) {
        queueSatus = osMessageQueueGet (posCtrlTaskArg->positionSettingQueue, &posCtrlData, 0, pdMS_TO_TICKS (PositionControlTaskTimeOut));
        if (queueSatus == osOK) {
            if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
                float posDiff = posCtrlData.positionSettingValue - (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset);
                if (posDiff != 0) {
                    sign                  = posDiff > 0 ? 1 : -1;
                    startPosition         = *posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset;
                    movementPhase         = startPhase;
                    moveCmdTimeoutCounter = 0;
                    timeLeftMS            = 0;
#ifdef DBG_POSITION
                    printf ("Axe %c start phase\n", posCtrlTaskArg->axe);
#endif
                }
                *(posCtrlTaskArg->positionSetting) = posCtrlData.positionSettingValue;
                osMutexRelease (sensorsInfoMutex);
//                if (osMutexAcquire (positionSettingMutex, osWaitForever) == osOK) {
//                    osMutexRelease (positionSettingMutex);
//                }
            }
        } else if (queueSatus == osErrorTimeout) {
            if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
                if (((*posCtrlTaskArg->motorStatus != 0) && (movementPhase != idlePhase)) || (movementPhase == startPhase) ) {
                    if (((*posCtrlTaskArg->switchLimiterDownStat == 1) && (*posCtrlTaskArg->switchLimiterUpStat == 1)) ||
                    ((*posCtrlTaskArg->switchLimiterUpStat == 1) && (*posCtrlTaskArg->switchLimiterCenterStat == 1))) {
                        movementPhase = idlePhase;
                        motorStatus   = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle, 0,
                          0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
                        *posCtrlTaskArg->motorStatus = motorStatus;
#ifdef DBG_POSITION
                        printf ("Axe %c limiters wrong state - idle phase\n", posCtrlTaskArg->axe);
#endif
                    }


                    timeLeftMS += PositionControlTaskTimeOut;
                    if (prevPosition == (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset)) {
                        moveCmdTimeoutCounter += PositionControlTaskTimeOut;
                    } else {
                        moveCmdTimeoutCounter = 0;
                    }
                    prevPosition = *posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset;
                    if (moveCmdTimeoutCounter > NO_MOVE_TIMEOUT_MS) {
                        movementPhase = idlePhase;
                        motorStatus   = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle, 0,
                          0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
                        *posCtrlTaskArg->motorStatus = motorStatus;
#ifdef DBG_POSITION
                        printf ("Axe %c no movement idle phase\n", posCtrlTaskArg->axe);
#endif
                    }
                    switch (movementPhase) {
                    case startPhase:
                        motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
                        sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
                        *posCtrlTaskArg->motorStatus = motorStatus;
                        if (motorStatus == 1) {
                            *posCtrlTaskArg->motorPeakCurrent = 0.0;
#ifdef DBG_POSITION
                            printf ("Axe %c speed up phase\n", posCtrlTaskArg->axe);
#endif
                            movementPhase         = speedUpPhase;
                            timeLeftMS            = 0;
                            moveCmdTimeoutCounter = 0;
                        } else {
                            movementPhase = idlePhase;
#ifdef DBG_POSITION
                            printf ("Axe %c idle phase\n", posCtrlTaskArg->axe);
#endif
                        }

                        break;
                    case speedUpPhase:
                        if ((abs ((*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) - startPosition) >= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
                            pwmValue    = MOTOR_HIGH_SPEED_PWM_VALUE;
                            motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
                            sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
                            *posCtrlTaskArg->motorStatus = motorStatus;
                            movementPhase                = movePhase;
#ifdef DBG_POSITION
                            printf ("Axe %c move phase\n", posCtrlTaskArg->axe);
#endif
                        }
                        break;
                    case movePhase:
                        if (abs ((*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) - *posCtrlTaskArg->positionSetting) <= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) {
                            movementPhase = slowDownPhase;
#ifdef DBG_POSITION
                            printf ("Axe %c slow down phase\n", posCtrlTaskArg->axe);
#endif
                        }
                        break;
                    case slowDownPhase:
                        pwmValue    = MOTOR_START_STOP_PWM_VALUE;
                        motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
                        sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
                        *posCtrlTaskArg->motorStatus = motorStatus;
                        movementPhase                = stopPhase;
                        timeLeftMS                   = 0;
#ifdef DBG_POSITION
                        printf ("Axe %c stop phase\n", posCtrlTaskArg->axe);
#endif
                        break;
                    case stopPhase:
                    	float posDiff = sign > 0 ? posCtrlData.positionSettingValue - (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) : (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) - posCtrlData.positionSettingValue;
                        if ((posDiff <= 0) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
                            motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
                            0, 0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
                            *posCtrlTaskArg->motorStatus = motorStatus;
                            movementPhase                = idlePhase;
#ifdef DBG_POSITION
                            printf ("Axe %c idle phase\n", posCtrlTaskArg->axe);
#endif
                        }
                        break;
                    default: break;
                    }
                } else {
                    if ((*posCtrlTaskArg->motorStatus == 0) && (movementPhase != idlePhase)) {
                        movementPhase = idlePhase;
#ifdef DBG_POSITION
                        printf ("Axe %c idle phase\n", posCtrlTaskArg->axe);
#endif
                    }
                }
                osMutexRelease (sensorsInfoMutex);
            }
        }
    }
}