//Begin page main
void update()
    {
    // My and opposite ZRState
    api.getMyZRState(mystate);
    api.getOtherZRState(oppstate);
    
    game.getMyEulerState(myeulstate);
    game.getOtherEulerState(oppeulstate);
    
    for(int x = 0; x < 3; x++)
        {
        mypos[x] = mystate[x];
        myvel[x] = mystate[x+3];
        myatt[x] = mystate[x+6];
        myrot[x] = mystate[x+9];
        
        opppos[x] = oppstate[x];
        oppvel[x] = oppstate[x+3];
        oppatt[x] = oppstate[x+6];
        opprot[x] = oppstate[x+9];
        
        myeul[x] = myeulstate[x+6];
        oppeul[x] = oppeulstate[x+6];
        }
    
    // My thruster health and fuel
    fuel = game.getFuelRemaining();
    myscore = game.getScore();
    }

float distance(float *p1, float *p2)
    {  // Distance between 2 positions (meters)
    float p3[3];
    mathVecSubtract(p3, p1, p2, 3);
    
    return mathVecMagnitude(p3, 3);
    }

// ---------------------------------------------------------BREAK--------------------------------------------------------



// Variables
float mystate[12], myeulstate[12], mypos[3], myvel[3], myatt[3], myeul[3], myrot[3];  // My state
float oppstate[12], oppeulstate[12], opppos[3], oppvel[3], oppatt[3], oppeul[3], opprot[3];  // Opposite state
 
float tpos[3], tvel[3], tatt[3], teul[4], trot[3],thirdgain;  // target array
 
float dist, alpha, mag, myscore, e, fuel; //distance, stop, scores
 
int step, count, temp,health;  //Fuel %
 
// INIT ----------------------------------------------------------------------------------------------
void init()
    {
    // Initialize variables
    step = count = temp = alpha =  e = 0;
    health=game.getThrusterHealth();
    // Call functions
    update();
    thirdgain=7.57-0.034*health;     //7.5655-0.0366*health;
    }
 
// LOOP ----------------------------------------------------------------------------------------------
void loop()
    {
    update();
     
    //Debug step
    DEBUG(("STEP: %d", step));
     
    if(step == 0)
        {  // going -Y
        if(mypos[1] < 0.5) //0.55
            step++;
            else
                {
                tvel[0] = 0;
                tvel[1] = -15;
                tvel[2] = 0;
                api.setPosGains(4.23,1.1,1.1);
                api.setPositionTarget(tvel);
            }
        }
    if(step == 1)
        {  //HOOK
        if(game.getGamePhase() == 4)
            step++;
            else
                {
                //Variable gains
                dist = distance(mypos, tpos);
                DEBUG(("%f", dist));
             
                if(dist > 0.20)
                    {
                    api.setPosGains(1.4212, 8.21, 7.518); //        1.3/n/n
                    api.setAttGains(1.1, 7, 8.2);
                    }
                    else if(dist > 0.07)
                        {
                        api.setPosGains(1.212, 9, 9.9);      //      n/n/7.65
                        api.setAttGains(1.05, 8, 8.5);
                        }
                        else
                            {
                            api.setPosGains(1.22,0.3,thirdgain);
                            if(health == 100)
                                api.setAttGains(1.0, 15, 5.55);
                                else if(health >= 90)
                                    api.setAttGains(1.0, 15, 5.55);
                                    else if(health >= 75)
                                        api.setAttGains(1.0, 15, 6.55);
                                        else if(health >= 65)
                                        api.setAttGains(1.0, 15, 6.55);
                                        else
                                            api.setAttGains(1.0, 15, 7.15);//7.55
                            }
             
                mag = 0.339;      //0.338 or 0.341
                tpos[0] = opppos[0] + oppatt[0]*mag;
                tpos[1] = opppos[1] + oppatt[1]*mag;
                tpos[2] = opppos[2] + oppatt[2]*mag;
                if(opppos[0]<0.005 && opppos[0]>-0.005 && opppos[1]<-0.495 && opppos[1]>-0.505 && opppos[2]<0.005 && opppos[2]>-0.005)
                    tpos[1] = opppos[1] + oppatt[1]*mag+0.00025;
                api.setPositionTarget(tpos);
                 
                teul[0] = -oppeul[0];
                teul[1] = -oppeul[1];
                teul[2] = oppeul[2] - PI; //oppeul[2] - PI
                if(opppos[0]<0.005 && opppos[0]>-0.005 && opppos[1]<-0.495 && opppos[1]>-0.505 && opppos[2]<0.005 && opppos[2]>-0.005)
                    {
                    teul[0]=-oppeul[0]-0.42875;                                                                                            //////////CHANGE A LITTLE -oppeul[0]-0.x
                    teul[1]=0.00;
                    teul[2]=-PI/2;
                    }
                game.setEulerTarget(teul);
                }
        }
    if(step == 2)
        {  //towing
        if(mypos[1] > 0.2)
            step++;
            else
                {
                mag = 0.5;  
                tvel[0] = oppatt[0]*mag;
                tvel[1] = oppatt[1]*mag;
                tvel[2] = oppatt[2]*mag;
                api.setVelocityTarget(tvel);
                 
                teul[0] = -oppeul[0];
                teul[1] = -oppeul[1];
                teul[2] = oppeul[2] - PI;
                game.setEulerTarget(teul);
                }
        }
    if(step == 3)
        {  //unhook and escape
        dist = distance(mypos, opppos);
         
        if(dist > 1)
            {
            DEBUG(("Perform a wheelie!"));
            step++;
            }
            else
                {
                trot[0] = trot[2] = 0;
                trot[1] = 1;
                api.setAttRateTarget(trot);
                     
                tpos[0] = -0.8;
                tpos[1] = 0.1;
                tpos[2] = -4;
                mathVecSubtract(tvel, tpos, mypos, 3);
                     
                api.setVelocityTarget(tvel);
            }
        }
    if(step == 4)
        {  // FINISHED
        // stop
        tvel[0] = tvel[1] = tvel[2] = 0;
        api.setVelocityTarget(tvel);
         
        // Improved wheelie
        e = 0.1;
        if(myeul[1] > PI/2 - e && myeul[1] < PI/2 + e)
            {
            trot[1] = trot[2] = 0;
            trot[0] = 1;
             
            api.setAttRateTarget(trot);
            }
            else
                {
                teul[0] = myeul[0];
                teul[1] = PI/2;
                teul[2] = myeul[2];
             
                game.setEulerTarget(teul);
                }
        }
    }
//End page main