File content as of revision 47:199042980678:

/*

- NOTE: I could instantiate at least 960 points when NOT USING THE ETHERNET LIBRARY! (as 30 objects containing each 32 points). 
  ... but then again I could not make only 36 1-point object on a grid? what's going on? an object takes MUCH MORE MEMORY than just
  the trajectory and 3d vector array... but where?
  Anyway, using the ETHERNET LIBRARY uses more than 20kB, and then I cannot instantiate more than a few letters... 
*/

//#define WITH_OSC // this consumes about 20kb or RAM!!!

#include "mbed.h"
#include "WrapperFunctions.h"

#ifdef WITH_OSC
#include "mbedOSC.h"
#endif

// The following is because I still did not "wrap" ALL the LaserRenderer methods (so we have to use the object lsr directly, for instance to set a matrix: lsr.setIdentityPose()...)
#include "LaserRenderer.h"
extern LaserRenderer lsr;

// mbed IP address (server):
#ifdef WITH_OSC
#ifdef DHCP
EthernetNetIf eth;
#else
EthernetNetIf eth(
    IpAddr(10,0,0,2), //IP Address of the mbed
    IpAddr(255,255,255,0), //Network Mask
    IpAddr(10,0,0,1), //Gateway
    IpAddr(10,0,0,1)  //DNS
);
#endif

//uint8_t serverMac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
uint8_t serverIp[]  = { 10, 0, 0, 2 };
int  serverPort  = 10000;

//uint8_t destIp[]  = {10, 0, 0, 3};
uint8_t destIp[]  = {255, 255, 255, 255};  // broadcast, so we can use several computers for sound, etc
int  destPort = 12000;

char *topAddress="/mbed";
char *subAddress[3]= { "/test1" , "/test2" , "/test3" };

OSCMessage recMes;
OSCMessage sendMes;

OSCClass osc;
//OSCClass osc(&recMes);  // instantiate OSC communication object, and set the receiver container from the OSC packets
#endif

// for tests:
extern Scene scene;
extern laserSensingDisplay lsd;

// =======================================================

// Timers:
Timer t; // we can read in ms or us

// Tickers:
//Ticker timerForRendering; // now in the WrapperFunctions.cpp file
//Ticker timerForSendingData; // better use a timer, so as not to interrupt the exact laser display ticker
unsigned long lastTimeCreated;
unsigned long lastTimeSensed;

// ======================================================================================================================
// DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics).
// -> would be better to load them from a system file!! (TO DO)
// Note: last row is completed automatically in the loadExtrinsicMatrix and loadProjMatrix methods
// Extrinsics of usb camera to projector (note: calibration was in cm, so the translation vector is in cm)
float E1[12] = {9.8946330287356954e-01, -5.0979372852926171e-03, -1.4469410251272136e-01,  5.8356271792647112e+00,
                -4.7536907255693065e-03,  9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00,
                1.4470582120637832e-01,  6.7633532232509466e-02,  9.8716053943963022e-01, -8.2545447546057940e+00
               };

// 9.9540692993883650e-01, -8.5578652572728450e-03, -9.5350966287597705e-02, 2.2256570914518332e+00,
//                1.4872282274740087e-02,  9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00,
//                 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01,  1.0699927478132011e+01
//};


// INTRINSICS of laser projector:
float K1[6] = { 5.9957148864529627e+03, 0.0000000e+000, 2.8869455045867635e+03,
                0.0000000e+000,         5.9647451141521778e+03, 2.9005238051336592e+03
              };

// 7.0776317995422105e+03, 0., 2.4203185896583996e+03,
//                0.,                     7.2889265345161484e+03, 1.7718110988625751e+03};

// SCALE FACTOR for the SCAN:
float scaleFactorProjector1 = 1.0; //scale factor for the projector image (the scan is, say, 600x600, but the resolution is 4096x4096)

// ======================================================================================================================
// LASER TERMINAL:
// We assume that the "terminal" is operating on an a4 page. Units are in cm.
V2 cursorPosition(0,0);

void createTextScene();
void addText(string, int, float, float);

float angle;
string textToDisplay = "O";
float fontWidth = 2.5, fontHeight = 2.5; // if calibration is in cm, then this is in cm.

// ==============================================================================================================
// Some global functions and variables (find a better way to integrate this!)
extern "C" void mbed_reset();

void createSceneTest();
void setOrthographicView();

#ifdef WITH_OSC
void processOSC(UDPSocketEvent e);
#endif
void interpretCommand();
void processSerial();

int touchedTimes = 0;
enum sensingModes {NO_SENSING, SENSING_WHOLE_SCENE, SENSING_PER_OBJECT};
sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING;

