/**************************************************************************
Данный файл содержит функции для симуляции МК в Simulink (S-Function).

**************************************************************************/
#include "simstruc.h"
#include "mcu_wrapper_conf.h"
//#include <windows.h>
//#include <synchapi.h>

DEL_MCUHandleTypeDef hmcu;          // для управления контекстом программы МК
HANDLE hThread;
uint32_t idThreads;
unsigned flag_to_end;

double SystemClockDouble = 0;       // для симуляции системных тиков, прока просто по приколу
uint64_t SystemClock;               // для симуляции системных тиков, прока просто по приколу

/* Extern function of control MCU App from asmjmp.asm */
extern void GET_RSP_ORIGIN(DEL_MCUHandleTypeDef* hmcu);
extern void RESTORE_CONTEXT_AND_JUMP(DEL_MCUHandleTypeDef* hmcu);
extern void READ_REGS(_JUMP_BUFFER* hmcu);

//-------------------------------------------------------------//
//-----------------CONTROLLER SIMULATE FUNCTIONS---------------//
/** MCU STEP SIMULATION */
/**
  * @brief  Read from simulink S-Block Inputs and write to MCU I/O ports.
  * @param  in - inputs of S-Function.
  * @param  disc - discrete array of S-Function. Outputs would be written from disc.
  * @param  time - current time of simulation (in second).
  * @note   Вызывает главную функцию main из пользовательского кода и управляет её ходом:
  *         Прерывает её, если она попала в бесконечный while, симулирует периферию 
  *         и возвращает в точку остановки на следующем шаге.
  */
/* simulate controller step */
extern int main(void); // extern while from main.c
unsigned __stdcall MCU_App_Thread(void) {
    main(); // симуляция кода МК 
    return 0;
}
void MCU_Step_Simulation(SimStruct* S, real_T* in, real_T* disc, time_T time)
{
    //-----------INIT WRAPPER-----------
    if (hmcu.Get_RSP_Origin)    // если надо получть указатель на "начало" стека
    {
        hmcu.Get_RSP_Origin = 0;    // сброс флага, чтобы сюда больше не попадать
        // указатель на "начало" стека - берется в начале главной управляющей функции
        //GET_RSP_ORIGIN(&hmcu);

        // инициализация потока, который будет выполнять код МК
        hThread = (HANDLE)_beginthreadex(NULL, 0, MCU_App_Thread, 0, 0x00000004, &idThreads);

        ResumeThread(hThread);

        //Sleep(100);
        for (int i = TIME_FOR_MCU_APP*10000; i > 0; i--)
        {
        }

        SuspendThread(hThread);
        return;
    }
    //----------------------------------

    // Get uwTick (in ms)
    //uwTick = time * 1000; // from simulation time 

    SystemClockDouble += SystemCoreClock * SIM_Sample_Time; // emulate core clock 
    SystemClock = SystemClockDouble;
    uwTick = SystemClock / (SystemCoreClock / 1000);

    MCU_readInputs(in); // считывание портов

    MCU_Periph_Simulation(); // simulate peripheral

    //// Return to MCU app at line, where it was stopped
    //if (hmcu.Resume_MCU_App) // если необходимо вернутся в код МК
    //{
    //    // Сброс флагов
    //    hmcu.Resume_MCU_App = 0;
    //    hmcu.Stop_MCU_App = 0;
    //    // Возврат в код МК
    //    RESTORE_CONTEXT_AND_JUMP(&hmcu);
    //}

    //// сохранение контекста до перехода в код МК
    //if (!hmcu.Stop_MCU_App && (setjmp(hmcu.main_start) == 0))
    //{   // если контекст сохранен (setjmp == 0) и МК не остановлен
    //    main(); // симуляция кода МК 
    //}
    // note: при вызове longjmp, setjmp != 0, поэтому программа не войдет заново в main_loop



    ResumeThread(hThread);
    for (int i = TIME_FOR_MCU_APP; i > 0; i--)
    {
    }
    SuspendThread(hThread);


    MCU_writeOutputs(disc); // запись портов (по факту запись в буфер. запись в порты в mdlOutputs)
}

/** SIMULATE MCU PERIPHERAL */
/**
  * @brief  Simulate peripheral of MCU
  * @note   Пользовательский код, который симулирует работу периферии МК.
  */
void MCU_Periph_Simulation(void)
{
    Simulate_TIMs();
}

/** READ INPUTS S-FUNCTION */
/**
  * @brief  Read from simulink S-Block Inputs and write to MCU I/O ports.
  * @param  in - inputs of S-Function.
  * @note   Пользовательский код, который записывает в порты ввода-вывода из disc.
  */
void MCU_readInputs(real_T *in)
{
    SFUNC_to_GPIO(in);
}

/** WRITE OUTPUTS S-FUNCTION */
/**
  * @brief  Read from MCU I/O ports and write to simulink S-Block Outputs.
  * @param  disc - discrete array of S-Function. Outputs would be written from disc.
  * @note   Пользовательский код, который записывает в disc порты ввода-вывода.
  */
void MCU_writeOutputs(real_T *disc)
{		
    GPIO_to_SFUNC(disc);
}
//-----------------CONTROLLER SIMULATE FUNCTIONS---------------//
//-------------------------------------------------------------//


//-------------------------------------------------------------//
//----------INITIALIZE/DEINITIALIZE SIMULATE FUNCTIONS---------//
/** MCU WRAPPER DEINITIALIZATION */
/**
  * @brief  Initialize structures and variables for simulating MCU.
  * @note   Пользовательский код, который будет настраивать все структуры.
  */
void SIM_Initialize_Simulation(void)
{
    /* user initialization */
    Initialize_Periph_Sim();

    /* wrapper initialization */
    //SystemClock_step = SystemCoreClock * SIM_Sample_Time;    // set system clock step
    hmcu.Get_RSP_Origin = 1;                        // set flag to store pionter to "beginning" pf the stack
}
/** MCU WRAPPER DEINITIALIZATION */
/**
  * @brief  Deinitialize structures and variables for simulating MCU.
  * @note   Пользовательский код, который будет очищать все структуры.
  */
void SIM_deInitialize_Simulation(void)
{
    // simulate structures of peripheral deinitialization
    deInitialize_Periph_Sim();
    // mcu peripheral memory deinitialization
    deInitialize_MCU();
}
//-------------------------------------------------------------//


//-------------------------------------------------------------//
//-------------------  SAVE CONTEXT FUNCTIONS -----------------//
/** SAVE CONTEXT OF PROGRAMM */
/**
  * @brief  Save context (stack, registers) of running program.
  * @note   Данная функция устанавливает счетчик комманд на начало функции.
  *         Так что возврат будет произведен в начало этой функции.
  */
void saveProgramContext(void)
{
    _SAVE_RIP_(hmcu);
    hmcu.REGS.Rip -= 5;     // Сдвиг RIP с этой команды на первую команду в функции: sizeof(call getNextAddr) = 5
    READ_REGS(&hmcu.REGS);  // Считывание регистров
    _SAVE_STACK_(hmcu);     // Считывание регистров
}

/** GET NEXT INSTRUCTION ADDRESS */
/**
  * @brief  Gives the address where this function will return.
  */
void* getNextAddr(void) {
    return _ReturnAddress();
}
//-------------------------------------------------------------//