#include "stm32f1xx_matlab_gpio.h"
#include "upp.h"


#define detect_front(_in_numb_, _var_, _val_) {             \
if ((in[_in_numb_] > 0.5) && (prev_in[_in_numb_] <= 0.5))   \
{                                                           \
    _var_ = _val_;                                          \
} }

#define detect_rise(_in_numb_, _var_, _val_) {              \
if ((in[_in_numb_] < 0.5) && (prev_in[_in_numb_] >= 0.5))   \
{                                                           \
    _var_ = _val_;                                          \
} }



void WriteFromSFunc(real_T* disc)
{

    for (int i = 0; i < PORT_WIDTH; i++)
    {
        if (GPIOA->ODR & (1 << i))
        {
            disc[i] = 1;
        }

        if (GPIOB->ODR & (1 << i))
        {
            disc[PORT_WIDTH + i] = 1;
        }
    }
    disc[2 * PORT_WIDTH + 0] = phase_A.ctrl.angle.delay_us;
    disc[2 * PORT_WIDTH + 1] = (uint16_t)((uint16_t)TIMER->CNT - phase_A.ctrl.angle.start_delay_tick);
    disc[2 * PORT_WIDTH + 2] = phase_A.ctrl.angle.start_delay_tick;
    disc[2 * PORT_WIDTH + 3] = TIMER->CNT;
}



void ReadToSFunc(real_T* in)
{
    static real_T prev_in[IN_PORT_WIDTH];

    detect_front(0, phase_A.zc_detector.f.EXTIZeroCrossDetected, 1);
    detect_rise(0, phase_A.zc_detector.f.EXTIZeroCrossDetected, 1);

    detect_front(1, phase_B.zc_detector.f.EXTIZeroCrossDetected, 1);
    detect_rise(1, phase_B.zc_detector.f.EXTIZeroCrossDetected, 1);

    detect_front(2, phase_C.zc_detector.f.EXTIZeroCrossDetected, 1);
    detect_rise(2, phase_C.zc_detector.f.EXTIZeroCrossDetected, 1);

    detect_front(3, Upp.GoSafe, 1);
    detect_rise(3, Upp.GoSafe, 0);

    detect_front(4, Upp.Prepare, 1);
    detect_rise(4, Upp.Prepare, 0);

    detect_front(5, Upp.ForceStop, 1);
    detect_rise(5, Upp.ForceStop, 0);

    detect_front(6, Upp.ForceDisconnect, 1);
    detect_rise(6, Upp.ForceDisconnect, 0);


    Upp.sine_freq = in[7];
    Upp.Duration = in[8];


    for (int i = 0; i < IN_PORT_WIDTH; i++)
    {
        prev_in[i] = in[i];
    }
}