float myZRState[12], myPos[3], positionA[2], positionB[3], positionC[3], positionD[3], attitude[3], att[3], vector[3],
      height, min, distance, vel, angVel;
int counter, step, br, geyser, n, p, k;

void init(){
    //This function is called once when your code is first loaded.
    attitude[0] = 0.0f;
    attitude[1] = 0.0f;
    attitude[2] = -1.0f;
    positionA[2] = 0.26f;
    positionB[2] = 0.26f;
    positionC[0] = 0.0f;
    positionC[1] = 0.0f;
    positionC[2] = 0.0f;
    counter = 0;
    step = 0;
    geyser = 0;
    p = 0;
    k = 0;
    //IMPORTANT: make sure to set any variables that need an initial value.
    //Do not assume variables will be set to 0 automatically!
}

void loop(){
    api.getMyZRState(myZRState);
    att[0] = myZRState[6];
    att[1] = myZRState[7];
    att[2] = 0.0f;
    mathVecNormalize(att, 3);
    for (int i = 0; i < 3; i++)
    {
        myPos[i] = myZRState[i];
    }
    if (counter == 0)
    {
        if (myZRState[0] > 0)
        {
            br = 1;
        }
        else
        {
            br = -1;
        }
        coordinates();
        counter++;
    }
    if (step == 0 || step == 4)
    {
        api.setAttitudeTarget(att);
        vel = sqrt(myZRState[3] * myZRState[3] + myZRState[4] * myZRState[4] + myZRState[5] * myZRState[5]);
        angVel = sqrt(myZRState[9] * myZRState[9] + myZRState[10] * myZRState[10] + myZRState[11] * myZRState[11]);
        move(1);
        if ((myZRState[0] < (positionB[0] + 0.03f)) && (myZRState[0] > (positionB[0] - 0.03f)) && (vel < 0.01f) && (fabs(myZRState[8]) < 0.13f) &&
            (myZRState[1] < (positionB[1] + 0.03f)) && (myZRState[1] > (positionB[1] - 0.03f)) && (myZRState[2] >= 0.255f) && (angVel < 0.04f))
        {
            step = 10;
            geyser = 0;
            n = 0;
            p = 0;
        }
    }
    else if (step == 9)
    {
        if (k < 4)
        {
            move(2);
            k++;
        }
        else
        {
            move(4);
        }
        api.setAttitudeTarget(attitude);
        if (game.atBaseStation() == 1)
        {
            for (int i = 0; i < 5; i++)
            {
                game.dropSample(i);
            }
            coordinates();
            step = 0;
            geyser = 0;
            k = 0;
        }
    }
    else
    {
        if (geyser == 0)
        {
            drill();
        }
        if ((game.getDrills(myPos) == 3 && game.getNumSamplesHeld() == 4) || game.getNumSamplesHeld() == 5)
        {
            game.stopDrill();
            step = 9;
        }
        else if (game.getDrills(myPos) == 3 || geyser == 1)
        {
            if (n == 0)
            {
                coordinates();
                game.stopDrill();
                n++;
            }
            move(3);
            if (geyser == 1)
            {
                step -= 3;
            }
            else
            {
                step = 4;
            }
        }
    }
    //This function is called once per second.  Use it to control the satellite.
}

void coordinates()
{
    min = 1.0f;
    for (int i = 0; i < 16; i++)
    {
        for (int j = 0; j < 10; j++)
        {
            positionA[0] = -0.6f + 0.08f * i;
            positionA[1] = (0.04f + 0.08f * j) * br;
            if (game.getDrills(positionA) > 0)
            {
                continue;
            }
            height = game.getTerrainHeight(positionA);
            if (height == 0.4f && (fabs(positionA[0]) > 0.16f || fabs(positionA[1]) > 0.16f))
            {
                distance = sqrtf((positionA[0] - myZRState[0]) * (positionA[0] - myZRState[0]) +
                                 (positionA[1] - myZRState[1]) * (positionA[1] - myZRState[1]));
                if (distance < min)
                {
                    min = distance;
                    positionB[0] = positionA[0];
                    positionB[1] = positionA[1];
                }
            }
        }
    }
}

void drill()
{
    geyser = 0;
    api.setPositionTarget(positionB);
    if (game.getDrillEnabled() == 0)
    {
        game.startDrill();
    }
    if (game.getDrillError() == true)
    {
        game.stopDrill();
        coordinates();
        step = 4;
    }
    if (game.getDrillEnabled() == 1)
    {
        for (int i = 0; i < 3; i++)
        {
            positionD[i] = positionB[i];
        }
        mathVecCross(positionD, att, attitude);
        mathVecNormalize(positionD, 3);
        api.setAttitudeTarget(positionD);
    }
    if (game.checkSample() == 1)
    {
        game.pickupSample();
        if (game.isGeyserHere(positionB) == true)
        {
            game.stopDrill();
            geyser = 1;
        }
    }
}

void move(int variant)
{
    switch (variant)
    {
    case 1:
        mathVecSubtract(vector, positionB, myPos, 3);
        if (api.getTime() < 20)
        {
            if (myZRState[5] < 0.03f && myZRState[2] < 0.1f)
            {
                api.setVelocityTarget(vector);
            }
            else if (myZRState[2] > 0.15f)
            {
                api.setPositionTarget(positionB);
            }
        }
        else
        {
            if (((vel < 0.032f) && (p == 0 || p == 1) && (myZRState[5] < 0.02f) && (geyser == 1)) ||
                ((geyser == 0) && (p == 0 || p == 1) && (vel < 0.011f) && (step == 4)) ||
                ((step == 0) && (p == 0 || p == 1) && (vel < 0.02f)))
            {
                api.setVelocityTarget(vector);
                p = 1;
                DEBUG(("YCKORENIE"));
            }
            else
            {
                if (p == 1 && myZRState[2] > 0.24f && myZRState[5] > 0.0f)
                {
                    DEBUG(("BLOKIROVKA"));
                    p = 2;
                }
                api.setPositionTarget(positionB);
                DEBUG(("ZAMEDLENIE"));
            }
        }
        break;
    case 2:
        mathVecSubtract(vector, positionC, myPos, 3);
        api.setVelocityTarget(vector);
        break;
    case 3:
        mathVecSubtract(vector, positionB, myPos, 3);
        for (int i = 0; i < 3; i++)
        {
            vector[i] = vector[i] * 100;
        }
        api.setVelocityTarget(vector);
        break;
    case 4:
        for (int i = 0; i < 3; i++)
        {
            positionC[i] = myZRState[i] * 0.2201f / mathVecMagnitude(myPos, 3);
        }
        api.setPositionTarget(positionC);
        break;
    }
}