File content as of revision 21:bc9b9383f4b6:

#include "rigidLoop.h"

// SHOULD NOT BE HERE: (only because I am using AD_MIRRIOR... max and min in the set region function that should not be here)
#include "hardwareIO.h"

rigidLoop::rigidLoop() {
}

rigidLoop::~rigidLoop() {

}


// Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type).
void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2Df _initPos, vector2Df _initSpeed) {
    // (1) set ID:
    identifier=_id;

    updateMode=_elasticBlobMode;
    
    startCenter=_initPos;
    startSpeed=_initSpeed;
    
    // (2) Initialize common variables of all blobs (base class):
    initCommonVariables();

    // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...)
    // Sending data:
    periodSendingData=10; // in ms
    sendingRecenteringAngle=true;
    sendingAnchorPosition=true;
    sendingOnlyWhenTouch=true;
    
    // Gravity field:
    gravity.set(.5,0);

// (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)

    switch (updateMode) {
        
        case SPOT_TEST:
            // Name of this kind of spot:
            sprintf(spotName,"spot_test");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);

            saccadeRadius=250;

            // default (initial) shape (the scafold belongs to the base class):
            // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
            massCenter=0.01;
            dampMotionCenterMass=0.001;
            
            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES

            break;

        case SPOT_FOLLOWING:

            // Name of this kind of spot:
            sprintf(spotName,"rigid_following");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);

            // default (initial) shape (the scafold belongs to the base class):
            saccadeRadius=25;
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
            massCenter=0.01;
            dampMotionCenterMass=0.001;

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            slidingDirection=true; //  For contour following (will change direction when touching wall)
            speedContourFollowing=1.3*saccadeRadius;
            justSearched=false;

            // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
            // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
            // BUT because we may want to see the blue laser where there is dark zone, then we would try to adjust mirror delay as close as possible to the 
            // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
            displaySensingBuffer.setDelayMirrors(3); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
            angleCorrectionForceLoop= -5;// good for ONE spot: -5;// in DEGREES

            break;

        case  SPOT_BOUNCING:
            // Name of this kind of spot:
            sprintf(spotName,"rigid_bouncing");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);

            saccadeRadius=85;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.0012;
            dampMotionCenterMass=0.0005;//0.00015;//00003;
            factorBouncingForce=0.0018; // this is because we will use a force on the central mass

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(3);
            angleCorrectionForceLoop=-5;// in degrees

            break;
    }
    
    saccadeRadius_initial=saccadeRadius; // this is for search mode for instance. 
    
    // Excursion limits (for all points). Tthis will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius:
    setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);


}


void rigidLoop::initSizeBlob(int _numPoints) {
    // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
    numPoints=_numPoints;

    // Sensing and Display trajectory:
    displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?).
}

void rigidLoop::createLoopFromScafold(void)  {

    initSizeBlob(bluePrint.scafold.size());  // very simple here (only need to set the size of the lsd buffer)

    centerMass.mass=massCenter;
    centerMass.dampMotion = dampMotionCenterMass;

    // note: the following may not be required in case of contour following:
    centerMass.setIntegrationStep(0.23); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
    centerMass.setInitialCondition(startCenter, startSpeed);
    // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);
}

void rigidLoop::setRegionMotion(float mmix, float mmiy, float mmax, float mmay) { // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass
   // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
   // Use the static method of the pointMass class: 
   pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
}


