%% 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 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.
%%
%% 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

    /* RPP includes */
    #include "rpp/rpp.h"

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

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


    %%%%%%%%
    %openfile tmpBuf

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

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


    %%%%%%%%
    %openfile tmpBuf

    static boolean_t WORKING = FALSE;
    static boolean_t SHUTDOWN = FALSE;
    static xSemaphoreHandle step_signal = NULL;
    static uint32_t steps_control = 0;
    static uint32_t steps_working = 0;

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


    %%%%%%%%
    %openfile tmpBuf

    void working_task(void* p);

    /**
     * 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();

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

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

        if(xTaskCreate(working_task, (const signed char*)"working_task",
            %<rppModelTaskStack>, NULL, WORKING_PRIORITY, NULL) != pdPASS) {
            #ifdef DEBUG
            rpp_sci_printf((const char*)
                "ERROR: Cannot spawn model task.\r\n"
            );
            #endif
            while(TRUE) {
                asm(" nop");
            }
        }

        while(!SHUTDOWN) {

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

            if(WORKING) {
                /* Overrun detected */
                %<LibSetRTModelErrorStatus("\"Overrun\"")>;
                /* FIXME: Call overrun routine or set some overrun flag. */
            } else {
                /* Release semaphore */
                xSemaphoreGive(step_signal);
            }
            steps_control++;

        }

        /* 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)>\
                steps_working++;
                WORKING = FALSE;
            }
        }

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


    /**
     * Hardware initialization, task spawning and OS scheduler start.
     */
    void main(void)
    {
        /* Initialize RPP board */
        rpp_init();

        // Speed up the SCI
        rpp_sci_setup(115200);

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

        /* Create tasks to step model and model task */
        if(xTaskCreate(control_task, (const signed char*)"control_task",
            configMINIMAL_STACK_SIZE, NULL, CONTROL_PRIORITY, NULL) != pdPASS) {
            #ifdef DEBUG
            rpp_sci_printf((const char*)
                "ERROR: Cannot spawn control task.\r\n"
            );
            #endif
            while(TRUE) {
                asm("nop");
            }
        }

        /* Start FreeRTOS Scheduler */
        vTaskStartScheduler();

        /* We should never get here */
        %<LibCallModelTerminate()>\
        #ifdef DEBUG
        rpp_sci_printf((const char*)
                "ERROR: Problem allocating memory for idle task.\r\n"
            );
        #endif
        while(TRUE) {
            asm(" nop");
        }
    }


    #if configUSE_MALLOC_FAILED_HOOK == 1
    /**
     * FreeRTOS malloc() failed hook.
     */
    void vApplicationMallocFailedHook(void) {
        #ifdef DEBUG
        rpp_sci_printf((const char*)
                "ERROR: manual memory allocation failed.\r\n"
            );
        #endif
    }
    #endif


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

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

%endfunction