////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Port S4 High-Speed RS-485 Full Duplex Token Ring
//
// Autor  : Tiago Gonçalves (ogait87)
// Release: 0.0.3
// Date   : 30/08/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#pragma platform(NXT)

//#define HSPEED_DEBUG

long nXmitSuccesses = 0;
long nRcvChars = 0;

void setupHighSpeedLink(const bool bMaster)
{
   //
   // Initialize port S4 to "high speed" mode.
   //
   nxtEnableHSPort();
   //nxtSetHSBaudRate();
   nxtHS_Mode = bMaster ? hsMsgModeMaster : hsMsgModeSlave;

   //
   // Configure the link for raw read and write. User program will have complete control over the link.
   // User program will be responsible for managing the half-duplex operation and must prevent collisions!
   //
   nxtHS_Mode = hsRawMode;

   return;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Driver Task
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
ubyte HSpeedTxData[17] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int   HSpeedTxDataLen = 0;
ubyte HSpeedRxData[17] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int   HSpeedRxDataLen = 0;

int   MaxNXT = 0;
int   IAmNXT = 0;

task HSpeed()
{
   bool IAmMaster = false;
   int timeout;
   int dest, len;
   ubyte TxData = (IAmNXT << 6);
   ubyte lixo = 0x00;
   ubyte head;
   int i;
   ClearTimer(T1);

#ifdef HSPEED_DEBUG   
   int master_count = 0;
   int head_count = 0;   
   int loop_count = 0;
   int timeout_count = 0;
#endif

   while(true)
   {
#ifdef HSPEED_DEBUG
      loop_count++;
        nxtDisplayTextLine(4, "LOOP: %d", loop_count);
#endif
      if ( IAmMaster )
      {
         if ( HSpeedTxDataLen > 0)
         {
            HSpeedTxData[0] = HSpeedTxData[0] << 4;
            HSpeedTxData[0] = HSpeedTxData[0] | (IAmNXT << 6);
            HSpeedTxData[0] = HSpeedTxData[0] | HSpeedTxDataLen;
            nxtWriteRawHS(HSpeedTxData, HSpeedTxDataLen+1, 0); // == ioRsltSuccess
            HSpeedTxDataLen = 0;
         }
         else
         {
            nxtWriteRawHS(TxData, 1, 0);
         }
         // Dar tempo ao recetor receber e procesar a mensagem
         IAmMaster = false;
#ifdef HSPEED_DEBUG
         master_count++;
         nxtDisplayTextLine(6, "MASTER: %d", master_count);
#endif
         wait1Msec(100);
      }
      timeout = random[1000] + 2000;
      ClearTimer(T1);

      while( nxtGetAvailHSBytes() <= 0 && time1[T1] < timeout)
         wait1Msec(10);

      // Houve erro na comunicao e niguem sabe quem e a vez de falar
      if ( time1[T1] >= timeout )
      {
         IAmMaster = true;
#ifdef HSPEED_DEBUG
         timeout_count++;
         nxtDisplayTextLine(7, "TIMEOUT: %d", timeout_count);
#endif
         continue;
      }

      // Recebi pelo menos 1 byte
      nxtReadRawHS(head, 1);

#ifdef HSPEED_DEBUG
      head_count++;
      nxtDisplayTextLine(5, "HEAD: %d C: %d", (int) head&0xFF, head_count);
#endif

      // Sou o proximo?
      if ( ((head >> 6) + 1) % MaxNXT == IAmNXT)
      {
         IAmMaster = true;
      }

      // Processar o corpo da mensagem
      len = head&0x0F;
      if( len != 0 )
      {
         dest = (head>>4) & 0x3;
#ifdef HSPEED_DEBUG
         nxtDisplayTextLine(3,"PM Master:%d", (byte) IAmMaster);
#endif
         i = 0;
         while( i < len)
                {
            while(nxtGetAvailHSBytes() <= 0)
            wait1Msec(10);

            nxtReadRawHS(lixo, 1);

            if ( dest == IAmNXT )
               HSpeedRxData[i+1] = lixo;

            i++;
         }

         if ( dest == IAmNXT )
         {
            HSpeedRxData[0] = head>>6;
            HSpeedRxDataLen = len;
         }
         else
         {
            HSpeedRxDataLen = 0;
         }
#ifdef HSPEED_DEBUG
         if ( dest == IAmNXT )
         {
            nxtDisplayTextLine(3, "PARA MIM");
         }
         else
         {
            nxtDisplayTextLine(3, "IGNORE");
         }
#endif
      }
      wait1Msec(100);
   }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Main Task
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
   string sFriendlyName;
   int count = 0;
   long loop_count = 0;
   long send_count=0;

   setupHighSpeedLink(true);

   MaxNXT = 2;

   getFriendlyName(sFriendlyName);
   IAmNXT = sFriendlyName == "HRF1" ? 0 : 1;

   eraseDisplay();
   bNxtLCDStatusDisplay = true; // Enable top status line display

   nxtDisplayTextLine(2, "IAmNXT: %d MAX: %d", IAmNXT, MaxNXT);

   StartTask(HSpeed);

   ClearTimer(T2);

   while(true)
   {
      nxtDisplayTextLine(3, "Loop: %d", loop_count++);

      while(HSpeedRxDataLen == 0 && time1[T2] < 1000)
      {
         nxtDisplayTextLine(3, "Loop: %d %d", HSpeedRxDataLen, time1[T2]);
         wait1Msec(10);
      }

      if ( HSpeedRxDataLen != 0 ) // Recebi Algo
      {
         count ++;
         nxtDisplayTextLine(6, "Rx: ID %d Len %d", (int)HSpeedRxData[0], HSpeedRxDataLen);
         nxtDisplayTextLine(7, "Rx: %d", count);
         HSpeedRxDataLen = 0;
      }

      if ( time1[T2] > 1000)
      {
         send_count++;
         HSpeedTxData[0] = IAmNXT == 0 ? 1 : 0;
         HSpeedTxData[1] = 1;
         HSpeedTxDataLen = 1;
         nxtDisplayTextLine(4, "Send: %d", send_count);
         ClearTimer(T2);
      }
   }
}

   IAmNXT = sFriendlyName == "HRF1" ? 0 : 1;

if SensorValue[0]>=100 // send in case a wall (e.g., raw=400) has been detected
{
  myByteArray[0]=0; // addressee: Master=00
  myByteArray[1]=1; // own address 01
  myByteArray[2]=00 // sensor port 0 --- or alternatively  a message ID (to answer to a request?)
  myByteArray[3]=02 // 2 byte integer (short)
  myByteArray[4]= 0xC0 // value=400 => lowByte of SensorValue=400-256=144 
  myByteArray[5]= 0x01 // value=400 => hiByte of SensorValue=1*256
  myByteArray[6]= 0x00  // needed only for 4 byte int
  myByteArray[7]= 0x00  // needed only for 4 byte int
  myByteArray[8] =... // needed only for 8 byte int/float
  myByteArray[9] =... // needed only for 8 byte int/float
  myByteArray[10]=... // needed only for 8 byte int/float
  myByteArray[11]=... // needed only for 8 byte int/float   
  SendRS485Message(myByteArray);
}

 
// transmission block: 16 bytes
// header block: 8 bytes
  TBlock[0] =0x00; // addressee: 00
  TBlock[1] =0x01; // own address 01
  TBlock[2] = ...  // free; maybe low byte of mesage ID or sth. else
  TBlock[3] = ...  // free; maybe hi byte  of mesage ID or sth. else
  TBlock[4] =0x00; // value type: sensor port 0  (see below)
  TBlock[5] = ...  // in case of variable value:  low byte variable ID 
  TBlock[6] = ...  // in case of variable value:  hi byte variable ID   
  TBlock[7] =0x21;  // value encoded as 2 byte integer (short)  (see below)
// data block: 8 bytes
  TBlock[8] = 0xC0; // lowByte of (short)SensorValue
  TBlock[9] = 0x01; // hiByte of (short)SensorValue
  TBlock[10]= 0x00; // needed only for 4 byte int
  TBlock[11]= 0x00; // needed only for 4 byte int
  TBlock[12]= 0x00; // needed only for 8 byte int/float
  TBlock[13]= 0x00; // needed only for 8 byte int/float
  TBlock[14]= 0x00; // needed only for 8 byte int/float
  TBlock[15]= 0x00; // needed only for 8 byte int/float