//Begin page main
//-----------------------------------------------------------------------------------------
//ISS Version Log
//-----------------------------------------------------------------------------------------
//9.5.2 Adjust approach settings
//9.4 hookOffset to hookTarget and uses Arnauts Numbers
//9.3 Rex Numbers
//9.2Arnauts Hooking Numbers
//9.1 Kierans Hooking Numbers
//9.0 Began ISS Version
//---- Now tugs as soon as in position
//----------------------------------------------------------------------------------------

//Begin page Testing
//-----------------------------------------------------------------------------------------
//Page: Testing
//This page will calculate data about the game to analyze 
//-----------------------------------------------------------------------------------------

void printHookOffset()
{
    float hookPos[3];
    
    float yRot[4] = {0,-0.707,0,0.707};
    float zRot[4] = {0,0,0.707,0.707};
    
    float yQuat[4];
    float zQuat[4];
    
    //float xVec[3];
    float yVec[3];
    float zVec[3];
    
    float refVec[3] = {1,0,0};
    float hookVec[3];
    float xOffset;
    float yOffset;
    float zOffset;
    float otherHook[3];
    state_vector otherQuat;
    state_vector otherState;
    
    api.getOtherSphState(otherQuat);
    api.getOtherZRState(otherState);
    
    mathVecAddTerm(hookPos,myState,&myState[6],0.17095);
    mathVecAddTerm(otherHook,otherState,&otherState[6],0.17095);
    mathVecSubtract(hookVec,hookPos,otherHook,3);

    quatMult(yQuat,&otherQuat[6],zRot);
    api.quat2AttVec(refVec,yQuat,yVec);
    
    quatMult(zQuat,&otherQuat[6],yRot);
    api.quat2AttVec(refVec,zQuat,zVec);
    
    
    xOffset = mathVecInner(hookVec,&otherState[6],3);
    yOffset = mathVecInner(hookVec,yVec,3);
    zOffset = mathVecInner(hookVec,zVec,3);
    DEBUG((",Hook Offset, %.3f, %.3f, %.3f", xOffset, yOffset, zOffset));
}

//End of Testing ------------------------------------------------------------------------------
//End page Testing
//Begin page berbas

void setHookPosition(float posTarget[3], float hookTarget[3], float xOffset, float yOffset, float zOffset, int method=2)
{
    float yRot[4] = {0,-0.707,0,0.707};
    float zRot[4] = {0,0,0.707,0.707};
    
    float yQuat[4];
    float zQuat[4];
    
    float xVec[3];
    float yVec[3];
    float zVec[3];
    
    float refVec[3] = {1,0,0};
    
    float sumVec[3];
    float totalVec[3];
    
    float savedHook[3];
    float savedAtt[3];
   
    api.quat2AttVec(refVec,savedQuat,savedAtt);
    //sets magnitude of other att of x offset 
    mathVecSetMag(xVec,savedAtt,xOffset);
    
    //rotates saved quat by 90 around z axis 
    //gets roated att and sets mag to y offset
    quatMult(yQuat,savedQuat,zRot);
    api.quat2AttVec(refVec,yQuat,yVec);
    mathVecSetMag(yVec,yVec,yOffset);
    
    //rotates saved quat by 90 around -y axis 
    //gets roated att and sets mag to z offset
    quatMult(zQuat,savedQuat,yRot);
    api.quat2AttVec(refVec,zQuat,zVec);
    mathVecSetMag(zVec,zVec,zOffset);
    
    //adds vectors to get 3d vector
    mathVecAdd(sumVec,xVec,yVec,3);
    mathVecAdd(totalVec,sumVec,zVec,3);
    
   mathVecAddTerm(savedHook,savedPos,savedAtt, 0.17095);
   mathVecAdd(hookTarget,savedHook,totalVec,3);
   mathVecAddTerm(posTarget,hookTarget,&myState[6],-0.17095);

    //if(!end){setAccPath(myState,&myState[3],posTarget,v_max);}//targets position
    if(method==2){setAccPosition(myState,&myState[3],posTarget,v_max);}//targets position
    
    //DEBUG((",Hook Target, %.3f, %.3f, %.3f",xOffset,yOffset,zOffset));
}