// ================= AUXILIARY VARIABLES FOR INTERPRETING COMMAND FROM SERIAL OR OSC =========================
// NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretCommand function):
// max of two addresses (top and sub), of a max length of 24 characters:
char address[2][24];
//long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
int data[2]; // this is to have -1 as NO DATA, to detect errors.
int numArgs;

// An auxiliary buffer to store parameters data, or matrices, or points to load as vertices.
// Note: for matrices (pose, projection), the maximum size we need is 12 floats, but as we will also receive vertices, let's give enough space (assuming
// we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot)
float auxDataBuffer[128];
int auxDataBuffer_index=0;

// Commands:
string command1, command2;

// this is a hack for the time being:
int objectID;
int xx, yy; // initial position of object or displacement
bool firstTimeDisplay=true;

// ==============================================================================================================
int main()
{

    t.start();
    lastTimeCreated = t.read_ms();
    lastTimeSensed = t.read_ms();
    angle = 0;

    // Setup:
    // (1) Hardware init (laser powers, positions...):
    IO.init();

#ifdef WITH_OSC
    // OSC initialization:
    // Set the Ethernet port:
    printf("Setting up...\r\n");
    EthernetErr ethErr = eth.setup();
    if (ethErr) {
        printf("Error %d in setup.\r\n", ethErr);
        return -1;
    }
    printf("Setup OK\r\n");

    //(1) Sending message:
    // Set IP and Port:
    sendMes.setIp( destIp );
    sendMes.setPort( destPort );
    // Set data:
    sendMes.setTopAddress(topAddress);

    //setting osc functionnality:
    //(2) Receiving:
    recMes.setIp( serverIp ); // not needed?
    osc.setReceiveMessage(&recMes); // this sets the receiver container for the OSC packets (we can avoid doing this if we use osc.getMessage() to get messages)
    osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function
    wait(1);
#endif

    //===================================================================

    /*
        //EXAMPLE 1: simple geometry ======================================================
        // (1) Calibration matrices (Projection, and extrinsics - if needed):
        // Typically, the projection matrix is set from the PC side, or loaded by default (from file system).
        lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.

        // (2) Create the scene in "local" coordinates:
        createSceneTest(); // Create a default scene (for tests)

         // (3) Set a default GLOBAL POSE and PROJECTION MATRIX (if we changed it) for rendering and start displaying:
         // - Using the current K (that correct for the center of the camera too!):
        //lsr.setIdentityPose();
        //lsr.translate(0, 0, 2000);
        // - or doing orthoprojection:
        lsr.setOrthoProjection();
        lsr.setIdentityPose();
        lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
        lsr.rotateZ(45);
        drawScene();

         // *** AND IMPORTANT *** the FIRST time, we need to set the display engine (ie, attach the interrupt). This is not needed afterwards, ONLY if we change the scene.
          lsr.startDisplay();

        // (4) Main loop:
        // Change the POSE when necessary (also called "viewing transformation" or "global modelview" in OpenGL jargon), render and display:
        while (1) {

            if (pc.readable())
                processSerial(); // here, the pose can be changed by a pc command
            hardwareKnobs();

            if (0) { //((t.read_ms()-lastTimeCreated)>10) {
                // for tests:
                myLed2 = !myLed2;

                angle += 1;
                if (angle > 360)
                    angle = 0;

                // Set the current global pose:
               // lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
               // lsr.translate(0, 0, 1000);
               // lsr.rotateZ(angle);
               // lsr.rotateX(2.5 * angle);

                lsr.setOrthoProjection();
                lsr.setIdentityPose();
                lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
                lsr.rotateZ(angle);
               // lsr.rotateY( angle);

                // Then, render (and display) the whole scene (note: we DEFINITELY don't want to call this all the time, only when the RT matrix changes)
                drawScene();

                lastTimeCreated = t.read_ms();
            } // end auto-change pose loop

            if ((t.read_ms() - lastTimeSensed) > 50) { // check data sensing every xx ms
                // Check objects and change colors or other properties as a function of the touch:
                // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
                // (note: we assume we are drawing the text).
                // There are two ways to query for an object input: using the ID, or from its pointer (scene->objectArray[i]) if we know it. This is faster.
                for (int i = 0; i < scene.totalObjects(); i++) {
                    BaseObject* ptrObj = scene.objectArray[i];
                    if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
                        ptrObj->setColor(0x02);

                        // char str[15];
                        // sprintf(str, "%d", touchedTimes);
                        // textToDisplay=string(str);//string(itoa(touchedTimes));
                        // touchedTimes++;
                        // createTextScene();

                    } else
                        ptrObj->setColor(0x01);
                }
                lastTimeSensed = t.read_ms();
            } // end sensing loop

        } // "end" infite loop
          // =================================
    */

    // EXAMPLE 2: 3d text  ======================================================

    // (1) Calibration matrices:
    // Typically, the projection and extrinsic matrix is set from the PC side, or loaded by default (from file system).
    lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
    lsr.loadExtrinsicsMatrix(E1);

    // (2) Create the scene in "local" coordinates:
    createTextScene(); // Clear scene, and create a default scene (for tests), and update scene (but not yet rendered)

   // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
    lsr.loadProjMatrix(K1, scaleFactorProjector1);
    lsr.setIdentityPose();
    lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()

    // DEFAULT position in CAMERA coordinates (calibration was in cm):
    lsr.translate(0, 0, 130);

     drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
     updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
    // rendering is done in the drawScene() function.
    // *** AND IMPORTANT *** the FIRST time, we need to set the display engine (ie, attach the interrupt). This is not needed afterwards, ONLY if we change the scene.
    
    startDisplay();

    // Add more text (testing):
  //   textToDisplay="1";
  //   addText();
     
      // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
  //  lsr.loadProjMatrix(K1, scaleFactorProjector1);
  //  lsr.setIdentityPose();
  //  lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()
    // DEFAULT position in CAMERA coordinates (calibration was in cm):
  //  lsr.translate(0, 0, 130);
     
   //  drawScene(); 
   //  updateScene();

    // (4) Main loop:
    while(true) {

        // =========== Query hardware events (knobs, ethernet, serial port) ============
       #ifdef WITH_OSC
        Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC!
        #endif
        
        if (pc.readable()) processSerial(); // here, the pose can be changed by a pc command
        hardwareKnobs();
        // =============================================================================

        // Automatic change of pose for tests
        if (0) {//(t.read_ms()-lastTimeCreated)>20) {
            angle+=.2;
            if (angle>360) angle=0;

            lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
            lsr.flipY();
            lsr.translate(0,0,1000);
            lsr.rotateZ(angle);
            //lsr.rotateX(angle);

            drawScene();

            lastTimeCreated=t.read_ms();
        }

        if ((currentSensingMode!=NO_SENSING)&&((t.read_ms()-lastTimeSensed)>70)) { // check data sensing every xx ms
            // Check objects and change colors or other properties as a function of the touch:
            // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
            // (note: we assume we are drawing the text).
            // There are two ways to query for an object input: using the ID, or from its pointer (scene->objectArray[i]) if we know it. This is faster.
            switch(currentSensingMode) {
                case SENSING_WHOLE_SCENE: // Sensing whole scene ----------
                    if (scene.sense())   {

                        // COUNTING:
                        // char str[15];
                        //  sprintf(str, "%d", touchedTimes);
                        //  textToDisplay=string(str);//string(itoa(touchedTimes));
                        //  touchedTimes++;
                        //  createTextScene();
                        // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
                        // lsr.loadProjMatrix(K1, scaleFactorProjector1);
                        // lsr.setIdentityPose();
                        // lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()
                        // This means that the current coordinate frame is the CAMERA, and calibration was in cm:
                        // lsr.translate(0, 0, 100);
                        ////lsr.flipY();
                        // drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
                        // }
                    }
                    break;
                case SENSING_PER_OBJECT: // Per-object sensing and action:
                    for (int i=0; i<scene.totalObjects(); i++) {
                        BaseObject* ptrObj=scene.objectArray[i];
                        if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
                            ptrObj->setColor(0x02); // make it green

                            // for fun: let's rotate THIS letter:
                            //lsr.pushPoseMatrix();
                            //lsr.setIdentityPose();
                            //lsr.rotateX(180);
                            //transformObject(ptrObj);
                            //lsr.popPoseMatrix();
                            // we need to redraw the scene (ie, reproject), or at least this object:
                            //lsr.renderObject(ptrObj, lsr.RT);

                            // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have
                            //addCartoucheObject(ptrObj);

                             //pc.putc(ptrObj->ID());
                             //pc.printf("%d \n", ptrObj->ID());
                             pc.printf("%d", ptrObj->ID());
                        } else
                            ptrObj->setColor(0x01); // make it blue
                    }
                    break;
                default:
                    break;
            }
            lastTimeSensed=t.read_ms();
        } // end sensing

    } // infinite while loop
    // =================================
}

