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

/* OSEK declarations */
DeclareCounter(SysTimerCnt);
DeclareTask(EventDispatcher);
DeclareTask(EventCallback);
DeclareTask(TaskLCD);

DeclareEvent(TouchSensorOnEvent); /* Event declaration */
DeclareEvent(TouchSensorOffEvent); /* Event declaration */

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

void ecrobot_device_terminate(void)
{
  nxt_motor_set_speed(NXT_PORT_A, 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);
  }
}

/* EventDispatcher executed every 1ms */
TASK(EventDispatcher)
{
  static U8 TouchSensorStatus_old = 0;
  U8 TouchSensorStatus;

  TouchSensorStatus = ecrobot_get_touch_sensor(NXT_PORT_S1);

  if (TouchSensorStatus == 1 && TouchSensorStatus_old == 0)
  {
    /* Send a Touch Sensor ON Event to the Handler */
    SetEvent(EventCallback, TouchSensorOnEvent);
  }
  else if (TouchSensorStatus == 0 && TouchSensorStatus_old == 1)
  {
    /* Send a Touch Sensor OFF Event to the Handler */
    SetEvent(EventCallback, TouchSensorOffEvent);
  }

  TouchSensorStatus_old = TouchSensorStatus;

  TerminateTask();
}

/* EventCallback executed by OSEK Events */
TASK(EventCallback)
{
  while(1)
  {
    WaitEvent(TouchSensorOnEvent); /* Task is in waiting status until the Event comes */
    ClearEvent(TouchSensorOnEvent);
    nxt_motor_set_speed(NXT_PORT_A, 50, 1);

    WaitEvent(TouchSensorOffEvent); /* Task is in waiting status until the Event comes */
    ClearEvent(TouchSensorOffEvent);
    nxt_motor_set_speed(NXT_PORT_A, 0, 1);
  }

  TerminateTask();
}

/* TaskLCD executed every 500ms */
TASK(TaskLCD)
{
  ecrobot_status_monitor("EDS");

  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 Touch Sensor ON Event */
  EVENT TouchSensorOnEvent{
    MASK = AUTO;
  };

  /* Definition of Touch Sensor OFF Event */
  EVENT TouchSensorOffEvent{
    MASK = AUTO;
  };

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

  /* Definition of EventCallback */
  TASK EventCallback
  {
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
    };
    PRIORITY = 2;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* Stack size */
    EVENT = TouchSensorOnEvent;
    EVENT = TouchSensorOffEvent;
  };

  /* 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 EventDispatcher execution timing */
  ALARM cyclic_alarm1
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = EventDispatcher;
    };
    AUTOSTART = TRUE
    {
      ALARMTIME = 1;
      CYCLETIME = 1; /* EventDispatcher is executed every 1msec */
      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;
    };
  };
};