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


void init()
{
    counter=0;
//mettere le coordinate dei punti
//coordinate punti
    cornerA[0] = 0.0;
	cornerA[1] = 0.5;
	cornerA[2] = 0.25;
	cornerB[0] = 0.0;
	cornerB[1] = -0.5;
	cornerB[2] = 0.5;
	cornerC[0] = 0.0;
	cornerC[1] = 0.5;
	cornerC[2] = 0.75;
	cornerD[0] = 0.0;
	cornerD[1] = 0.0;
	cornerD[2] = 1.0;
	
//rotazione su asse z	
	
	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));
    
//dichiarazione bilanciamento

    float bilanciamento1[3]={0,0,0};
    memcpy(bilanciamento, bilanciamento1, 3*sizeof(float));
    
    coeff_bilan=-2.5;
    dist_debug  =0.045;     //--m
    dist_squared=0.0012;    //--m (0.035m)
    conta=0;

    
//forze

    float forceA1[3]={0,1,0.25};
    float forceB1[3]={0,-1,0.2};
    float forceC1[3]={0,1,0.15};
    float forceD1[3]={0,-1,0.15};
    memcpy(forceA, forceA1, 3*sizeof(float));
    memcpy(forceB, forceB1, 3*sizeof(float));
    memcpy(forceC, forceC1, 3*sizeof(float));
    memcpy(forceD, forceD1, 3*sizeof(float));
}

void loop()
{   
    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];
        bilanciamento[0]= coeff_bilan * pos[9];
        bilanciamento[1]= coeff_bilan * 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{
            if(isCloseTo2(cornerA)){
                api.setPositionTarget(cornerA);
                api.setAttRateTarget(bilanciamento);
            }else{
                api.setForces(forceA);
                api.setAttRateTarget(bilanciamento);
            }
        }
    }


    if(counter==1){
        dist=distP[1];
        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{
                    
                if(isCloseTo3(cornerB)){
                    api.setPositionTarget(cornerB);
                    api.setAttRateTarget(bilanciamento);
                }else{
                    api.setForces(forceB);
                    api.setAttRateTarget(bilanciamento);

                }
            }
    }
    
    if(counter==2){
        dist=distP[2];
        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{
                    
                if(isCloseTo3(cornerC)){
                    api.setPositionTarget(cornerC);
                    api.setAttRateTarget(bilanciamento);
                }else{
                    api.setForces(forceC);
                    api.setAttRateTarget(bilanciamento);

                }
            }
    }
    
    if(counter==3){
        dist=distP[3];
        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) {
    
    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 distanceSquared < dist_squared; //--m
}

bool isCloseTo2 (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 distanceSquared < 0.45;  //--mq = 0.67m 
}

bool isCloseTo3 (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 distanceSquared < 0.49;  //--mq = 0.7m
}

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