//Begin page main



float pf[4][3], p1[3], last_vel[3];                  //migliorare lo spostamento del punto
float zred[3];
int stato;                                                                                   //                                                                              //                                                                                //codesizeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee
bool attiva, champion;

void init(){
	
	stato = 0;

    attiva = champion = true;
}

void loop(){
    
    DEBUG(("go go giorgioooooo go"));
    
   /*game.getOtherEulerState(otherState);
    game.getMyEulerState(mystate);
    
    float zred[3], yred[3], yblue[3], zblue[3];
    
    zred[0] = cos(otherState[6]) * sin(otherState[7]) * cos(otherState[8]) + sin(otherState[6]) * sin(otherState[8]);
    zred[1] = cos(otherState[6]) * sin(otherState[7]) * sin(otherState[8]) - sin(otherState[6]) * cos(otherState[8]);
    zred[2] = cos(otherState[6]) * cos(otherState[7]);
    
    yred[0] = cos(otherState[8]) * sin(otherState[7]) * sin(otherState[6]) - sin(otherState[8]) * cos(otherState[6]);
    yred[1] = sin(otherState[8]) * sin(otherState[7]) * sin(otherState[6]) + cos(otherState[8]) * cos(otherState[6]);
    yred[2] = cos(otherState[7]) * sin(otherState[6]);
    
    yblue[0] = cos(mystate[8]) * sin(mystate[7]) * sin(mystate[6]) - sin(mystate[8]) * cos(mystate[6]);
    yblue[1] = sin(mystate[8]) * sin(mystate[7]) * sin(mystate[6]) + cos(mystate[8]) * cos(mystate[6]);
    yblue[2] = cos(mystate[7]) * sin(mystate[6]);
    
    zblue[0] = cos(mystate[6]) * sin(mystate[7]) * cos(mystate[8]) + sin(mystate[6]) * sin(mystate[8]);
    zblue[1] = cos(mystate[6]) * sin(mystate[7]) * sin(mystate[8]) - sin(mystate[6]) * cos(mystate[8]);
    zblue[2] = cos(mystate[6]) * cos(mystate[7]);*/
    
    float mystate[12], otherState[12], distance[3], m;
    
    api.getMyZRState(mystate);
    api.getOtherZRState(otherState);
    
    api.setPosGains(0.52f,1.f,4.5f);
    api.setAttGains(0.1f,1.f,4.5f);
    
    if(mathVecMagnitude(&otherState[3],3) > 0.002) champion = false;
    //DEBUG(("%d", champion));
    
    //distance[0] = -0.5 - (otherState[7] * otherState[0] / otherState[6]);
    //DEBUG(("%f", distance[0]));

    if(game.getGamePhase() < 4) {
    
        if(stato == 0) {                                    //andare verso il punto di hooking
        
            CanYouShowMeDaWaeeee();
            
            mathVecSubtract(distance,p1,mystate,3);
            m = mathVecMagnitude(distance,3);
            if(m < 0.012) stato++;
            else if(!champion)                               mathVecScalar(distance,((0.085 * (1-expf(-m/0.21)))));
            else                                            mathVecScalar(distance,((0.058 * (1-expf(-m/0.11)))));
        }
        
        if(stato == 1) {  //hooking
        
            CanYouShowMeDaWaeeee();
            
            mathVecSubtract(distance,p1,mystate,3);
            m = mathVecNormalize(distance,3);
    
            /*if(champion) mathVecScalar(distance,(m * 0.6));
            else         */if(!champion) mathVecScalar(distance,((0.025 * (1-expf(-m/0.105)))));
            else                        mathVecScalar(distance,((0.0275 * (1-expf(-m/0.11)))));
        }
    }
    
    if(game.getGamePhase() == 4) {                       //towning

        //api.getOtherZRState(otherState);
        
        //pf[3][0] -= 0.05;
        //pf[3][1] -= 0.2;
        
        game.getOtherEulerState(otherState);
        pf[3][0] =  -otherState[6] - (champion ? 0.3 : 0);
        pf[3][1] =  -otherState[7];
        pf[3][2] =   otherState[8] + 3.14;
        
        if(otherState[1] > -0.48) {    
            
            //game.getOtherEulerState(otherState);
    
            //float zred[3];
            
            /*zred[0] = (cosf(otherState[6]) * sinf(otherState[7]) * cosf(otherState[8]) + sinf(otherState[6]) * sinf(otherState[8])) * -1;
            zred[1] = (cosf(otherState[6]) * sinf(otherState[7]) * sinf(otherState[8]) - sinf(otherState[6]) * cosf(otherState[8])) * -1;
            zred[2] = (cosf(otherState[6]) * cosf(otherState[7])) * -1;*/
            trans(1,-1);
            
            float Xp[3], started_quat[4], newQuat[4], started_att[3];
    
            Xp[0] =  1;
            Xp[1] =  0;
            Xp[2] =  0;
        
            mathVecNormalize(zred,3);
            game.eulerToQuaternion(pf[3],started_quat);
            api.quat2AttVec(Xp,started_quat,started_att);
            api.attVec2Quat(started_att,zred,started_quat,newQuat);
            game.quaternionToEuler(newQuat,pf[3]); 
            //memcpy(pf[3],last_vel,3 * sizeof(float));
        }
        
        api.getOtherZRState(otherState);
        mathVecScalar(&otherState[6],0.1);
        if((otherState[1] > -0.4 && !champion) || (otherState[1] > 0 && champion))  {  //-0.1
            
            //pf[3][0] -= 0.4;
            //pf[3][1] -= 0.4;

            if(!attiva) {
                
                memcpy(p1,mystate,3 * sizeof(float));
                trans(7,0);
                
                attiva =true;
            }
            
            mathVecSubtract(distance,p1,mystate,3);
            //m = mathVecMagnitude(distance,3);
            
            /*if(m > 0.1) */mathVecScalar(distance,0.5);  //0.5
            //else        mathVecScalar(distance,0.00000001);
        }
        else memcpy(distance,&otherState[6],3 * sizeof(float));
    }
    
    api.setVelocityTarget(distance);
    
    game.setEulerTarget(pf[3]);
}

