// Lego NXT RS485 demo Master
// modified from John Hansen nxcsamples
// http://bricxcc.sourceforge.net/nbc/nxcsamples/485.zip

void SendRS485Bool(const bool value)
{
  byte msg[];
  ArrayBuild(msg, value, 0);
  SetHSOutputBuffer(0, 2, msg);
  SetHSOutputBufferOutPtr(0);
  SetHSOutputBufferInPtr(2);
  SetHSState(HS_SEND_DATA);
  SetHSFlags(HS_UPDATE); //send it
}

void SendRS485Number(const int value)
{
  string msg = Flatten(value);
  SetHSOutputBuffer(0, 5, msg);
  SetHSOutputBufferOutPtr(0);
  SetHSOutputBufferInPtr(5);
  SetHSState(HS_SEND_DATA);
  SetHSFlags(HS_UPDATE); //send it
}

void SendRS485String(const string msg)
{
  byte mlen = ArrayLen(msg);
  SetHSOutputBuffer(0, mlen, msg);
  SetHSOutputBufferOutPtr(0);
  SetHSOutputBufferInPtr(mlen);
  SetHSState(HS_SEND_DATA);
  SetHSFlags(HS_UPDATE); //send it
}

void WaitForMessageToBeSent()
{
  while(HSOutputBufferOutPtr() < HSOutputBufferInPtr())
    Wait(1);
}

task main()
{
  // set port 4 to RS485 type
  SetSensorType(IN_4, SENSOR_TYPE_HIGHSPEED);
  SetHSState(HS_INITIALISE);
  SetHSFlags(HS_UPDATE);
  // set ports 1 and 2 to touch sensor type
  SetSensor(S1, SENSOR_TOUCH);
  SetSensor(S2, SENSOR_TOUCH);

  byte mlen;  // message length
  string buffer; // message buffer
  string SlaveMsgNum; // incoming message number
  int SlaveMsgLen;  // incoming message length
  string SlaveNum;  // out going message number
  string msg; // out going  message
  string SlaveMsg;  // message text
  int TimeOut;
  int x;
  int PollCount;
  int PollTotal;  // total number of slave to poll
  bool ReadMsg;
  int ar_Polling[3]; // declare an array

  // set array of slave numbers
  // each slave NXT must be in this list
  ar_Polling[0] = 2;
  ar_Polling[1] = 5;

  PollTotal = 1; // Total number of slaves - 1
  PollCount = 0;
  Wait(10);

  while (true) {
    // Poll one slave in ar_Polling on each pass
    SlaveNum = NumToStr(ar_Polling[PollCount]);
    SlaveMsg = "P"; // P = Polling
    msg = SlaveNum + "," + SlaveMsg;

    // if touch sensor 1 pressed create message for slave number = 5
    if(Sensor(S1) > 0 && ar_Polling[PollCount] == 5) {
      msg = "5,C";
    }
    // if touch sensor 2 pressed create message for slave number = 2
    if(Sensor(S2) > 0 && ar_Polling[PollCount] == 2) {
      msg = "2,C";
    }

    // display out going message
    TextOut(0, LCD_LINE1, msg, true);

    ReadMsg = true;
    // Send message
    SendRS485String(msg);
    WaitForMessageToBeSent();

    msg = "";
    TimeOut = 100; // set time out if not response from slave

    // start with empty input buffer
    SetHSInputBufferInPtr(0);
    SetHSInputBufferOutPtr(0);
    // wait for a message to arrive.
    mlen = 0;
    while (mlen == 0 && TimeOut > 0){
      TimeOut = TimeOut-1;
      if(TimeOut == 0) { // no message
        ReadMsg = false;
        SlaveMsgNum = NumToStr(ar_Polling[PollCount]);
        SlaveMsg = "error";
      }
      mlen = HSInputBufferInPtr();
    }
    Wait(2); // wait to get the whole message
    mlen = HSInputBufferInPtr();

    // get message
    if(ReadMsg == true) {
      GetHSInputBuffer(0, mlen, buffer);
      // clear the incoming buffer
      SetHSInputBufferInPtr(0);
      // display buffer message
      TextOut(0, LCD_LINE2, buffer);

      // phrase message
      SlaveMsgLen = StrLen(buffer);
      x = 0;
      while(x < SlaveMsgLen){
        if(SubStr(buffer, x, 1) == ",") { // look for comma
          SlaveMsgNum = SubStr(buffer, 0, x);
          SlaveMsg = SubStr(buffer, x+1, SlaveMsgLen);
          break;
        }
        x = x+1;
      }

      // read message and act on it
      if(SubStr(SlaveMsg, 0, 1) == "R") { // look for R
        if(SubStr(SlaveMsg, 2, 1) == "2") { // if from slave 2
          // Turn on Motor A
          OnRev(OUT_A, 75);  // turn Motor port A in reverse
        }
        if(SubStr(SlaveMsg, 2, 1) == "5") { // if from slave 5
          // Turn on Motor A
          OnFwd(OUT_A, 75);  // turn Motor port A forward
        }
      }
    }

    // display message
    TextOut(0, LCD_LINE3, SlaveMsgNum);
    TextOut(0, LCD_LINE5, SlaveMsg);
    PollCount = PollCount+1;
    if(PollCount > PollTotal) {
      PollCount = 0;
    }
    Wait(500); // time delay so humans can watch it

    Coast(OUT_A);
  }
}

