// master.c
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Remote NXT Control Client Example
//
//  Program to serve as an example to how to use the 'Remote.c' functions
//
// Autor  : Tiago Costa Gonçalves (ogait87)
// Release: 0.0.1
// Date   : 07/11/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "Remote.c"

task main()
{
   // Setup
   string sFriendlyName;
   getFriendlyName(sFriendlyName);
   IAmNXT = sFriendlyName == "HRF2" ? 0 : 1;
   MaxNXT = 2;
   setupHighSpeedLink();

   //
   int other = (IAmNXT+1) % MaxNXT;
   ubyte motor_pos;
   ubyte sensor_value;

   //
   int count = 0;
   while( 1 )
   {
      count++;
      nxtDisplayTextLine(2, "i: %d", count);

      SetHSRemoteMotor( other, 0, 50);
      wait1Msec(500);
      SetHSRemoteMotor( other, 0, 00);
      wait1Msec(500);

      GetHSRemoteEncoderValue( other, 0, motor_pos);
      nxtDisplayTextLine(4, "Motor Pos: %d", motor_pos);

      GetHSRemoteSensorValue( other, 0, sensor_value);
      nxtDisplayTextLine(5, "Sensor Value: %d", sensor_value);
   }
}

// slave.c
#pragma config(Sensor, S1,     ss,                  sensorTouch)
#pragma config(Motor,  motorA,          aaa,                  tmotorNxtEncoderClosedLoop)
#pragma config(Motor,  motorB,          aab,                  tmotorNxtEncoderClosedLoop)
#pragma config(Motor,  motorC,          aac,                  tmotorNxtEncoderClosedLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Remote NXT Control Client Server
//
//  Task to process a remote controler command
//
// Autor  : Tiago Costa Gonçalves (ogait87)
// Release: 0.0.1
// Date   : 07/11/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "HSpeed.c"

task main()
{
   // Setup
   string sFriendlyName;
   getFriendlyName(sFriendlyName);
   IAmNXT = sFriendlyName == "HRF2" ? 0 : 1;
   MaxNXT = 2;
   setupHighSpeedLink();

   //
   int count=0;
   while( 1 )
   {
      HSpeedRxDataLen = 0;
      while( HSpeedRxDataLen == 0 ) wait1Msec(10);

      count++;
      nxtDisplayTextLine(2, "i: %d Len: %d", count, HSpeedRxDataLen);

      if ( HSpeedRxDataLen == 2 && HSpeedRxData[0] >= 1 && HSpeedRxData[0] <= 3 )
      {
         motor[HSpeedRxData[0]-1] = HSpeedRxData[1];
         nxtDisplayTextLine(4, "M: %d Speed: %d", HSpeedRxData[0]-1, HSpeedRxData[1]);
      }

      if ( HSpeedRxDataLen == 1 && HSpeedRxData[0] >= 4 && HSpeedRxData[0] <= 6 )
      {
         HSpeedSendByte(HSpeedRxHead, nMotorEncoder[HSpeedRxData[0]-4]);
         nxtDisplayTextLine(5, "MPos: %d MPos: %d", (int)HSpeedRxData[0]-4, nMotorEncoder[HSpeedRxData[0]-4]);
      }

      if ( HSpeedRxDataLen == 1 && HSpeedRxData[0] >= 7 && HSpeedRxData[0] <= 10 )
      {
         HSpeedSendByte(HSpeedRxHead, SensorValue[HSpeedRxData[0]-7]);
         nxtDisplayTextLine(6, "SP: %d Value: %d", (int)HSpeedRxData[0]-7, SensorValue[HSpeedRxData[0]-7]);
      }
   }
}

// remote.c
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Remote NXT Control API
//
//  Functions to control a remote NXT
//
// Autor  : Tiago Costa Gonçalves (ogait87)
// Release: 0.0.1
// Date   : 07/11/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "HSpeed.c"

void SetHSRemoteMotor(ubyte remote, ubyte motor, ubyte speed)
{
   HSpeedSendByte(remote, 1 + 0*3 + motor, speed);
}

void GetHSRemoteEncoderValue(ubyte remote, ubyte motor, ubyte &pos)
{
   HSpeedSendByte(remote, 1 + 1*3 + motor);
   HSpeedRxDataLen = 0;
   while( HSpeedRxDataLen == 0 ) wait1Msec(10);
   pos = HSpeedRxData;
   HSpeedRxDataLen = 0;
}

