//Begin page main
//Begin page main
float cornerA[3], cornerB[3];
float cornerC[3], cornerD[3];
float radstop;
int counter;
float dist;
ZRState pos;
float attitudeA[3];
float attitudeB[3];
float attitudeC[3];
float attitudeD[3];
float bilanciamento[3];
float distP[4];      //--m
float dist_debug;    //--m
float dist_squared;  //--m
float coeff_bilan;
int conta;

void init()
{
//mettere le coordinate dei punti
	cornerA[0] = -0.5;
	cornerA[1] = 0.0;
	cornerA[2] = -0.25;
	cornerB[0] = 0.5;
	cornerB[1] = 0.0;
	cornerB[2] = -0.5;
	cornerC[0] = -0.5;
	cornerC[1] = 0.0;
	cornerC[2] = -0.25;
	cornerD[0] = 0.0;
	cornerD[1] = 0.0;
	cornerD[2] = 0.0;
	float attitudeA1[3]={0,0,1};
	float attitudeB1[3]={0,0,-1};
	float attitudeC1[3]={0,0,0};
	float attitudeD1[3]={0,0,0.5};
	memcpy(attitudeA, attitudeA1, 3*sizeof(float));	memcpy(attitudeB, attitudeB1, 3*sizeof(float));
 	memcpy(attitudeC, attitudeC1, 3*sizeof(float));
 	memcpy(attitudeD, attitudeD1, 3*sizeof(float)); 

 	coeff_bilan = -2.5;
	dist_debug  =0.025;      //--m
	dist_squared=0.000225;   //--m  (0.015m)
	
	counter	=	0;
    conta = 0;
	
}

void loop()   //ci devo fare qualche cosa
{
    api.getMyZRState(pos);

    calcolaDistanze();      //--m

	if(distP[0] < dist_debug || distP[1] < dist_debug 
	|| distP[2] < dist_debug || distP[3] < dist_debug){
	        DEBUG(("dist= %5.3f %5.3f %5.3f %5.3f coord: %5.3f %5.3f %5.3f ang: %5.3f %5.3f %5.3f\n", 
	        distP[0], distP[1], distP[2], distP[3], 
	        pos[0], pos[1],  pos[2],
	        pos[9], pos[10], pos[11]));     //--m
	}
    
    if(counter==0){
        dist=distP[0];                      //--m
        bilanciamento[0]= -2 * pos[9];
        bilanciamento[1]= -2 * pos[10];
        bilanciamento[2]= attitudeA[2];
        if(isCloseTo(cornerA)){
            counter++;
            conta=0;
            DEBUG(("Punto A: x=%5.3f y=%5.3f z=%f wx=%5.3f wy=%5.3f wz=%5.3f d=%5.3f END",
            pos[0], pos[1], pos[2], pos[9], pos[10], pos[11], dist));
        
        }else{
            api.setPositionTarget(cornerA);
            api.setAttRateTarget(bilanciamento);
        }
    }
    
    if(counter==1){
        dist=distP[1];                      //--m
        if (conta < 3){
            conta++;
            bilanciamento[0]= coeff_bilan * pos[9];
            bilanciamento[1]= coeff_bilan * pos[10];
            bilanciamento[2]= attitudeA[2]; //--ancora Att A
            DEBUG(("\nconta++"));
        }
        else {
        	bilanciamento[0]= coeff_bilan * pos[9];
        	bilanciamento[1]= coeff_bilan * pos[10];
        	bilanciamento[2]= attitudeB[2]; //---- poi  Att B
        }
        if(isCloseTo(cornerB)){
            counter++;
            conta=0;
            DEBUG(("Punto B: x=%5.3f y=%5.3f z=%f wx=%5.3f wy=%5.3f wz=%5.3f d=%5.3f END",
            pos[0], pos[1], pos[2], pos[9], pos[10], pos[11], dist));
        }
        else{
            api.setPositionTarget(cornerB);
            api.setAttRateTarget(bilanciamento);
        }
    }
    
    if(counter==2){
        dist=distP[2];                      //--m
        if (conta < 3){
            conta++;
            bilanciamento[0]= coeff_bilan * pos[9];
            bilanciamento[1]= coeff_bilan * pos[10];
            bilanciamento[2]= attitudeB[2]; //--ancora Att B
            DEBUG(("\nconta++"));
        }
        else {
        	bilanciamento[0]= coeff_bilan * pos[9];
        	bilanciamento[1]= coeff_bilan * pos[10];
        	bilanciamento[2]= attitudeC[2]; //---- poi  Att C
        }
        if(isCloseTo(cornerC)){
            counter++;
            conta=0;
            DEBUG(("Punto C: x=%5.3f y=%5.3f z=%f wx=%5.3f wy=%5.3f wz=%5.3f d=%5.3f END",
            pos[0], pos[1], pos[2], pos[9], pos[10], pos[11], dist));
        
        }else{
            api.setPositionTarget(cornerC);
            api.setAttRateTarget(bilanciamento);
        }
    }
    
    if(counter==3){
        dist=distP[3];                      //--m
        if (conta < 3){
            conta++;
            bilanciamento[0]= coeff_bilan * pos[9];
            bilanciamento[1]= coeff_bilan * pos[10];
            bilanciamento[2]= attitudeC[2];	//--ancora Att C
            DEBUG(("\nconta++"));
        }
        else {
        	bilanciamento[0]= coeff_bilan * pos[9];
        	bilanciamento[1]= coeff_bilan * pos[10];
        	bilanciamento[2]= attitudeD[2]; //---- poi  Att D
        }

            api.setPositionTarget(cornerD);         //--m
            api.setAttRateTarget(bilanciamento);    //--m
    }
}

bool isCloseTo (float *target) {
    ZRState myState;
    api.getMyZRState(myState);
    
    float distanceSquared = (target[0] - myState[0]) * (target[0] - myState[0])
    + (target[1] - myState[1]) * (target[1] - myState[1])
    + (target[2] - myState[2]) * (target[2] - myState[2]);

    return distanceSquared < dist_squared;  //--m
}

float distanza (float *target) {

    float distanceSquared = (target[0] - pos[0]) * (target[0] - pos[0])
    + (target[1] - pos[1]) * (target[1] - pos[1])
    + (target[2] - pos[2]) * (target[2] - pos[2]);
    return sqrt(distanceSquared);
}

void  calcolaDistanze(){    //--m
	distP[0] = distanza(cornerA);
	distP[1] = distanza(cornerB);
	distP[2] = distanza(cornerC);
	distP[3] = distanza(cornerD);
}    
//End page main
//End page main