%% Copyright (C) 2013-2015 Czech Technical University in Prague
%%
%% Authors:
%%     - Carlos Jenkins <carlos@jenkins.co.cr>
%%     - Michal Sojka <sojkam1@fel.cvut.cz>
%%
%% This document contains proprietary information belonging to Czech
%% Technical University in Prague. Passing on and copying of this
%% document, and communication of its contents is not permitted
%% without prior written authorization.
%%
%% File : rpp_mrmain.tlc
%% Abstract:
%%     Custom TLC file to generate an RPP "main" file.
%%
%%     This file generates the "main" file for the RPP target on top of the RPP
%%     library and the FreeRTOS operating system.
%%
%%     The mr prefix is standard to mark Multi Tasking main, which is the case. See 
%%     rpp_file_process.m description above for more information about this.
%%
%%     !!!!!!!!!!!!!!!!!!!!!!!!
%%     !!! This is not ready to use source code. It is just a concept with some ideas
%%     !!! from our experiments and has to be reworked.
%%     !!!!!!!!!!!!!!!!!!!!!!!!
%% References:
%%     Example in <matlabroot>/rtw/c/tlc/mw/bareboard_mrmain.tlc


%selectfile NULL_FILE

%function FcnMultiTaskingMain() void

    %if GenerateSampleERTMain
        %assign CompiledModel.GenerateSampleERTMain = TLC_FALSE
    %endif


    %assign cFile = LibCreateSourceFile("Source", "Custom", "ert_main")


    %%%%%%%%
    %openfile tmpBuf

    /* Model includes */
    #include "%<LibGetMdlPubHdrBaseName()>.h"

    /* RPP runtime includes */
    #include "rpp_simulink_runtime.h"

    %if extMode == 1
      /* External mode header file */
      #include "ext_work.h"
      #include <drv/sci.h>
    %endif

    %closefile tmpBuf
    %<LibSetSourceFileSection(cFile, "Includes", tmpBuf)>
    %%%%%%%%


    %%%%%%%%
    %openfile tmpBuf

    /* Definitions */
    #define STEP_SIZE_MILLIS %<CompiledModel.FundamentalStepSize>*1000.0
    #define CONTROL_PRIORITY 3
    #define WORKING_PRIORITY 2
    #define EXTMODE_PRIORITY 1

    %closefile tmpBuf
    %<LibSetSourceFileSection(cFile, "Defines", tmpBuf)>
    %%%%%%%%


    %%%%%%%%
    %openfile tmpBuf

    static boolean_t working = FALSE;
    static xSemaphoreHandle step_signal = NULL;
    static xSemaphoreHandle initialized_signal = NULL;
    %if extMode == 1
      static xSemaphoreHandle ext_mode_ready = NULL;
    %endif
    static uint32_t steps_control = 0;
    static uint32_t steps_working = 0;
    boolean_t overrun_flag = FALSE;

    %closefile tmpBuf
    %<LibSetSourceFileSection(cFile, "Declarations", tmpBuf)>
    %%%%%%%%


    %%%%%%%%
    %openfile tmpBuf

    /**
     * Model step control and overrun detection task.
     */
    void control_task(void* p)
    {

      xSemaphoreTake(initialized_signal, portMAX_DELAY);
      %if extMode == 1
        xSemaphoreTake(ext_mode_ready, portMAX_DELAY);
      %endif

        static const portTickType freq_ticks = STEP_SIZE_MILLIS / portTICK_RATE_MS;
        portTickType last_wake_time = xTaskGetTickCount();

    %% This condition has been added because of the warning:
    %% "ert_main.c", line 46: warning #238-D: controlling expression is constant
    %% caused by the expression (void*) 0 passed to the while statement. The rtmGetStopRequested
    %% macro is defined to this constant if and only if the GenerateSampleERTMain is not set. So
    %% The condition is not needed in this case as well.
    %%
    %% See <MATLAB_ROOT>/rtw/c/tlc/mw/rtmspecmacs.tlc at line 198.

    %if !GenerateSampleERTMain
        while (rtmGetErrorStatus(%<modelName>_M) == NULL) {
    %else
        while (rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
    %endif
            /* Wait until next step */
            vTaskDelayUntil(&last_wake_time, freq_ticks);

            if (working) {
                /* Overrun detected */
                overrun_flag = TRUE;
            } else {
                overrun_flag = FALSE;
                /* Release semaphore */
                xSemaphoreGive(step_signal);
            }
            steps_control++;

            %if extMode == 1
              rtExtModeCheckEndTrigger();
            %endif

        }

        /* In case of shutdown, delete this task */
        vTaskDelete(NULL);
    }

    /**
     * Model step logic execution task.
     */
    void working_task(void* p)
    {
        %% This idea might be useful in the MR implementation
            %if LibNumSynchronousSampleTimes() > 1
                static  int_T taskCounter[%<FcnNumST()>] = %<FcnInitializeTaskCounter()>;
            %endif
        %% ----
        /* Initialize model */
        %<LibCallModelInitialize()>\
        /* Step the model for any subrate */
        xSemaphoreGive(initialized_signal);

        while (1) {
            if (xSemaphoreTake(step_signal, portMAX_DELAY)) {
                working = TRUE;

                %<LibCallModelStep(0)>\
		        
                %% This idea might be useful in the MR implementation

                %if LibNumSynchronousSampleTimes() > 1
                  %foreach idx = LibGetNumSyncPeriodicTasks() - 1
                    %assign tid = 1 + idx + tid01Eq
                    /* Task %<tid> handling */
                    if (taskCounter[%<tid>] == 0) {
                      %<LibCallModelStep(tid)>\
                    }
                    if (++taskCounter[%<tid>] == %< FcnComputeTaskTickLimit(tid)>)
                      taskCounter[%<tid>]=0;
                  %endforeach
                %endif
                
                %% ----
		        		        
                steps_working++;
                working = FALSE;
            }
        }

	%% /* In case of shutdown, delete this task */
	%% vTaskDelete(NULL);
    }

    %if extMode == 1
      void ext_mode_comm_task(void* p)
      {
        drv_sci_set_crlf_conv_en(FALSE); /* Disable CR->CRLF conversion */

        %<SLibGenERTExtModeInit()>
        xSemaphoreGive(ext_mode_ready);

        while (rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
          rtExtModeOneStep(rtmGetRTWExtModeInfo(%<modelName>_M), %<numSampleTimes>, (boolean_T *)&rtmGetStopRequested(%<modelName>_M));
        }
        rtExtModeShutdown(%<numSampleTimes>);
        /* In case of shutdown, delete this task */
        vTaskDelete(NULL);
      }
    %endif

    /**
     * Hardware initialization, task spawning and OS scheduler start.
     */
    void main(void)
    {
        /* Initialize RPP board */
        %if EXISTS(::rpp_ain_present)
            rpp_adc_init();
        %endif
        %if EXISTS(::rpp_din_present) || EXISTS(::rpp_dout_present)
            rpp_gio_init(RPP_GIO_PORT_ALL);
        %endif
        rpp_sci_init();

        // Speed up the SCI
        rpp_sci_setup(115200);

        %if rppPrintMeta
        %assign model_info = SPRINTF("'%s' - %s (TLC %s)\\r\\n", LibGetMdlPubHdrBaseName(), TLC_TIME, TLC_VERSION)
        rpp_sci_printk("%<model_info>");
        %endif

        /* Create and lock semaphore */
        vSemaphoreCreateBinary(step_signal);
        xSemaphoreTake(step_signal, 0);
        vSemaphoreCreateBinary(initialized_signal);
        xSemaphoreTake(initialized_signal, 0);
        %if extMode == 1
          vSemaphoreCreateBinary(ext_mode_ready);
          xSemaphoreTake(ext_mode_ready, 0);
        %endif

        /* Create tasks to step model and model task */
        if (xTaskCreate(control_task, "control_task",
            configMINIMAL_STACK_SIZE, NULL, CONTROL_PRIORITY, NULL) != pdPASS) {
            rpp_sci_printk("ERROR: Cannot spawn control task.\r\n");
            return;
        }
        if (xTaskCreate(working_task, "working_task",
            %<rppModelTaskStack>, NULL, WORKING_PRIORITY, NULL) != pdPASS) {
            rpp_sci_printk("ERROR: Cannot spawn model task.\r\n"
            );
            return;
        }
        %if extMode == 1
          /* External mode */
          rtParseArgsForExtMode(0, NULL);

          if (xTaskCreate(ext_mode_comm_task, "ext_mode_comm_task", 1024, NULL, EXTMODE_PRIORITY, NULL) != pdPASS) {
            rpp_sci_printk("ERROR: Cannot spawn model task.\r\n");
            return;
          }
        %endif

        /* Start FreeRTOS Scheduler */
        vTaskStartScheduler();

        /* We should never get here */
        %<LibCallModelTerminate()>
        rpp_sci_printk("ERROR: Problem allocating memory for idle task.\r\n");
    }


    #if configUSE_MALLOC_FAILED_HOOK == 1
    /**
     * FreeRTOS malloc() failed hook.
     */
     void vApplicationMallocFailedHook(void) {
       rpp_sci_printk("ERROR: Memory allocation failed.\r\n");
     }
    #endif


    #if configCHECK_FOR_STACK_OVERFLOW > 0
    /**
     * FreeRTOS stack overflow hook.
     */
    void vApplicationStackOverflowHook(xTaskHandle xTask, signed portCHAR *pcTaskName)
    {
      rpp_sci_printk("ERROR: Stack overflow : \"%s\".\r\n", pcTaskName);
    }
    #endif

    %closefile tmpBuf
    %<LibSetSourceFileSection(cFile, "Functions", tmpBuf)>
    %%%%%%%%

%endfunction