//Begin page main
//Begin page main
#define EPS 0.00001
#define moved (g.otherState[2] > -0.195f)
#define pwTwo(x) ((x) * (x))
#define copyArray(res, src, n) memcpy(res, src, 3*sizeof(float))


struct global{
    float myState[12], otherState[12], lastOtherState[12];
    float force[3], force1[3], force2[3];

    float attother[3];
    float tg2[3];
    int elapsed_time;

    float target[3], angle[3];
    
    float myEuler[12], otherEuler[12];

    int counter;
    
    float safepoint[3];
    float complexhook[3], imaginaryhook[3], imaginaryhook2[3];
    float redhook[3], bluehook[3], vfcon[3];
    float centre[3], quat[4], forces[3], basetarget[3];
    float predict[3], red_sphere_origin[3];
    int contorbase;
    float vecbetween[3];
    
    float tg[3];
    bool calculated;
}g;

enum phase{GET_THERE, HOOK, TUG};
phase state;

void init(){
	memset(&g, 0, sizeof(g));
	
	api.getMyZRState(g.myState);
	api.getOtherZRState(g.otherState);
    game.getMyEulerState(g.myEuler);

	state = GET_THERE;
    g.vfcon[1] = -1.634f;
   

    copyArray(g.lastOtherState, g.myState, 3);
    copyArray(g.red_sphere_origin, g.otherState, 3);
}


void loop(){
	api.getMyZRState(g.myState);
	api.getOtherZRState(g.otherState);
    game.getOtherEulerState(g.otherEuler);
    game.getMyEulerState(g.myEuler);
    
    api.setPositionTarget(g.target);
    
    if(g.elapsed_time >= 3.0f){
        DEBUG(("%f", pwTwo(0.2f + 0.2f)));
        if(!g.calculated)
            getPointAtDistanceOnLine(g.predict, g.otherState, g.red_sphere_origin, 0.2f, 1);
        getPointAtDistanceOnLine(g.tg, g.vfcon, g.predict, 2 * 0.17095f, -1);
        g.tg2[1] = g.tg[1];
        DEBUG(("TG2 %f %f %f", g.tg2[0], g.tg2[1], g.tg2[2] ));
        getPointAtDistanceOnLine(g.target, g.tg2, g.tg, 0.057f, 1);
        g.target[1] -= 0.012f;

    }
    DEBUG(("PRED: %f %f", g.predict[0], g.predict[2]));
    DEBUG(("TARGET: %f %f %f", g.target[0], g.target[1], g.target[2]));
    DEBUG(("Red sphere moved %d", moved));

	// Calculate hooks 
    getPointAtDistanceOnLine(g.imaginaryhook2, g.centre, &g.otherState[6], 0.17095f, -1);
    g.redhook[0] = g.imaginaryhook2[0] - g.otherState[6] + g.otherState[0];
    g.redhook[1] = g.imaginaryhook2[1] - g.otherState[7] + g.otherState[1];
    g.redhook[2] = g.imaginaryhook2[2] - g.otherState[8] + g.otherState[2];

    getPointAtDistanceOnLine(g.imaginaryhook, g.centre, &g.myState[6], 0.17095f, -1);
    g.bluehook[0] = g.imaginaryhook[0] - g.myState[6] + g.myState[0];
    g.bluehook[1] = g.imaginaryhook[1] - g.myState[7] + g.myState[1];
    g.bluehook[2] = g.imaginaryhook[2] - g.myState[8] + g.myState[2];
    // End of hook calculations
    
    copyArray(g.angle, &g.myEuler[6], 3);
    g.angle[0] = -g.otherEuler[6]; // align with other tank
    
    
    if(game.getGamePhase() == 4)
        state = TUG;
	DEBUG(("state: %d", state));
	switch(state){
	    case GET_THERE:
            DEBUG(("kk"));
            //move(g.target);
            if(calculateDistance(g.myState, g.target) < 0.03f)
                state = HOOK;
            if(g.elapsed_time % 2 == 0){
                copyArray(g.attother, &g.otherState[6], 3);
                ourVecScale(g.attother, g.attother, -1.0f, false);
                api.setAttitudeTarget(g.attother);
            } else {
                game.eulerToQuaternion(g.angle, g.quat);
                api.setQuatTarget(g.quat);
            }
            break;
        case HOOK:
            //setGains(3.0,2.0);
            DEBUG(("hook johnule hook"));

            getPointAtDistanceOnLine(g.complexhook, g.bluehook, g.redhook, 0.0001f, -1);
            copyArray(g.target, g.myState, 3);
            g.target[0] += g.complexhook[0] - g.bluehook[0];
            g.target[1] += g.complexhook[1] - g.bluehook[1];
            g.target[2] += g.complexhook[2] - g.bluehook[2];
            api.setPositionTarget(g.target);
            if(g.counter <= 6 || g.counter % 5 == 4){
                game.eulerToQuaternion(g.angle, g.quat);
                api.setQuatTarget(g.quat);
            } else {
                mathVecSubtract(g.vecbetween, g.otherState, g.myState, 3);
                api.setAttitudeTarget(g.vecbetween);
            }
            g.counter++;
            break;
        case TUG:
            if(g.contorbase == 0){
                copyArray(g.basetarget, g.myState, 3);
                g.basetarget[1] = +0.5f;
                g.safepoint[0] = -0.6f;
                g.safepoint[1] = 0.8f;
                g.safepoint[2] = -0.65f;
            }
            mathVecSubtract(g.forces, g.basetarget, g.myState, 3);
            ourVecScale(g.forces, g.forces, 100, true);
            if(g.contorbase < 8)
                 api.setForces(g.forces);
            else {
                float rot[3];
                copyArray(rot, &g.myEuler[6], 3);
                rot[0] += 5;
                game.setEulerTarget(rot);
                api.setPositionTarget(g.safepoint);
            }
            g.contorbase++;
            break;
	}
	g.elapsed_time++;
	copyArray(g.lastOtherState, g.otherState, 12);
}   

//End page main
//Begin page utils
void getPointAtDistanceOnLine(float result[3], float firstPoint[3], float secondPoint[3], float dist, int dir){
    float t = -dir * dist / calculateDistance(firstPoint, secondPoint) + 1;
    result[0] = (secondPoint[0] - firstPoint[0]) * t + firstPoint[0];
    result[1] = (secondPoint[1] - firstPoint[1]) * t + firstPoint[1];
    result[2] = (secondPoint[2] - firstPoint[2]) * t + firstPoint[2];
}

float calculateDistance(float point1[], float point2[]){
    float vectorBetween[3];
    mathVecSubtract(vectorBetween, point1, point2, 3);
    return mathVecMagnitude(vectorBetween, 3);
}

void ourVecScale(float res[], float src[], float scl, bool norm){
  memcpy(res, src, 3*sizeof(float));
  if(norm) mathVecNormalize(res, 3);
  for(int i=0; i<3; i++)res[i]*=scl;
}

//End page utils
//End page main