#include "stm32f4xx_matlab_gpio.h"

#include "modbus.h"

void GPIO_to_SFUNC(real_T* disc)
{
    for (int i = 0; i < PORT_WIDTH; i++)
    {
        if (GPIOB->ODR & (1 << i))
        {
            disc[i] = 1;
        }

        if (GPIOD->ODR & (1 << i))
        {
            disc[PORT_WIDTH + i] = 1;
        }

        if (GPIOE->ODR & (1 << i))
        {
            disc[2*PORT_WIDTH + i] = 1;
        }
    }
}

void SFUNC_to_GPIO(real_T* in)
{
    // write pwm ctrl registers
    for (int i = 0; i < 5; i++)
    {
        pwm_ctrl[i] = in[i];
    }
    // write pwm ctrl coils
    if (in[5] > 0.5)
    {
        MB_Set_Coil_Local(coils_regs, COIL_PWM_DC_MODE);
    }
    else
    {
        MB_Reset_Coil_Local(coils_regs, COIL_PWM_DC_MODE);
    }
    if (in[6] > 0.5)
    {
        MB_Set_Coil_Local(coils_regs, COIL_PWM_CH_MODE);
    }
    else
    {
        MB_Reset_Coil_Local(coils_regs, COIL_PWM_CH_MODE);
    }
    if (in[7] > 0.5)
    {
        MB_Set_Coil_Local(coils_regs, COIL_PWM_PHASE_MODE);
    }
    else
    {
        MB_Reset_Coil_Local(coils_regs, COIL_PWM_PHASE_MODE);
    }
}