void setOrthographicView()
{
    // clearScene();
    // set color:
    lsr.setColor(0x02); // green
    //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera

    lsr.setOrthoProjection();
    lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
}

void createSceneTest()
{
    lsr.pushPoseMatrix();

    // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
    clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)

    // (2) Set displaying attributes (colors and local pose):
    // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
    lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
    // (b) set color:
    lsr.setColor(0x02);

    // Add a first object: using openGL-like syntaxis (here, a closed square):
    // begin(1); // start creating object with identifier = 1
    //  vertex(10,10,1);
    //  vertex(10,100,1);
    //  vertex(100,100,1);
    //  vertex(10,100,1);
    //  vertex(10,10,1); //
    // end();

    // Add another objects using "object primitives":
    //begin(2);
    //lsr.translate(-100,-100,0); // recenter the square
    //square(200, 10); // this square has a corner in (0,0), and by default, z=0
    //end();

    // A "cube" (with one corner at the origin):
    //begin(3);
    //lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
    //lsr.translate(100,-100,-100); // recenter the cube (no need to push pop because we are seting the identity above)
    //lsr.rotateY(30);
    //cube(200,10); // cube(200, 20);
    //end();

    // A grid:
    // IMPORTANT: a grid is a multi-object built, and DOES NOT NEED to be between a begin and end
    lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
    lsr.translate(-500, -500, 0);
    grid(1000, 1000, 5, 5, 1); //grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
    //gridCircles(300, 250, 6, 5, 10, 6); // gridCircles(float sizeX, float sizeY, int nx, int ny, float radius, int nbpointsCircle)

    // A circle:
    //begin(4);
    //circle(200, 100);
    //end();

    updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
    // rendering is done in the drawScene() function.

    // pc.printf("Number of objects in the scene: %d\n", scene.totalObjects());
    // pc.printf("Number of points in the scene: %d\n", scene.totalPoints());

    lsr.popPoseMatrix();
}

