%%
%% linux_ert_target_multitasking_main.tlc
%%
%% description:
%%  This TLC script generates ert_main for multi-rate / multi-tasking
%%  case.
%%
%% date: 3nd Feb 2009
%%
%% author: Lukas Hamacek
%% e-mail: hamacl1@fel.cvut.cz
%% Department of Control Engineering
%% Faculty of Electrical Engineering
%% Czech Technical University in Prague
%%
%% modifications:
%%  2009/2/3 - Lukas Hamacek
%%      creation of the file
%%

%% GENERATEDECLARATIONS
%%  This function generates main function declarations.
%%
%function generateDeclarations() Output

  /* Multirate - Multitasking case main file */

  #include <stdio.h>                     /* This ert_main.c example uses printf/fflush */
  #include <pthread.h>                   /* Thread library header file */
  #include <sched.h>                     /* OS scheduler header file */
  #include <semaphore.h>                 /* Semaphores library header file */
  #include "pthread_periodic.h"          /* Periodic threads library header file*/
  #include "%<modelName>.h"              /* Model's header file */
  #include "rtwtypes.h"                  /* MathWorks types */
  %if extMode == 1
    #include "ext_work.h"                  /* External mode header file */
  %endif

  /**
   * Maximal priority used by main loop thread.
   */
  #define MAX_PRIO sched_get_priority_max(SCHED_FIFO) - 4

  /**
   * Thread handler of the main loop thread.
   * Fundamental sample time = %<fundamentalStepSize>s
   */
  pthread_t main_loop_thread;

  %foreach i = numSampleTimes
    %assign s = sampleTime%<i>
    %assign o = offset%<i>

    /**
     * Thread handler of the sample time %<i> loop thread.
     * Sample time = %<s>s, offset = %<o>s
     */
    pthread_t rt_OneStep%<i>_thread;

    /**
     * Semaphore used to suspend rt_OneStep%<i> loop until next simulation step.
     */
    sem_t step%<i>_semaphore;
  %endforeach

  /**
   * Flag if the simulation has been terminated.
   */
  int simulationFinished = 0;

%endfunction


%% GENERATERTONESTEP
%%  This function generates code of rt_OneStep function.
%%
%function generateRtOneStep() Output

  %foreach i = numSampleTimes
    %assign s = sampleTime%<i>
    %assign o = offset%<i>

    /**
     * This is thread function of the simulation one step cycle.
     * Sample time = %<s>s, offset = %<o>s
     */
    void * rt_OneStep%<i>()
    {
        /* Setting up the pririty of the sample time loop */
        struct sched_param scheduling_parameters;
        scheduling_parameters.sched_priority = MAX_PRIO - 1 - %<i>;
        if (0 != pthread_setschedparam(pthread_self(), SCHED_FIFO, &scheduling_parameters))
        {
            %%log_message("[ERROR] Failed to set sample time thread priority, Login as root and start again.", VERBOSEL);
            printf("[ERROR] Failed to set the loop %<i> thread priority. Login as root and start again.\n");
        }

        while(!simulationFinished) {
            sem_wait(&step%<i>_semaphore);    /* sem_val = 1 */
            %<modelName>_step%<i>();
            sem_wait(&step%<i>_semaphore);    /* sem_val = 0 */
        }
    }
  %endforeach
%endfunction


