//Begin page main
float myState[12], otherState[12], otherSphState[13];
    #define myPos myState
    #define otherPos otherState
    #define myVel &myState[3]
    #define mySpeed mathVecMagnitude(myVel, 3)
    #define otherVel &otherState[3]
    #define otherSpeed mathVecMagnitude(otherVel, 3)
    #define myAtt &myState[6]
    #define otherAtt &otherState[6]
    #define myAngVel &myState[9]
    #define otherAngVel &otherState[9]
    #define otherQuat &otherSphState[6]

float Z_ROT_QUAT[4];
int step;
    #define HOOKING 3
    #define TOWING 4
int passage;
float targetPos[3];

void init(){ 
    api.setPosGains(0.67f, 0.00f, 4.00f);
    api.setAttGains(0.50f, 8.00f, 17.0f);
    step = HOOKING;
    passage = 0;
    
    targetPos[1] = 0.7f;
    targetPos[2] = 0.0f;
    targetPos[0] = 0.2f;
    
    Z_ROT_QUAT[0] = 0.0f;
    Z_ROT_QUAT[1] = 0.0f; 
    Z_ROT_QUAT[2] =-1.0f;
    Z_ROT_QUAT[3] = 0.0f;
}

void loop() { 
    api.getMyZRState(myState);
    api.getOtherZRState(otherState);
    api.getOtherSphState(otherSphState);
    
    if(step == HOOKING) {
        
        float quat[4];
        quatMult(quat, otherQuat, Z_ROT_QUAT);
        api.setQuatTarget(quat);
        
        float to[3];
        if(passage == 0) {
            to[0] = 0.33f;
            to[1] = 0.03f;
            to[2] = -0.015f;
            
            float mat[3][3];
            float matLine[9];
            float to2[3];
            mathBody2Global(mat, otherSphState);
            for(int i = 0; i < 3; i++) {
                for(int j = 0 ; j < 3; j++) {
                    matLine[(3*i)+j] = mat[i][j];
                }
            }
            mathMatVecMult(to2, matLine, to, 3, 3);
            mathVecAdd(to, to2, otherState ,3);
            api.setPositionTarget(to);
            if(dist(myState, to) < 0.02f) {
                passage++;
            }
        } else {
            for(int i = 0; i < 3; i++) { 
                to[i] = otherState[i+6];
            }
            setLength(to, to, 2*0.165f, 3);
            mathVecAdd(to, to, otherState, 3);
            api.setPositionTarget(to);
        }
        
        DEBUG(("passage= %d", passage));
        if(game.getGamePhase()==4 || dist(myPos, to) < 0.003f) {
            step=TOWING;
        }
    }
    
    if(step == TOWING) {
        if(myState[1]<0.3)
        api.setPosGains(3.0f, 0.0f, 0.2f);
        else
        api.setPosGains(0.6f, 0.0f, 4.0f);
        
        api.setPositionTarget(targetPos);
        if (myPos[0] > 0.1) {
            DEBUG(("Test"));
            float targetPos2[3];
            targetPos2[0] = 0.0;
            targetPos2[1] = 0.7;
            targetPos2[2] = 0.0;
            api.setPositionTarget(targetPos2);
        } else if (myPos[1] >= 0.6) {
            float targetPos3[3];
            targetPos3[0] = 0.0;
            targetPos3[1] = 0.7;
            targetPos3[2] = 0.0;
            api.setPositionTarget(targetPos3);
        }
    }
}

void mul(float* b, float* a, float k, int n) { for(int i = 0; i < n; i++) { b[i] = a[i]*k; } }
void setLength(float* b, float* a, float k, int n) { memcpy(b, a, n*sizeof(float)); mathVecNormalize(b, n); mul(b, b, k, n); } 
float dist(float* a, float* b) { float v[3]; mathVecSubtract(v, a, b, 3); return mathVecMagnitude(v, 3); }





//End page main