void GetHSRemoteSensorValue(ubyte remote, ubyte sensorPort, ubyte &value)
{
   HSpeedSendByte(remote, 1 + 2*3 + sensorPort);
   HSpeedRxDataLen = 0;
   while( HSpeedRxDataLen == 0 ) wait1Msec(10);
   value = HSpeedRxData;
   HSpeedRxDataLen = 0;
}

// highspeed.c
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Port S4 High-Speed RS-485 Full Duplex Token Ring
//
// Autor  : Tiago Costa Gonçalves (ogait87)
// Release: 0.0.6
// Date   : 07/11/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

//#define HSPEED_DEBUG

#ifndef __HSPEED_C__
#define __HSPEED_C__

#ifndef HSPEED_TIMEOUT_BASE
#define HSPEED_TIMEOUT_BASE 1000
#endif

#ifndef HSPEED_TIMEOUT_RAND
#define HSPEED_TIMEOUT_RAND 1000
#endif

//
// Public
//
int  MaxNXT = 1;
int  IAmNXT = 0;

ubyte HSpeedRxHead[1];
ubyte HSpeedRxData[16];
ubyte HSpeedRxDataLen = 0;

task HSpeed;

void setupHighSpeedLink();

void HSpeedSendByte(ubyte destino, ubyte data);
void HSpeedSendByte(ubyte destino, ubyte data_a, ubyte data_b);
void HSpeedSendByte(ubyte destino, ubyte data_a, ubyte data_b, ubyte data_c);

/////////////////////////////////////////////////////////////////////////////////////////
//                                     Private
/////////////////////////////////////////////////////////////////////////////////////////
ubyte HSpeedTxData[1+16];
ubyte HSpeedTxLen = 0;

ubyte chr[16];

void setupHighSpeedLink()
{
   nxtEnableHSPort();
   //nxtSetHSBaudRate();
   nxtHS_Mode = hsMsgModeMaster ;
   nxtHS_Mode = hsRawMode;

   StartTask(HSpeed);

   return;
}

void HSpeedSendByte(ubyte destino, ubyte data)
{
   while(HSpeedTxLen != 0)
      EndTimeSlice();

   HSpeedTxData[0] = (IAmNXT << 6) | (destino << 4) | 1;
   HSpeedTxData[1] = data;
   HSpeedTxLen     = 1;
}

void HSpeedSendByte(ubyte destino, ubyte data_a, ubyte data_b)
{
   while(HSpeedTxLen != 0)
      EndTimeSlice();

   HSpeedTxData[0] = (IAmNXT << 6) | (destino << 4) | 2;
   HSpeedTxData[1] = data_a;
   HSpeedTxData[2] = data_b;
   HSpeedTxLen     = 2;
}

void HSpeedSendByte(ubyte destino, ubyte data_a, ubyte data_b, ubyte data_c)
{
   while(HSpeedTxLen != 0)
      EndTimeSlice();

   HSpeedTxData[0] = (IAmNXT << 6) | (destino << 4) | 3;
   HSpeedTxData[1] = data_a;
   HSpeedTxData[2] = data_b;
   HSpeedTxData[3] = data_c;
   HSpeedTxLen     = 3;
}

void HSpeedMaster()
{
#ifdef HSPEED_DEBUG
   static int master_count = 0;
   master_count++;
   nxtDisplayTextLine(6, "MASTER: %d", master_count);
#endif

   if ( HSpeedTxLen > 0)
   {
      nxtWriteRawHS(HSpeedTxData, 1+HSpeedTxLen, 0);
      HSpeedTxLen = 0;
   }
   else
   {
      nxtWriteRawHS((IAmNXT << 6), 1, 0);
   }
}

bool HSpeed_ReadRawHS(int n)
{
   int i, timeout = HSPEED_TIMEOUT_BASE + random[HSPEED_TIMEOUT_RAND];

   ClearTimer(T1);
   for(i=0; i<n; )
   {
      i += nxtReadRawHS(chr[i], n-i);
      if ( time1[T1] >= timeout )
      {
         return false;
      }
   }

   return true;
}

