//Begin page main
// Version 
// README Basta che capiamo noi
// Variables declaration 


float myState[12];
float otherState[12];
float myPos[3];
float targetPos[3];
float myAtt[3];
float otherAtt[3];

float togo[3];
float vb[3];
float tugForce[3];

float disVertical;
float disHook;
float disRB;
float v;
int count;

// Function prototypes
void moveTo(float, float);
void copyVector(float, float, int);
void setEndpointZ(float, float);
void redZVector(float);
void endPoint(float, float, float);

// Variables initialization
void init() {
    count = 0;
    disVertical = 0.0f;
    disHook = 0.341748f; //748f;
    
    tugForce[0] = 0.0f;
    tugForce[1] = 1.0f;
    tugForce[2] = 0.0f;
}

void loop(){
    
    api.getMyZRState(myState);
    api.getOtherZRState(otherState);
    for(int i = 0; i < 3; i++) {
        targetPos[i] = otherState[i];
        myPos[i] = myState[i];        
        otherAtt[i] = otherState[i+6];
    }
    
    if(game.getGamePhase() == 3) {
        copyVector(togo, targetPos);    
        mathVecSubtract(vb, myPos, targetPos, 3);
        disRB = mathVecMagnitude(vb, 3);
        if(disRB > 0.7f)
            v = 0.028f; //28f;
        else if(disRB > 0.37f)
            v = 0.011f;
        else 
            v = 0.004f;
        
        if(disRB > 0.37f) {
            disVertical = 0.045f;
            disHook = 0.7f;
            disHook -= api.getTime() * 0.015; 
        }
        else if(disRB > 0.355f) {
            disVertical = 0.034f;
            disHook = 0.325;
        }
        else if(count++ < 15) {
            disVertical = ((0.034f) - (count * 0.0034f)); //225f; //5f;
            if(disVertical <= 0.0025f)  //0.0053f) // 0.0053f altezza aggiuntiva
                disVertical = 0.0025f; //025f;
            if(count < 13)
                disHook =  0.323525f; //0.323525f;
            else
                disHook = 0.341748f;
        }
        else {
            disVertical = 0.0025f;
            disHook = 0.341748f;
        }
        setAngleTarget();
        setEndpointZ(togo, disVertical);
        endPoint(togo, otherAtt, disHook);
        moveTo(togo, v);
    }
    else {
        if(targetPos[1] < 0.0f) {
            api.setForces(tugForce);
            setAngleTarget();
        }
        else {
            tugForce[0] = 1.0f;
            tugForce[1] = 0.0f;
            tugForce[2] = 1.0f;
            api.setForces(tugForce);
            if(game.getHooked())
                api.setTorques(tugForce);
        }
    }
}

void moveTo(float target[3], float value = 0.007f)  {
    
    //we suggest to always be calling movement at bottom of code, and just changing the target before.
    //For example, in our code, we just call movement ONCE at bottom of loop, and change target to what it needs to be.
    //This movement function does use a little fuel even when stopped, but we think that is okay because it isn't much.
    //no need to add extra code to prevent.
    float myState[12]; //Our information
    float futurePos[3]; //Where we WILL be in the next second
    float futureToTarget[3]; //vector between target and future position
    float distToTarget; //distance to target(absolute value)
    float vectToTarget[3]; //vector to target
    float vectToSetMag; //multiplier, function of distance over time.
    //We graphed SPHERE movement and linearized to save code space, and then took the function.
    float futureDist; //Distance from futurePos to target
    float currentVelMag; //Current Speed
    
    api.getMyZRState(myState);
    for(int y=0;y<3;y++){
        futurePos[y] = myState[y] + myState[y+3]; //myposition+myVelocity
    }
    
	currentVelMag=fabsf(mathVecMagnitude(myState+3,3)); //magnitude of speed.
	mathVecSubtract(futureToTarget, target, futurePos, 3); //target-futureposition=futuretotarget
	futureDist = fabsf(mathVecNormalize(futureToTarget, 3)); //get distance from futurePos to target
	mathVecSubtract(vectToTarget, target, myState, 3); //vector between target and our current position
	distToTarget = fabsf(mathVecNormalize(vectToTarget, 3)); //distance from target to our current position
	vectToSetMag = fabsf(sqrt(value*futureDist)); //function that we got from graphing SPHERE movement
	//can change number(0.007) to get faster but more fuel, or slower and less fuel.
	for(int i=0;i<3;i++) { 
	    futureToTarget[i] *= vectToSetMag;
	}
	api.setVelocityTarget(futureToTarget);
}

void copyVector(float a[], float b[], int size = 3) {
    for(int i = 0; i < size; i++) {
        a[i] = b[i];
    }
}

void setEndpointZ(float pointToGo[], float dist) {
    float attVecZ[3]; 
    redZVector(attVecZ);
    endPoint(pointToGo, attVecZ, dist);
}

void redZVector(float vector[]) {
    float quaternionState[13];
    float zVec[3] = { 0.0f, 0.0f, -1.0f };
    api.getOtherSphState(quaternionState);
    float quatAtt[4];
    for(int i = 0; i < 4; i++) {
        quatAtt[i] = quaternionState[i+6];
    }
    api.quat2AttVec(zVec, quatAtt, vector);
}

void setAngleTarget() {
  float eulerRedState[12];
  float eulerState[3];
  game.getOtherEulerState(eulerRedState);
  for(int i = 0; i < 3; i++)
    eulerState[i] = eulerRedState[i+6];
  float eulerPoint[3];
  eulerPoint[0] = -eulerState[0]; //+ M_PI;
  eulerPoint[1] = -eulerState[1];
  eulerPoint[2] = eulerState[2] + M_PI;

  game.setEulerTarget(eulerPoint);
}

void endPoint(float vector[], float unit[], float distance) {
    vector[0] = vector[0] + distance * unit[0];
    vector[1] = vector[1] + distance * unit[1];
    vector[2] = vector[2] + distance * unit[2];
}
//End page main