void rigidLoop::update() {

    // (I) process loop geometry: not needed (rigid)
    // Just check if the blob touched the borders (only need to do this with the central mass):
    blobWallCollision=centerMass.bWallCollision;

    // (II) Process sensing buffer and compute light forces:
    displaySensingBuffer.processSensedData(); // note: region with light is -1, and without is 2 (TO CHANGE!!! then we don't need to do "if" in the moment computation, but just a product)

    // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
    // ATTENTION!! for this simple method (of "first order moment") to work, we have either to have numPoints very large, OR an EVEN quantity - so the
    // sum in the circle is 0).
    vector2Df momentVector(0,0);
    int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!!
    for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
        if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
            
             momentVector.x+=(float)bluePrint.scafold[i].x; // note: casting is happening here automatically (unsigned short to float), but I put (float) to remember that types are different
             momentVector.y+=(float)bluePrint.scafold[i].y;
            
            // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd... 
            // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
            
            counterDarkZone++;
        }
    }
    momentVector=momentVector*(2*PI/numPoints);
    float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
    
    vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
    if (momentNorm==0) {
        unitTowardsLight.set(0,0);
        recenteringVectorLoop.set(0,0);
        normRecenteringVector=0;
        angleRecenteringVector=0;
    } else {
         unitTowardsLight=momentVector/(-1.0*momentNorm);
         // Apply correction angle (delay mirrors):
        unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
         
         // Compute "recenteringVectorLoop" (in fact, the vector making the spot goes completely away form the dark zone):
         float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
         if (aux>1) aux=1.0; // can happen because of the discrete integration!
         if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
            recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
            } else {
             recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
             }
    
      
        // Compute redundant quantities (if necessary, for sending through OSC, etc):
        normRecenteringVector=recenteringVectorLoop.length();
        angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
    }

    // Now, depending on the mode of operation, we have two types of behaviour:
    vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
    switch (updateMode) {
        
        case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc: 
        // do nothing for the time being 
        // NOTE: it is not so easy to show the recentering vector without affecting the mirror delay BECAUSE I AM USING THE INTERRUPT METHOD for display.
        // A possible solution is to instantiate ANOTHER blob with a shape just equal to a line, and rotate it using data from the this blob. Make a new class? Seems a good idea. 
        
        break;

        case SPOT_FOLLOWING:
            // we need to compute the tangencial "speed":
            // vector2D slidingVector;
            if (momentNorm>0) {
                //momentVector/=momentNorm;
                // We can now compute the sliding vector as:
                slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
                
                // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing)
                // This is used to update the position of the central mass - WITHOUT INTEGRATION (or with it, but for the time being, we don't do that):
      
                centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
               // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the 
               // re-entry and avoid oscillations). 

                // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
                centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
                
                centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
                
                if (justSearched) {
                    saccadeRadius=saccadeRadius_initial;
                    bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                    justSearched=false;
                    }
                   

            } else {
                // not on something. SEARCH MODE (or go to spot_bouncing mode?)
                saccadeRadius+=30; if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
                bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                justSearched=true;
            }

            break;

        case  SPOT_BOUNCING:
            // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
            centerMass.resetForce();

        if (displaySensingBuffer.lightTouched) {
            // add force; MANY POSSIBILITIES:
            // (1) Constant in norm:
            //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
            // Exactly what is needed to have an elastic bouncing:
            
            // Proportional to the penetration depth in the dark zone (spring):
             centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
             // Or proportional to the square (or something else) of the penetration:
             //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
             
            }
            
            // Gravity?
            centerMass.addForce(gravity*centerMass.mass);

            // update dynamics for the central mass::
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
            
             if (displaySensingBuffer.lightTouched) {
            // do collision damping:
               centerMass.setSpeed(centerMass.getSpeed()*0.99);
             }


            break;

    }

    // OTHER PARTICULAR THINGS:
    if (displaySensingBuffer.lightTouched)  {
          // setGreenPower(1); // setColor(0x02); // green on
           setColor(0x02|blobColor); // set green ON, regardless of the initial color
    } else {
          // setGreenPower(0); //setColor(0x00); // green off
           setColor(blobColor); // set green OFF (IF it was off from the start)
     }
    
    
    // change sliding direction (for countour following):
    if (blobWallCollision) {
        if (wallCounter>5) {
            slidingDirection=!slidingDirection;
            wallCounter=0;
        }
    }
    wallCounter++;
}


// Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser
void rigidLoop::draw() {
    // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
    float cx= centerMass.pos.x;
    float cy= centerMass.pos.y;
    for (int i = 0; i < numPoints; i++) {
        // The shape is drawn by translating the scafold shape (centered on centerMass):
        displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
        displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );
        
        // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
        // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;
        
        displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
    }
    // global color for the whole loop:
    displaySensingBuffer.displayColor=blobColor;
}

void rigidLoop::computeBoundingBox() {
}