void CanYouShowMeDaWaeeee() {
    
    float O[3];
    float mystate[12], otherState[12], distance[3], m;
    
    
   if((champion ? 1 : (!game.getHookCollision()))) {
        
        game.getOtherEulerState(otherState);
        pf[3][0] =  -otherState[6] - (champion ? 0.3 : 0);
        pf[3][1] =  -otherState[7];
        pf[3][2] =   otherState[8] + 3.14;
    }
    
    api.getMyZRState(mystate);
    api.getOtherZRState(otherState);
    
    if(!champion) {
        
        //mathVecSubtract(distance,p1,mystate,3);
        //if((mathVecMagnitude(&otherState[3],3) < 0.007 && mathVecMagnitude(distance,3) < 0.2) || !attiva) {
            
            mathVecScalar(&otherState[6],(stato == 0 ? 0.334 : 0.341));
            mathVecAdd(p1,otherState,&otherState[6],3);
            
            /*mathVecScalar(&otherState[6],0.17095);   //0.15
            mathVecAdd(O,&otherState[6],otherState,3);
            
            mathVecScalar(&mystate[6],0.17095);     //0.15
            mathVecAdd(last_vel,&mystate[6],mystate,3);*/
            
            //trans(,(stato == 0 ? -0.03 : -0.005));
            
            if(stato == 0) {
                
                trans(0.00338,-0.018);
            }
            /*else {
            
                p1[0] = p1[0] - (last_vel[0] - O[0]);
                p1[2] = p1[2] - (last_vel[2] - O[2]);
                
                //trans(0,-0.002);
            }*/
            
            //attiva = false;
        /*}
        else {             //algoritmo
            
            otherState[1] = 0;
            mathVecScalar(otherState,0.2);
            otherState[1] = -0.5;
            
            O[0] = O[2] = 0;
            O[1] = -1.634256364;
            mathVecSubtract(last_vel,otherState,O,3);
            mathVecScalar(last_vel,0.332);
            mathVecAdd(p1,otherState,last_vel,3);
            
            //game.getOtherEulerState(otherState);
            
            zred[0] = cosf(pf[3][0]) * sinf(pf[3][1]) * cosf(pf[3][2]) + sinf(pf[3][0]) * sinf(pf[3][2]);
            zred[1] = cosf(pf[3][0]) * sinf(pf[3][1]) * sinf(pf[3][2]) - sinf(pf[3][0]) * cosf(pf[3][2]);
            zred[2] = cosf(pf[3][0]) * cosf(pf[3][1]);
            
            yred[0] = cosf(pf[3][2]) * sinf(pf[3][1]) * sinf(pf[3][0]) - sinf(pf[3][2]) * cosf(pf[3][0]);
            yred[1] = sinf(pf[3][2]) * sinf(pf[3][1]) * sinf(pf[3][0]) + cosf(pf[3][2]) * cosf(pf[3][0]);
            yred[2] = cosf(pf[3][1]) * sinf(pf[3][0]);*/
            
            //trans(0.00342,-0.018);
        //}
    }
    else {
        
        //api.getMyZRState(mystate);
        //api.getOtherZRState(otherState);
        
        mathVecSubtract(distance,p1,mystate,3);
        if(mathVecMagnitude(distance,3) < 0.05) {
            
            mathVecScalar(&otherState[6],(stato == 0 ? 0.332 : 0.3408));
            mathVecAdd(p1,otherState,&otherState[6],3);
            
            
            
            
            
            /*mathVecScalar(&otherState[6],0.17095);
            mathVecAdd(O,&otherState[6],otherState,3);
            
            mathVecScalar(&mystate[6],0.17095);
            mathVecAdd(last_vel,&mystate[6],mystate,3);*/
            
            
            
            
            
            if(stato == 0) trans(0.015,-0.005);
            /*else {
                
                p1[0] = p1[0] - (fabs((O[0] - last_vel[0])) * ((last_vel[0] > O[0]) ? 1 : -1));
                p1[2] = p1[2] - (fabs((O[2] - last_vel[2])) * ((last_vel[2] > O[2]) ? 1 : -1));
            }*/
        }
        else {
            
            mathVecScalar(&otherState[6],0.332);
            mathVecAdd(p1,otherState,&otherState[6],3);
            
            trans(0.01,-0.04);
        }
    }
    
    if(stato != 0) {
        
        //api.getMyZRState(mystate);
        //api.getOtherZRState(otherState);
        
        mathVecScalar(&otherState[6],0.16895);
        mathVecAdd(O,&otherState[6],otherState,3);
            
        mathVecScalar(&mystate[6],0.16895);
        mathVecAdd(last_vel,&mystate[6],mystate,3);
        
        p1[0] = p1[0] - (last_vel[0] - O[0]);
        p1[2] = p1[2] - (last_vel[2] - O[2]);
    }
    attiva= false;
}

