/* Real Time Scheduler for a robot with two motors connected to
   output ports A and C.
   Last updated 24.2.02
 */

#include "H8.h"
#include "RCX_Interrupts.h"
#include "Timers.h"



#include "TimeCounters.h"
#include "Scheduler.h"

/* Time-driven modules used in the BugRTS */
#include "MotorControl.h"

#include "Math.h"

#ifndef   RTS_H_DEFINED
#define   RTS_H_DEFINED

#define forever while (1)


/* The timer T0 is used to interrupt periodically. 
 * On each interrupt the T0Handler calls routines 
 *
 *   to update counters related to real time, and
 *   to schedule processes.
 *
 *   to produce the puls waveform needed to control the motors.
 *
 * These routines should be called so the one with the highest
 * time precision requirement is called first. E.g. a
 * sound modulation routine is called before the motor routine.
 *
 * The period is 1 msec.
 *
 */


static word SavedSP;

void T0Handler ( void ) 
{
  /* Stack on entry:
        C prolog         r4 of interrupted process
                         r6 address of T0Handler
        RCX dispatcher   return to T0 dispatcher
                         r6 and 
        H8 hardware      cc and
                         pc of interrupted process
     This is used in StartProcess of the Scheduler. Change initial
     stack if stack on entry changes because of register saving
     in the C prolog.
  */
  /* Save remaining registers of interrupted process */
  asm("
        push r0
        push r1
        push r2
        push r3 
        push r5
      "
     );

  /* Save stack pointer in C variable */
  asm("
        mov.w sp,%0
      "
       : "=r" (SavedSP)
      );

  DisableT0;
  Enable;

  /* RobotRTS time-driven routines */
  MotorControlUpdate(); 

  /* Time keeping */  
  TimeCountersUpdate();   

  /* Schedule processes */
  SavedSP = ScheduleProcess( SavedSP );

  Disable;
  EnableT0;

  T0_TCSR &= ~bit6; /* Clear CMFA flag */

  /* Restore stack pointer from C variable */
  asm("
        mov.w %0,sp
      "
       : 
       : "r" (SavedSP)
     ); 
     
  /* Restore registers and start scheduled process */
  asm("
        pop r5
        pop r3
        pop r2
        pop r1
        pop r0
      "
     );
}	

/* For behaviour control only an accuracy of seconds or tenth of 
seconds is needed */

uint16 TenthSec( void )
{
  return ( imul( 10, RealTime ) + idiv( MSTime + 50 , 100 ) );
}

byte WaitUntilSEC( byte * Break, uint16 s )
{
  byte stop; uint16 now;

  now = RealTime; stop = FALSE;
  while ( ! stop & ( RealTime < now + s ) )
          stop = WaitUntil( Break, 50 );
  return stop;
}

byte WaitUntilTenthSEC( byte * Break, uint16 ts )
{
  byte stop; uint16 now;

  now = TenthSec(); stop = FALSE;
  while ( ! stop & ( TenthSec() < now + ts ) )
          stop = WaitUntil( Break, 50 );
  return stop;
}


void  RTSInit ( void )
{
  DisableT0;

  /* Install T0 interrupt handler */
  T0_Interrupt_Handler = T0Handler;

  /* Set T0 to interrupt every msec. An internal clock is
     used to increment the time counter TCNT each 4 usec. 
     Time counter A is set to 250 to request an interrupt on
     compare-match A, i.e. an interrupt period of 1 msec.
   */
  T0_TCSR &= 0;    /* Clear all flags and select no output */
  /* Internal clock 4 usec, T0_TCNT = 0 on compare-match A */
  T0_TCR  |= ( bit3 | bit1 ); STCR = 0;  
  T0_TCORA = 250;  
  T0_TCNT  =   0; 

  /* Initialize modules that need an event each msec. */
  TimeCountersInit();
  SchedulerInit();

  /* Initialize RobotRTS time-driven modules */
  MotorControlInit();

  EnableT0;
}	

void RTSClose( void )
{
  /* Disable interrupts from timer 0 */
  DisableT0;

  /* Close down modules */
  TimeCountersClose();
  SchedulerClose();

  /* Close down RobotRTS modules */
  MotorControlClose();
}
		   
#endif /* RTS_H_DEFINED */