void rigidLoop::sendDataSpecific() {
    char auxstring[10];
    myled2=1; // for tests...

    // First, set the top address of the message to the ID of the blob (not the name):
 //   sprintf(auxstring, "%d", identifier);
 //   sendMes.setTopAddress("0");//auxstring);

    // =====================   OSC  ======================
    if (sendOSC) {

        // (a) Anchor mass:
        if (sendingAnchorPosition) {
           sprintf(auxstring, "/p %d",identifier);
            sendMes.setSubAddress(auxstring);
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(centerMass.pos.x);
            y=(long)(centerMass.pos.y);
            sendMes.setArgs( "ii", &x, &y);
            osc.sendOsc( &sendMes );
        }

        // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
        if (sendingLoopPositions) {
#ifdef SEND_AS_POINTS
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            float cx= centerMass.pos.x;
            float cy= centerMass.pos.y;
            for (int i = 0; i < numPoints; i++) {
                sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
                sendMes.setSubAddress(auxstring);  // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...)
                x=(long)(bluePrint.scafold[i].x + cx);
                y=(long)(bluePrint.scafold[i].y + cy);
                sendMes.setArgs( "ii", &x, &y);
                osc.sendOsc( &sendMes );
            }

#endif
#ifdef SEND_AS_BLOB
            sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
            uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
            float cx= centerMass.pos.x;
            float cy= centerMass.pos.y;
            for (int i = 0; i < numPoints; i++ ) {
                // note: massesLoop[i].pos.x is a "float"
                uint16_t x=(uint16_t)(bluePrint.scafold[i].x +  cx);
                blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+1]=(uint8_t)x;

                uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
                blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+3]=(uint8_t)y;
            }
            osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
#endif
#ifdef SEND_AS_STRING
            sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
            uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
            float cx= centerMass.pos.x;
            float cy= centerMass.pos.y;
            for (int i = 0; i < numPoints; i++ ) {
                // note: massesLoop[i].pos.x is a "float"
                uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
                blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+1]=(uint8_t)x;

                uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
                blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+3]=(uint8_t)y;
            }
            osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
#endif
        }
        if (sendingLoopRegions) {
            long    x;   //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            for (int i = 0; i < numPoints; i++) {
                sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
                sendMes.setSubAddress(auxstring);  // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...)
                x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
                sendMes.setArgs( "i", &x);
                osc.sendOsc( &sendMes );
            }
        }
        if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
            long    wall;   //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            sprintf(auxstring, "/bWall");
            sendMes.setSubAddress(auxstring);
            wall=(long)(blobWallCollision? 1 : 0);
            sendMes.setArgs( "i", &wall);
            osc.sendOsc( &sendMes );
        }

        // (d) Light sensing statistics:
        if (sendingBlobMaxMin) {
            sendMes.setSubAddress("/maxmin");
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(displaySensingBuffer.maxI);
            y=(long)(displaySensingBuffer.minI);
            sendMes.setArgs( "ii", &x, &y);
            osc.sendOsc( &sendMes );
        }

        // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
        if (sendingRecenteringVector) {
            sendMes.setSubAddress("/rvector");
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(recenteringVectorLoop.x);
            y=(long)(recenteringVectorLoop.y);
            sendMes.setArgs( "ii", &x, &y);
            osc.sendOsc( &sendMes );
        }
        if (sendingRecenteringAngle) {
            sprintf(auxstring, "/v %d",identifier);
            sendMes.setSubAddress(auxstring);
            long    x;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(angleRecenteringVector);
            sendMes.setArgs( "i", &x);
            osc.sendOsc( &sendMes );
        }
        if (sendingRecenteringNorm) {
            sendMes.setSubAddress("/rnorm");
            long    x;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(normRecenteringVector);
            sendMes.setArgs( "i", &x);
            osc.sendOsc( &sendMes );
        }

        if (sendingTouched) {
            if (displaySensingBuffer.lightTouched) {
                sendMes.clearArgs(); // there are no arguments to send
                sendMes.setSubAddress("/touched");
                osc.sendOsc( &sendMes );
            }
        }

    } // end of OSC sending per-spot

    // =====================   SERIAL  ======================
    if (sendSerial) {
        //.. to do
    }

    myled2=0; // for tests...
}