File content as of revision 1:3be7b7d050f4:

#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()
    totalBlobs=ptBlobCfToRender->blobArray.size(); // equal in fact to blobArray.size()

    // NOTE: the wait times (normal, start and end point) can be BLOB dependent. This may be a nice future (TO DO?).

// SET STATE MACHINE TO INITIAL VALUES:
    waitFirst=0;
    waitFirstLaser=0;
    waitNormal=0;
    waitLaser=0;
    waitLast=0;
    currentBlob=-1; // this is only for the very first time we initialize the state machine (we could have another initial state, but this would be inefficient)
    stateLsd=MOVE_NEXT_BLOB;

    /*
     // For tests: case of unique blob:
      currentBlob=0;// in case of unique blob (for tests)
    // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->delayMirrorSamples; // per blob delay!
     currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
     currentColor=tBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
     IO.setRGBPower(currentColor|0x04); // Note: RED always on...
     */

    // overlap display to avoid deformed saccade and give time to the mirrors to be well in the saccade trajectory
    // NOTE: ideally, numOverlapPoints depends on the number of points of EACH blob, as well as the distance between the spots. 
    //       But for the time being, this will be a fixed quantity (DEFAULT_OVERLAP_POINTS).
    if (totalBlobs>1) numOverlapPoints=DEFAULT_OVERLAP_POINTS; 
    else numOverlapPoints=0;

    configTotalPoints=0;
    for (int i=0; i<totalBlobs ; i++) {
        configTotalPoints+=ptBlobCfToRender->blobArray[i]->displaySensingBuffer.lsdTrajectory.size();
    }
    // configTotalPoints contains the number of points of the config, and will be used to ensure that a FULL DISPLAY has been done BEFORE updating and "re-drawing" the trajectory in the buffer,
    // wherever the current point being displayed when we start the update/draw.
    pointDisplayCounter=0;
  
}

bool simpleLaserSensingRenderer::endedFullDisplay() {
    return(pointDisplayCounter>configTotalPoints);
}

bool simpleLaserSensingRenderer::endedFractionDisplay(int frac) {
    if (frac==0) return(true); 
    else  return(pointDisplayCounter>configTotalPoints/frac);
}

void simpleLaserSensingRenderer::startFullDisplay() {
    pointDisplayCounter=0;
}