void mathVecScalar(float vect[], float n) {
    
    mathVecNormalize(vect,3);
    for(int i = 0; i < 3; i++)
        vect[i] *= n;
}

/*void attitudeToEuler(float attitude[3], float started_euler[3]) {
    
    float Xp[3], started_quat[4], newQuat[4], started_att[3];
    
    Xp[0] =  1;
    Xp[1] =  0;
    Xp[2] =  0;

    mathVecNormalize(attitude,3);
    game.eulerToQuaternion(started_euler,started_quat);
    api.quat2AttVec(Xp,started_quat,started_att);
    api.attVec2Quat(started_att,attitude,started_quat,newQuat);
    game.quaternionToEuler(newQuat,last_vel);                             //last_vel is a public variable, in which we put the result
}*/

void trans(float y, float z) {
    
    float yred[3], otherState[12];
    
    game.getOtherEulerState(otherState);
            
    zred[0] = cosf(otherState[6]) * sinf(otherState[7]) * cosf(otherState[8]) + sinf(otherState[6]) * sinf(otherState[8]);
    zred[1] = cosf(otherState[6]) * sinf(otherState[7]) * sinf(otherState[8]) - sinf(otherState[6]) * cosf(otherState[8]);
    zred[2] = cosf(otherState[6]) * cosf(otherState[7]);
    
    yred[0] = cosf(otherState[8]) * sinf(otherState[7]) * sinf(otherState[6]) - sinf(otherState[8]) * cosf(otherState[6]);
    yred[1] = sinf(otherState[8]) * sinf(otherState[7]) * sinf(otherState[6]) + cosf(otherState[8]) * cosf(otherState[6]);
    yred[2] = cosf(otherState[7]) * sinf(otherState[6]);
    
    mathVecScalar(zred,z);
    mathVecAdd(p1,p1,zred,3);
    mathVecScalar(yred,y);
    mathVecAdd(p1,p1,yred,3);
}
//(sei un grande davide ;)

//lo so XD


//End page main