#include <At89x55.h>    
#include <RTXtiny.h>

#define RECV_BOT 0
#define SONAR_BOT 1
#define SEND_BOT 2

#define MEASUREMENTS 3

/* timer 0 is for the system */
/* timer 1 is for the serial I/O */
/* timer 2 is for the sonar time measurement */

/* globals */
bit ackPending;           // set when an acknowledgement is pending
bit newMeasurement;       // set when a new distance measurement is completed
bit delayMode;            // tells T2 ISR that we are in delay mode, not listen mode
byte pendingMeasurements; // determines number of pending measurements
byte sum;                 // sum of previous measurements in this roundint timeOfFlight;         
int timeOfFlight;         // time of flight in this measurement (machine cycles)


// T2 interrupt handler, really two tasks SONAR_DELAY and SONAR_ECHO
void sonar_top interrupt T2 {
  if (delayMode) {
    delayMode = FALSE;
    setT2CaptureMode();   // now we pay attention to ECHO
  } else {
    INIT = 0;
    timeOfFlight = ((T2CAPH<<8) + T2CAPL)/scaleFactor;
    newMeasurement = TRUE;
    isr_send_signal(SONAR_BOTTOM);
  }
}

/* handles end of transmission (send or receive) */
void serial_top interrupt SERIAL {
  if (RI) {
    os_send_signal(RECV_BOT);
    RI = 0;
  }
  if (TI) 
    TI = 0;
}

void sonar_bottom _tast_ SONAR_BOTTOM {
  while (1) {
    os_wait(K_SIG, 0, 0);                        // wait to be signalled 
    disableT2();                                 // no harm in doing this
    if (newMeasurement) {                        // only true after SONAR_TOP executes
      newMeasurement = FALSE;
      pendingMeasurements--;                     
      distance = time2Distance(timeOfFlight);    // no danger of interruption by SONAR_TOP!!
      enq(distance);                             // what if no room in the queue?
      sum += distance;                            
      os_send_signal(SEND_BOT);                 // iform SEND_BOT of new data
    }
    if (pendingMeasurements) {
      INIT = 1;                                 // starts measurement
      delayMode = TRUE;                        // 
      setT2OverflowMode(500);                   // will cause T2 interrupt in 500uS
    } else {
      sum = sum / MEASUREMENTS;                 // compute average
      enq(sum);                                 // put average in send queue
      os_send_signal(SEND_BOT);                 // inform SEND_BOT of new data
    }
  }
}

void send_bottom() _task_ SEND_BOT {
  char c;
  while (1) {
    if (!ackPending) {                          // do nothing until acknowledgement received
      if (deq(&c)) {							// get data from queue
      	disable();                              // begin critical section
		SBUF = c;                               // starts serial transmission
		ackPending = TRUE;                      // set ackPending flag
		enable(); 								// critical section here!
      }
    }
    os_wait(K_SIG, 0, 0);   // wait for an acknowledgement or byte to send
  }
}

void recv_bottom() _task_ RECV_BOT  {
  char c;

  sonarReset();               
  setupTimer1AsSerial();
  enableSerialInterrupts();

  // create other tasks
  os_create(SEND_BOT);
  os_create(SONAR_BOT);

  // process incoming bytes
  while (1) {
    os_wait(K_SIG, 0, 0);    // wait until byte received;
    c = SBUF;                // read received character from the buffer
    if (c == 'A') {
      if (ackPending) {
	ackPending = FALSE;
	os_send_signal(SEND_BOT);  // send next byte if in queue
      } // otherwise ignore the acknowledgement
    }
    if (c == 'M') {          // start a new measurement
      sonarReset();        
      os_send_signal(SONAR_BOT);
    }
  }
}

void sonarReset() {
      EA = 0;
      disableT2();           // stop whatever measurement is in progress
      INIT = 0;              // Turn off the sonar
      head = tail = 0;       // reset the queue
      pendingMeasurements = MEASUREMENTS;	
      ackPending = FALSE;
      newMeasurement = FALSE;
      sum  = 0;
      EA = 1;
}

