%% 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 TLC file to generate a "main" file.
%%
%%     This file generated the "main" file for the RPP target on top of the RPP
%%     library and the FreeRTOS operating system.
%%
%% 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>

    /* Kernel includes */
    #include "FreeRTOS.h"
    #include "task.h"
    #include "semphr.h"

    /* 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) {

        static 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
                %<LibSetRTModelErrorStatus("\"Overrun\"")>;
                // 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 model */
        %<LibCallModelInitialize()>\
        _enable_IRQ();

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

        /* Create tasks to step model and start scheduler */
        xTaskCreate(control_task, (signed char*) "control_task", 128, NULL,
            CONTROL_PRIORITY, NULL);
        xTaskCreate(working_task, (signed char*) "working_task", %<rppModelTaskStack>, NULL,
            WORKING_PRIORITY, NULL);
        vTaskStartScheduler();

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


    /**
     * FreeRTOS malloc() failed hook.
     */
    void vApplicationMallocFailedHook(void) {
        // FIXME: Implement.
    }

    /**
     * FreeRTOS stack overflow hook.
     */
    void vApplicationStackOverflowHook(xTaskHandle xTask,
                                       signed portCHAR *pcTaskName) {
        // FIXME: Implement.
    }

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

%endfunction