/*******************************************************************************
 *   MASTER CONNECTION for Bluetooth GPS Receiver -GPS Data in NMEA Format-    *
 *******************************************************************************
 *                                                                             *
 *                                                                             *
 *      Workarround to handle bluetooth GPS receiver Data stream               *
 *      with NXC language from John Hansen version 1.0.1.32beta                *
 *      and NXT-Lego Firmware 1.04. Bluetooth GPS-Receiver in this             *
 *      example is a Navilock BT-348 with a NMEA GPS-Data output               *
 *      format, but other models of such kind should also work.                *
 *      Both devices (NXT and GPS receiver) work with 4600 baud.               *
 *      Normally, this workarround should not be possible, because             *
 *      of some NXC-Language or NXT-Firmware lacks, but so what,               *
 *      I have beaten the devil!                                               *
 *                                                                             *
 *      Author: Antonio Scarfone                                               *
 *      Email:  a.scarfone@web.de                                              *
 *      Date:   10.09.2007                                                     *
 *                                                                             *
 *                                                                             *
 *******************************************************************************/

#include "NXCDefs.h"
#define BT_CONN 1   //Bluetooth connection port
#define INBOX 1     //Mailbox or QUEUE number
#define INBUFFER 128   //The input buffer, where the GPS data are written
#define INBUFFERSMALL 58 //Workarround buffer of 58 bytes, because nxc is only able to read max 58 bytes
#define INBUFFERSMALL2 12  //Workarround buffer 2 x 58 = 116, remains 12 bytes to 128 bytes
#define STARTOFFSET 0     //Offset start
#define FILENAME "GPSData.txt" //Name of the file to write the GPS data


/*Struct, where to hold the gps data stream sentences for one read cycle, one read cycle brings up to 2 or 3 sentences.
For security reason, there is a fourth sentence struct variable implemented*/
struct sentence_struct {
  string sentence1;
  string sentence2;
  string sentence3;
  string sentence4;
};

sentence_struct gpsSentences;  //Global struct variable for the GPS sentences
string out,tmpstr;  //out: output for the sentences, all in one string . tmpstr: debug string
// temp. buffer for the workarround input buffer to read splitted in 58,58,12 bytes buffers
byte tmpbuffer0[];
byte tmpbuffer1[];
byte tmpbuffer2[];
//Buffer to clear or init the real 128 byte input buffer
byte clearbuffer[];
//Buffer for init the request of the GPS Data
byte startreadbuf[];
//Create or append file for output data only for debug phase
bool filewritten;

//Sub routine to check Bluetooth connection, exits the program if no connection
sub BTCheck(int conn){
   if (!BluetoothStatus(conn) == NO_ERR){
      TextOut(5,LCD_LINE2,"Error");
      Wait(1000);
      Stop(true);
   }
}


long _Power(int X, int Y)
{
long Ret,v;

  Ret=1;
  for (v=1;v<=Y;v++)
  {
    Ret=Ret*X;
  }

  return  Ret;
}

long HexToDec(string Hex) {

long   T1,T2,DEC;
bool   Error;
byte   hexlen;
byte   hexdigit[];
byte   ahex;
  hexlen=StrLen(Hex);
  Error=false;
  T1=0;T2=0;DEC=0;
  ArrayInit(hexdigit,0,2);
  StrToByteArray(Hex, hexdigit);
  for (T1=0;T1<hexlen;T1++)
  {
   T2=(hexlen-T1)-1;
   ahex = hexdigit[T1];

   switch (ahex) {
     case 48://"0"
          DEC=DEC+0;
          break;
     case 49://"1"
          DEC=DEC+_Power(16,T2);
          break;
     case 50://"2"
          DEC=DEC+2*_Power(16,T2);
          break;
     case 51://"3"
          DEC=DEC+3*_Power(16,T2);
          break;
     case 52://"4"
          DEC=DEC+4*_Power(16,T2);
          break;
     case 53://"5"
          DEC=DEC+5*_Power(16,T2);
          break;
     case 54://"6"
          DEC=DEC+6*_Power(16,T2);
          break;
     case 55://"7"
          DEC=DEC+7*_Power(16,T2);
          break;
     case 56://"8"
          DEC=DEC+8*_Power(16,T2);
          break;
     case 57://"9"
          DEC=DEC+9*_Power(16,T2);
          break;
     case 65://"A"
          DEC=DEC+10*_Power(16,T2);
          break;
     case 66://"B"
          DEC=DEC+11*_Power(16,T2);
          break;
     case 67://"C"
          DEC=DEC+12*_Power(16,T2);
          break;
     case 68://"D"
          DEC=DEC+13*_Power(16,T2);
          break;
     case 69://"E"
          DEC=DEC+14*_Power(16,T2);
          break;
     case 70://"F"
          DEC=DEC+15*_Power(16,T2);
          break;
     default: Error=true;
   }
  }
  if (Error)
  {
    return 0;
  }
  else
  {
    return DEC;
  }
}