/* the queue and support routines */
byte array[MEASUREMENTS+1];    // enough space for each measurement and the average.
volatile byte head, tail;

void enq(char data) {
  while (next(head) == tail);  // make enq blocking. This stops the measurement process when queue full
  array[head] = data;
  EA = 0;                      // queue access is atomic
  head = next(head);
  EA = 1;
  return;
}

bit deq(char *data) {
  if (head == tail) return 0;  // if queue is empty then return 0
  *data = array[tail];
  EA = 0;
  tail = next(tail);           // queue access is atomic
  EA = 1;
  return 1;
}

byte next(int ptr) {           
  // not declared as reentrant because 
  // it is protected by the critical sections
  if (ptr++ == MEASUREMENTS) ptr = 0;
  return ptr;
} 

  
Schedulability analysis:
Worst case analysis:

SERIAL_TOP
P(SERIAL_TOP) = .83ms (continuous 9600 baud)
U(SERIAL_TOP) = .018ms/.833ms = ~2%

SONAR_ECHO
P(SONAR_ECHO) = .83ms (sonar round trip at .14 meters)
U(SONAR_ECHO) = .018ms/.83 = ~2%

SONAR_DELAY
P(SONAR_TOP) = .83ms (sonar round trip at .14 meters)
U(SONAR_TOP) = .005ms/.83 = negligible

Pt = .83ms. At such low utilizations it is clear that we can run all of the other tasks at least once in one Pt period. So we know we won't fall behind on total work. But is this a strong enough statement? There are some critical deadlines that affect performance and quality.

Key deadlines:
    INIT hi --> ECHO enabled > 500us. 
This is ensured by the SONAR_ECHO task. T2 is first used to count out 500us, then it is used to capture the time of flight for the signal. We probably need to add some offset to the T2 capture reading to account for the first 500us.

    T2 Overflow --> Capture Mode Enabled (SONAR_DELAY) < (minimum time of flight-.5ms) = .33ms 
This is probably our tightest deadline
The worst case latency to SONAR_DELAY is R(SERIAL_TOP) which is dominated by the isr_send_signal() system call. We are probably okay. 330us is 880 machine cycles at 32MHz.  Subtract the 20 machines cycles that the OS addes to interrupt latency to get 860 available machine cycles for R(SERIAL_TOP). 

    ECHO hi --> time of flight captured
This is done in hardware using capture mode so the guaranteed latency is at the clock cycle level. The input sample rate determine the impact on precision. Not sure if ECHO is sampled on machine cycles or clock cycles. Even at 32Mhz/12 our distance error due to sampling rate is at most .0001 meters! 

    ECHO hi --> INIT hi for next measurement ... time from end of one measurement to start of another.
This is a soft deadline, the sooner the better. More readings in less time. 
The worst case time to completion of SONAR_BOT from ECHO-hi is indefinite if the queue is full and the host does not send an acknolwedgement. However, the system is designed so that the queue can never be full (M causes the queue to be reset and all pending measurements to be abandoned), and the queue is large enough for a full complement of measurements and the average. The best way to determine the worst case is with a call tree, though a timing diagram would help to visualize events.

Worst case is:
(RI followed closely by ECHO-hi)  that would lead to the the following execution sequence:
SERIAL_TOP->RECV_BOT->SEND_BOT, SONAR_TOP->SONAR_BOT. We have to account for the execution times of each routine as well as the three context switches that occur. Since we have limited stack use, we can assume ~200 machine cycles / switch, which dominates the run time. So the worst case is on the order of 600 machine cycles or .225ms between measurements. But, usually much less. Note that the OS time tick is likely to be much longer than the execution time of the routines so we don't consider its affects.

    There are no other important deadlines in the system.