task  HSpeed()
{
   int senderID, reciverID, length;

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

   if ( IAmNXT == 0 )   // So start roling the comunication
   {
      HSpeedMaster();
   }

   while(true)
   {
#ifdef HSPEED_DEBUG
      loop_count++;
        nxtDisplayTextLine(3, "LOOP: %d", loop_count);
#endif

      // Receve 1 byte, the header
      if ( !HSpeed_ReadRawHS(1) )
      {
#ifdef HSPEED_DEBUG
         timeout_count++;
         nxtDisplayTextLine(7, "TIMEOUT1: %d", timeout_count);
#endif
         HSpeedMaster(); // so be me now :)
         continue;
      }

      // Get Head fields
      senderID  = (chr[0] >> 6) & 0x3;
      reciverID = (chr[0] >> 4) & 0x3;
      length    = (chr[0] >> 0) & 0xF;

#ifdef HSPEED_DEBUG
      nxtDisplayTextLine(4, "SRC:%d DEST:%d", senderID, reciverID);
      head_count++;
      nxtDisplayTextLine(5, "LEN: %d COUNT: %d", length, head_count);
#endif

      if ( length > 0 )
      {
         if ( !HSpeed_ReadRawHS(length) )
         {
#ifdef HSPEED_DEBUG
            timeout_count++;
            nxtDisplayTextLine(7, "TIMEOUT2: %d", timeout_count);
#endif
            HSpeedMaster(); // so be me now :)
            continue;
         }

         if ( reciverID == IAmNXT )
         {
            HSpeedRxHead[0] = senderID;
            memcpy(HSpeedRxData, chr, length);
            HSpeedRxDataLen = length;
         }
      }

      // I am the next NXT to transmit?
      if ( (senderID + 1) % MaxNXT == IAmNXT )
      {
         HSpeedMaster();
      }
   }
}

#endif

// teste_ligacao.c
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Port S4 High-Speed RS-485 Full Duplex Token Ring Tester
//
//  Program to test the code
//
// Autor  : Tiago Costa Gonçalves (ogait87)
// Release: 0.0.2
// Date   : 07/11/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#define HSPEED_DEBUG

#include "HSpeed.c"

task main()
{
   // Setup
   string sFriendlyName;
   getFriendlyName(sFriendlyName);
   IAmNXT = sFriendlyName == "HRF2" ? 0 : 1;
   MaxNXT = 2;
   setupHighSpeedLink();

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

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

   while(true)
   {
      wait1Msec(10);
   }
}

// test_ligacao2.c
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                         Port S4 High-Speed RS-485 Full Duplex Token Ring Tester
//
//  Program to test the efficiency of the HSpeed Connection
//
// Autor  : Tiago Costa Gonçalves (ogait87)
// Release: 0.0.2
// Date   : 07/11/2008
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
#define HSPEED_TIMEOUT_BASE 10000
//#define HSPEED_DEBUG

#include "HSpeed.c"

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        recieve Data
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
void recieve()
{
   int counta = 0;

   while(true)
   {
      if( HSpeedRxDataLen != 0  )
      {
         counta++;
         nxtDisplayTextLine(6, "Rx: ID %d Len %d", HSpeedRxHead[0]     , HSpeedRxDataLen);
         nxtDisplayTextLine(7, "Rx: %d %d"       , (int)HSpeedRxData[0], counta          );
         HSpeedRxDataLen = 0;
      }
   }
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        send Data
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
void send()
{
   int count = 0;

   ClearTimer(T2);
   while(true)
   {
      if( time1[T2] > 20 )
      {
         count++;
         HSpeedSendByte( (IAmNXT+1) % MaxNXT, count);
         nxtDisplayTextLine(4, "Send: %d", count);
         ClearTimer(T2);
      }
   }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Main Task
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
   // Setup
   string sFriendlyName;
   getFriendlyName(sFriendlyName);
   IAmNXT = sFriendlyName == "HRF2" ? 0 : 1;
   MaxNXT = 2;
   setupHighSpeedLink();

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

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

   if ( IAmNXT == 0 )
   {
      send();
   }
   else
   {
      recieve();
   }
}

  nxtDisplayTextLine(4, "Motor Pos: %d", (int)motor_pos);
  //...
  nxtDisplayTextLine(5, "Sensor Value: %d", (int)sensor_value);