void createTextScene()
{
    lsr.pushPoseMatrix();
    //This function just creates text in local coordinates. We have two options: to create it as a unique object, or as separate objects (one per letter).
    // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):

    // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
    clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)

    // (2) Set displaying attributes (colors and pose):
    // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
    lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
    //lsr.flipY();
    //lsr.translate(2,6,0);

    lsr.flipY();
    lsr.flipX();
    lsr.translate(-15,-10,0);

    // (b) set current color:
    lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).

    // A string as a UNIQUE object:
    // begin(1); // start creating an object with index 1
    // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
    //    string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
    //end();

    // A string as a *collection* of object-letters:
    //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
    for (unsigned short i = 0; i < textToDisplay.length(); i++) {
        char ch = textToDisplay.at(i);
        if (ch != ' ') {
            begin(i);
            letter3d(ch, fontWidth, fontHeight);
            end();
        }
        lsr.translate(1.1*fontWidth, 0, 0);
    }

    lsr.popPoseMatrix();

}

void addText(string text, int id, float posx, float posy)
{
    //clearScene();
    lsr.pushPoseMatrix();
    //This function just creates text in local coordinates. We have two options: to create it as a unique object, or as separate objects (one per letter).
    // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):

    // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
    // clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)

    // (2) Set displaying attributes (colors and pose):
    // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
    lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
    //lsr.flipY();
    //lsr.translate(2,6,0);

    lsr.flipY();
   // lsr.flipX();
    lsr.translate(posx,posy,0);

    // (b) set current color:
    lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).

    // A string as a UNIQUE object:
    // begin(1); // start creating an object with index 1
    // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
    //    string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
    //end();

    // A string as a *collection* of object-letters:
    //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
     
    for (unsigned short i = 0; i < text.length(); i++) {
        char ch = text.at(i);
        if (ch != ' ') {
          begin(id);
            letter3d(ch, fontWidth, fontHeight);
        end();
        }
        lsr.translate(1.1*fontWidth, 0, 0);
    }
    lsr.popPoseMatrix();

}