// Lego NXT RS485 demo Slave
// modified from John Hansen nxcsamples
// http://bricxcc.sourceforge.net/nbc/nxcsamples/485.zip

void SendRS485String(const string msg)
{
  byte mlen = ArrayLen(msg);
  SetHSOutputBuffer(0, mlen, msg);
  SetHSOutputBufferOutPtr(0);
  SetHSOutputBufferInPtr(mlen);
  SetHSState(HS_SEND_DATA);
  SetHSFlags(HS_UPDATE); //send it
}

void WaitForMessageToBeSent()
{
  while(HSOutputBufferOutPtr() < HSOutputBufferInPtr())
    Wait(1);
}

task main()
{
  // set port 4 to RS485 type
  SetSensorType(IN_4, SENSOR_TYPE_HIGHSPEED);
  SetHSState(HS_INITIALISE);
  SetHSFlags(HS_UPDATE);
  // set ports 1 and 2 to touch sensor type
  SetSensor(S1, SENSOR_TOUCH);
  SetSensor(S2, SENSOR_TOUCH);

  byte mlen;  // message length
  string buffer; // message buffer
  string msg; // incoming message
  string SlaveMsgNum; // incoming message number
  string SlaveMsg;  // incoming message text
  int SlaveMsgLen;  // incoming message length
  int SlaveNum;  // number of slave NXT
  string strSlaveNum; // string form of slave NXT number
  string SendMsg;  // outgoing message
  int x;
  int result; // result of Open File
  byte handle; // handle for Open File
  string buf; // buf for Open File

  // Get slave number
  // each slave NXT must have a different number
  // can be assigned to SlaveNum the compiled to each slave NXT
  //SlaveNum = 5;
  // or a txt file can be uploaded to each slave NXT with a
  // different number in it then
  result = OpenFileRead("SlaveNum.txt", handle, handle);
  ReadLnString(handle,buf);
  result = CloseFile(handle);
  SlaveNum = StrToNum(buf);
  strSlaveNum = NumToStr(SlaveNum);

  // start with empty input buffer
  SetHSInputBufferInPtr(0);
  SetHSInputBufferOutPtr(0);

  Wait(10);
  while (true) {
    // wait for a message to arrive.
    mlen = 0;
    while (mlen == 0)
      mlen = HSInputBufferInPtr();

    Wait(2); // wait to get the whole message
    mlen = HSInputBufferInPtr();

    // read it.
    GetHSInputBuffer(0, mlen, buffer);
    // clear the incoming buffer
    SetHSInputBufferInPtr(0);

    // display buffer message
    TextOut(75, LCD_LINE1, strSlaveNum, true);
    TextOut(0, LCD_LINE1, buffer);

    // get SlaveNum of message
    SlaveMsgLen = StrLen(buffer);
    x = 0;
    while(x < SlaveMsgLen){
      if(SubStr(buffer, x, 1) == ",") { // look for comma
        SlaveMsgNum = SubStr(buffer, 0, x);
        SlaveMsg = SubStr(buffer, x+1, SlaveMsgLen);
        break;
      }
      x = x+1;
    }

    // Check sensors
    SendMsg = "";
    if(Sensor(S2) > 0) { // if touch sensor 2 is pressed
      SendMsg = "R-" + strSlaveNum + "-S2"; // create message to send to master
    }

    // Message for Me
    if(StrToNum(SlaveMsgNum) == SlaveNum) {
      TextOut(0, LCD_LINE3, SlaveMsgNum);
      TextOut(0, LCD_LINE5, SlaveMsg);
      // phrase message
      if(SubStr(SlaveMsg, 0, 1) == "C") {
        // Turn on Motor A
        OnFwd(OUT_A, 75); // turn on port A
      }

      // Respone to master polling message
      if(StrLen(SendMsg) == 0) { // if no return message
        SendMsg = "A-" + strSlaveNum; // A = acknowledge
      }
      // send return message to master
      msg = strSlaveNum + "," + SendMsg;
      Wait(10);
      SendRS485String(msg);
      WaitForMessageToBeSent();
    }
    Wait(10);
    Coast(OUT_A); // turn off port A
  }
}

set_sensor_type  (BrickNumber, PortNumber, SensorType)
set_sensor_mode  (BrickNumber, PortNumber, SensorMode)
get_sensor_value (BrickNumber, PortNumber, &SensorValue)
set_motor_speed  (BrickNumber,  PortNumber, &MotorSpeedValue)
get_motor_encoder (BrickNumber, PortNumber, &MotorEncoderValue)