%% Copyright (C) 2013-2015 Czech Technical University in Prague
%%
%% Authors:
%%     - Carlos Jenkins <carlos@jenkins.co.cr>
%%     - Michal Sojka <sojkam1@fel.cvut.cz>
%%     - Michal Horn <hornmich@fel.cvut.cz>
%%
%% Permission is hereby granted, free of charge, to any person
%% obtaining a copy of this software and associated documentation
%% files (the "Software"), to deal in the Software without
%% restriction, including without limitation the rights to use,
%% copy, modify, merge, publish, distribute, sublicense, and/or sell
%% copies of the Software, and to permit persons to whom the
%% Software is furnished to do so, subject to the following
%% conditions:

%% The above copyright notice and this permission notice shall be
%% included in all copies or substantial portions of the Software.

%% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
%% EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
%% OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
%% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
%% HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
%% WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
%% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
%% OTHER DEALINGS IN THE SOFTWARE.
%%
%% 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.
%%
%% 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")

    %if LibIsSingleTasking()
      %assign taskCount = 1
      %assign firstTask = 0
    %else
      %assign taskCount = numSampleTimes - tid01Eq
      %assign firstTask = tid01Eq
    %endif


    %%%%%%%%
    %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>
      #include <drv/emac.h>
    %endif

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


    %%%%%%%%
    %openfile tmpBuf

    /* Definitions */
    #define STEP_SIZE_MILLIS (%<CompiledModel.FundamentalStepSize>*1000.0)
    #define NUM_TASKS    %<taskCount>
    #define CONTROL_PRIORITY (configMAX_PRIORITIES - 1)

    %% Worker task priority assignment - rate transition block
    %% requires rate monotonic priority assignment
    %foreach i = taskCount
        #define WORKING%<i + firstTask>_PRIORITY    (configMAX_PRIORITIES - %<2 + i>)
    %endforeach

    %if extMode == 1
      #define EXTMODE_PRIORITY 1
    %else
      #define IDLE_PRIORITY 0
      #define EXTMODE_PRIORITY IDLE_PRIORITY /* Extmode not used */
    %endif

    %foreach i = taskCount
        %assign j = i + firstTask
        #if (WORKING%<j>_PRIORITY <= EXTMODE_PRIORITY) || (WORKING%<j>_PRIORITY >= CONTROL_PRIORITY)
        #error Too many tasks. Increase configMAX_PRIORITIES.
        #endif
    %endforeach

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


    %%%%%%%%
    %openfile tmpBuf
    typedef struct sub_rate_st {
        xTaskHandle      thread;
        xSemaphoreHandle sem;
    } sub_rate_t;

    %if extMode == 1
      static xSemaphoreHandle ext_mode_ready = NULL;
    %endif
    static uint32_t steps_control = 0;
    static uint32_t steps_working[NUM_TASKS];
    boolean_t overrun_flag = FALSE;
    static sub_rate_t sub_rate[NUM_TASKS];
    static boolean_t simulationFinished = 0;

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


    %%%%%%%%
    %openfile tmpBuf

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

        %if !LibIsSingleTasking()
          uint32_t i = 0;

          %% Variables used by LibCallSetEventForThisBaseStep (static => zero initialized)
          static uint32_t taskCounter[NUM_TASKS];
          static boolean_t eventFlags[NUM_TASKS];
          static boolean_t OverrunFlags[NUM_TASKS];

          (void)OverrunFlags; /* Not used in multitasking mode, but required by TLC macro */
        %endif

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

      /* Initialize model */
      %<LibCallModelInitialize()>\

      /* Start periodic execution */
      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 !LibIsSingleTasking()
              /* Mark rates that need to run this time step */
              %<LibCallSetEventForThisBaseStep("eventFlags")>\
            %endif

            /* Trigger working threads */
            %foreach j = taskCount
                %assign i = j + firstTask
                %assign s = CompiledModel.SampleTime[i].PeriodAndOffset[0]
                %assign o = CompiledModel.SampleTime[i].PeriodAndOffset[1]
                /* Sampling rate %<i>, sample time = %<s>, offset = %<o> */
            %endforeach

            %if LibIsSingleTasking()
              %assign i = "0"
            %else
              %assign i = "i"
              for (%<i> = %<firstTask>; %<i> < NUM_TASKS; %<i>++) {
                  if (%<i> == 0 || eventFlags[%<i>]) {
                    eventFlags[%<i>] = FALSE;
                %endif
                    uint32_t step_sem_value = uxQueueMessagesWaiting(sub_rate[%<i>].sem);
                    if (step_sem_value) {
                        %% Commented out so that overrun does stop the whole execution
                        %% rtmSetErrorStatus(%<modelName>_M, "Overrun");
                        overrun_flag = TRUE;
                        continue;
                    }
                    xSemaphoreGive(sub_rate[%<i>].sem);
                    xSemaphoreGive(sub_rate[%<i>].sem);
                %if !LibIsSingleTasking()
                }
              }
            %endif

            steps_control++;

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

        }
        simulationFinished = 1;

        /* Final step */
        %if !LibIsSingleTasking()
        for (%<i> = %<firstTask>; %<i> < NUM_TASKS; %<i>++) {
        %endif
            xSemaphoreGive(sub_rate[%<i>].sem);
            xSemaphoreGive(sub_rate[%<i>].sem);
        %if !LibIsSingleTasking()
        }
        %endif

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

    %foreach j = taskCount
        %assign i = j + firstTask
        /**
         * Model step logic execution task for sample time %<CompiledModel.SampleTime[i].ClockTickStepSize>.
         */
        void working_task%<i>(void *p)
        {
            steps_working[%<i>] = 0;

            while(!simulationFinished) {
                xSemaphoreTake(sub_rate[%<i>].sem, portMAX_DELAY); /* Sem_val = 1 */
                %<LibCallModelStep(i)>\
                steps_working[%<i>]++;
                xSemaphoreTake(sub_rate[%<i>].sem, portMAX_DELAY);  /* Sem_val = 0 */
            }

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

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

            %<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 */
         /* Initialize RPP board */
        %if EXISTS(::rpp_ain_present)
            rpp_adc_init();
        %endif
        %if EXISTS(::rpp_aout_present)
            rpp_dac_init();
        %endif
        %if EXISTS(::rpp_din_present)
            rpp_din_init();
        %endif
        %if EXISTS(::rpp_hbr_present)
            rpp_hbr_init();
        %endif
        %if EXISTS(::rpp_irc_present)
            rpp_irc_init();
        %endif
        %if EXISTS(::rpp_lout_present)
            rpp_lout_init();
        %endif
        %if EXISTS(::rpp_mout_present)
            rpp_mout_init();
        %endif
        %if EXISTS(::rpp_sdr_present)
            rpp_sdr_init();
        %endif
        %if EXISTS(::rpp_gio_in_present) || EXISTS(::rpp_gio_out_present)
            rpp_gio_init(RPP_GIO_PORT_ALL);
        %endif
        rpp_sci_init();

        %if extMode == 1
            rpp_eth_init();
        %endif
        
        // 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
 
        %if extMode == 1
          vSemaphoreCreateBinary(ext_mode_ready);
          xSemaphoreTake(ext_mode_ready, 0);
        %endif

        %foreach j = taskCount
            %assign i = j + firstTask
            %assign s = CompiledModel.SampleTime[i].PeriodAndOffset[0]
            %assign o = CompiledModel.SampleTime[i].PeriodAndOffset[1]

            /* Initializing the step semaphore of the loop %<i> */
            sub_rate[%<i>].sem = xSemaphoreCreateCounting(2, 0);
            if (sub_rate[%<i>].sem == NULL) {
                rpp_sci_printk("ERROR: Can not create semaphore for task %<i>.\r\n");
                while (1) ;
            }

            /* Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s. */
            if (xTaskCreate(working_task%<i>, "working_task%<i>",
                            %<rppModelTaskStack>, NULL, WORKING%<i>_PRIORITY,
                            &sub_rate[%<i>].thread ) != pdPASS) {
                rpp_sci_printk("ERROR: Cannot spawn working task %<i>.\r\n");
                while (1);
            }
        %endforeach

        /* Create tasks to step model and model task */

        if (xTaskCreate(control_task, "control_task",
                        %<rppModelTaskStack>, NULL, CONTROL_PRIORITY,
                        NULL) != pdPASS) {
            rpp_sci_printk("ERROR: Cannot spawn control task.\r\n");
            while (1);
        }

        %if extMode == 1
          /* External mode */

          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");
            while (1);
          }
        %endif

        /* Start FreeRTOS Scheduler */
        vTaskStartScheduler();

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


    #if configUSE_MALLOC_FAILED_HOOK == 1
    /**
     * FreeRTOS malloc() failed hook.
     */
     void vApplicationMallocFailedHook(void) {
       rpp_sci_printk("ERROR: Memory allocation failed.\r\n");
       while (1);
     }
    #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);
      while (1);
    }
    #endif

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

%endfunction