// ============ COMMUNICATION PROTOCOL ======================================
// String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC):
// TO DO: use string objects!
char receivedStringData[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int...
int indexStringData = 0; //position of the byte in the string

// String to store COMMAND WORDS (not used yet):
char stringCommand[24];
int indexStringCommand = 0;
bool commandReady = false; // will become true when receiving the byte 0 (i.e. the '/0' string terminator)


#ifdef WITH_OSC
//============= RECEIVE OSC COMMANDS =========================
// This is the callback function called when there are packets on the listening socket. It is not nice to have it
// here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer
// as handler to the upd object).
void processOSC(UDPSocketEvent e)
{
    osc.onUDPSocketEvent(e);

    if (osc.newMessage) {

      //  pc.printf("RECEIVED NEW OSC MESSAGE ----\n");

        // Acquire the addresses and arguments and put them in the GLOBAL variables to be processed by the interpretCommand function (common to serial and OSC):
        command1="";
        command2="";
        for (int i=0; i<recMes.getAddressNum(); i++)  {
            strcpy(address[i],recMes.getAddress(i)); // NOTE: up to the rest of the program to check if address[1] is really not null
          //  pc.printf("Address %d = %s \n",i, address[i]);
        }
        // Acquire data:

        numArgs=recMes.getArgNum();
        data[0]=-1;
        data[1]=-1;
        for (int i=0; i<numArgs; i++) {
            data[i]=(int)recMes.getArgInt(i);//recMes.getArgFloat(i);
            //pc.printf("%4.2f %4.2f \n", data[i], recMes.getArgFloat(i));
           // pc.printf("%i %i \n", data[i], recMes.getArgInt(i));
        }

        // Finally, interpret the command or data:
        interpretCommand();

    }
}
#endif


//interpretCommand(const char& address[2][], const int& data[2]) {
void interpretCommand()
{
    // ==========  DATA (all this is a hack because my OSC class does not accept BUNDLES...) =============
    // Add data to auxiliary data buffer:
    if (command1 == "data") {
        // numArgs is the number of arguments (in my OSC version this is max 2):
     //   pc.printf("--- Data pack: %d\n", numArgs);
        for (int i=0; i<numArgs; i++) {
            auxDataBuffer[auxDataBuffer_index+i] = 1.0*data[i];
            // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received.
     //       pc.printf("buffer index / data: %i %f \n", auxDataBuffer_index+i, auxDataBuffer[auxDataBuffer_index+i]);//data[i]);
        }
        auxDataBuffer_index+=numArgs;
        //pc.putc('/n');
    }
    // ========== COMMANDS (using data on auxDataBuffer or directly as arguments =========================

    else if (command1 == "objectID") { // ID of the object
            receivedStringData[indexStringData] = 0;
            indexStringData = 0;
            objectID=atoi(receivedStringData);
       }

    else if (command1 == "mbedReset" ) mbed_reset();

    else if (command1 == "testPower" ) {
        // TO DO
    }

    // Enable/disable projection:
    else if ( command1 == "stopDisplay" ) stopDisplay();
    else if (command1 =="resumeDisplay")  resumeDisplay();
    else if (command1 =="showLimits") showLimitsMirrors(50, 1);
    else if (command1 == "setSensingMode") {
        int value=data[0];
        if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
            switch(value) {
                case 0:
                    currentSensingMode=NO_SENSING;
                    break;
                case 1:
                    currentSensingMode=SENSING_WHOLE_SCENE;
                    break;
                case 2:
                    currentSensingMode=SENSING_PER_OBJECT;
                    break;
                default:
                    break;
            }
        }
    }

    // Change state machine color (objects created from this point on will have the state machine color)
    else if (command1 == "setStateColor") { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
        int value=data[0];
        if (value!=-1) lsr.setColor(value);
    }

    // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
    else if (command1 == "setAllObjectsColor") { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
        int value=data[0];
        if (value!=-1) changeColorScene(value);
    }


// ===================  CREATION and DESTRUCTION of objects =====================================================
    else if (command1 == "clearScene") {
        clearScene();
    }

    else if (command1 =="deleteObject") { // second address is object identifier (a string that will be converted
        deleteObject(objectID);
    }
    
       else if (command1=="initialPosition") { // this is for setting the POSITION of the object to be created (or displacement):
           xx=auxDataBuffer[0]; yy=auxDataBuffer[1];
           auxDataBuffer_index=0;
       }
    
// ===================  TRANSFORMATION OF OBJECT POINTS =========================================================
// NOTE: this may in the end destroy the object rigidity because approximations... but without an RT per object, this is the only way to 
// modify object pose independently of the whole scene...
else if (command1 =="moveObject") {
       translateObject(objectID,  xx,  yy, 0);
    }


// ==============================================================================================================


    // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
    // been translated previously - this may be optional, we could first send the pose, then the points...)
    // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
    // setting Z to 1, or by setting the projection matrix to a unit diagonal but the last term in the diagonal is set to 0.
    // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
    // NOTE: the following string of ASCII characters is assumed to have been input:
    //               posX#posY#sizeX#sizeY#nx#ny#:
    // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
    // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
    else if (command1 =="gridScene") {
        // Data is now ordered in the auxDataBuffer array:
        float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
        auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer'
        clearScene();
        // set color:
        lsr.setColor(0x02); // green
        //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera

        // set local pose matrix to identity:
        lsr.setIdentityPose();
        // translate to the required position:
        lsr.translate(posX, posY, 0); // since the projection  and pose are going to be set
        // to Identity (projection identity will automatically make the projection orthographic), this means that these
        // values are in "laser projector pixels" (in fact angles).
        // Create the collection of dot objects:
        // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
        grid(sizeX, sizeY, nx, ny, 1);
        updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
        // rendering is done in the drawScene() function.

        // Now we can render the scene once:
        // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
        lsr.setOrthoProjection();
        lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
        drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
    }
    // NOTE: now that the grid is set, we don't need to do it again when changing the pose: JUST SEND THE POSE MATRIX and things will be re-rendered!

    // This means we received "2D" vertex data (z assumed 0) to create A SINGLE SCENE out of these points (we will create a single object)
    // using orthographic projection.
    // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
    // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
    // NOTE: the POSE is reset to ID, but we could use the current pose (i.e., send it first by the computer). But for the time being, and in order to modify the
    // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
    else if (command1 == "arbitraryGridScene") {
        // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object:
        // The present value of auxDataBuffer_index contains the number of points x 2
        clearScene();
        lsr.setIdentityPose();
        //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
        lsr.setColor(0x03); // blue+green
        IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera

        // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
        for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
            begin(i);
            for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
                vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
            end();
        }
        updateScene();
        // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
        // properly called lsr.pushPoseMatrix())
        lsr.setOrthoProjection();
        lsr.setIdentityPose();
        drawScene();
        auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
    }

    // This means we received 2D vertex array to add an object assuming it's planar (we will create an object with all points having z=0 in current pose):
    else if (command1 == "createObject2D") {
        unsigned char numPoints= auxDataBuffer_index/2;
         if (objectID!=-1) {

                // First, create the object:
                lsr.pushPoseMatrix();
                lsr.setIdentityPose();
                lsr.translate(xx,yy,0);

                lsr.setColor(0x02); // we will render in current color

                begin(objectID);
                for (unsigned char i = 0; i < numPoints; i++) {
                    begin(i);
                    vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1],0); // z=0 for 2d objects (in current pose)
                    //letter3d('A', fontWidth, fontHeight);
                    //vertex(10*cos(2*PI/numPoints*i), 10*sin(2*PI/numPoints*i), 0);
                    //vertex(5*cos(2*PI/numPoints*i+.1), 5*sin(2*PI/numPoints*i+.1), 0);
                    //pc.printf("local (x,y): %f, %f \n", auxDataBuffer[2 * i],  auxDataBuffer[2 * i+1]);
                    // lsr.translate(1.1*fontWidth, 0, 0);
                }
                end();
                lsr.popPoseMatrix();

                // Set the global projection and modelview:
                lsr.loadProjMatrix(K1, scaleFactorProjector1);
                lsr.setIdentityPose();
                lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()
                // DEFAULT position in CAMERA coordinates (calibration was in cm):
                lsr.translate(0, 0, 130);

                // RENDER the scene:
               // update the scene for display
                drawScene();
                updateScene();
            }
    }

    // This means we received 3D vertex data to create an object.
    // It will be rendered in the CURRENT pose.
    else if (command1 == "createObject3D") {
        int objectID=data[1];
        if (objectID!=-1) {
            // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
            // The present value of auxDataBuffer_index contains the number of points x 3
            //lsr.pushPoseMatrix(); // we will render on the current pose
            //clearScene();
            //lsr.setIdentityPose();
            //lsr.setColor(0x02); // we will render in current color
            begin(objectID);
            for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
                vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
                       auxDataBuffer[3 * i + 2]);
            }
            end();
            updateScene();
            // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
            // properly called lsr.pushPoseMatrix())
            //lsr.setOrthoProjection();
            //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
            //lsr.popPoseMatrix();
            drawScene();
            auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
        }
    }
    
      else if (command1=="createText") {
            receivedStringData[indexStringData] = 0; // termination for the string
            indexStringData = 0;
            // Serial.println(receivedStringData);
            textToDisplay = string(receivedStringData);
           
           if (firstTimeDisplay) {clearScene(); firstTimeDisplay=false;}
           
           addText(textToDisplay, objectID, xx, yy);
           drawScene();
           updateScene();
        }

    // Create text:
    else if (command1 == "createTextObject") {

        if (objectID!=-1) {

            // CREATE OBJECT:
            lsr.pushPoseMatrix();
            lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
            lsr.flipY();
            //lsr.flipX();
            lsr.translate(xx,yy,0);

            begin(objectID);
            for (unsigned short i = 0; i < textToDisplay.length(); i++) {
                char ch = textToDisplay.at(i);
                if (ch != ' ') {
                   // begin(i);
                    letter3d(ch, fontWidth, fontHeight);
                   // end();
                }
                lsr.translate(1.1*fontWidth, 0, 0);
            }
            end();

            lsr.popPoseMatrix();

            // Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
            lsr.loadProjMatrix(K1, scaleFactorProjector1);
            lsr.setIdentityPose();
            lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()
            // DEFAULT position in CAMERA coordinates (calibration was in cm):
            lsr.translate(0, 0, 130);

            drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
            updateScene();
        }
    }


    // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
    else if (command1 == "poseMatrix") { // when receiving this, it means that the WHOLE matrix data (4x4 values) have been sent in row/column format
        auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer'
        // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
        lsr.setIdentityPose();               // RT=ID
        lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()
        lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates

        drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project)
    }

    // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
    else if (command1 == "projectionMatrix") {// when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
        auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
        // store in projection matrix:
        lsr.loadProjMatrix(auxDataBuffer, 1.0);

        drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
    }

    //(e) TERMINATOR indicating the data was for the extrinsic matrix:
    else if (command1 == "extrinsicMatrix")  { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
        auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
        // store in projection matrix:
        lsr.loadExtrinsicsMatrix(auxDataBuffer);

        drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
    }
}

