provided code + sensors + heap for dijkstras

include/generic-heap.h

+#ifndef GENERIC_HEAP_H
+#define GENERIC_HEAP_H
+
+int printHeap(MinHeap* Heap);
+
+int GetMinHeap(MinHeap* Heap, TrackNode* MinNode);
+
+void RemoveMinHeap(MinHeap* Heap);
+
+void InsertHeap(MinHeap* Heap, TrackNode* NewNode);
+
+void BubbleDownHeap(MinHeap* Heap, int StartIndex);
+
+void BubbleUpHeap(MinHeap* Heap, int Last);
+
+
+
+#endif
+

include/sensors.h

@@ -58,11 +58,13 @@ The second side of the s88 is read the same way, only it will be in the second b
 The s88 units can be told to reset its memory after a dump to the computer, or it can remain as it was and continue adding data to the byte. A reset would make each position in memory a "0" again. If you want it to reset, send the single byte 192 without adding any numbers to it. If the reset mode is to be "off", then send the code 128. 
 */
 
-void PrintSensors(char* SensorChars, int* SensorNums);
+void SensorPrintTask();
 
 void ProcessData (int* Sensors, char* SensorChars, int* SensorNums);
 
-void SensorTask();
+void SensorNotifier();
+
+void SensorServer();
 
 
 #endif
\ No newline at end of file

include/track_data.h

+/* THIS FILE IS GENERATED CODE -- DO NOT EDIT */
+
+#include <types.h>
+
+// The track initialization functions expect an array of this size.
+#define TRACK_MAX 144
+
+void init_tracka(track_node *track);
+void init_trackb(track_node *track);
\ No newline at end of file

include/types.h

@@ -81,6 +81,61 @@
 #define CURVED 34
 #define SOLENOID_OFF 32
 