void setHookQuaternion(float xRot)
{
    float negX[3]={-1,0,0};
    float zQuat[4] = {0,0,1,0};
    float quat1[4];
    float quat2[4];

    //attackAngle=180-attackAngle;
    //getQuaternion(axis,0.01745327777f*attackAngle,quatMultp);
    //quatMult(quatTarget,savedQuat,quatMultp);

    quatMult(quat1,savedQuat,zQuat);
    getQuaternion(negX,xRot,quat2);
    quatMult(quatTarget,quat1,quat2);
    mathVecNormalize(quatTarget,4);
    api.setQuatTarget(quatTarget);//targets quat
}




//End page berbas
//Begin page berbas2
bool isClose (float* current, float *target, float max_dist)
{
    float distance = getDistance(current, target);
    
    return (distance <= max_dist);
}

//Calcuates quaterion by using axis and angle:
//x,y,z = Axis of rotation (vector rotating around)	
//angle = Amount of rotation about axis	
//qx	=sin(angle/2) * x
//qy	=sin(angle/2) * y
//qz	=sin(angle/2) * z
//qw	=cos(angle/2)
//angle in radians
void getQuaternion(float axis[3], float angle, float quat[4])
{
    for(int i=0;i<3;i++)
    {
       quat[i] = sinf(angle/2.0) * axis[i];
    }
    
    quat[3] = cosf(angle/2.0);
    mathVecNormalize(quat,4);
}

void mathVecSetMag(float targetVector[3], float startingVector[3], float mag)
{
    float scalar;
    if(mathVecMagnitude(startingVector, 3)<0.0001)
    {
        //DEBUG(("mathVecSetMag:startingVector is zero"));
        return;
    }
    scalar=mag/mathVecMagnitude(startingVector, 3);
    mathVecMultiply(targetVector,startingVector,scalar);
}


//Calls getAccPath, then calls getForces to apply those forces
void setAccPath(float x0[3],float v0[3], float x1[3],float vMax)
{
    float a[3];
    float force[3];
    getAccPath(a,x0,v0,x1,vMax);
    getForces(force,a);
    api.setForces(force);
}

//Calls getAccPosition, then calls getForces to apply those forces.
void setAccPosition(float x0[3],float v0[3], float x1[3],float vMax)
{
    float a[3];
    float force[3];
    getAccPosition(a,x0,v0,x1,vMax);
    getForces(force,a);
    api.setForces(force);
}

//a = acceration
//x0 = starting position
//v0 = starting velocity 
//x1 = final position
//v_max = max velocity
//Gets acceleration required to follow a path (no stopping)
void getAccPath(float a[3], float x0[3], float v0[3], float x1[3], float v_max)
{      
    if(getDistance(x0,x1) > v_max)
    {
        mathVecSetDistance(x1,x1,x0,v_max);
    }

    for(int i=0;i<3;i++)
    {
        a[i]=2*(x1[i] - x0[i] - v0[i]);
    }
    limitAcc(a,v0,v_max);
}

//Go to a specific position (assumes stopping at that position)
//Gets acc to go to position 
void getAccPosition(float a[3],float x0[3],float v0[3], float x1[3],float vMax)
{
    float d[3];
    mathVecSubtract(d,x1,x0,3);
    float dMag =mathVecMagnitude(d,3);
    float sec=sqrtf((2*dMag)/a_max);
    sec=ceilf(sec);
    if(sec<1){sec=1;}
    mathVecMultiply(a,d,(-2/(sec*sec)),3);
    float m0[3];//ideal slope at x0 given max acceleration
    mathVecMultiply(m0,a,-sec,3);
    float nextX[3];
    mathVecAddTerm(nextX,x0,m0,2);
    
    mathVecAddTerm(nextX,nextX,a,2);
    if(sec<2){mathVecCopy(nextX,x1,3);}
    mathVecSubtract(a,nextX,x0,3);
    mathVecAddTerm(a,a,v0,-2);
    mathVecMultiply(a,a,0.5,3);
    
    limitAcc(a,v0,vMax);
}

float getDistance(float *current, float *target)
{
    float vector [3];
    //Turn 2 points into vectors
    //Test distance (min 0.05 away)

    mathVecSubtract (vector, target, current, 3);
    return mathVecMagnitude (vector,3);
}    


//getAngle:
//this compares angles to eachother using math
float getAngle(float *vec1, float *vec2)
{   
    
    mathVecNormalize(vec1,3);
    mathVecNormalize(vec2,3);
    return acosf(mathVecInner(vec1, vec2, 3));
}

