File content as of revision 3:b44ff6de81bd:

#include "simpleLaserRenderer.h"

void simpleLaserSensingRenderer::setConfigToRender(blobConfig* ptBlobCf) {
    //Note: when setting the config to render, we assume the number of blobs is fixed, and the number of points per blob is also fixed.
    ptBlobCfToRender=ptBlobCf;

    totalBlobs=ptBlobCfToRender->numBlobs; // equal in fact to blobArray.size()

    // initialization of current indexes:
    currentBlob=0;
    currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->delayMirrorSamples; // per blob delay!
    currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
    currentColor=ptBlobCfToRender->blobArray[currentBlob]->blobColor;

    // Blanking:
    //IO.setRGBPower(0);
    // or start with the correct color (case of unique blob)
    // IO.setRGBPower(currentColor|0x04); // Note: RED always on better? (to avoid loosing LockIn signal)
    // OR RED ONLY:
    IO.setRGBPower(0x04);

    // Set mirrors to the start of FIRST blob:
    x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].x;
    y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].y;
    IO.writeOutX(x);
    IO.writeOutY(y);

    // ATTENTION !!!!!!!!!!!!!!!!!!
    // Reset current point to special "start value":
    currentPoint=-1;
    // or start on first point (case of unique blob rendering function):
//     currentPoint=0;

    pauseCounter=20; 

}

void simpleLaserSensingRenderer::laserRenderThread() {

    if (currentPoint==-1) { // this means that the mirrors are (supposedly) well positionned in the start of the new blob.
        // Restart the laser with the new current color:
        IO.setRGBPower(currentColor|0x04); // Note: RED always on...

        // First of all, read the light at currentPoint=0
       currentPoint=0;
    int delayedPoint=currentMirrorDelay%currentTotalPoints;//just equal to (currentPoint+currentMirrorDelay)%currentTotalPoints;

#ifdef debugDelayMirrors
        if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) {
            IO.setBluePower(0);
            // myled3=0;
        } else {
            IO.setBluePower(1);
            // myled3=1;
        }
        //IO.setRGBPower(0x04); else  IO.setRGBPower(0x07);
#endif


        // Use direct reading:
        // ptBlobCfToRender->blobArray[currentBlob].displaySensingBuffer.lsdTrajectory[currentMirrorDelay%currentTotalPoints]=lockin.getMedianValue();//lockin.getSmoothValue(); //lockin.getLastValue();//
        // Or corrected by lookup table (NOTE: x and y are supposed to be positioned at the currenPoint=0 already)
        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].intensity=IO.lockInCorrectedValue(x,y);

        // Then, set current point to the SECOND saccade point and position the mirrors (the first was already drawn and measured...):
        // currentPoint=1; // already done
        x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].x;
        y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].y;
        IO.writeOutX(x);
        IO.writeOutY(y);

        // Re-schedule interruption:
        // Timeout
    } else if (currentPoint==currentTotalPoints-1) { // this means that we ended rendering the current blob
        // Start processing next blob:
        currentBlob=(currentBlob+1)%totalBlobs;

        currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->delayMirrorSamples; // per blob delay!
        currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
        currentColor=ptBlobCfToRender->blobArray[currentBlob]->blobColor;

        // Blanking:
        // IO.setRGBPower(0);
        // or start with the correct color (case of unique blob)
        // IO.setRGBPower(currentColor|0x04); // Note: RED always on better? (to avoid loosing LockIn signal)
        // OR RED ONLY:
        IO.setRGBPower(0x04);

        // Set mirrors to the start of next blob:
        x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].x;
        y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].y;
        IO.writeOutX(x);
        IO.writeOutY(y);

        // Reset current point to special "start value":
        currentPoint=-1;

    } else { // proceed with the laser rendering/sensing (note: the mirrors are positioned already on the next point to be measured)

        // (1) SENSING (on the current blob and particle index):
        // Either using direct value:
        // ptBlobCfToRender->blobArray[currentBlob].sensingBuffer[(currentPoint+currentMirrorDelay)%currentTotalPoints]=lockin.getMedianValue(); //lockin.getLastValue();//
        // Or corrected by lookup table (NOTE: x and y are supposed to be positioned at the currenPoint already)

        int delayedPoint=(currentPoint+currentMirrorDelay)%currentTotalPoints;


#ifdef debugDelayMirrors
        if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) {
            IO.setBluePower(0);
            // myled3=0;
        } else {
            IO.setBluePower(1);
            // myled3=1;
        }
        //IO.setRGBPower(0x04); else  IO.setRGBPower(0x07);
#endif

        // Make the current measurement:
        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].intensity=IO.lockInCorrectedValue(x,y);


        // (2) increment the current point index and position the mirrors:
        currentPoint++;
        x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].x;
        y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].y;
        IO.writeOutX(x);
        IO.writeOutY(y);
    }

}

void simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY() {
    // When we arrive here, we ASSUME the mirrors are well positioned at the currentPoint-1, so we need to process the currentPoint:

    // Current mirror position:
    x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].x;
    y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].y;

    // (2) Send command to position the mirrors to the next position:
    IO.writeOutX(x);
    IO.writeOutY(y);

    int delayedPoint=(currentPoint+currentMirrorDelay)%currentTotalPoints;

#ifdef debugDelayMirrors
    if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) {
        IO.setBluePower(0);
        // myled3=0;
    } else {
        IO.setBluePower(1);
        // myled3=1;
    }
    //IO.setRGBPower(0x04); else  IO.setRGBPower(0x07);
#endif

    // (1) SENSING (on the current blob and particle index with mirror delay: )
    ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].intensity=IO.lockInCorrectedValue(x,y);
    //=lockin.getMedianValue(); //lockin.getLastValue();//

    // increment the current point index:
    currentPoint=(currentPoint+1)%currentTotalPoints;

}

/*
// Non threaded rendering (will produce a hot spot):
  void simpleLaserSensingRenderer::laserRender(blobConfig* ptBlobCf) {

   for (int i=0; i<ptBlobCf->numBlobs; i++) {
   if (ptBlobCf->blobArray[i].alive) {  // render only active blobs
    int blobMirrorDelay=ptBlobCf->blobArray[i].delayMirrorSamples; // per blob delay
    int blobPoints=ptBlobCf->blobArray[i].numMasses;
    for (int j=0; j<blobPoints; j++) {
        // Note: Inter point pause in the same loop is not needed: it's a thread, so we first do the sensing, THEN move the mirrors in the thread.
        // wait_us(10);

        // (1) SENSING (on the current blob and particle index: )
        ptBlobCf->blobArray[i].sensingBuffer[(j+blobMirrorDelay)%blobPoints]=lockin.getSmoothValue(); //lockin.getLastValue();//

        // (2) Send command to position the mirrors to the next position:
         IO.writeOutX(int( ptBlobCf->blobArray[i].massesLoop[j].pos.x));
         IO.writeOutY(int( ptBlobCf->blobArray[i].massesLoop[j].pos.y));

    }
    // INTER BLOB PAUSE (could depend on the distance between them):
    // wait_us(20); // question: does "wait_us" stop the micro even in user context? use something else if it does!!!
    }
   }
}
*/