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

/* OSEK declarations */
DeclareCounter(SysTimerCnt);
DeclareTask(TaskInitialize);
DeclareTask(TaskControl);
DeclareTask(TaskSonar);
DeclareTask(TaskLCD);

/* Definitions */
#define MOTOR_STEERING     NXT_PORT_A
#define MOTOR_LEFT         NXT_PORT_B
#define MOTOR_RIGHT        NXT_PORT_C
#define STEERING_LIMIT             40 /* degree */
#define STEERING_P_GAIN             2 /* proportinal gain */
#define DIFF_GAIN_MAX            0.7F /* 1.0-DIFF_GAIN_MAX: max. differential effect */
#define NEUTRAL_DEAD_ZONE           2 /* degree */
#define PWM_OFFSET                 10 /* friction compensator */
#define EDC_ON                     -1 /* Electronic Differential Control: ON */
#define EDC_OFF                     1 /* Electronic Differential Control: OFF */

static S8 EDC_flag;                   /* EDC flag */

/* Prototypes */
S32 FrictionComp(S32 ratio, S32 offset);

/* OSEK hooks */
void StartupHook(void){}
void ShutdownHook(StatusType ercd){}
void PreTaskHook(void){}
void PostTaskHook(void){}
void ErrorHook(StatusType ercd){}

/* nxtOSEK hooks */
void ecrobot_device_initialize(void)
{
  ecrobot_set_light_sensor_active(NXT_PORT_S1);
  ecrobot_set_light_sensor_active(NXT_PORT_S3);
  ecrobot_init_sonar_sensor(NXT_PORT_S2);
  ecrobot_init_bt_connection();
}

void ecrobot_device_terminate(void)
{
  nxt_motor_set_speed(MOTOR_STEERING, 0, 1);
  nxt_motor_set_speed(MOTOR_RIGHT, 0, 1);
  nxt_motor_set_speed(MOTOR_LEFT, 0, 1);
  ecrobot_set_light_sensor_inactive(NXT_PORT_S1);
  ecrobot_set_light_sensor_inactive(NXT_PORT_S3);
  ecrobot_term_sonar_sensor(NXT_PORT_S2);
  ecrobot_term_bt_connection();
}

void user_1ms_isr_type2(void)
{
  StatusType ercd;

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

/* TaskInitialize */
TASK(TaskInitialize)
{
  nxt_motor_set_speed(MOTOR_STEERING, 0, 1);
  nxt_motor_set_speed(MOTOR_RIGHT, 0, 1);
  nxt_motor_set_speed(MOTOR_LEFT, 0, 1);
  nxt_motor_set_count(MOTOR_STEERING, 0);
  nxt_motor_set_count(MOTOR_RIGHT, 0);
  nxt_motor_set_count(MOTOR_LEFT, 0);

  EDC_flag = EDC_OFF;

  TerminateTask();
}

/* TaskControl executed every 10msec */
TASK(TaskControl)
{
  S32 analog_stick_left;  /* speed command data from GamePad */
  S32 analog_stick_right; /* steering command data from GamePad */
  S32 steering_angle;
  S32 steering_err;
  S32 steering_speed;
  S32 diff_gain;
  U8 touch_sensor;
  static U8 touch_sensor_state = 0;
  static U8 bt_receive_buf[32]; /* buffer size is fixed as 32 */

  /* receive NXTGamePad command
   * byte0 speed_data -100(forward max.) to 100(backward max.)
   * byte1 steering_data -100(left max.) to 100(right max.)
   */
  ecrobot_read_bt_packet(bt_receive_buf, 32);
  analog_stick_left = -(S32)(*(S8 *)(&bt_receive_buf[0])); /* reverse the direction */
  analog_stick_right = (S32)(*(S8 *)(&bt_receive_buf[1]));

  /* read Touch Sensor to switch Electronic Differential Control */
  touch_sensor = ecrobot_get_touch_sensor(NXT_PORT_S4);
  if (touch_sensor == 1 && touch_sensor_state == 0)
  {
    EDC_flag = ~EDC_flag + 1; /* toggle */
  }
  touch_sensor_state = touch_sensor;

  /* steering control */
  steering_angle = nxt_motor_get_count(MOTOR_STEERING);
  steering_err = (STEERING_LIMIT*analog_stick_right)/100 - steering_angle;
  steering_speed = STEERING_P_GAIN*steering_err;
  nxt_motor_set_speed(MOTOR_STEERING, FrictionComp(steering_speed,PWM_OFFSET), 1);

  /* wheel motors control with Electric Differential Control */
  diff_gain = 10;
  if (steering_angle > NEUTRAL_DEAD_ZONE) /* turn right */
  {
    if (EDC_flag == EDC_ON)
    {
      diff_gain = (S32)((1.0F - (float)steering_angle*DIFF_GAIN_MAX/STEERING_LIMIT)*10);
    }
    nxt_motor_set_speed(MOTOR_RIGHT,
      FrictionComp((analog_stick_left*diff_gain)/10,PWM_OFFSET), 1);
    nxt_motor_set_speed(MOTOR_LEFT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);
  }
  else if (steering_angle < -NEUTRAL_DEAD_ZONE) /* turn left */
  {
    if (EDC_flag == EDC_ON)
    {
      diff_gain = (S32)((1.0F + (float)steering_angle*DIFF_GAIN_MAX/STEERING_LIMIT)*10);
    }
    nxt_motor_set_speed(MOTOR_RIGHT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);
    nxt_motor_set_speed(MOTOR_LEFT,
      FrictionComp((analog_stick_left*diff_gain)/10,PWM_OFFSET), 1);
  }
  else /* go straight */
  {
    nxt_motor_set_speed(MOTOR_RIGHT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);
    nxt_motor_set_speed(MOTOR_LEFT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);
  }

  /* send NXT status data to NXT GamePad */
  ecrobot_bt_data_logger((S8)analog_stick_left, (S8)analog_stick_right);

  TerminateTask();
}

/* TaskSonar executed every 50msec */
TASK(TaskSonar)
{
  S32 sonar;

  /* Sonar Sensor is invoked just for data logging */
  sonar = ecrobot_get_sonar_sensor(NXT_PORT_S2);

  TerminateTask();
}

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

  TerminateTask();
}

/* Sub functions */
S32 FrictionComp(S32 ratio, S32 offset)
{
  if (ratio > 0)
  {
    return ((100-offset)*ratio/100 + offset);
  }
  else if (ratio < 0)
  {
    return ((100-offset)*ratio/100 - offset);
  }
  else
  {
    return ratio;
  }
}

#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 TaskInitialize */
  TASK TaskInitialize
  {
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
    };
    PRIORITY = 4;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* Stack size */
  };

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

  /* Definition of TaskSonar */
  TASK TaskSonar
  {
    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 TaskControl execution timing */
  ALARM cyclic_alarm_TaskControl
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = TaskControl;
    };
    AUTOSTART = TRUE
    {
      ALARMTIME = 1;
      CYCLETIME = 10; /* executed every 10msec */
      APPMODE = appmode1;
    };
  };

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

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