//multiples old vector by scalar or multiple to get a scaled vector
void mathVecMultiply(float *newVector, float *oldVector, float multiple, int n=3)
{
    for (int i=0; i<n; i++)
    {
        newVector[i] = multiple*oldVector[i];
    }
}

//f is the force a is the acceleration
void getForces (float f[3],float a[3])
{
    //float th = game.getThrusterHealth();
    //if( th < 50 )
    //{
        //th = 50;
    //}
    //float k = 474.1/th;
    float k = 6.25;
    mathVecMultiply(f,a,k);
}

//returns point newTarget that is distance mm from point start
//on the line connecting start to oldTarget
void mathVecSetDistance( float *newTarget, float *oldTarget, float *start, float mm)
{
    mathVecSubtract(newTarget,oldTarget,start,3);
    mathVecSetMag(newTarget,newTarget,mm);
    mathVecAdd(newTarget, newTarget, start,3);
}

void limitAcc(float a[3], float v0[3], float vMax)
{
    if(mathVecMagnitude(a,3)>a_max)//from 0.008 to a_max
    {
        mathVecSetMag(a,a,a_max);//from 0.008 to a_max
    }
    
    float nextV[3];
    mathVecAddTerm(nextV,v0,a,2);
    if(mathVecMagnitude(nextV,3)>vMax)
    {
        mathVecSetMag(nextV,nextV,vMax);
        mathVecMultiply(a,nextV,0.5);
        mathVecAddTerm(a,a,v0,-0.5);
    }
}

//c output
//a number that is just added
//b multiply by term and then added
void mathVecAddTerm (float c[3], float a[3], float b[3], float term)
{
    float cTerm[3];
    mathVecMultiply(cTerm,b,term,3);
    mathVecAdd(c,cTerm,a,3);
}

void mathVecCopy(float *setVec,float *setter, int length)
{
    for(int i=0; i<length; i++)
    {
        setVec[i]=setter[i];
    }
}


//End page berbas2
//Begin page main
state_vector myState;

//float vel[3];
//float angVel[3];
//float Deg2Rad;
int t;
int stage; 
int counter;
float target[3];//where we are going
float quatTarget[4];
float a_max;
float savedQuat[4];
float savedPos[3];
float prevPos[3];
float v_max;

float vel[3];
 float otherHook[3];

bool time;
int timer;
void init()
{
    DEBUG(("Code Base  9.6"));
    stage=0;
    a_max = 0.0083;
    v_max = 0.1000;
    prevPos[0]=0;
    prevPos[1]=0;
    prevPos[2]=0;
    // DEBUG(("ROLL X = %f",myEState[6]));
    // DEBUG(("PITCH Y = %f",myEState[7]));
    // DEBUG(("YAW Z = %f",myEState[8]));
}

