//Begin page main

float pid[3];


float rotation[4];

float MyPosition[13];

int phase;
float dist;

bool CHM;
float second;
void init()
{
    
    api.setAttGains(4,4,0.1);
    phase = 1;
    second = 0;
    
}
void loop()
{
    CHM = false;
    
    api.getMySphState (MyPosition); 
    float smt[12];
    
    float punto[3];
    game.getOtherEulerState(smt);
    
    int gamePhase;
    gamePhase = game.getGamePhase();
    
    //api.setPosGains(0.5,1,3);
	pid[0]=0.5;
	pid[1]=1;
	pid[2]=3;
	
    Find_Quaternion();

            
    float epls;
    
    epls = 0;

    if(phase == 1 && gamePhase != 4)
    {
		pid[1]=0;
		pid[2]=4;
        if(MyPosition[1]>-0.13)
        {
		    pid[1]=0.6;

        }else if (MyPosition[1]>-0.165){
			pid[0]=0.5;                  
		}else{
		    pid[0]=0.3; 
	        pid[2]=2.6;
						
        }

        Pos_allingment(0.32,0.03,true,punto);
                    

        if(CHM)
            epls = 0.01;
                        
        if(calcDistance(punto) < 0.015+epls-second)
            phase++;
        }
        else 
        { 

                
            if(phase == 2 )
                {
                     
                    dist = 0.32;
                    Pos_allingment(dist,0.005,false,punto);
                        
                    if(calcDistance(punto) < 0.01)
                    {
                        phase++;
                        dist = 0.36;
                    }
                        
                }
                    
                    
                     if(gamePhase == 4)
                        {
                            dist = 0.6;
                            
                            phase = 23;
                            

                           
                        }
                        
                    
                    Pos_allingment(dist,0.005,false,punto);

                    float oth[3];
                    
                    oth[0] = smt[0];
                    oth[1] = smt[1];
                    oth[2] = smt[2];
    
                       if(phase == 3 && calcDistance(oth) > 0.348)
                            {
                                phase = 1;
                                second = 0.008;
                                
                                if(CHM)
                                    second = 0.002;
                            }
                    
                    if(MyPosition[1] > 0.1)
                    {
                        
                        Pos_allingment(0.3,0.9,true,punto);
						pid[0]=5; 
						pid[1]=0;
					    pid[2]=0;       
						}

                }
                
          api.setPosGains(pid[0],pid[1],pid[2]);

        api.setQuatTarget(rotation); 
        
        api.setPositionTarget(punto);
        

        
}



void Find_Quaternion()
{
    
    float smt[12];
    float riferimento[4];

    game.getOtherEulerState(smt);
    
    float target[3];
    float factor = 0;


    if(fabsf(smt[0]) < 0.05 && fabsf(smt[2]) < 0.05)
    {
        
        factor = 0.00666666 * 25;

        CHM = true;
    }  
    
    target[0] = smt[6] + factor;
    target[1] = smt[7];
    target[2] = smt[8];
    
    game.eulerToQuaternion(target,riferimento);
  
    float rotY[4];
     
    rotY[0] = 0;
    rotY[1] = 0;
    rotY[2] = 1;
    rotY[3] = 0;

    quatMult(rotation,riferimento,rotY);
    
    
}


void Pos_allingment(float dist,float dist_trav,bool flag,float punt[3])
{
    float ang,ang1;
    float att[12];
    api.getOtherZRState(att);
    
    ang= atan2(att[7],att[6]);//Finds the angle of the XY plane
    ang1 = atan2(att[8], sqrt(att[7]*att[7] + att[6]*att[6]));//Finds the angle of the YZ plane
        

    float ops = dist*cos(ang1);
    
    punt[0] = ops*cos(ang);
    punt[1] = ops*sin(ang);
    punt[2] = dist*sin(ang1);
    
    
    
    if(flag)
    {
        
        float smt[12];
        game.getOtherEulerState(smt);
        float tp = 4;
        
        
        if(!CHM)
        {
            tp = 2;
        }
        else
        {
            tp = 2;
        }
        float y = smt[6]-PI/tp;

        
        punt[0] += dist_trav*sin(y);
        punt[2] += dist_trav*cos(y);
            

        
    }
    punt[0] += att[0];
    punt[1] += att[1];
    punt[2] += att[2];
    
    
    

    
}


float calcDistance (float p2[]){
    float vctBtw[3];
    mathVecSubtract(vctBtw,MyPosition,p2,3);
    return mathVecMagnitude(vctBtw,3);
}
//End page main