%% Copyright (C) 2013 Czech Technical University in Prague
%%
%% Authors:
%%     - Carlos Jenkins <carlos@jenkins.co.cr>
%%
%% This program is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published by
%% the Free Software Foundation; either version 2 of the License, or
%% (at your option) any later version.
%%
%% This program is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty of
%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%% GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public License
%% along with this program.  If not, see <http://www.gnu.org/licenses/>.
%%
%% File : rpp_srmain.tlc
%% Abstract:
%%     Custom file processing to generate a "main" file.
%%
%% References:
%%     Example in <matlabroot>/rtw/c/tlc/mw/bareboard_srmain.tlc


%selectfile NULL_FILE

%function FcnSingleTaskingMain() void

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


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


    %%%%%%%%
    %openfile tmpBuf

    /* Standard includes */
    #include <stdbool.h>
    #include <math.h>

    /* Kernel includes */
    #include "FreeRTOS.h"
    #include "os_task.h"
    #include "os_semphr.h"
    %% FIXME: Return to original file names
    %%#include "task.h"
    %%#include "semphr.h"

    /* System includes */
    #include "sys_common.h"
    #include "system.h"

    /* Library includes */
    // FIXME: Implement conditional #include's
    %% IF GIO (General Input/Outputs)
    %% IF HOUT (PWM outputs)
    %% IF MOUT (Motor outputs)
    %% IF ADC (Analog to Digital Converter)
    %% IF SCI (Serial Communication Interface)
    %% IF CAN (CAN Bus)
    %% IF SDRAM

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

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


    %%%%%%%%
    %openfile tmpBuf

    #define STEP_SIZE_MILLIS (%<CompiledModel.FundamentalStepSize>*1000.0)
    #define CONTROL_PRIORITY 2
    #define WORKING_PRIORITY 1

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


    %%%%%%%%
    %openfile tmpBuf

    bool WORKING = false;
    bool SHUTDOWN = false;
    xSemaphoreHandle step_signal = NULL;

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


    %%%%%%%%
    %openfile tmpBuf

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

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

        while(!SHUTDOWN) {

            // Wait until next step
            vTaskDelayUntil(&last_wake_time, freq_ticks);

            if(WORKING) {
                // Overrun detected
                // FIXME: Call overrun routine
            } else {
                // Release semaphore
                xSemaphoreGive(step_signal);
            }

        }

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

    /**
     * Model step logic execution task.
     */
    void working_task(void *p) {

        while(!SHUTDOWN) {

            // Lock semaphore
            if(xSemaphoreTake(step_signal, portMAX_DELAY)) {
                WORKING = true;
                %<LibCallModelStep(0)>\
                WORKING = false;
            }
        }

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


    /**
     * Hardware initialization, task spawning and OS scheduler start.
     */
    void main(void)
    {

        /* Initialize peripherals */
        // FIXME: Implement conditional init()'s
        _enable_IRQ();

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

        /* Create and lock semaphore */
        vSemaphoreCreateBinary(step_signal);
        xSemaphoreTake(step_signal, 0);

        /* Create tasks to step model and start scheduler */
        // FIXME: How to calculate / policy about model stack size
        xTaskCreate(control_task, (signed char*) "control_task", 128,  NULL,
            CONTROL_PRIORITY, NULL);
        xTaskCreate(working_task, (signed char*) "working_task", 4096, NULL,
            WORKING_PRIORITY, NULL);
        vTaskStartScheduler();

        // We should never get here
        %<LibCallModelTerminate()>\
        while(true) {
        }
    }

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

%endfunction