void loop()
{   
state_vector otherState;
    t = api.getTime();
    api.getOtherZRState(otherState);
    api.getMyZRState(myState);
    
    float hookTarget[3];
    float hookPos[3];
    mathVecAddTerm(hookPos,myState,&myState[6],0.17095);
    
   
    mathVecAddTerm(otherHook,otherState,&otherState[6],0.17095);

    //api.getMySphState (myState);
    api.getOtherSphState(otherState);
    mathVecCopy(savedQuat, &otherState[6],4);
    mathVecCopy(savedPos,otherState,3);
    float xRot=otherState[10];

        float deltaVec[4];
    
    if(t>3 && t<15)
   {
        mathVecSubtract(deltaVec,otherState,prevPos,3);
        mathVecAddTerm(savedPos,otherState,deltaVec,(15-t));
        //DEBUG(("predicted Pos %f, %f,%f",savedPos[0],savedPos[1],savedPos[2]));
    }
    


mathVecCopy(prevPos,otherState,3);
    //----------------------------------------------------------------------
    //Update Hooking
    //-----------------------------------------------------------------------
    //0	Idle (set up conditions and move to next stage)
    //1	Determine hooking area and hook position
    //2	Move hook to target hook 
    //3	Check if hooked if yes go to 9 if not repeat from 1 
    //4 towing
    //9	Complete (phase has ended, select next phase)
    
    // printHookStatus();
    // V6.2 Hook Parameters
    //printHookOffset();
   
    if (stage == 0)
    {
        //Version 5.9.2 - Keep these entries (comment if using new settings)
        DEBUG((", Stage Offset,-0.020, 0.016, -0.016,0.014"));
        DEBUG((", Stage Offset,-0.008, 0.000,  0.000,0.008"));
        DEBUG((", Stage Offset,0,0,0,0.004"));
        stage=1;
        //setHookQuaternion();
        //setHookPosition(target,hookTarget,0.02,0,-0.025,2);
        //if(isClose(hookPos,hookTarget,0.02)){stage = 1;}
        //DEBUG(("STAGE 0"));
    }
    
    if (stage == 1)
    {
        DEBUG(("STAGE 1"));
        //v_max=0.10;
        setHookQuaternion(xRot);
        setHookPosition(target,hookTarget,-0.020, 0.016, -0.016,2);
       
        if(isClose(hookPos,hookTarget,0.014)){stage=2;}
       
    }
    if(stage == 2)
    {
        DEBUG(("STAGE 2"));
        //Stage 2 is below hook, and closer to sphere
        
        setHookQuaternion(xRot);
        setHookPosition(target,hookTarget,-0.008, 0.000,  0.000,2);
        if(isClose(hookPos,hookTarget,0.008)){stage=3;}
        
    }
    if(stage==3)
    {
         v_max=0.02;
         DEBUG(("STAGE 3"));
         setHookPosition(target,hookTarget,0,0,0,0);
         if(!isClose(hookTarget,hookPos,0.004))
         {
            setHookQuaternion(xRot);
            setHookPosition(target,hookTarget,0,0,0,2);
            //DEBUG(("Setting Pos"));
         }
         if(isClose(otherHook,hookPos,0.004) || game.getGamePhase()==4)
{
	DEBUG(("I am hooked at %d",t));
	stage = 4;
}
    }

    if(stage==4) 
    {
         DEBUG(("STAGE 4"));
         if(myState[1] > 0.25){
            if(!time){
                timer = api.getTime();
                time = true;
                vel[1] = 0;
            }
             vel[2] = 1;
             vel[0] = 0.8;
            if((api.getTime()) == (timer + 2)){
                vel[0] = 0.8;
            }
        api.setAttRateTarget(vel);

        }else{
            vel[1] = 0.9;
            vel[2] = 0;
            vel[0] = -0.2;
	setHookQuaternion(0);
        }
        
        api.setVelocityTarget(vel);
        
        
    }
}

//SETS THE EULER ANGLES TO 0
// bool RestoreAxis()
// {
//     if(boolangiu)
//     {   
//         oilar[0] = 0;//roll x
//         oilar[1] = 0;//pitch y
//         oilar[2] = 0;//yaw z
//     }
    
//     if(laOilar(oilar) && boolangiu)
//     {
//         DEBUG(("RESTORED"));
        
//         boolangiu = false;
//     }
    
//     game.setEulerTarget(oilar);  
// }



//-------------------------------------------

/*
//FUNCTION THAT CHECKS IF THE BLUE SATTELITE HAS THE RIGHT EULER ANGLES
bool laOilar(float oilar[3])
{//laPozitia sequel
    float myEState[12], eroareDeRotatie = PI/180;
    game.getMyEulerState(myEState);
    if(myEState[6] > oilar[0]-eroareDeRotatie && myEState[6] < oilar[1]+eroareDeRotatie &&
      myEState[7] > oilar[1]-eroareDeRotatie && myEState[7] < oilar[1]+eroareDeRotatie &&
      myEState[8] > oilar[2]-eroareDeRotatie && myEState[8] < oilar[2]+eroareDeRotatie)
        return true;
    return false;
}
*/
//FUNCTION THAT CHECKS IF THE BLUE SATTELITE IS IN THE RIGHT POSITION
// bool laPozitia(float poz0, float poz1, float poz2){
//     float eroareDePozitie = 0.003;
//     if(myState[0]>=poz0 - eroareDePozitie && myState[0]<=poz0 + eroareDePozitie&&       //daca e aprox la destinatie
//       myState[1]>=poz1 - eroareDePozitie && myState[1]<=poz1 + eroareDePozitie&&
//       myState[2]>=poz2 - eroareDePozitie && myState[2]<=poz2 + eroareDePozitie){
//             return true;
//     }else 
//             return false;
// }

//End page main



//End page main