void simpleLaserSensingRenderer::laserRenderThread() {

    switch (stateLsd) {
        case NORMAL_POINT:
            if (currentPoint<currentTotalPoints+numOverlapPoints) { // Attention: use modulo currentTotalPoints when accessing trajectory index.
                if (waitNormal==0) { // Send mirrors position the first time (note: I don't put this inside the waitNormal<WAIT_NORMAL, because WAIT_NORMAL can be 0!
                
                    uint8_t currentPointWrap=currentPoint%currentTotalPoints; 
                    x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
                    y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;

                    IO.writeOutX(x);
                    IO.writeOutY(y);
                }
                if (waitNormal<WAIT_NORMAL)  {
                    waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
                } else {   // if we got here, it means the mirrors are well positionned: activate laser:
                    if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
#ifndef debugDelayMirrors
                        IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR). 
                        // BUT enable the blue activation if one wants (in particular elastic blobs...)
                        if (ptBlobCfToRender->blobArray[currentBlob]->blueTouch) {
                            uint8_t delayedPoint=currentPoint; 
                        if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
                            //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
                            // Note: better not use complicated calls?
                            IO.setBluePower(0);
                            IO.setGreenPower(currentColor&0x02);
                        } else {
                            //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
                            IO.setBluePower(1);
                            IO.setGreenPower(currentColor&0x02);
                        }
                            }
#else               // TEST MODE for delay using blue laser:
                    // NOTE: we can either CORRECT the delay (and see if the correction is good), or show the "raw" detection (in this case, we need to 
                    // compute delayedPoint, but exactly the reverse as the calculation made in the classLaserSensingTrajectory...
                        // (a) "raw":
                        //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
                        // (b) corrected delay:
                        uint8_t delayedPoint=currentPoint; 
                        if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
                            //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
                            // Note: better not use complicated calls?
                            IO.setBluePower(0);
                            IO.setGreenPower(currentColor&0x02);
                        } else {
                            //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
                            IO.setBluePower(1);
                            IO.setGreenPower(currentColor&0x02);
                        }
#endif
                    }
                    if (waitLaser<WAIT_LASER) {
                        waitLaser++; // increment wait laser counter
                    } else { // If we got here, it means that mirrors and laser power are both properly set:
                        // Read the intensity and move to the next point:

                        uint8_t currentPointWrap=currentPoint%currentTotalPoints;
                        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
                        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));

                        // Move to next point:
                        currentPoint++;

                        waitNormal=0;
                        waitLaser=0;

                        // Update the point display counter (meaning: this point has been properly acquired - we need (at least) configTotalPoints of those good acquisitions before updating and re-draw)
                        pointDisplayCounter++;
                    }
                }
            } else { // this means we ended rendering this blob, with or without partial duplication
                if (totalBlobs>1) stateLsd=LAST_POINT;
                else { // this means we are rendering a unique blob:
                    // currentBlob does not change (equal to 0 always), stateLsd is always NORMAL_POINT
                    // The only thing we need to do is to reset "currentPoint" to 0, and eventually change the color of the blob.
                    // NOTE that if only doing this, the point 0 will take two ISR cycles; therefore it is better to move the mirrors NOW and set the
                    // currentPoint to 1:
                    currentPoint=0; // and we copy the code in the NORMAL mode (this will increment currentPoint):
                    
                    if (waitNormal==0) { // Send mirrors position the first time (note: I don't put this inside the waitNormal<WAIT_NORMAL, because WAIT_NORMAL can be 0!
                
                    uint8_t currentPointWrap=currentPoint%currentTotalPoints; 
                    x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
                    y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;

                    IO.writeOutX(x);
                    IO.writeOutY(y);
                }
                if (waitNormal<WAIT_NORMAL)  {
                    waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
                } else {   // if we got here, it means the mirrors are well positionned: activate laser:
                    if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
#ifndef debugDelayMirrors
                        IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR). 
#else               // TEST MODE for delay using blue laser:
                    // NOTE: we can either CORRECT the delay (and see if the correction is good), or show the "raw" detection (in this case, we need to 
                    // compute delayedPoint, but exactly as the reverse of the calculation made in the classLaserSensingTrajectory...
                        // (a) "raw":
                        //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
                        // (b) corrected delay:
                        uint8_t delayedPoint=currentPoint; 
                        if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
                            //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
                            // Note: better not use complicated calls?
                            IO.setBluePower(0);
                            IO.setGreenPower(currentColor&0x02);
                        } else {
                            //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
                            IO.setBluePower(1);
                            IO.setGreenPower(currentColor&0x02);
                        }
#endif
                    }
                    if (waitLaser<WAIT_LASER) {
                        waitLaser++; // increment wait laser counter
                    } else { // If we got here, it means that mirrors and laser power are both properly set:
                        // Read the intensity and move to the next point:

                        uint8_t currentPointWrap=currentPoint%currentTotalPoints;
                        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
                        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));

                        // Move to next point:
                        currentPoint++;

                        waitNormal=0;
                        waitLaser=0;

                        // Update the point display counter (meaning: this point has been properly acquired - we need (at least) configTotalPoints of those good acquisitions before updating and re-draw)
                        pointDisplayCounter++;
                    }
                }
                    
                    currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
                }
            }
            break;
        case LAST_POINT:
            // We need to pause for a while (this is for avoiding a deformed end of a blob when there are more than one blob AND we did not properly correct the mirror delay - this may be because
            // we want a faster display, in which case we will need to adjust the mirrorDelay variable to something different from 0)
            if (waitLast<WAIT_LAST) waitLast++;
            else {
                // switch off laser (NOTE: there is no need to wait for switch off time)
#ifdef RED_BLANKING
               IO.setRGBPower(0x00);
#else
               IO.setRGBPower(0x04); // Note: RED always on, or really 0
#endif
                waitLast=0;
                stateLsd=MOVE_NEXT_BLOB;
            }
            break;
        case MOVE_NEXT_BLOB:
            // TO DO: line and counter to avoid overshoot?

            // Start processing next blob:
            currentBlob=(currentBlob+1)%totalBlobs;

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

            if (totalBlobs>1) stateLsd=START_POINT;
            else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)

            break;

        case START_POINT:
            if (waitFirst==0) {
                // Send command to position the mirrors on the first point of NEXT blob (laser is flying in between during this time... )
                x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].x;
                y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].y;
                IO.writeOutX(x);
                IO.writeOutY(y);
            }
            if (waitFirst<WAIT_FIRST) waitFirst++; // time for positioning of mirrors on next blob.
            else { //mirrors are positioned: activate laser and lock in (needs time):
                if (waitFirstLaser==0) {
                    // activate laser - important in particular for giving time to the Lock-in to catch signal, then laser rouge:
                    IO.setRGBPower(currentColor|0x04); // Note: RED always on...
                }
                if (waitFirstLaser<WAIT_FIRST_LASER) waitFirstLaser++;
                else  {
                    waitFirst=0;
                    waitFirstLaser=0;
                    stateLsd=NORMAL_POINT; // start normal point
                }
            }
            break;
    }
}

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[currentPoint].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[currentPoint].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
    //=lockin.getMedianValue(); //lockin.getLastValue();//

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

}