//Begin page main
//Begin page main
#define max(a, b) ((a>b)? (a):(b))
#define min(a, b) ((a<b)? (a):(b))

float my_state[12];
float my_euler[12];
float other_state[12];
float other_euler_state[13];
float target[3];
float other_hook[3];
float my_hook[3];
float other_angle[3];

float compare_other[12];

float pos[3];
float dist;

float yeet;

bool champion;

void init() {
    champion = 0;
    DEBUG(("muie pesky"));
    api.setPosGains(0.258f*1.8f,0.0086f,1.935f*1.5f);//d=1.4
    api.setAttGains(0.258f*1.8f,0.0086f,1.935f*1.5f);//d=1.4
    api.getMyZRState(my_state);
    api.getOtherZRState(compare_other);
    yeet = 1.5f; //1.5
    
    game.enableHookGT();
    
    dist = 0.12f;  /// boss 15 cm era o gramada
    pos[1] = -0.17f;
}

void loop() {
    api.getMyZRState(my_state);
    api.getOtherZRState(other_state);
    
    if(distance(my_state, other_state) < ( 2 * 0.17095f + 0.01f))
        yeet = 0;
    
    DEBUG(("%f", yeet));
    
    game.getMyEulerState(my_euler);
    HookPos(other_hook, other_state, yeet);
    HookPos(my_hook, my_state, 0);
    
    if(game.getGamePhase() == 3) {
        if(api.getTime() >= 10 && api.getTime() <= 35 && distance(other_state, compare_other) < 0.01f && Velocity(other_state) > 0.05f){
            Angles(other_angle);
            pos[0] = sin(other_angle[0]) * dist;
            pos[2] = cos(other_angle[0]) * dist;
            api.setPositionTarget(pos);
            if(dist > 0.0025f)  
                dist -= 0.0023f;   /// lasam acolo 0.0002 XD, si marim putin pasul sa convearga mai repede
            
            DEBUG(("CHAMPION!!"));
            
            champion = 1;
        }
        else{
            Angles(other_angle);
            DEBUG(("dist hook: %f", distance(my_hook, other_hook)));
            other_angle[0] *= -1;
            other_angle[1] *= -1;
            other_angle[2] += PI;

            for(int i = 0 ; i < 3; i++)
                target[i] = other_state[i] + 0.3403 * other_state[6 + i];
                
            if(champion)
                other_angle[0] -= PI / 180.0 * 9;
                
            api.setPositionTarget(target);
            game.setEulerTarget(other_angle);
        }
    }
    if(game.getGamePhase() == 4) {
        game.disableHookGT();
        if(other_state[1] < 0){
            game.setEulerTarget(other_angle);
            float force[3];
            force[0] = other_state[3];
            force[1] = other_state[4] + 10;
            force[2] = other_state[5];
            api.setForces(force);
        }else{
            float torque[] = {0, 0, 0};
            if(game.getHooked())
                torque[1] = -1;
            api.setTorques(torque);
            float velocity[] = {-1, 0, 2};
            api.setVelocityTarget(velocity);
        }
    }
}

float Velocity(float sph[]){
    float vel[3];
    vel[0] = sph[9];
    vel[1] = sph[10];
    vel[2] = sph[11];
    return mathVecMagnitude(vel, 3);
}

void set(float *vec3, float a, float b, float c){
    vec3[0] = a;
    vec3[1] = b;
    vec3[2] = c;
}
void copy(float *dest, float *source){
    dest[0] = source[0];
    dest[1] = source[1];
    dest[2] = source[2];
}
float distancePointLine(float* pt, float *a, float *b){
    float v1[3], v2[3], v3[3], v4[3];
    mathVecSubtract(v1, pt, a, 3);
    mathVecSubtract(v2, pt, b, 3);
    mathVecCross(v3, v1, v2);
    mathVecSubtract(v4, b, a, 3);
    float sq = mathVecMagnitude(v3, 3) / mathVecMagnitude(v4, 3);
    if(sq < 0)
        sq = -sq;
    return sq;
}
float distance(float first[], float second[]) {
    float result[3];
    mathVecSubtract(result, second, first, 3); 
    return mathVecMagnitude(result, 3);
}
void move(float target[]) {
    float my_state[12];
    float vector_between[3];
    api.getMyZRState(my_state);
    mathVecSubtract(vector_between, target, my_state, 3);
    if(distance(my_state, target) > 0.4f)
        api.setForces(vector_between);
    else
        api.setPositionTarget(target);
}
void Angles(float angle[3]) {
    float state[12];
    game.getOtherEulerState(state);
    for(int i = 0; i < 3; i++)
        angle[i] = state[i + 6];
}
void HookPos(float point[], float sph[], float jmk){
    for(int i = 0; i < 3; i++){
        point[i] = sph[i] + sph[i + 6] * 0.17095f;
    }
    point[1] = sph[1] + (sph[1 + 6] + jmk) * 0.17095f;
}
//End page main

//End page main