void processSerial()
{
    while (pc.readable() > 0) {
        char incomingByte = pc.getc();

        // For test:
        // pc.putc(incomingByte);

        // (a) TEXT to display or NUMBERS (in ASCII) to convert to numerical value (rem: limited set of characters for the time being):
        // Note: numbers are float, so we need also the "." and "-"
        if (       (incomingByte >= '0' && incomingByte <= '9')  // numbers
                   || (incomingByte >= 'A' && incomingByte <= 'Z')  // capital letters
                   || (incomingByte >= 'a' && incomingByte <= 'z')  // small letters (in fact, will be used for other symbols)
                   || (incomingByte == ' ') // space
                   || (incomingByte == '-') // minus sign
                   || (incomingByte == '.') // decimal point
           ) {
            receivedStringData[indexStringData] = incomingByte;
            indexStringData++;
        }

        // (b) a NUMBER to convert to float and to add to the auxiliary data buffer:
        else if (incomingByte == '#') { // this means that we received a whole number to convert to float (but we don't know yet if it's for
            // the modelview, the projection matrix or some other data):
            receivedStringData[indexStringData] = 0;
            indexStringData = 0;
            // Serial.println(receivedStringData); // for tests
            // convert to float and store in auxiliary "matrix" array:
            auxDataBuffer[auxDataBuffer_index] = atof(receivedStringData);
            //Serial.println( auxDataBuffer[auxDataBuffer_index]); // for tests
            auxDataBuffer_index++;
        }

        // Enable/disable projection:
        else if (incomingByte == '}') {
            stopDisplay();
        } else if (incomingByte == '{') {
            resumeDisplay();
        }

        // Show maximum excursion of mirrors:
        else if (incomingByte == '*') {
            showLimitsMirrors(50, 1);
        }

        else if (incomingByte=='_') {
            clearScene();
        }

        else if (incomingByte == '+') {
            receivedStringData[indexStringData] = 0;
            indexStringData = 0;
            switch(atoi(receivedStringData)) {
                case 0:
                    currentSensingMode=NO_SENSING;
                    break;
                case 1:
                    currentSensingMode=SENSING_WHOLE_SCENE;
                    break;
                case 2:
                    currentSensingMode=SENSING_PER_OBJECT;
                    break;
                default:
                    break;
            }
        }

        // Performs a scan of the surface and send all the data serially (N>, with N the resolution of the scan)
        else if (incomingByte == '>') {
            receivedStringData[indexStringData] = 0;
            indexStringData = 0;
            //pc.printf("scan command issued¥n");
            scanSerial(atoi(receivedStringData));
        }

        // Recompute the LUT table
        else if (incomingByte == '<') {
            recomputeLookUpTable();
        }

        // Change state machine color (objects created from this point on will have the state machine color)
        else if (incomingByte == '(') { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
            receivedStringData[indexStringData] = 0;
            indexStringData = 0;
            // convert to integer and set the state machine color:
            lsr.setColor(atoi(receivedStringData));
        }

        // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
        else if (incomingByte == ')') { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
            receivedStringData[indexStringData] = 0;
            indexStringData = 0;
            // convert to integer and set the color:
            changeColorScene(atoi(receivedStringData));
        }


        // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
        // been translated previously - this may be optional, we could first send the pose, then the points...)
        // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
        // setting Z to 1, or by setting the projection matrix to a unit diagonal but the last term in the diagonal is set to 0.
        // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
        // NOTE: the following string of ASCII characters is assumed to have been input:
        //               posX#posY#sizeX#sizeY#nx#ny#:
        // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
        // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
        else if (incomingByte == ':') {
            // Data is now ordered in the auxDataBuffer array:
            float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
            auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
            clearScene();
            // set color:
            lsr.setColor(0x02); // green
            //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera

            // set local pose matrix to identity:
            lsr.setIdentityPose();
            // translate to the required position:
            lsr.translate(posX, posY, 0); // since the projection  and pose are going to be set
            // to Identity (projection identity will automatically make the projection orthographic), this means that these
            // values are in "laser projector pixels" (in fact angles).
            // Create the collection of dot objects:
            // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
            grid(sizeX, sizeY, nx, ny, 1);
            updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
            // rendering is done in the drawScene() function.

            // Now we can render the scene once:
            // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
            lsr.setOrthoProjection();
            lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
            drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
        }
        // NOTE: now that the grid is set, we don't need to do it again when changing the pose: JUST SEND THE POSE MATRIX and things will be re-rendered!

        // This means we received "2D" vertex data (z assumed 0) to create a single scene out of these points (we will create a single object)
        // using orthographic projection.
        // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
        // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
        // NOTE: the POSE is reset to ID, but we could use the current pose (i.e., send it first by the computer). But for the time being, and in order to modify the
        // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
        else if (incomingByte == '@') { // ID of the object
            command1="objectID";
            interpretCommand();
       }
       
       else if (incomingByte == '[') {
        command1 =="deleteObject";
        interpretCommand();
       }
       
       else if (incomingByte == '}') {
        command1 =="moveObject";
       interpretCommand();; // xx, yy contains already the necessary displacement
    }

        
       else if (incomingByte == '~') { // this is for setting the POSITION of the object to be created:
            command1="initialPosition";
            interpretCommand();
       }
        
        
        else if (incomingByte == '=') { // Create 2d object:
            command1 = "createObject2D"; 
            interpretCommand();
        }

        // This means we received 3D vertex data to create a single scene out of these points (we will create a single object).
        // It will be rendered in the CURRENT pose.
        else if (incomingByte == ';') {
            // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
            // The present value of auxDataBuffer_index contains the number of points x 3
            lsr.pushPoseMatrix(); // we will render on the current pose...
            clearScene();
            lsr.setIdentityPose();
            //lsr.setColor(0x02);
            begin(1);
            for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
                vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
                       auxDataBuffer[3 * i + 2]);
            }
            end();
            updateScene();
            // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
            // properly called lsr.pushPoseMatrix())
            //lsr.setOrthoProjection();
            //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
            lsr.popPoseMatrix();
            drawScene();
            auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
        }


        /*
         else if (incomingByte=='!') { // sets the width of the letter
         receivedStringData[indexStringData]=0;
         indexStringData=0;
         fontWidth=atof(receivedStringData);
         createTextScene(); // needed because we changed the geometry
         }

         else if (incomingByte=='"') { // sets the height of the letter
         receivedStringData[indexStringData]=0;
         indexStringData=0;
         fontHeight=atof(receivedStringData);
         createTextScene(); // needed because we changed the geometry
         }
         */

        // (c) TERMINATOR indicating the data was text to display:
        else if (incomingByte == '"') { // this means that the previous data was TEXT to display
            command1="createText";
            interpretCommand();
            }

        // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
        else if (incomingByte == '$') { // when receiving this character, it means that the WHOLE matrix data (4x4 values) have been sent (with '#' separator), in row/column format
            auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
            // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
            lsr.setIdentityPose();               // RT=ID
            lsr.multPoseMatrix(lsr.EXTRINSICS);  // RT=E // we can simply do: lsr.setExtrinsicsPose()
            lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates

            drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry, just project)

            // Handshake:
            pc.putc(13);
        }

        // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
        else if (incomingByte == '%') { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
            auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
            // store in projection matrix:
            lsr.loadProjMatrix(auxDataBuffer, 1.0);

            drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
        }

        //(e) TERMINATOR indicating the data was for the extrinsic matrix:
        else if (incomingByte == '&') { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
            auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
            // store in projection matrix:
            lsr.loadExtrinsicsMatrix(auxDataBuffer);

            drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
        }

    }
}