File content as of revision 42:5f21a710ebc5:

/*

- NOTE: I could instantiate at least 960 points! (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?
*/

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

// 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 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


// 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();
float angle;
string textToDisplay = "HELLO";
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();

void processOSC(UDPSocketEvent e);
void interpretData();
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 interpretData 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;


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

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

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

    // 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);

    //while(true) {
//     // COMMUNICATION:
//        // (a) Reading commands:
//        // Ethernet:
//        Net::poll(); // this will take care of calling processOSC(UDPSocketEvent e) when a new packet arrives.
//
//         wait(.5);
//         sendMes.setTopAddress("osc");
//         sendMes.setSubAddress("working");
//         osc.sendOsc( &sendMes );
//    }

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

    /*
        //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(); // Create a default scene (for tests)

    // or start with nothing:
    //clearScene();
    //setOrthographicView();

    // (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

    // *** 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();

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

        // =========== Query hardware events (knobs, ethernet, serial port) ============
        Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC!
        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)>80)) { // 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<textToDisplay.length(); 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(i+48);

                        } 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);
    }

    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.

    lsr.popPoseMatrix();

}

void addText()
{
    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);
    }

    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.

    lsr.popPoseMatrix();

}

//==================  OSC messageReceivedCallback function =====================
//============= 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 interpretData function (common to serial and OSC):
        strcpy(address[0],"");
        strcpy(address[1],"");
        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:
        interpretData();

    }
}



//interpretData(const char& address[2][], const int& data[2]) {
void interpretData()
{

    // ==========  DATA (all this is a hack because my OSC class does not accept BUNDLES...) =============
    // Add data to auxiliary data buffer:
    if (!strcmp(address[0], "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 ( !strcmp(address[0], "mbedReset" ) ) mbed_reset();

    else if ( !strcmp(address[0], "testPower" ) ) {
        // TO DO
    }

    // Enable/disable projection:
    else if ( !strcmp(address[0], "stopDisplay" )) stopDisplay();
    else if (!strcmp(address[0], "resumeDisplay"))  resumeDisplay();
    else if (!strcmp(address[0], "showLimits")) {
        int value=data[0]; // argument is the number of seconds for display
        if (value!=-1) {
            showLimitsMirrors(50, value);
        }
    } else if (!strcmp(address[0], "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 (!strcmp(address[0], "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 (!strcmp(address[0], "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 (!strcmp(address[0], "clearScene")) {
        clearScene();
    }

    else if (!strcmp(address[0], "deleteObject")) { // second address is object identifier (a string that will be converted
        int idobject=atoi(address[1]);
        deleteObject(idobject);
    }

    // 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 (!strcmp(address[0], "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 (!strcmp(address[0], "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 (!strcmp(address[0], "createObject2D")) {
        unsigned char numPoints= 11;//auxDataBuffer_index/2;
        //auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port

       // pc.printf("creating 2d object with %d points\n", numPoints);

        int IDObject=atoi(address[1]); // the object identifier is on the second address
        // the initial position is on data[0-1]
        // pc.printf("Object ID is %d \n", IDObject);
        //pc.printf("Position at: %4.2f, %4.2f \n", (float)data[0], (float)data[1]);
        // pc.printf("Position at: %d, %d \n", (int)data[0], (int)data[1]);

        if (IDObject!=-1) {

            if(true) {

                clearScene();

                // First, create the object:
                lsr.pushPoseMatrix();
                lsr.setIdentityPose();

                // lsr.translate(-15,-10,0);

                lsr.translate(20,20,0);

                // lsr.translate(5,10,0);
                // lsr.translate((int)data[0], (int)data[1], 0);

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

                //begin(IDObject);
                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);
                    end();
                    //pc.printf("local (x,y): %f, %f \n", auxDataBuffer[2 * i],  auxDataBuffer[2 * i+1]);
                    // lsr.translate(1.1*fontWidth, 0, 0);
                }
                lsr.popPoseMatrix();

                // update the scene for display
                updateScene();

                //createTextScene();

                // 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:
                drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
            }

        }
    }

    // This means we received 3D vertex data to create an object.
    // It will be rendered in the CURRENT pose.
    else if (!strcmp(address[0], "createObject3D")) {
        int IDObject=data[1];
        if (IDObject!=-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(IDObject);
            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
        }
    }

    // Create text:
    else if (!strcmp(address[0], "createTextObject")) {


        // //NOTE: the actual text will be in the second address, and the argument will be the ID of this new text object:
        textToDisplay = address[1];
        int IDObject=data[0];
        if (IDObject!=-1) {


            //createTextScene();
//                         // 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


          //  clearScene();
          stopDisplay();

            // 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(-15,-10,0);

            //begin(IDObject);
//            string3d(textToDisplay, fontWidth*textToDisplay.length(), fontHeight);
//            end();

            //begin(IDObject);
            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();

            updateScene();

            // 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
        }
    }


    // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
    else if (!strcmp(address[0], "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 (!strcmp(address[0], "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 (!strcmp(address[0], "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.
    }
}

//============= RECEIVE SERIAL COMMANDS =========================
// 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)

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++;
        }

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

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


        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 == '=') {
            // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y)
            // 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 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
        }

        // (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++;
        }

        /*
         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
            receivedStringData[indexStringData] = 0; // termination for the string
            indexStringData = 0;
            // Serial.println(receivedStringData);
            textToDisplay = string(receivedStringData);
           // stopDisplay();
           // addText();
           createTextScene();
            drawScene(); // needed because we changed the geometry, and we need to compute the projection once for display (of course, this will
            // happens anyway when the computer send the pose matrix).
            //  resumeDisplay();
        }

        // (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.
        }

    }
}