+typedef enum TrackServerType {
+	GetSensorNode,
+	GetNextSensorNode,
+	GetPathSrcDest
+} TSType;
+
+typedef struct trackServerMessage {
+	TSType Type;
+	TrackNode* Src, Dest;
+} TSMsg;
+
+
+#define INT_MAX 2147483647; 
+typedef struct track_edge TrackEdge;
+typedef struct track_node TrackNode;
+
+/* BELOW Taken from the track data on the website BELOW */
+typedef enum {
+  NODE_NONE,
+  NODE_SENSOR,
+  NODE_BRANCH,
+  NODE_MERGE,
+  NODE_ENTER,
+  NODE_EXIT,
+} node_type;
+
+#define DIR_AHEAD 0
+#define DIR_STRAIGHT 0
+#define DIR_CURVED 1
+
+struct track_node;
+typedef struct track_node track_node;
+typedef struct track_edge track_edge;
+
+struct track_edge {
+  track_edge *reverse;
+  track_node *src, *dest;
+  int dist;             /* in millimetres */
+};
+
+struct track_node {
+  const char *name;
+  node_type type;
+  int num;              /* sensor or switch number */
+  track_node *reverse;  /* same location, but opposite direction */
+  track_edge edge[2];
+
+  /* for Dijkstras */
+  char bVisited;
+  track_node * Parent;
+  int PathDistance;
+};
+
+/* ABOVE Taken from the track data on the website ABOVE */
+
 typedef enum SensorServerMessageType {
 	SENSOR_RESPONSE,
 	SENSOR_NOTIFIER,

lib/display.c

@@ -265,7 +265,7 @@ void InitLayout(){
 	Printf(COM2,"|\n\r");
 	//Printf(COM2, "|       Current Location:  A00            Speed: 00                 |\n\r"); // 21
 	Printf(COM2, "+-------------------------------------------------------------------+\n\r"); // 22
-	Printf(COM2, "> Prompt34: ");
+	Printf(COM2, "> PromptDelayNopass: ");
 }
 
 void Display() {

lib/generic-heap.c

+#include <types.h>
+#include <generic-heap.h>
+#include <bwio.h>
+
+int printHeap(MinHeap* Heap) {
+	if (Heap == NULL || Heap->Num == 0) return FAILURE;
+	int i;
+	TrackNode** Storage = (TrackNode**)(Heap->Storage);
+	for (i = 0; i < Heap->Num; i++) {
+		TrackNode* Temp = Storage[i];
+		bwprintf(COM2,"(%s) ", Temp->name);
+	}
+	bwprintf(COM2, "\n\r");
+	return SUCCESS;
+}
+
+// returns min Tick
+int GetMinHeap(MinHeap* Heap, TrackNode* MinNode) {
+  if (Heap->Num <= 0) return 0;
+  TrackNode** Storage = (TrackNode**)Heap->Storage;
+  
+  MinNode =  Storage[0];
+//   *DelayedTick = MinNode->DelayedTick;
+  
+  return 1; // don't change this to SUCCESS, need 1 for ClockServer to work
+}
+
+// One Tough Cookie: Move Last to Top then Bubble Down
+void RemoveMinHeap(MinHeap* Heap) {
+	TrackNode** Storage = (TrackNode**)(Heap->Storage)
+	TrackNode* LastNode = Storage + Heap->Last -1;
+	TrackNode* MinNode = Storage[0];
+
+	MinNode = NULL;
+	Storage[0] = LastNode;
+
+	Heap->Num -= 1;
+	Heap->Last -= 1;
+
+	//bwprintf(COM2,"RM: ");
+	//printMinHeap(Heap);
+	BubbleDownMinHeap(Heap, 0); 
+}
+
+// inserts into Min Heap
+void InsertHeap(MinHeap* Heap, TrackNode* NewNode) {
+  //bwprintf(COM2,"HInsert:%d\n\r",TaskID);
+	TrackNode** Storage= (TrackNode**)(Heap->Storage);
+
+	Storage[Heap->Last] = NewNode;
+
+	Heap->Num += 1;
+	BubbleUpMinHeap(Heap, Heap->Last);
+	Heap->Last += 1;
+}
+
+void BubbleDownHeap(MinHeap* Heap, int StartIndex) {
+	int LeftChildIndex, RightChildIndex, NextIndex;
+	TrackNode Temp;
+
+	LeftChildIndex = StartIndex*2 + 1;
+	RightChildIndex = StartIndex*2 + 2;
+
+	TrackNode** Storage = (TrackNode**)(Heap->Storage);
+	TrackNode* Cur = Storage[StartIndex];
+	TrackNode* Left = Storage[LeftChildIndex];
+	TrackNode* Right = Storage[RightChildIndex];
+
+	Temp = *Cur;
+	if (Right == NULL || (Left->PathDistance < Right->PathDistance)) { // Right Branch is NULL
+		if (Cur->PathDistance > Left->PathDistance && Left != NULL) {
+			*Cur = *Left; // TODO hmm shallow copy
+			*Left = Temp;
+			NextIndex = LeftChildIndex;
+		}
+	}
+	else {
+		if (Cur->PathDistance > Right->PathDistance) {
+			*Cur = *Right;
+			*Right = Temp;
+			NextIndex = RightChildIndex;
+		}
+	}
+	//bwprintf(COM2,"BDW:");
+	//printMinHeap(Heap);
+	if ((2*NextIndex+1) < Heap->Last) BubbleDownMinHeap(Heap, NextIndex);
+}
+
+void BubbleUpHeap(MinHeap* Heap, int Last) {
+	int ParentIndex = (Last-1)/2;
+	TrackNode Temp;
+
+	if (ParentIndex < 0) return;
+	TrackNode** Storage = (TrackNode**)(Heap->Storage);
+	TrackNode* Parent = Storage[ParentIndex];
+	TrackNode* LastNode = Storage[Last];
+	if (Parent->PathDistance > LastNode->PathDistance) {
+		Temp = *Parent;
+		*Parent = *LastNode;
+		*LastNode = Temp;
+	}
+	//bwprintf(COM2,"BUP: ");
+	//printMinHeap(Heap);
+	if (ParentIndex != 0) BubbleUpMinHeap(Heap, ParentIndex);
+}
+
+void DecreaseKey(MinHeap* Heap, void* NewKey) {
+	
+}
\ No newline at end of file

lib/k4.c

@@ -140,11 +140,11 @@ void FirstUserTask() {
   
 
   // COM2 IOServer
-  Args.arg0 = (void*) 1;
+  Args.arg0 = (void*) 8;
   Args.arg1 = (void*) (&IServer2);
   tid = Create(&Args);
 
-  Args.arg0 = (void*) 1;
+  Args.arg0 = (void*) 8;
   Args.arg1 = (void*) (&OServer2);
   tid = Create(&Args);
 
@@ -156,12 +156,12 @@ void FirstUserTask() {
   Args.arg1 = (void *)(&IdleTask);
   tid = Create(&Args);
   
-  Args.arg0 = (void *) 0;
+  Args.arg0 = (void *) 7;
   Args.arg1 = (void *) (&TermInNotifier);
   tid = Create(&Args);
 
   
-  Args.arg0 = (void *) 0;
+  Args.arg0 = (void *) 7;
   Args.arg1 = (void *) (&TermOutNotifier);
   tid = Create(&Args);
   // COM1 IO server
@@ -240,29 +240,29 @@ void FirstUserTask() {
    */
    
   //Pass();
-  
-  //bwprintf(COM2,"FUT: exiting()\n\r");
-   // Args.arg0 = (void *)12;
-  // Args.arg1 = (void *)(&SensorPrintTask);
 
-  // tid = Create(&Args);
+  Args.arg0 = (void *)9; //TODO what priority
+  Args.arg1 = (void *)(&SensorServer);
+  tid = Create(&Args);
+  Delay(clkid,1);
+
+  // Pass();  
+  //bwprintf(COM2,"FUT: exiting()\n\r");
+  Args.arg0 = (void *)10;
+  Args.arg1 = (void *)(&SensorPrintTask);
+  tid = Create(&Args);
+  Delay(clkid,1);
   //       // bwprintf(COM2,"FUT: before delay\n\r");
 
   // Delay(clkid,1);
       // bwprintf(COM2,"FUT: after delay\n\r");
 
-  // Args.arg0 = (void *)0;
-  // Args.arg1 = (void *)(&SensorNotifier);
-  // tid = Create(&Args);
-  // Delay(clkid,1);
-  // // bwprintf(COM2,"FUT: added SN\n\r");
-
+  Args.arg0 = (void *)8;
+  Args.arg1 = (void *)(&SensorNotifier);
+  tid = Create(&Args);
+  Delay(clkid,1);
+  // bwprintf(COM2,"FUT: added SN\n\r");
 
-  // Args.arg0 = (void *)1; //TODO what priority
-  // Args.arg1 = (void *)(&SensorServer);
-  // tid = Create(&Args);
-  // Delay(clkid,1);
-  
   Args.arg0 = (void *)5;
   Args.arg1 = (void *)(&TCServer);
   tid = Create(&Args);

lib/path-finding.c

+#include <types.h>
+#include <generic-heap.h>
+#include <track_data.h>
+
+// returns Path's length or 0 if there's no Paths
+int Dijkstra(TrackNode* TrackNodes, TrackNode* Src, TrackNode* Dest) {
+    MinHeap Heap;
+    TrackNode* tPath[TRACK_MAX];
+    int PathLen, Index, i;
+
+    TrackEdge Edge[2];
+    Heap.Storage = (void*)tPath;
+    Heap.Num = 0;
+    Heap.Last = 0;
+    
+    for (i = 0; i < TRACK_MAX; i++) {
+        TrackNodes[i]->PathDistance = INT_MAX;
+        TrackNodes[i]->Parent = NULL;
+        TrackNodes[i]->bVisited = 'N';
+    }
+
+    Src->PathDistance = 0;
+    Insert(&Heap, Src);
+
+    while(Heap.Num > 0) {
+        Index = 0;
+        TrackNode* MinNode, Revrs;
+        GetMinHeap(&Heap, MinNode);
+        MinNode->bVisted = 'Y';
+        if (MinNode == Dest) break;
+        switch(MinNode->type) {
+            case NODE_BRANCH:
+                Edge[Index++] = MinNode->edge[DIR_STRAIGHT];
+                Edge[Index++] = MinNode->edge[DIR_CURVED];
+            case NODE_SENSOR:
+            case NODE_MERGE:
+            case NODE_ENTER:
+                Edge[Index++] = MinNode->edge[DIR_AHEAD];
+                break;
+            default: // including NODE_EXIT
+                break;
+        }
+        for (i = 0; i < Index; i++){
+            TrackNode* TempDest = Edge[Index].dest;
+            if (TempDest->bVisted != 'Y' && (MinNode->PathDistance + Edge[Index].dist < TempDest->PathDistance)) {
+                TempDest->PathDistance = MinNode->PathDistance + Edge[Index].dist;
+                if (TempDest->Parent == NULL) InsertHeap(&Heap, TempDest);
+                else DecreaseKey(&Heap, TempDest);
+                TempDest->Parent = MinNode;
+            }
+        }
+        Revrs = 
+    }
+
+}
+
+TrackEdge* GetNextEdge(TrackNode* N, ) {
+    if(N->type == NODE_BRANCH) {
+        //TODO
+    }
+    else return &(N->edge[DIR_AHEAD]);
+}
+
+TrackNode* GetNextNode(TrackNode* N) {
+    TrackEdge* E = GetNextEdge(N);
+    return E->dest;
+}
+
+void GetNextSensor() {}
+
+
+// returns distance in mm if there's a path between 2 nodes
+int GetDistance2Nodes(TrackNode* A, TrackNode* B) {
+    int Distance = 0;
+    TrackNode* E = GetNextEdge(A);
+    Distance += E->dist;
+
+    TrackNode* NextNode = GetNextNode(A);
+    while (NextNode != B && NextNode->type != NODE_EXIT) {
+        E = GetNextEdge(NextNode);
+        Distance += E->dist;
+        NextNode = E->dest;
+    }
+    if (NextNode == B) return Distance;
+    return -1;
+}
+
+
+void InitTrack(TrackNode* TrackNodes, char Tr) {
+    if (Tr == 'a') init_tracka(TrackNodes);
+    else init_trackb(TrackNodes);
+} 
+
+void TrackServer() {
+    TrackNode TrackNodes[TRACK_MAX];
+    Message RcvSlot;
+    TSMsg RcvMsg, ReplyMsg;
+
+    InitTrack(TrackNodes);
+
+    char TrackServerName[] = "TrackServer";
+    RegisterAs(TrackServerName);
+
+    FOREVER {
+	  if(!bReceived){
+	    Res = BufferFIFO(&SendQ, (void*)(&SenderID));
+	    BufferPopHead(&SendQ);
+	  }
+	  if (Res == 0) {
+	    if (bReceived == 0) {
+	      Res = Receive(SenderID, (void*)&RcvSlot, sizeof(RcvMsg));
+	      SenderID = RcvSlot.SenderID;
+	      //Printf(COM2,"TC: Rcv %d\n\r",SenderID);
+	    }
+	    //Printf(COM2,"TC: Got %d\n\r",SenderID);
+	    bReceived = 0;
+        RcvMsg = *((TSMsg*)RcvSlot.Addr);
+        
+	    switch (RcvMsg.Type){
+	    case GetPathSrcDest:
+            Dijkstra(TrackNodes, RcvMsg.Src, RcvMsg.Dest);
+            break;
+	    case GetSensorNode:
+            break;
+	    case GetNextSensorNode: 
+	        break;
+        case GetDistance2Nodes:
+            break;
+	    default:
+	        break;
+	    }
+	  }
+	  else {
+	    bReceived = 1;
+	    //Printf(COM2, "TC->Null\n\r");
+	    Receive(-1, (void*)&RcvSlot, sizeof(RcvMsg));
+	    SenderID = RcvSlot.SenderID;
+	    Res = 0;
+	    //Printf(COM2,"TC: Null->%d\n\r",SenderID); 
+	  }
+	}
+}
+

lib/sensors.c

@@ -7,44 +7,87 @@
 #include <syscall.h>
 #include <nameserver.h>
 #include <buffer.h>
+#include <clockserver-api.h>
 
-void PrintSensors(char* SensorChars, int* SensorNums) {
+void SensorPrintTask() {//,char* SensorChars, int* SensorNums) {
+	SSMsg SendMsg, ReplyMsg;
+	Message RcvSlot;
 	int i, ColPos;
-	for (i = 0; i < NUM_SENSORS_DISPLAYED; i++) {
-		ColPos = 11 + i*4;
-		Printf(COM2, "%s\033[19;%dH%c%d%s", SAVE_CURSOR, ColPos, SensorChars[i], SensorNums[i], RESTORE_CURSOR);
+	char* SensorChars;
+	int* SensorNums;
+
+	char SensorServerName[] = "SensorServer";
+	// Printf(COM2,"SPT first\n\r");
+
+	int SensorServerID = WhoIs(SensorServerName);//MyParentTid();
+	int SPTID = MyTid();
+
+	char ClkServerName[] = "ClockServer";
+	int ClkServerID = WhoIs(ClkServerName);
+	// Pass();
+
+	FOREVER {
+		SendMsg.Type = SENSOR_QUERY;
+		Printf(COM2,"SPT bout2Send\n\r");
+		RcvSlot.SenderID = SPTID;
+		RcvSlot.Addr = (void*)&SendMsg;
+		Printf(COM2, "SSID %d\n\r", SensorServerID);
+		Send(SensorServerID, (void*)&RcvSlot, sizeof(RcvSlot), (void*)&ReplyMsg, sizeof(ReplyMsg));
+		Printf(COM2,"SPT afterSend\n\r");
+
+		SensorChars = ReplyMsg.SensorChars;
+		SensorNums = ReplyMsg.SensorNums;
+
+		for (i = 0; i < NUM_SENSORS_DISPLAYED; i++) {
+			ColPos = 11 + i*5;
+			// Printf(COM2,"%c\n\r",SensorChars[i]);
+			// Printf(COM2,"xxxxxxxxx\n\r");
+			// Printf(COM2,"%d\n\r",SensorNums[i]);
+			Printf(COM2, "%s%c[19;%dH%c%d%s", SAVE_CURSOR, 27, ColPos, SensorChars[i], SensorNums[i], RESTORE_CURSOR);
+			// Printf(COM2,"SPT %c%d\n\r",SensorChars[0],SensorNums[0]);	
+			// Delay(ClkServerID,50);
+		}
 	}
+	// Printf(COM2,"Done 5\n\r");	
+
 }
 
 
 void ProcessData (int* Sensors, char* SensorChars, int* SensorNums) {
-  int i, j, Num, SensorNum, Index, First;
-  char SensorChar;
-  
-  for (i = 0; i < NUM_SENSORS_READ; i++) {
-    SensorChar = 'A' + (i/2);
-    First = i % 2;
-    Index = 1;
-    Num = 128;
-    FOREVER {
-      if (Sensors[i] == 0) break;
-      if (Sensors[i] >= Num) {
-	SensorNum = First*8 + Index;
-	Sensors[i] -= Num;
-	if ((SensorChar != SensorChars[0]) || (SensorNum != SensorNums[0])) {
-	  for (j = NUM_SENSORS_DISPLAYED - 1; j > 0; j--) {
-	    SensorChars[j] = SensorChars[j-1];
-	    SensorNums[j] = SensorNums[j-1];
-	  }
-	  SensorChars[0] = SensorChar;
-	  SensorNums[0] = SensorNum;
+	int i, j, Num, SensorNum, Index, First;
+	char SensorChar;
+	int Temp = 0;
+
+	for (i = 0; i < NUM_SENSORS_READ; i++) {
+		SensorChar = 'A' + (i/2);				
+		First = i % 2;
+		Index = 1;
+		Num = 128;
+		FOREVER {
+			if (Sensors[i] == 0) break;
+		// Printf(COM2,"Char %c\n\r", SensorChar);
+		// Printf(COM2,"NumTotal %d\n\r", Sensors[i]);		
+			if (Sensors[i] >= Num) {
+				SensorNum = First*8 + Index;
+				// Printf(COM2,"Sensor Num %d\n\r", SensorNum);
+				Sensors[i] -= Num;
+				if ((SensorChar != SensorChars[0]) || (SensorNum != SensorNums[0])) {
+					Temp++;
+					for (j = NUM_SENSORS_DISPLAYED - 1; j > 0; j--) {
+						SensorChars[j] = SensorChars[j-1];
+						SensorNums[j] = SensorNums[j-1];
+					}
+					SensorChars[0] = SensorChar;
+					SensorNums[0] = SensorNum;
+					// Printf(COM2,"ProcessData %c%d\n\r",SensorChars[0],SensorNums[0]);	
+				}
+				// if (Sensors[i] == 0) break;
+			}
+			Num /= 2;
+			Index++;
+		}
 	}
-	// if (Sensors[i] == 0) break;
-      }
-      Num /= 2;
-      Index++;
-    }
-  }
+	// Printf(COM2,"In array:%d\n\r", Temp);
 }
 
 void SensorServer() {
@@ -56,6 +99,8 @@ void SensorServer() {
 	Buffer SendQ;
 	int* SensorNums;
 	char* SensorChars;
+	int i;
+	AP Args;
 
 	Buffer** AllSendQ = GetAllSendQ();
 	SendQ.Storage = (void *)QStorage;
@@ -70,74 +115,115 @@ void SensorServer() {
 	char SensorServerName[] = "SensorServer";
 	RegisterAs(SensorServerName);
 
+	SSMsg* Temp;
+
 	// int OServer = WhoIs("OServer");
 	int bReceived = 0;
 	FOREVER {
-	  //SenderID = 0;
-	  if(!bReceived){
-	    Res = BufferFIFO(&SendQ, (void*)(&SenderID));
-	    BufferPopHead(&SendQ);
-	  }
-	  if (Res == 0) {
-	    if (bReceived == 0){
-	      Res = Receive(SenderID, (void*)&RcvSlot, sizeof(RcvMsg));
-	      SenderID = RcvSlot.SenderID;
-	    }
-	    bReceived = 0;
-	    switch (RcvMsg.Type){
-	    case SENSOR_QUERY:
-	      ReplyMsg.Type = SENSOR_RESPONSE;
-	      ReplyMsg.SensorNums = SensorNums;
-	      ReplyMsg.SensorChars = SensorChars;
-	      Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
-	      break;
-	    case SENSOR_INPUT:
-	      ReplyMsg.Type = SENSOR_RESPONSE;
-	      Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
-	      PrintSensors(SensorChars, SensorNums);
-	      Putc(COM2,'z');					
-	      SensorNums = RcvMsg.SensorNums;
-	      SensorChars = RcvMsg.SensorChars;
-	      break;
-	    default:
-	      break;
-	    }
-	  }
-	  else {
-	    bReceived = 1;
-	    // bwprintf(COM2, "SS bout2block\n\r");
-	    Receive(-1, (void*)&RcvSlot, sizeof(RcvMsg));
-	    SenderID = RcvSlot.SenderID;
-	    Res = 0;
-	  }
+		//SenderID = 0;
+		if(!bReceived){
+			Res = BufferFIFO(&SendQ, (void*)(&SenderID));
+			BufferPopHead(&SendQ);
+		}
+		if (Res == 0) {
+			if (bReceived == 0){
+				Res = Receive(SenderID, (void*)&RcvSlot, sizeof(RcvMsg));
+				Temp = (SSMsg*)(RcvSlot.Addr);
+				SenderID = RcvSlot.SenderID;
+			}
+			bReceived = 0;
+			switch (Temp->Type){
+				case SENSOR_QUERY:
+				//   ReplyMsg.Type = SENSOR_RESPONSE;
+				//   ReplyMsg.SensorNums = SensorNums;
+				//   ReplyMsg.SensorChars = SensorChars;
+				//   Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
+					// Printf(COM2,"SensorQuery ID %d SendQLen %d\n\r", SenderID, SendQ.Length);
+					FeedBuffer(&SendQ, (void*)SenderID);
+					// Printf(COM2,"SendQLen %d\n\r", SendQ.Length);
+					break;
+				case SENSOR_NOTIFIER:
+					// Printf(COM2,"SensorNotifier\n\r");
+					ReplyMsg.Type = SENSOR_RESPONSE;
+					// Printf(COM2,"SenderID Notif%d SendQLen %d\n\r", SenderID, SendQ.Length);
+					SensorChars = Temp->SensorChars;
+					SensorNums = Temp->SensorNums;
+					Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
+					// SensorNums = RcvMsg.SensorNums;
+					// SensorChars = RcvMsg.SensorChars;
+					// for (i = 0; i < NUM_SENSORS_DISPLAYED; i++) {
+					// 	Printf(COM2,"xxx");
+					// 	Printf(COM2,"Serverrrrrrrrrrr %c%d\n\r",SensorChars[i],SensorNums[i]);	
+					// }	
+					while (SendQ.Length > 0) {
+						BufferFIFO(&SendQ, (void*)(&SenderID));
+						// Printf(COM2,"SenderID in loop %d", SenderID);
+						ReplyMsg.SensorChars = SensorChars;
+						ReplyMsg.SensorNums = SensorNums;
+						Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
+						BufferPopHead(&SendQ);
+					}
+					// Printf(COM2,"xxxx\n\r");
+					break;
+				default:
+					break;
+			}
+		}
+		else {
+			bReceived = 1;
+			// bwprintf(COM2, "SS bout2block\n\r");
+			Receive(-1, (void*)&RcvSlot, sizeof(RcvMsg));
+			SenderID = RcvSlot.SenderID;
+			Temp = (SSMsg*)(RcvSlot.Addr);
+			Res = 0;
+		}
 	}
 }
 
-void SensorTask() {
-  int i;
-  int Sensors[NUM_SENSORS_READ];
-  char SensorChars[NUM_SENSORS_DISPLAYED];
-  int SensorNums[NUM_SENSORS_DISPLAYED];
-  SSMsg SendMsg, ReplyMsg;
-  
-  char SensorServerName[] = "SensorServer";
-  int SensorServer = WhoIs(SensorServerName);
-  for (i = 0; i < NUM_SENSORS_DISPLAYED; i++) {
-    SensorChars[i] = '\0';
-    SensorNums[i] = 0;
-  }
-  FOREVER {
-    Putc(COM1, SENSOR_RESET);
-    Putc(COM1, SENSOR_REQUEST); // 128 + 5 modules
-    for (i = 0; i < NUM_SENSORS_READ; i++) {
-      // Putc(COM2,i+65);
-      Sensors[i] = Getc(COM1);
-    }
-    ProcessData(Sensors, SensorChars, SensorNums);
-    SendMsg.Type = SENSOR_INPUT;
-    SendMsg.SensorChars = SensorChars;
-    SendMsg.SensorNums = SensorNums;
-    Send(SensorServer, (void*)&SendMsg, sizeof(SensorChars), (void*)&ReplyMsg, sizeof(ReplyMsg));
-  }
+void SensorNotifier() {
+	int i;
+	int Sensors[NUM_SENSORS_READ];
+	char SensorChars[NUM_SENSORS_DISPLAYED];
+	int SensorNums[NUM_SENSORS_DISPLAYED];
+	SSMsg SendMsg, ReplyMsg;
+	Message RcvSlot;
+
+	// char SensorNotifierName[] = "SensorNotifier";
+	// RegisterAs(SensorNotifierName);
+	// Pass();
+	char SensorServerName[] = "SensorServer";
+	int SensorServerID = WhoIs(SensorServerName);
+	// int SensorServerID = MyParentTid();
+	int SSNot = MyTid();
+	// Printf(COM2, "SensorServer ID = %d\n\r", SensorServerID);
+	
+	for (i = 0; i < NUM_SENSORS_DISPLAYED; i++) {
+		SensorChars[i] = '\0';
+		SensorNums[i] = 0;
+	}
+
+	FOREVER {
+		Putc(COM1, SENSOR_RESET);
+		Putc(COM1, SENSOR_REQUEST); // 128 + 5 modules
+		for (i = 0; i < NUM_SENSORS_READ; i++) {
+			// if (i==9)Putc(COM2,i+65);
+			Sensors[i] = Getc(COM1);
+		}
+		ProcessData(Sensors, SensorChars, SensorNums);
+		SendMsg.Type = SENSOR_NOTIFIER;
+
+		// for (i = 0; i < NUM_SENSORS_DISPLAYED; i++) {
+		// 	Printf(COM2,"Notifier %c%d\n\r",SensorChars[i],SensorNums[i]);	
+		// }	
+		SendMsg.SensorChars = SensorChars;
+		SendMsg.SensorNums = SensorNums;
+		
+		// Putc(COM2, 'x');
+		Printf(COM2, "Notifier SSID = %d\n\r", SensorServerID);
+		RcvSlot.SenderID = SSNot;
+		RcvSlot.Addr = (void*)&SendMsg;
+		// Printf(COM2, "Notif Sent!\n\r
+		Send(SensorServerID, (void*)&RcvSlot, sizeof(RcvSlot), (void*)&ReplyMsg, sizeof(ReplyMsg));
+	}
 }