%% GENERATEMAIN
%%  This function generates code of the main function function.
%%
%function generateMain() Output

  /**
   * This is the thread function of the main loop.
   * Fundamental sample time = %<fundamentalStepSize>s
   */
  void * main_loop()
  {
    struct timespec start;
    struct timespec period;
    boolean_T eventFlags[%<numSampleTimes>];             /* Model has %<numSampleTimes> rates */
    int_T i;

    /* Setting up the pririty of the main loop */
       struct sched_param scheduling_parameters;
       scheduling_parameters.sched_priority = sched_get_priority_max(SCHED_FIFO) - 4;
    if (0 != pthread_setschedparam(pthread_self(), SCHED_FIFO, &scheduling_parameters))
    {
      %%log_message("[ERROR] Failed to set main thread priority, Login as root and start again", VERBOSEL);
      printf("[ERROR] Failed to set main thread priority. Login as root and start again.\n");
    }

    period.tv_sec = (unsigned long long)%<fundamentalStepSize>;
    period.tv_nsec = (unsigned long long)((%<fundamentalStepSize>-period.tv_sec)*1000000000);

    clock_gettime(CLOCK_REALTIME, &start);
    pthread_make_periodic_np(pthread_self(), &start, &period);

    int step_sem_value;

    /* Main loop, running until all the threads are terminated */
    while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
      pthread_wait_np();
      %<modelName>_SetEventsForThisBaseStep(eventFlags);

      /* Base sampling rate */
      sem_getvalue(&step0_semaphore, &step_sem_value);
      if(step_sem_value) {
	rtmSetErrorStatus(%<modelName>_M, "Overrun");
	printf("Loop 0 overrun, sample time=%<sampleTime0>s, offset=%<offset0>s is too fast\n");
	break;
      }
      sem_post(&step0_semaphore);
      sem_post(&step0_semaphore);

      %foreach i = numSampleTimes-1
	%assign s = sampleTime%<i+1>
	%assign o = offset%<i+1>
	/* Sampling rate %<i+1>, sample time = %<s>, offset = %<o> */
	if (eventFlags[%<i+1>]) {
	  sem_getvalue(&step%<i+1>_semaphore, &step_sem_value);
	  if(step_sem_value) {
	    rtmSetErrorStatus(%<modelName>_M, "Overrun");
	    printf("Loop %<i+1> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");
	    break;
	  }
	  sem_post(&step%<i+1>_semaphore);
	  sem_post(&step%<i+1>_semaphore);
	}
      %endforeach

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

    simulationFinished = 1;
    /* Final step */
    %foreach i = numSampleTimes
      sem_post(&step%<i>_semaphore);
      sem_post(&step%<i>_semaphore);
    %endforeach
  }

  /**
   * This is the main function of the model.
   * Multirate - Multitasking case main file
   */
  int_T main(int_T argc, const char_T *argv[])
  {
    %if extMode == 1
      /* External mode */
      rtERTExtModeParseArgs(argc, argv);
    %else
      (void)(argc);
      (void *)(argv);
    %endif

    /* Initialize periodic thread library */
    pthread_periodic_init_np();

    /* Initialize model */
    %<modelName>_initialize(1);
    simulationFinished = 0;

    %foreach i = numSampleTimes
      %assign s = sampleTime%<i>
      %assign o = offset%<i>

      /* Initializing the step semaphore of the loop %<i>*/
      if(sem_init(&step%<i>_semaphore, 0, 0) < 0) {
	printf("[ERROR] Step semaphore %<i> initialization failed!\n");
	%%log_message("[ERROR] Step semaphore %<i> initialization failed", VERBOSEL);
	return(1);
      }

      /* Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s. */
      pthread_create(&rt_OneStep%<i>_thread, NULL, rt_OneStep%<i>, NULL);
    %endforeach

    /* Starting the main loop */
    pthread_create(&main_loop_thread, NULL, main_loop, NULL);
    pthread_join(main_loop_thread, NULL);
    %foreach i = numSampleTimes
      pthread_join(rt_OneStep%<i>_thread, NULL);
    %endforeach

    /* Terminate model */
    %<modelName>_terminate();

    const char_T *errStatus = rtmGetErrorStatus(%<modelName>_M);
    int_T i;
    if(errStatus != NULL && strcmp(errStatus, "Simulation finished")) {
      %%printf("%s\n", rtmGetErrorStatus(%<modelName>_M));
      if(!strcmp(errStatus, "Overrun")) {
	printf("ISR overrun - sampling rate too fast\n");
      }
      return(1);
    }
    return 0;
  }
%endfunction

%% [EOF] linux_ert_target_miltitasking_main.tlc