// Writes the sentences stored in out to a file, only for debug
void WriteData (string afile, string Data)
{
byte handle;
int slen;
int fsize;


     if (!filewritten) {
       if (CreateFile(afile, 10048, handle) == NO_ERR ) {
          slen = StrLen(Data);
          WriteBytes(handle, Data, slen);
          CloseFile(handle);
          filewritten = true;
       }
     } else {
        if (OpenFileAppend(afile, fsize, handle) == NO_ERR ) {
          slen = StrLen(Data);
          WriteBytes(handle, Data, slen);
          CloseFile(handle);
       }
     }
}



int GetSentences (sentence_struct &ret,byte buffer1[],byte buffer2[],byte buffer3[],byte asciibegin, byte asciiend)
{
byte sarray[],rarray[];
string astr = "";
int beg = -1;
int ends = -1;
int ii = 0;
int xx = 0;
int num = -1;
byte alen = 0;
byte lensplit = 0;
bool startsplit = true;
bool startchecksum = false;
int checksum = -1;
byte thehexdigit[];
string thehex = "";
long dec = 0;
int l;

ArrayInit(sarray, 0, INBUFFER);
ArrayInit(rarray, 0, INBUFFER);
ArrayBuild(sarray, buffer1, buffer2, buffer3);
alen = ArrayLen(sarray);
ArrayInit(thehexdigit, 0, 2);



for (ii=0;ii<alen;ii++)
   {
     if (sarray[ii] == asciiend) // end of a sentence, get checksum in hex format
     {
        startchecksum = false;
        thehexdigit[0] = sarray[ii+1];
        thehexdigit[1] = sarray[ii+2];
        ByteArrayToStrEx(thehexdigit, thehex);
        dec = HexToDec(thehex); //calculate decimal value of the hex
     }
     if (startchecksum) {
        checksum ^= sarray[ii]; //calc xor checksum of the sentence, starts with char '$' and ends with '*', but without '*'
     }


     if (sarray[ii] == asciibegin)
     {

       if (checksum == -1) { //init checksum to calculate
          checksum = sarray[ii];
          checksum ^= sarray[ii];
          startchecksum = true;
       }
       if (startsplit == true && beg == -1 && ends == -1)  //mark position of split char '$' beginning
        {
          beg = ii;  //mark position
          startsplit=false;
          continue; //continue with next char
        }

       if (startsplit == false && beg > -1 && ends == -1)  //mark position of split char '$' ending
        {
          ends = ii;
          num++;
          lensplit = (ends - beg);
          ArraySubset(rarray, sarray, beg, lensplit); //extract sentence
          ByteArrayToStrEx(rarray, astr);

          // if checksum ok, save sentence
          if (xx == 0 && checksum == dec)ret.sentence1 = astr;
          if (xx == 1 && checksum == dec)ret.sentence2 = astr;
          if (xx == 2 && checksum == dec)ret.sentence3 = astr;
          if (xx == 3 && checksum == dec)ret.sentence4 = astr;
          //clear buffers and init all variables
          ArrayInit(rarray, 0, INBUFFERSMALL);
          ArrayInit(thehexdigit, 0, 2);
          dec = 0;
          thehex = "";
          checksum = -1;
          astr = "";
          startsplit = true;
          ends = -1;
          beg = -1;
          xx++;
          ii--; //important, go with iteration one back to mark next beginning sentence, end = begin
        }
     }
   }

   num++;
   return num;
}


