/* rms.c */
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

/* OSEK declarations */
DeclareCounter(SysTimerCnt);
DeclareTask(Task1);
DeclareTask(Task2);
DeclareTask(TaskLCD);

/* nxtOSEK hooks */
void ecrobot_device_initialize(void)
{
  nxt_motor_set_speed(NXT_PORT_A, 0, 1);
  nxt_motor_set_speed(NXT_PORT_B, 0, 1);
}

void ecrobot_device_terminate(void)
{
  nxt_motor_set_speed(NXT_PORT_A, 0, 1);
  nxt_motor_set_speed(NXT_PORT_B, 0, 1);
}

/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void)
{
  StatusType ercd;

  ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */
  if (ercd != E_OK)
  {
    ShutdownOS(ercd);
  }
}

/* Task1 executed every 1msec */
TASK(Task1)
{
  if (ecrobot_get_touch_sensor(NXT_PORT_S1))
  {
    nxt_motor_set_speed(NXT_PORT_A, 0, 1);
  }
  else
  {
    nxt_motor_set_speed(NXT_PORT_A, 50, 1);
  }

  TerminateTask();
}

/* Task2 executed every 500msec */
TASK(Task2)
{
  if (ecrobot_get_touch_sensor(NXT_PORT_S1))
  {
    nxt_motor_set_speed(NXT_PORT_B, 0, 1);
  }
  else
  {
    nxt_motor_set_speed(NXT_PORT_B, 50, 1);
  }

  TerminateTask();
}

/* TaskLCD executed every 500msec */
TASK(TaskLCD)
{
  ecrobot_status_monitor("RMS");

  TerminateTask();
}

#include "implementation.oil"

CPU ATMEL_AT91SAM7S256
{
  OS LEJOS_OSEK
  {
    STATUS = EXTENDED;
    STARTUPHOOK = FALSE;
    ERRORHOOK = FALSE;
    SHUTDOWNHOOK = FALSE;
    PRETASKHOOK = FALSE;
    POSTTASKHOOK = FALSE;
    USEGETSERVICEID = FALSE;
    USEPARAMETERACCESS = FALSE;
    USERESSCHEDULER = FALSE;
  };

  /* Definition of application mode */
  APPMODE appmode1{};

  /* Definition of Task1 */
  TASK Task1
  {
    AUTOSTART = FALSE;
    PRIORITY = 3;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* Stack size */
  };

  /* Definition of Task2 */
  TASK Task2
  {
    AUTOSTART = FALSE;
    PRIORITY = 2;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* Stack size */
  };

  /* Definition of TaskLCD */
  TASK TaskLCD
  {
    AUTOSTART = FALSE;
    PRIORITY = 1;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* Stack size */
  };

  /* Definition of OSEK Alarm Counter */
  COUNTER SysTimerCnt
  {
    MINCYCLE = 1;
    MAXALLOWEDVALUE = 10000;
    TICKSPERBASE = 1; /* One tick is equal to 1msec */
  };

  /* Definition of Task1 execution timing */
  ALARM cyclic_alarm1
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = Task1;
    };
    AUTOSTART = TRUE
    {
      ALARMTIME = 1;
      CYCLETIME = 1; /* Task1 is executed every 1msec */
      APPMODE = appmode1;
    };
  };

  /* Definition of Task2 execution timing */
  ALARM cyclic_alarm2
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = Task2;
    };
    AUTOSTART = TRUE
    {
      ALARMTIME = 1;
      CYCLETIME = 500; /* Task2 is executed every 500msec */
      APPMODE = appmode1;
    };
  };

  /* Definition of TaskLCD execution timing */
  ALARM cyclic_alarmLCD
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = TaskLCD;
    };
    AUTOSTART = TRUE
    {
      ALARMTIME = 1;
      CYCLETIME = 500; /* TaskLCD is executed every 500msec */
      APPMODE = appmode1;
    };
  };
};