task main(){
   int numsent = 0;// Number of sentences received in one read cycle
   int n = 0;
   byte inbufPtr = 0;
   IOMapReadType InBufferfull;  // Struct to check if input is full and ready for the next read cycle
   //init the following buffers
   ArrayInit(tmpbuffer0, 0, INBUFFERSMALL);
   ArrayInit(tmpbuffer1, 0, INBUFFERSMALL);
   ArrayInit(tmpbuffer2, 0, INBUFFERSMALL2);
   ArrayInit(clearbuffer, 0, INBUFFER);
   ArrayInit(startreadbuf, 0, INBUFFERSMALL);

   InBufferfull.ModuleName = CommModuleName;
   InBufferfull.Offset = CommOffsetBtInBufInPtr;
   InBufferfull.Count = 1;
   InBufferfull.Buffer[0]=0;

   BTCheck(BT_CONN); //check slave connection
   //Check if file is already written
   filewritten = false;
   ReceiveRemoteString(INBOX, 1, startreadbuf); //Start a receive sequence to fill up the input buffer with GPS data

   while(true){
       //init the struct to hold the sentences
       gpsSentences.sentence1 = "";
       gpsSentences.sentence2 = "";
       gpsSentences.sentence3 = "";
       gpsSentences.sentence4 = "";
       //init the output string, which hold up the gpsSentences struct
       out = "";

       //Start a receive sequence to fill up the input buffer with GPS data , before reading the input buffer
       ReceiveRemoteString(INBOX, 1, startreadbuf);
       //make sure one receive sequence is not pending or not in error
       SysIOMapRead(InBufferfull);
       while(InBufferfull.Buffer[0]==0) { SysIOMapRead(InBufferfull); }
       SetBluetoothState(NO_ERR);
       InBufferfull.Buffer[0]=0;
       //Get the 128 byte large input buffer in three temp. buffer splitted up in 58,58 and 12 bytes long bytes arrays,
       //in order to workarround the ability of nxc to handle only 58 bytes large buffers.
       GetBTInputBuffer(STARTOFFSET, INBUFFERSMALL, tmpbuffer0);
       GetBTInputBuffer(INBUFFERSMALL, INBUFFERSMALL, tmpbuffer1);
       GetBTInputBuffer(2*INBUFFERSMALL, INBUFFERSMALL2, tmpbuffer2);

       //Submit the three temp. input buffers, in order to extract the sentences.
       //Each sentence starts with a '$' char ascii=36 and ends with a '*' char ascii=42 to follow a checksum and a CR+LF.
       numsent = GetSentences(gpsSentences, tmpbuffer0, tmpbuffer1, tmpbuffer2, 36, 42);
       //Output the extracted sentences to out string
       if (gpsSentences.sentence1 != "")out=StrCat(out,gpsSentences.sentence1);
       if (gpsSentences.sentence2 != "")out=StrCat(out,gpsSentences.sentence2);
       if (gpsSentences.sentence3 != "")out=StrCat(out,gpsSentences.sentence3);
       if (gpsSentences.sentence4 != "")out=StrCat(out,gpsSentences.sentence4);
       //Clear up the input buffer and set the input buffer pointer at the beginning of the buffer
       inbufPtr = BTInputBufferInPtr();
       if (inbufPtr == INBUFFER){
           SetBTInputBuffer(STARTOFFSET, INBUFFER, clearbuffer);
           SetBTInputBufferInPtr(0);
       }
       

       //clear up the temp. buffers
       ArrayInit(tmpbuffer0, 0, INBUFFERSMALL);
       ArrayInit(tmpbuffer1, 0, INBUFFERSMALL);
       ArrayInit(tmpbuffer2, 0, INBUFFERSMALL2);
       ArrayInit(clearbuffer, 0, INBUFFER);

       n+=numsent;

       NumOut(0,LCD_LINE5,n);
       //write the sentences in a file
       WriteData(FILENAME,out);
       numsent = 0;

   }
}