2016 Final Results

Page Contents:

2016 International Space Station Champions!

14 International alliances competed aboard the ISS in the Zero Robotics SPACESPHERES championship competition.

CONGRATULATIONS to:

2016 ISS Championship alliance: SpaceLinguine

2016 ISS Second Place alliance: FlyingFalconsTheQuarkCharmCode::Space

ISS Championship Alliance:SpaceLinguine

Alliance Name

 

 

  • ZRighi

ITI "Augusto Righi"

Italy

  • OverExtendedProgramming(OEP)

Centennial High School

AZ

United States

  • LSA Robotics Team

Liceo Scientifico Avogadro

Italy

SpaceLinguine

 :

ZRighi

ITI "Augusto Righi", Napoli, Italy

OverExtendedProgramming(OEP)

Centennial High School, Peoria, United States

LSA Robotics Team

Liceo Scientifico Avogadro, Vercelli, Italy
 :

ISS 2nd Place Alliance:FlyingFalconsTheQuarkCharmCode::Space

Alliance Name

 

 

  • Flying Falcons

North Sydney Boys High School

Australia

  • The Quark Charm

Storming Robots

NJ

United States

  • Code::Space

National College of Computer Science

Romania

FlyingFalconsTheQuarkCharmCode::Space

 :

Flying Falcons

North Sydney Boys High School, Sydney, Australia

The Quark Charm

Storming Robots, Branchburg, United States

Code::Space

National College of Computer Science, Piatra-Neamt, Romania
 :

Virtual Finalist Champions!

The top two international alliances from the virtual finals competed on the International Space Station to determine the Virtual finalist Champion

CONGRATULATIONS to the champion of the virtual finalist competition CYS BURGER

Virtual Finals Championship Alliance:CYS BURGER

Alliance Name

 

 

  • Team y0b0tics!

Montclair Community

NJ

United States

  • Cassiopeia

Grigore Moisil Theoretical Highschool

Romania

  • tE@m Segfault

Prospect Hill Academy

MA

United States

CYS BURGER

  • Team y0b0tics! Montclair Community, Montclair, United States
  • tE@m Segfault Prospect Hill Academy, Cambridge, United States
  • Cassiopeia Grigore Moisil Theoretical Highschool, Timisoara, Romania

ISS Match Details

 

The following tables show the official results of the ISS finals matches.

Notes:

1) All ISS matches were run using fixed item locations so that all teams were competing against equal conditions.

2) Scores shown are those reported from ISS unless noted otherwise. Scores from ISS were reported per the following rules: All scores were reported in whole numbers. If scores varied by decimal points, the winning team of each match was awarded an extra point.   All scores less than 1 were recorded as 1 and all scores greater than 22 were recorded as 22 except when the team earned an extra point for winning the match. For example, if both teams earned scores greater than 22 the match winner’s score read from ISS was 23.

3) The code for each of the finalist implementations has been posted in tables at the bottom of this page.

ISS Tournament Matches

Conference A

Bracket A-1

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

1. Team SANTA (3)

 

2. Wormhole (7)

18 (18)

23 (26)

link

2. Wormhole (7)

3. Vinci-NCSSM-ZRM !!!(11)

1 (1)

23 (24)

link

3. Vinci-NCSSM-ZRM !!!(11)

1. Team SANTA (3)

22 (23)

23 (25)

link

Bracket A-2

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

4. ProgNaughtical (5)

 

5. BACON-Cranbrery Pie (9)

14

25

link

5. BACON-Cranbrery Pie (9)

6. CosmicSparTech (13)

4 (4)

23 (28)

link

6. CosmicSparTech (13)

4. ProgNaughtical (5)

23

1

link

Conference A Semi-Finals

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

7. SpaceLinguine (1)

 

Vinci-NCSSM-ZRM !!

23 (35)

22 (25)

link

Vinci-NCSSM-ZRM !!!

CosmicSparTech

22 (23)

23 (30)

link

CosmicSparTech

7. SpaceLinguine (1)

22

23

link

Conference B

Bracket B-1

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

1. Zanneio Gunn Pointers (4)

 

2.CrabNebulaWaherlTeamAnomaly (8)

10 (9)

7 (7)

link

2.CrabNebulaWaherlTeamAnomaly-(8

3. Kuhl-Wall-Knights (12)

1 (0)

2 (1)

link

3. Kuhl-Wall-Knights (12)

1. Zanneio Gunn Pointers (4)

23 (24)

1 (1)

link

Bracket B-2

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

4. Keppler Hubble heROes (6)

 

5. FermiAsteroidsCraig (10)

17 (16.8)

16

link

5. FermiAsteroidsCraig (10)

6.FlyingFalconsTheQuarkCharmCode::Space (14)

13 (12)

10 (10)

link

6.FlyingFalconsTheQuarkCharmCode::Space (14)

4. Keppler Hubble heROes (6)

22

4 (4)

link

Conference B Semi-Finals

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

7. P.R.O. (2)

 

Kuhl-Wall-Knights

1 (1)

2 (2)

link

Kuhl-Wall-Knights

FlyingFalconsTheQuarkCharmCode::Space

1 (0)

23 (22)

link

FlyingFalconsTheQuarkCharmCode::Space

7. P.R.O. (2)

1 (0)

19 (18)

link

Virtual Finals Championship Match

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

8. CYS BURGER (V1)

 

8. Joined Unique International Coding Experts (J.U.I.C.E.) (V2)

21 (2)

1 (0)

link

ISS Finals Championship Match

Blue SPHERE

vs

Orange SPHERE

Blue Score

Orange Score

ISS Visuali-zation

Video Link

 

Sim Link

 

SpaceLinguine

 

FlyingFalconsTheQuarkCharmCode::Space

10 (9)

1 (1)

link

Photo Galleries from ISS Finals Events

Photo Gallery from MIT: Photo Gallery from ESTEC: Photo Gallery from Sydney, Australia:

ISS Alliance code

Conf B:

Alliance

Code

download
                        
                        //Begin page CONSTANT
#define SPSPLACEMENT 0
#define DOCK 1
#define DROP 10
#define STAY 11

#define FLOATSIZE3 12
//End page CONSTANT
//Begin page CORE
float getDistance(float *a, float *b){
    float distance[3];
    mathVecSubtract(distance,a,b,3);
    return mathVecMagnitude(distance,3);
}
bool isFacingPos(float myState[12], float lookPos[3], float tollerance, int valuePos){
    float realPos[3];  
    mathVecSubtract(realPos, lookPos, myState, 3); 
    mathVecNormalize(realPos,3);
    mathVecNormalize(myState+valuePos,3);
    //DEBUG(("TEST: %f",acosf(mathVecInner(realPos, myState+valuePos, 3))));
    return acosf(mathVecInner(realPos, myState+valuePos, 3))<tollerance;
}

bool newIsInBound(float *itemState){
    return !(fabs(itemState[0]) > 0.54 || fabs(itemState[1]) > 0.7f || fabs(itemState[2]) > 0.54f);
}


//End page CORE
//Begin page itemAvoid
bool willCollide(float myState[3], float itemFace[3], float itemPos[3]){
    return getDistance(myState, itemPos) < getDistance(myState, itemFace);
}
//End page itemAvoid
//Begin page itemFunctions
void calculateRightSecondSPS(float myState[12], float itemPos[12], float secondSPS[3], float A[3], float rightDistanceAverage){
    #define howIncrease 0.01f
    #define minArea 0.12500f
    float attitudeVector[3], CVERA[3], lastDist = 3.00f;
    do{
        game.getItemZRState(itemPos, actualID);
        memcpy(attitudeVector, itemPos, FLOATSIZE3);
        for(int i=0;i<3;i++) CVERA[i] = itemPos[i] + (itemPos[6+i] * rightDistanceAverage);
        canPickFirst = !(willCollide(myState, CVERA, itemPos));
        do{
            for(int i=0;i<3;i++) attitudeVector[i] = attitudeVector[i] + (itemPos[6+i] * howIncrease);
            for(float i = 0.0f; i<=3.0f; i+=0.01f){
                int axis = 0;
                for(int j = 0;j<6;j++){
                    float actualPoint[3];// = {attitudeVector[0], attitudeVector[1], attitudeVector[2]};
                    memcpy(actualPoint, attitudeVector, FLOATSIZE3);
                    // 0 0 1 1 2 2
                    axis += (j==2 || j==4);
                    actualPoint[axis] += (j%2 == 0) ? i : -i;
                    float AB[3], AC[3], crossBetween[3];
                    mathVecSubtract(AB, actualPoint, A, 3);
                    mathVecSubtract(AC, CVERA, A, 3);
                    mathVecCross(crossBetween, AB, AC);
                    float dist = getDistance(itemPos, actualPoint);
                    
                    if(getDistance(A, CVERA)>0.2f && dist<lastDist && ((mathVecMagnitude(crossBetween, 3)/2) > minArea) &&  ((!canPickFirst) ?  !willCollide(actualPoint, CVERA, itemPos) : true) ){
                        if(newIsInBound(actualPoint)){
                            memcpy(secondSPS, actualPoint, FLOATSIZE3);
                            lastDist = dist;
                        }
                    }
                } 
            }
        }while(newIsInBound(attitudeVector));
        
        if(lastDist == 3.0f) actualID+=2;
    }while(lastDist == 3.0f && actualID < 6);
}

//End page itemFunctions
//Begin page main
int Phase, actualID, Time, Color, droppedItems, idealID,otherID;
float A[3], calcSPS[3];
float allFacePOI[6][6];
float zoneInfo[4], enemyZone[3];
bool canPickFirst;

int idProva;
void init(){
    idProva = otherID = 10;
    Phase = actualID = droppedItems = Time = 0;
    game.dropSPS();
    calcSPS[0] = -20.0f;
}

void loop(){
    float whereToGo[3], whereAttitude[3], realItemState[12];
    float myState[12], enemyState[12];
    
    float kVel = 0.07f;
    
    api.getMyZRState(myState);
    api.getOtherZRState(enemyState);
    
    float distanceAvarage[3] = { .162f, .149f, .135f}; //DISTANZA GIUSTA PER IL DOCK IN BASE ALL'ITEM PRESO IN CONSIDERAZIONE
     
     
    if(Time++<1){ 
        actualID = Color = myState[1] < 0;
        memcpy(A, myState, FLOATSIZE3);
        for(int i = 0;i<6;i++){
            float tempState[12];
            game.getItemZRState(tempState, i);
            for(int j = 0;j<3;j++){
                allFacePOI[i][j] = tempState[j] + (tempState[6+j] * distanceAvarage[game.getItemType(i)]);
                allFacePOI[i][j+3] = tempState[j];
            }
        }
    }
    
    int SPSHeld = game.getNumSPSHeld();
    int itemType = game.getItemType(actualID);
    
    game.getItemZRState(realItemState, actualID);
    
    for(int i=0;i<3;i++){
        whereToGo[i] = realItemState[i] + (realItemState[6+i] * distanceAvarage[itemType]);
        whereAttitude[i] = -realItemState[6+i];
    }
    
    bool isGoingToB = ((SPSHeld==2 && !canPickFirst) || (canPickFirst && SPSHeld==1));
    if(Phase == SPSPLACEMENT){
        if(calcSPS[0] == -20.0f){
            calculateRightSecondSPS(myState, realItemState, calcSPS, A, distanceAvarage[itemType]);
        }
        
        memcpy(whereToGo, (isGoingToB) ? calcSPS : whereToGo, FLOATSIZE3);
        
        if(isGoingToB){ kVel = 0.0435; }
        
        if(getDistance(myState, whereToGo)<0.04f){
            if(canPickFirst && SPSHeld == 1){
                Phase = DROP;
            }
            if(isGoingToB) game.dropSPS();
        }
    }
    
    float realItemVel = mathVecMagnitude(realItemState+3, 3);
    float distanceToItem = getDistance(myState, realItemState);
    float enemyVel = mathVecMagnitude(enemyState+3,3);
    
    if(Phase<2){
        if(droppedItems==1){
            DEBUG(("distaaa : %f",getDistance(enemyState,enemyZone)));
            if(idProva != 10){
                DEBUG(("PROVA: %f",getDistance(myState,enemyZone)));
                if(isFacingPos(enemyState,enemyZone,0.55f,3)){
                    kVel = 0.16f;
                    DEBUG(("FACING"));
                    if(game.hasItem(idProva)==0){
                        actualID = (getDistance(myState,enemyZone)>0.28f) ? idProva : otherID;
                    }
                }
            }
            
            if(enemyVel>0.035f){
                if(isFacingPos(enemyState,zoneInfo,0.3f,3)){
                    Phase = STAY;
                    DEBUG(("It's so cold, I come back home"));
                }
            }
        }
        
        if((((realItemVel <= 0.0f) && realItemState[0] == allFacePOI[actualID][0+3]) || realItemVel > 0.0f) && ((!isGoingToB) || SPSHeld == 0)){
            DEBUG(("I'll wait"));
            memcpy(whereToGo, allFacePOI[actualID], sizeof(float)*3);
        }
        
        
        
        if(game.hasItem(actualID) == 0 && distanceToItem < distanceAvarage[itemType] + 0.011f && distanceToItem > distanceAvarage[itemType] - 0.011f && mathVecMagnitude(myState+3,3) < 0.01f ){
            if( isFacingPos(myState, realItemState, 0.25f, 6)){
                DEBUG(("posso DOCKARE"));
                if(game.dockItem(actualID)){
                    if(SPSHeld >=1) game.dropSPS(); 
                    if(SPSHeld <= 1 ) Phase = DROP;
                }
            }
        }
    }
    
    game.getZone(zoneInfo);
    for(int i = 0;i<3;i++) enemyZone[i] = -zoneInfo[i];
        
    //float distanceBetweenZones = getDistance(zoneInfo, enemyZone);
     
    if(Phase == DROP){
        mathVecSubtract(whereToGo,myState,zoneInfo,3);
        mathVecNormalize(whereToGo,3);
        
        for(int i = 0;i<3;i++){ whereToGo[i] *= distanceToItem; }
        
        mathVecAdd(whereToGo,zoneInfo,whereToGo,3);
        mathVecSubtract(whereAttitude,zoneInfo,myState,3);
        
        if(droppedItems == 0) kVel = 0.05f;
        
        if(getDistance(realItemState, zoneInfo) < 0.046f){ 
                game.dropItem();
                droppedItems++;
                Phase = 30;
        }
        
    }
    
    if(otherID>5){
        for(int i=0; i<6; i++){
            if(game.hasItem(i)==2)
                otherID = i;
        }
    }
    
    if(Phase==30){
        memcpy(A,whereToGo,sizeof(float)*3); //save item side in A
        
        Phase = STAY;
        float min = 5.0f;
        if(droppedItems<2){   
            for(int i=0; i<4; i++){
                float distance = getDistance(myState,allFacePOI[i]+3); 
                DEBUG(("DISTANCE %f ITEM %d",distance,i));
                if(distance<min && i!=actualID && i!=otherID){
                    min = distance;
                    idealID = i;
                }
            }
            float range = (getDistance(zoneInfo, enemyZone)>1.0f) ? 0.5f : 0.45f;
            
            if(min < range){
                Phase = DOCK;
                actualID = idealID;
                DEBUG(("PRENDO ITEM PIU VICINO"));
            }
            else{
                calcSPS[0] = 46.0f;
            }
            
        }
        
    }
    
    if(Phase==STAY){
        DEBUG(("STAY"));
        bool isWaiting = getDistance(enemyState,zoneInfo)<0.5 && enemyVel < 0.004f;
        if(isWaiting){
            DEBUG(("You can't steal at thief home."));
        }
        else{
            DEBUG(("Enemy is moving"));
        }
        
        for(int i = 0; i<4; i++){
            if( i!=actualID && i!=otherID && calcSPS[0]==46.0f ){
                if(itemType<=game.getItemType(otherID)){    
                    if((isFacingPos(enemyState,allFacePOI[i],0.3f,3) && enemyVel > 0.025f) || (getDistance(enemyState,allFacePOI[i])<0.3f&&enemyVel<0.02f)){    
                        if(getDistance(myState,enemyState)>0.45f){
                            DEBUG(("AVVERSARIO VERSO %d",i));
                            idProva = i;
                            Phase = DOCK;
                            actualID = otherID;
                        }
                    }
                }
            }
        }
        
        memcpy(whereToGo,(droppedItems>1) ? zoneInfo : A ,sizeof(float)*3);
        
        if(Time==134 && !game.itemInZone(otherID) && !isWaiting){
            Phase = DOCK;
            actualID = otherID;
        }

    }
    
    float distance = getDistance(myState, whereToGo);
	
	float velocity = (((distance*0.0775f)/(kVel))/0.56553f)/14.0f;
	if(velocity> 0.08) velocity = 0.08f;
    if(velocity>0.065f && SPSHeld==0) velocity = 0.065f; 
    if(distance>=0.47f && velocity>=0.06 && Phase==SPSPLACEMENT && !isGoingToB) velocity = 0.06f;
    
    DEBUG(("kVEL:    %f",kVel));
    DEBUG(("VEL: %f REALVEL: %f", velocity, mathVecMagnitude(myState+3,3)));
    DEBUG(("actualID: %d",actualID));
    
    float velocityVector[3];
    mathVecSubtract(velocityVector, whereToGo, myState, 3);
    mathVecNormalize(velocityVector, 3);
    for(int i=0;i<3;i++) velocityVector[i]*=velocity;
    api.setVelocityTarget(velocityVector);
    
    
    mathVecNormalize(whereAttitude, 3);
    api.setAttitudeTarget(whereAttitude);
}



//End page main

                        
                      
download
                        
                        //Begin page libs
//Begin page libs
// Multiply a vector and a scalar - norm=true -> a versor is created
void mVecMultAndNorm(float ret[3], float vinp[3], float mult, bool norm){
	memcpy(ret,vinp,sizeof(float)*3);
	if(norm) mathVecNormalize(ret,3);
	for(int i = 0;i <3;i++){
	    ret[i] *= mult;
	}
}
//
// Kinematic routines
//
//************************************************************************
float setPosAndGo(float k){
// a routine to move  towards the selected target
// dtoT = distance from Target
// if dtoT > 30 cm constant sphere speed (= 0.06 m/s)
// if dtoT <= 30 cm sepositiontarget is used
//
	float v[3],dtoT;
	dtoT=itemDist(v,Target,myState);
//  go, but slowly
    if(dtoT>0.25f){//0.35
        mVecMultAndNorm (v,v,0.055f,true);
        api.setVelocityTarget(v);
        return dtoT;
    }
//  go to a virtual more distant Target 
	mVecMultAndNorm (v,v,k,false);
	mathVecAdd (v,v,myState,3);
	api.setPositionTarget(v);
	return dtoT;
}
//************************************************************************
void moveToTarget(float kspeed){
// A smart way to Move towards the Target
// It should avoid sphere-item collision even if the item docking face is on the opposite side
//
    float v[3], versBis[3], vers1[3];
    float bendLim;
    if(Phase>0){
        mathVecSubtract(v,myState,Target,3);
        mVecMultAndNorm(vers1,v,1.0f,true);
//
        if(Phase==30)mVecMultAndNorm(&itemState[6],vers1,1.0f,false);
	    bendLim=mathVecInner(vers1,&itemState[6],3);
	    if(nItem!=oldItem){
//	        
	        if(bendLim>0.2f&&bendLim<1.0001f)Corr=dimn+0.018f;
	        else if(bendLim<-0.4f)Corr=dimn-0.018f;
	        else Corr=dimn;
//
	        oldItem=nItem;
	        veryCloseItem=false;
	    }    
//
        if(veryCloseItem)Corr=dimn-0.005f;
//
	    if(bendLim>0.7f){
            mVecMultAndNorm(v,&itemState[6],Corr,false);
	    } else {
	        mathVecAdd(versBis,vers1,&itemState[6],3);
	        mVecMultAndNorm(v,versBis,dimn+0.015f,true);
	    }
//
	mathVecAdd(Target,v,Target,3);
    alignToTarget();
    }  
//
    dist=setPosAndGo(kspeed);
}
//
//************************************************************************
void alignToTarget(){
// Alignment to the selected item
        float v[3];
	    mVecMultAndNorm(v,&itemState[6],-1.0f,true);
        angle=mathVecInner(v,&myState[6],3);
        api.setAttitudeTarget(v);
}
//*************************************************************************
float itemDist(float v[3], float *v1, float *v2){
    mathVecSubtract(v,v1,v2,3);
    return mathVecMagnitude(v,3);
}
//
//*************************************************************************
float AddAndDist(float *v, float *v1, float *v2){
    mathVecAdd(v,v1,v2,3);
    return mathVecMagnitude(v,3);
}
//
// Strategy routines
//************************************************************************
//short minDistItem(int ini, int fin){
short minDistItem(){
// The best item is selected on the basis of the Score that is 
// presumed to be obtained after docking that item.
// Items already picked up by opponent or dropped to our Ass zone are not considered.
// If an item is very close, it can be selected irrespective of other item Scores
// The choice of defending our Ass Zone is also performed if:
//      - our score is increasing more than 0.39 per cycle; AND
//      - our score > other score; 
//   OR
//      - an item has been dropped AND remaining fuel < 10 s
//
    short itemSelected,closestItem,i;
    float v[3], kCor[3]={0.162f,0.149f,0.135f};
    float myd, Score, maxScore, flItem, minDist, distSelected;
    itemSelected=6;maxScore=-100.f;Score=0.0f;
    minDist=10.0f;closestItem=9;distSelected=0.0f;
	for(i=0;i<6;i++){
// It is considered the location of items that have not already taken or that has not been picked up by the opponent
	    if(chIt[i]>=0){
            game.getItemZRState(itemState,i);
            myd=itemDist(v,itemState,myState);
            flItem=0.2f-(float)game.getItemType(i)*0.05f; 
            Score=(float)chIt[i]*flItem*(100-myd*40.f);
            if(myd<minDist){minDist=myd;closestItem=i;}
            if(Score>maxScore){maxScore=Score;distSelected=myd;itemSelected=i;}
	    }
	}
//
	if(firstChoice&&((distSelected>4.5f*minDist)||minDist<0.25f)){//4.0;0.3
	    itemSelected=closestItem;
	    if(minDist<0.25f)veryCloseItem=true;
	} else firstChoice=false;
//
	dimn=kCor[game.getItemType(itemSelected)];
//	
    return itemSelected;
}
//
//************************************************************************
void checkItems(){
//    
// checkItems: each Item is given a number which identifies its status (vector chIt[6]):
//   =  2 the Item is in the opponent Ass zone
//   =  1 the Item is available to be docked
//   =  0 the Item is protected by opponent sphere
//   = -1 the Item is not available (picked up by our/opp sphere)
//   = -2 the Item has been  dropped in our Ass Zone
//   = -3 the Item is moving
//
    bool movingItem;
    int i;
    float v1[3];
    float itemSpeed, dItem, dSphere;
	for(i=0;i<6;i++){
	    if((chIt[i]==-1&&game.hasItem(i)==0)||(chIt[i]==-2&&game.hasItem(i)==2))chIt[i]=1; // when item is released, it is available
	    if((chIt[i]>0)&&game.hasItem(i)>0)chIt[i]=-1; // as soon as anybody docks an item, it is unavailable
//
        game.getItemZRState(itemState,i);
        dItem=AddAndDist(v1, itemState, zoneInfo);
        dSphere=AddAndDist(v1, otherState, zoneInfo);
        if(dItem<0.15f||chIt[i]==0){
            if(dSphere<0.2f&&otherSpeed<0.01f) chIt[i]=0;
            else chIt[i]=2;
        }    
//
        itemSpeed=mathVecMagnitude(&itemState[3],3);
        movingItem=itemSpeed>0.001f;
        if(movingItem)chIt[i]=-3;
        if(chIt[i]==-3&&!movingItem)chIt[i]=1;
//
	}
}
//
//************************************************************************
bool checkDocking(){
    float v[3], distFromItem, farthest, closer;
//    
// checkDocking: conditions to dock an item are investigated
    distFromItem=itemDist(v,itemState,myState);
	farthest=dimn+0.011f;closer=dimn-0.011f;
    veryCloseItem=veryCloseItem||((distFromItem-farthest)>0.001f&&(distFromItem-farthest)<0.03f&&mySpeed<0.01f);
	if(mySpeed<0.01f&&game.isFacingCorrectItemSide(nItem)&&distFromItem<farthest&&distFromItem>closer){
	        return game.dockItem(nItem);
	}
	return false;
}
//
//************************************************************************
void choiceItem(){
//    
// choiceItem: the best item to be docked is selected
    nItem=minDistItem();
    game.getItemZRState(itemState,nItem);
    mVecMultAndNorm(Target,itemState,1.0f,false);
}
//
//************************************************************************
void firstTriangle(){
//    
float v[3],w[3],thirdP[3],ortho[3],SPSerror,itemD,prod,dd;
float Yerr=0.0f;
int k,ix;
//
    for(k=0;k<5;k=k+2){
        ix=k; 
        if(k==4)ix=0;
        if(iSide<0)ix++;
        game.getItemZRState(itemState,ix);
        itemD=itemDist(v,itemState,myState);
        mVecMultAndNorm (w,v,1.0f,true);
        prod=mathVecInner(w,&itemState[6],3);
        mathVecCross(ortho,w,&itemState[6]);
        mVecMultAndNorm (w,&itemState[6],0.15f,true);
        mathVecAdd(v,v,w,3);
//
        if(prod<0.0f){
            deltaY=0.24f/sqrtf(v[0]*v[0]+v[2]*v[2]);
            if(deltaY>0.7f)deltaY=0.7f;
            thirdP[0]=0.0f;thirdP[1]=iSide*deltaY;thirdP[2]=0.0f;
        } else {
            dd=0.25f/(itemD*sqrtf(1.0f-prod*prod));if(dd>0.6f)dd=0.6f;
            mVecMultAndNorm (thirdP,&itemState[6],dd,false);
            memcpy(Target,thirdP,sizeof(float)*3);            
            Target[1]=Target[1]+iSide*0.15f;
        }
//        
        mathVecCross(w,v,thirdP);
        SPSerror=0.02f/mathVecMagnitude(w,3);
        if(k==0)Yerr=SPSerror;
///DEBUG(("k = %i; itemd = %f; prod = %f; SPSerror = %f;\n",k,itemD, prod, SPSerror));
        if((k==0&&SPSerror<0.10f)||(k==2&&(SPSerror<Yerr))||k==4){
            chIt[ix]=10;
            break;
        } 
    }
//    
    if(SPSerror>0.12f){
        dd=0.3f/itemD;if(dd>0.6f)dd=0.6f;
        mVecMultAndNorm (Target,ortho,dd,true);
        prod=1.0f;
    }
//    
    if(prod<0.0f)Phase=15;
    return;
//    
}
//End page libs
//Begin page main
// SPACE-S ZR 2016 - PC ISS Phase - Final Candidate //
//
//
// date: 03/01/2017
//
// Definition of some of the main variables:
//
// BOOLEAN:
// veryCloseItem (if True the closest item is selected)
//
// INTEGERS:
// nItem: current selected item;
// oldItem: last selected item;
// Phase: current phase;
//      =  0:  placing of first two SPS
//      = 15: trying to dock the selected item
//      = 30: trying to drop a docked item to the Assembly Zone
// iSide: +1 BLU sphere - -1 RED sphere
// 
//
// FLOATING:
// dist: distance from actual target;
// dimn: mean distance to dock the selected item
// angle: cos(angle) - attitude error during items dropping 
// itemState[12]: item location, speed and attitude;
// zoneInfo[4]: location and uncertainty of the Assembly Zone;
// Target[3]: where my sphere is directed;
// distFromO: distance from opponent sphere;
//
// definition of main code Knobs
//
#define VERY_HIGH_SPEED 2.0f
#define HIGH_SPEED 1.55//1.75-1.65
//
bool veryCloseItem, firstChoice, debounce;
short chIt[6];
int iSide, nItem, Phase, oldItem;
float dist, dimn, angle, Corr, deltaY;
float mySpeed, otherSpeed;//, farthest, closer; 
float Target[3];
float zoneInfo[4];
float itemState[12];
ZRState myState;
ZRState otherState;

void init(){
    int i;
    dist=10.0f;
    veryCloseItem=false;firstChoice=false;debounce=true;
    iSide=1;Phase=0;oldItem=10;
//
    DEBUG(("P.R.O. Final Phase Candidate"));
//
    for(i=0;i<6;i++){chIt[i]=1;}
}
void loop(){
  for(int bleh=0;bleh<5;bleh++){//5
    float vet1[3], kvel;
//
// These are information needed at each time step
//
    api.getMyZRState(myState);api.getOtherZRState(otherState);
    mySpeed=mathVecMagnitude(&myState[3],3);
    otherSpeed=mathVecMagnitude(&otherState[3],3);
//
// At Time 0 the triangle is composed
//
    if(debounce){
        if(myState[1]<0.0f)iSide=-1;
        game.dropSPS();
        firstTriangle();
        debounce=false;
    }
//
/////DEBUG(("****FASE = %i\n", Phase));
// 
    checkItems();
//    
// Phase=0: A determined Target is reached
//    
    if(Phase==0&&dist<0.05f){
    		game.dropSPS();
    		if(game.getNumSPSHeld()>0)Phase=15;
    		else Phase=30;
    		dist=10.0f;
    }
//    
// Phase=15: Trying to dock the selected item. 
//           After docking the first large item , the third SPS is placed
//    
    if(Phase==15){
        choiceItem();
//
        if(checkDocking()){
      	    game.dropSPS();
      	    veryCloseItem=false;
      	    firstChoice=true;
      	    dist=10.0f;
      	    if(game.getNumSPSHeld()>0){
      	        Phase=0;
                mVecMultAndNorm(Target,myState,1.0f,false);
      	        Target[1]=Target[1]-iSide*deltaY;
      	        if(iSide*Target[2]<0.4f)Target[2]=iSide*0.4f;
      	    } else Phase=30;
        }    
    }
//
// Phase==30 The docked item is dropped to the Assembly Zone
//
    if(Phase==30){
        game.getZone(zoneInfo);
        mVecMultAndNorm (Target,zoneInfo,1.0,false);
        Corr=dimn+0.333f*mySpeed;
        if(angle>0.99f&&dist<0.025f){
           game.dropItem();
            if(game.itemInZone(nItem))chIt[nItem]=-2;
            Phase=15;
        }
    }
//
// Sphere speed is chosen according to selected Phase
//
    if(veryCloseItem)kvel=VERY_HIGH_SPEED;
    else kvel=HIGH_SPEED;
//
    moveToTarget(kvel);
//
  }
}

//End page main

                        
                      
download
                        
                        //Begin page main
// Team Singularity-AachenerNerds-Team Appreciate (2468)
// Version 1.6.8
// Date 2016.12.30
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) < (b) ? (b) : (a))
#define CPY(a, b) memcpy(a, b, 12)
#define IN3(v, c0, c1, c2) { v[0] = c0; v[1] = c1; v[2] = c2; }
#define MAG(a) mathVecMagnitude(a, 3)
#define NRM(p) mathVecNormalize(p, 3)
#define ADD(p, a, b) mathVecAdd(p, a, b, 3)
#define SUB(p, a, b) mathVecSubtract(p, a, b, 3)
#define CRS(p, a, b) mathVecCross(p, a, b)
#define DOT(a, b) mathVecInner(a, b, 3)
#define C2D(a) (acosf(a) * 57.3f)
#define R2D(a) (a * 57.3f)
#define DEBUG1(x) //DEBUG(x)
float DIR(float *out, float *a, float *b) { SUB(out, a, b); return NRM(out); }
float DST(float *a, float *b) { float c[4];	return DIR(c, a, b); }
void SKL(float *out, float *in, float f) { for (int i = 0; i < 3; i++) out[i] = in[i] * f; }
void OFS(float *p, float *a, float *b, float c) { float d[4]; SKL(d, b, c); ADD(p, a, d); }

int gfaise;
int gitemno;
int gaction;
int gspstime;
int gspscnt;
int gotguard;
int gowned;
float gmoveto[4];
float gpointo[4];
float gspsloc[4];

#define DOCKDI	0.0109f
#define DOCKDI2	0.0218f
#define ITEMSPD	0.003f
#define DROPERR 0.065f

void GetItemZRState(float itzr[12], float *ithotspot, float& itmind, int gitemno) {
	unsigned char ipickd[8] = { 151, 151, 138, 138, 124, 124, 124, 0 };
	game.getItemZRState(itzr, gitemno);
	itmind = 1.0005E-3f * ipickd[gitemno];
	OFS(ithotspot, itzr, itzr + 6, itmind + DOCKDI);
}

void GetZonne(float zone[8]) {
    gspscnt = game.getNumSPSHeld();
    if (gspscnt > 0)
		zone[0] = zone[1] = zone[2] = 0.0f;
	else
	{
		game.getZone(zone);
	}
	SKL(zone + 4, zone, -1.0f);
}

#define BRC0  100.0f
#define BRC1  130.0f
#define ACC0  0.25f
#define ACC1  0.1f

void SetMove(float myzr[12], float *tgt, float sRange, float tgtKe)
{
	float dirTo[4], *velo = myzr + 3;
	float dist = DIR(dirTo, tgt, myzr);
	float vAtt = DOT(velo, dirTo);
	float bdist = (gfaise == 40 ? BRC1 : BRC0) * vAtt * vAtt - tgtKe + (gaction == 35 ? 0.1f : 0.0f);
	bool accel = (dist > sRange && bdist < dist);
	OFS(gmoveto, tgt, dirTo, accel ? dist * ACC0 + ACC1 : tgtKe);
	gaction = accel ? 3 : 35;
	OFS(dirTo, velo, dirTo, -vAtt);
	dist = MAG(dirTo);
	if (dist > 0.001f)
		OFS(gmoveto, gmoveto, dirTo, dist < 0.02f ? -0.04f / dist : -2.0f);
}

void init() {
	gfaise = 0;
	gspstime = 180;
	gowned = 0;
	gotguard = 0;
}

void loop() {
	float myzr[12];
	api.getMyZRState(myzr);
	float otzr[12];
	api.getOtherZRState(otzr);
    float zone[8];
	float itzr[12];
	float ithotspot[4];
	float itmind = 0.0f;
	float fuel = game.getFuelRemaining();
	float approach[4], itemTo[4], v[4];
	bool outscore = game.getScore() > game.getOtherScore();
	int time = api.getTime();
	if (time == 0) {
		DEBUG(("--Alliance SANTA (ISS Final 2.6)--"));
		game.dropSPS();
		CPY(gspsloc, myzr);
		gaction = 1;
	}
	GetZonne(zone);
	if (time == 0 || gfaise == 10) {
		float doz = DIR(ithotspot, zone, otzr);
		float dmz = DST(zone, myzr);
		float otv = MAG(otzr + 3);
		float otvang = otv > 0.01f ? DOT(otzr + 3, ithotspot) / otv : -0.7f;
		char iItemv[8] = { 4, 4, 3, 3, 2, 2, 2, 0 };
		float eval, best = 0.0f;
		gitemno = -1;
		for (int i = 0; i < 6; i++) {
			if (game.itemInZone(i)) continue;
			GetItemZRState(itzr, ithotspot, itmind, i);
			int hasItem = game.hasItem(i);
			if (hasItem == 2) {
				if (i > 3 || MAG(zone) < 0.35f || gowned == 0) continue;
			}
			else {
				if (MAG(itzr + 3) > ITEMSPD) continue;
			}
        	float d1 = DIR(v, ithotspot, myzr);
        	float cangle = DOT(v, myzr);
        	if (time == 0) {
        	    if (i > 3 || cangle < -0.13f || d1 < 0.45f) continue;
        	}
        	float otime = 180.0f - time;
		    float value = 0.05f * iItemv[i];
        	float cosatt = DOT(v, itzr + 6);
        	float pktime = 20.0f * sqrtf(d1) + (cosatt > 0.24f ? 5.0f : 0.0f);
        	if (pktime < 6.0f && dmz < 0.2f)
        	    eval = 30.0f;
        	else {
            	float fuelEst1 = 10.0f * d1;
            	eval = fuel > 2.0f * fuelEst1 ? (otime - 2.0f * pktime) * value : 0.0f;
            	if (eval < 0.0f) eval = 0.0f;
            	if (game.hasItemBeenPickedUp(i)) {
        			float dooz = DST(zone + 4, otzr);
            		gotguard = (hasItem == 0 && dooz < 0.2f) ? gotguard + 1 : 0;
            		DEBUG1(("..ch: %d=%.2f d=%.2f dg=%.2f %s", i, value, d1, dooz, gotguard == 0 ? "left" : "guarded"));
            		if (gotguard > 12) continue;
        		    if (fuel > fuelEst1) eval += (otime - pktime) * value;
            	}
            	DEBUG1(("..ch: %d=%.2f, %d d=%.2f #mv=%.0f #fu=%.0f fe=%.1f ev=%.2f", i, value, game.hasItemBeenPickedUp(i), d1, pktime*2.0f, fuelEst1*2.0f, fuel, eval));
        	}
			if (eval > best) {
				best = eval;
				gitemno = i;
			}
		}
		if (time == 0 && gitemno < 0) { gitemno = myzr[1] < 0.0f ? 0 : 1; }
		if (gitemno < 0) gfaise = 60;
		if (gowned & 1) {
			if (dmz + 80.0f * DOT(myzr + 3, myzr + 3) < 0.35f)
				if ((otv > 0.03f && otvang > 0.866f) || (doz < 0.75f && otvang > -0.5f) || doz < 0.5f)
				    if (outscore || time < 140)
				        gfaise = 60;
			DEBUG1(("defense? ph=%d it=%d d=%.2f dg=%.2f v=%.3f vg=%.3f vang=%.0f", gfaise, gitemno, dmz, doz, MAG(myzr + 3), otv, C2D(otvang)));
		}
	}
	GetItemZRState(itzr, ithotspot, itmind, gitemno);
	float *tgt = gfaise == 40 ? zone : ithotspot;
	float *att = itzr + 6;
	float trdist = DIR(approach, tgt, myzr);
	if (time == 0) {
		float base = (trdist < 0.35f ? 0.35f : trdist);
		CRS(itemTo, att, approach);
		NRM(itemTo);
		CRS(v, approach, itemTo);
		SKL(v, v, 0.966f);
		OFS(itemTo, v, approach, -0.259f);
		gspsloc[3] = 0.22f / base;
		OFS(gmoveto, ithotspot, itemTo, 0.8f);
		DEBUG1(("Triangle: it=%d d=%.3f l=%.3f dir(%.1f,%.1f,%.1f)", gitemno, base, gspsloc[3], itemTo[0], itemTo[1], itemTo[2]));
		gfaise = 2;
	}
	if (gfaise == 2 && DST(myzr, gspsloc) > gspsloc[3]) {
		gspstime = time + 5;
		gfaise = 20;
	}
	if (gspscnt == 2 && time >= gspstime) {
		game.dropSPS();
	}

	float trofs = DIR(itemTo, itzr, myzr);
	float trcosAtt = DOT(itemTo, myzr + 6);
	//float trgap = DIR(itemTo, tgt, itzr);
	float trspeed = MAG(myzr + 3);

	if (gfaise == 10 || gfaise == 20) {
		int hasItem = game.hasItem(gitemno);
		bool bdocked = true;
		bool bsteal = game.hasItemBeenPickedUp(gitemno);
		if (hasItem != 1) {
			if (hasItem == 2 || MAG(itzr + 3) > ITEMSPD) {
				gfaise = 10;
				bdocked = false;
			}
			else {
				if (trdist < 0.1f) gfaise = 20;
				bdocked = (trofs > itmind + DOCKDI2 || trofs < itmind || trcosAtt < 0.969f || trspeed > 0.0099f) ? false :
					(game.isFacingCorrectItemSide(gitemno) && game.dockItem(gitemno));
			}
		}
		if (bdocked) {
			gaction = 1;
			gfaise = (bsteal && (gitemno > 1 || fuel < 18.0f) && outscore) ? 70 : 40;
			DEBUG1(("P%d %d d=%.3f v=%.3f att=%.0f (it=%d fu=%.0f)", gfaise, gaction, trofs, trspeed, C2D(trcosAtt), gitemno, fuel));
		}
		else {
			SKL(gpointo, att, -1.0f);
			float r = itmind + DOCKDI;
			DIR(itemTo, myzr, itzr);
			float cosA = DOT(att, itemTo);
			if (cosA < 0.971f && DOT(approach, att) > 0.24f && trofs > 0.12f) {
				OFS(v, itemTo, att, -cosA);
				NRM(v);
				float x = trofs * cosA;
				float y = trofs * DOT(v, itemTo);
				bool backPt = y < r && x < 0.0f;
				OFS(itemTo, itzr, v, backPt ? r : min(r, y));
				OFS(itemTo, itemTo, att, backPt ? max(-r, x) : r);
				SetMove(myzr, itemTo, 0.015f, backPt ? 0.16f : 0.022f);
			}
			else {
				SetMove(myzr, ithotspot, 0.05f, 0.0f);
			}
		}
	}

	if (gspscnt == 1 && (gfaise == 40 || time == 45)) {
		game.dropSPS();
		GetZonne(zone);
		DEBUG(("Zone=(%.2f,%.2f,%.2f) L=%.2f", zone[0], zone[1], zone[2], MAG(zone)));
	}
	
	if (gfaise == 40) {
		float trgap = DIR(itemTo, zone, itzr);
		trdist = DIR(approach, zone, myzr);
		DEBUG1(("P%d %d gap=%.3f d=%.3f v=%.3f vang=%.3f att=%.0f rot=%.0f", gfaise, gaction, trgap, trdist, trspeed, C2D((trspeed > 0.001f) ? DOT(approach, myzr + 3) / trspeed : 1.0f), C2D(trcosAtt), R2D(MAG(myzr + 9))));
		if (trgap <= DROPERR) {
			game.dropItem();
			DEBUG(("Drop %d in zone (%d): gap = %.3f / %.3f", gitemno, game.itemInZone(gitemno), trgap, DROPERR));
			if (game.itemInZone(gitemno)) gowned |= (gitemno < 2) ? 1 : 2;
			gfaise = 10;
			return;
		}
		bool bclose = trdist < 0.05f;
		SKL(gpointo, bclose ? itemTo : approach, bclose ? -1.0f : 1.0f);
		OFS(ithotspot, zone, gpointo, -trofs);
		SetMove(myzr, ithotspot, 0.25f, 0.09f);
	}

	if (gfaise == 70 || gfaise == 60) {
		bool bguard = (gfaise == 60);
		float *ozone = bguard ? zone : zone + 4;
		ADD(otzr, otzr, otzr + 3);
		DIR(gpointo, otzr, ozone);
		OFS(gmoveto, ozone, gpointo, bguard ? 0.1f : 0.2f);
		gaction = bguard ? 1 : 3;
		DEBUG1(("P%d d=%.2f dg=%.2f %s", gfaise, DST(myzr, ozone), DST(otzr, ozone), bguard ? "guard" : "block"));
		if (bguard) gfaise = 10;
	}

	if (gaction & 1) api.setPositionTarget(gmoveto);
	if (gaction & 2) api.setAttitudeTarget(gpointo);
}

//End page main

                        
                      
download
                        
                        //Begin page main
//#define DECISION_MESSAGES
#define DEBUG_MESSAGES
//#define ZONE_MESSAGES
//#define MOV_DEBUG_MESSAGES
//#define MOV_DEBUG_MESSAGES2

#define  tol  0.05f
#define  MIN_ANGLE  0.79f
#define  MINIM_FUEL    10.0f
#define  VEC_SIZE    12
#define  DROP_POINT   0.5f
#define  E  0.21f
#define BLUE    0
#define RED     1

#define  INITIAL_SPS         0
#define  SECOND_SPS          4
#define  NEAR_ITEM           8
#define  GRAB_ITEM           12
#define  ITEM_SELECTION      16
#define  MY_ZONE             20
#define  DROP_ITEM           24
#define  NEAR_ENEMY_ZONE     28
#define  GET_ENEMY_ITEMS     32
#define  GUARD_ZONE          36
#define  GRAB_DOCKING        40
#define  GRAB_SPECIAL        44


//Declare any variables shared between functions here
short               TIME, step, color, id0, id01, id23, id05, ide, ids, set_attitude, t0, t1, t2, t3;

float               destination[3], destination2[3], zone[4], *cpos, *cvel, *catt,
                    *opos, *ovel, velocity, fuel,
                    State[12], oState[12], itemState[12],
                    desired_attitude[3], distance_from_destination,
                    desired_velocity, heading[3];


void init(){
	//This function is called once when your code is first loaded.
	//IMPORTANT: make sure to set any variables that need an initial value.
	//Do not assume variables will be set to 0 automatically!
	step = 0;
	//zone[0]=0; zone[1]=0; zone[2]=0; zone[3]=0;
	set_attitude = 0;
	DEBUG(("Z-G-N: Version Alliance-B06"));
    desired_velocity = 0.068f;
    t1 = 0;
    t2 = 0;
    t3 = 0;
    ids = 10;
}



void loop(){
	//This function is called once per second.  Use it to control the satellite.
	
	float           r, flocal, flocal2, fvector[3], axes[3], angle, error1, error2, 
	                oponent_distance_from_my_zone, oponent_distance_from_other_zone,
	                d01,d23,d05;
	short           step_Guard_Or_Take;
	
	TIME = game.getCurrentTime();
	//Read state of opponent's Sphere
	api.getOtherZRState(oState);
	opos = oState;          //opponent's position
	ovel = &oState[3];      //oponent's velocity
	//oatt = &oState[6];      //opponent's attitude
	//ovelocity = mathVecMagnitude(ovel,3);
	
	//Read state of our Sphere
	api.getMyZRState(State);
	//copy information to vectors
	cpos = State;               //our position
	cvel = &State[3];           //our velocity
	catt = &State[6];           //attitude (the direction that our camera faces)
	//crot = &State[9];           //rotation vector
	/*if (TIME<2) //Red: 1  Blue:  0
	    color = cpos[1]>0  ?  BLUE  :  RED;
	*/
	//rot_vel = mathVecMagnitude(crot,3);             //rotational velocity
	velocity = mathVecMagnitude(cvel,3);            //velocity
	//fuel = game.getFuelRemaining();                  //fuel 
	
    
    
	
    //Messages (printed only if DEBUG_MESSAGES is defined)
	#ifdef DEBUG_MESSAGES
	DEBUG(("TIME: %d, Step: %d.\n", TIME, step));
	#endif

    
    #ifdef DEBUG_MESSAGES
    DEBUG(("T0 = %d, T1 = %d, T2 = %d, T3 = %d", t0, t1, t2, t3)); 
    #endif
    r = 0;
    
    
    game.getZone(zone);
    //DEBUG(("ZONE: %f %f %f %f", zone[0], zone[1], zone[2], zone[3]));
    
    mathVecSubtract(fvector,zone,opos,3);
    oponent_distance_from_my_zone = mathVecMagnitude(fvector,3);
    t1 = (oponent_distance_from_my_zone<0.48f) ? t1+1 : t1>>1;
    //DEBUG(("Oponent distance from zone %f", oponent_distance_from_my_zone));
      
    mathVecAdd(fvector,zone,opos,3);
    oponent_distance_from_other_zone = mathVecMagnitude(fvector,3);
    t2 = (oponent_distance_from_other_zone<0.27f) ? t2+1 : t2>>1;  
    
    //t3 = EnemyIsHeadingTowardsOurZone() ? t3+1 : 0;
    if (game.getNumSPSHeld()==0)
        if (EnemyIsHeadingTowardsOurZone())
            t3+=2;
        else
            t3>>=1;
    
    id01 = ClosestItem2(&d01,0, 1);
    id23 = ClosestItem2(&d23,2, 3); 
    id05 = ClosestItem2(&d05,0, 5);
    ide = EnemyZoneHasItems();
    
    step_Guard_Or_Take = (ids>>1)<=(ide>>1) ? GUARD_ZONE : NEAR_ENEMY_ZONE;
	        
    //-----------------------------------------------------------------
	//Strategic Steps (step is initialized at 0)
	switch(step){
	    
	    case INITIAL_SPS:
	        game.dropSPS();
            
            if (d01>0.35f){
                id0 = id01;
                //DEBUG(("YELLOW - equilateral"));
            }
            else
	            id0 = (d01>=d23) ? id01 : id23;
	            
	        DEBUG(("Large: %f, Pink %f", d01, d23));
	        //-------------------------------------
	        game.getItemZRState(itemState, id0);
            LinearComb(fvector, (float *) &itemState, (float *) &itemState[6], 1.0, 0.16f);
            mathVecSubtract(fvector,fvector,cpos,3);
	        flocal = mathVecMagnitude(fvector,3);
	        flocal2 = flocal*flocal;
	        LinearComb(axes, (float *) &itemState[6], fvector, 1.0f, 
	            -mathVecInner(fvector,(float *) &itemState[6],3)/(flocal2));
	        mathVecNormalize(axes,3);
	        flocal2 = 0.26f/flocal;
	        LinearComb(destination, fvector, axes, 0.5f, flocal2);
	        mathVecAdd(destination, destination, cpos, 3);
	        CutOffCoordinates(destination);
	        step = SECOND_SPS;
	    break;
	    
	    
	    
	    case SECOND_SPS:
            LinearComb(desired_attitude, (float*)&itemState[6], (float*)&itemState[6], 0, -1.0f);
            set_attitude = 2;
	        if (distance_from_destination<=tol){
	            game.dropSPS();
	            step=GRAB_ITEM;
	            goto STEP_GRAB_ITEM;
	        }
	    break;
	    
	    
	    
        STEP_GRAB_ITEM:
        case GRAB_ITEM: // Correct Large Item Selection
            game.getItemZRState(itemState, id0);
            if(mathVecMagnitude((float*)&itemState[3],3)==0){
                #ifdef DEBUG_MESSAGES
    	        DEBUG(("Item Picked %d.\n", id0));
    	        #endif
                flocal = (id0<2) ?  0.160f : (id0<5) ? 0.146f : 0.134f;
                //Add attitude
                LinearComb(destination, (float *) &itemState, (float*) &itemState[6], 1.0f, flocal);
                LinearComb(desired_attitude, (float*)&itemState[6], (float*)&itemState[6], 0, -1.0f);
                set_attitude = 2;
                if (CheckDockingConditions(id0)){
                    step = MY_ZONE;
                    game.dropSPS();
                    //goto STEP_MY_ZONE;
                }
                
                //curved
                mathVecSubtract(fvector,cpos,(float*)&itemState,3);
                mathVecNormalize(fvector,3);
	            flocal = mathVecInner(fvector,(float*)&itemState[6],3);
    	        if ((TIME>50)&(flocal<0.25f)){//possible colision with item
                    mathVecAdd(fvector,fvector,&itemState[6],3);
                    LinearComb(destination, (float*)&itemState, fvector, 1.0f, 0.22f);
    	            DEBUG(("Curved movement"));
    	        }
            }
            if (game.hasItem(id0)>1){//lost item
                //DEBUG(("LOST it"));
                id0 = (id0<2) ? id23 : id01;
            }
        break;

            
        STEP_MY_ZONE:    
        case MY_ZONE: //Set heading to my zone
            t0 = 0;
            set_attitude = 1;
            memcpy((void *)destination, (void*)zone, VEC_SIZE);
            step = DROP_ITEM;
        break;
            
            
        case DROP_ITEM: //Drop Item Inside Zone
            //set up destination
            r = 0.10f;
            //dropping conditions
            if(SmallInsideMyZone()){
                game.dropItem();
                DEBUG(("DROP the ITEM ---"));
                if ((game.itemInZone(id0))&(id0<ids))
                    ids = id0;
                step = GUARD_ZONE;
            }
            else{
                if ((t1>45)&(t0>15)){
                    game.dropItem();
                    step = GRAB_SPECIAL;
                    //goto STEP_GRAB_SPECIAL;
                }
            }
                
        break;
        

        
        STEP_NEAR_ENEMY_ITEM:
        case NEAR_ENEMY_ZONE: //Set heading to enemy's Zone
            set_attitude = 1;
            LinearComb(destination, zone, zone, 0, -1.0f);
            step = GET_ENEMY_ITEMS;
        break;
            
            

        case GET_ENEMY_ITEMS: //Get opponent items
            r = 0.4f;
            if ((distance_from_destination<=0.3f)&(oponent_distance_from_other_zone>0.18f)){
                //If enemy zone contains items
                //set_attitude = 0;
                id0=ide;
                if (id0<10){
                    step = GRAB_ITEM;
                    #ifdef DEBUG_MESSAGES
	                DEBUG(("Item Picked %d.\n", id0));
	                #endif
                    //goto STEP_GRAB_ITEM;
                }
            }
            else{
                if (t2>25){
                    step = GRAB_SPECIAL;
                }
            }
        break;
        
        
        STEP_GUARD_ZONE:
        case GUARD_ZONE:
            game.getItemZRState(itemState, ids);
            //Add attitude
            LinearComb(destination, (float *) &itemState, (float*) &itemState[6], 1.0f, 0.16f);
            //memcpy((void *)destination, (void*)zone, VEC_SIZE);
            r=0;
            set_attitude = 0;
            DEBUG(("-------- Closest item %d, : %f", id05, d05));
            if (t3==0){
                if ((d05<0.3f)&(t1==0)){//0.3f
                    step = GRAB_ITEM;
                    id0 = id05;
                }
                else
                    step = step_Guard_Or_Take;
                if (t1==0){
                    if (TIME>105)
                        step = GRAB_SPECIAL;
                    else if (t2==0){
                        step = NEAR_ENEMY_ZONE;
                    }
                }
            }
        break;
        
        
        STEP_GRAB_SPECIAL:
        case GRAB_SPECIAL:
            id0 = ClosestItem2(&flocal, 7, 8);
            game.getItemLoc(destination, id0);
            if (game.hasAdapter()){
                game.getItemLoc(destination, 6);
                set_attitude = 1;
                r = 0.134f;
                //DEBUG(("Distance = %f", distance_from_destination));
                if (CheckDockingConditions(6))
                    step = MY_ZONE;
            }
            else if (t3>6)
                step = step_Guard_Or_Take;
        break;
        
	}
	

	
    //--------------------------------------------------------
	//moving stuff
    mathVecSubtract(heading, destination, cpos, 3);
    mathVecNormalize(heading, 3);
    
    LinearComb(axes, destination,  heading, 1.0f, -r );
    
    mathVecSubtract(fvector,axes,cpos,3);
    distance_from_destination = mathVecNormalize(fvector,3);
    

    flocal = min(desired_velocity, 0.15f*distance_from_destination);
    //flocal = max(0.007f, flocal);
    if (flocal<0.007f)
        flocal = 0.007f;
    if (distance_from_destination<0.006f)
        flocal = 0.001f;
    
    //vrescale(fvector,flocal);
    LinearComb(fvector, fvector, fvector, 0, flocal);
    
    //wrong_heading = mathVecInner(cvel,heading,3)/velocity < 0.98f ? true : false;
    #ifdef MOV_DEBUG_MESSAGES
    DEBUG(("Velocity = %f, desired velocity = %f", velocity, flocal));
    #endif
    
    //DEBUG(("Distance = %f. Velocity = %f", distance_from_destination, velocity));
    if ((distance_from_destination<=0.040f)&(velocity>=0.01f)){
        LinearComb(fvector, cvel, cvel, 0, -5.0f);
        api.setForces(fvector);
        DEBUG(("BRAKES"));
    }
    else{
        api.setVelocityTarget(fvector);
    }
 
 
    #ifdef MOV_DEBUG_MESSAGES
    DEBUG(("GOING TO %f  %f  %f dist = %f", destination2[0], destination2[1], destination2[2], distance_from_destination));
    #endif
	//---------------------------------------------------------------------
    //Attitude Code
    switch(set_attitude){
        case 1:
            api.setAttitudeTarget(heading);
        break;
        case 2:
            api.setAttitudeTarget(desired_attitude);
        break;
    }
    
}




//------------------------------------------------------
//rescales a vector a --> l*a
void    vrescale(float *vector, float local)
{
    vector[0] *=local;
    vector[1] *=local;
    vector[2] *=local;
}


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


bool  EnemyCarriesItem(){
    short i;
    for (i=0;i<=7;i++){
        if (game.hasItem(i)==2)
            return true;
    }
    return false;
}


bool EnemyIsHeadingTowardsOurZone(){
    float temp[3], local;
    
    mathVecSubtract(temp, zone, opos, 3);
    mathVecNormalize(temp,3);
    local = mathVecMagnitude(ovel,3);
    DEBUG(("INNer Product : %f", mathVecInner(temp,ovel,3)/local));
    if ((mathVecInner(temp,ovel,3)/local>0.91f)&(local>0.010f))
        return true;
    else
        return false;
}

bool InsideMyZone(){
    float   fvector[3], flocal;
    
    LinearComb(fvector, cpos, catt, 1.0f, 0.10f);
    mathVecSubtract(fvector,zone,fvector,3);
    flocal = mathVecMagnitude(fvector,3);
    if (mathVecInner(catt,heading,3)>0.96f)
        if (flocal<=0.08f){
            LinearComb(fvector, cpos, catt, 1.0f, 0.17f);
            mathVecSubtract(fvector,zone,fvector,3);
            flocal = mathVecMagnitude(fvector,3);
            if(flocal<=0.08f){
                return true;
            }
        }
    return false;
}


bool SmallInsideMyZone(){
    float   fvector[3], flocal;
    
    LinearComb(fvector, cpos, catt, 1.0f, 0.13f);
    mathVecSubtract(fvector,zone,fvector,3);
    flocal = mathVecMagnitude(fvector,3);
    if (flocal<0.4f)
        t0++;
    if (flocal<=0.05f)
       return true;
    return false;
}



short EnemyZoneHasItems()
{
    short   i;
    float   temp[3];
    
    for (i=0;i<=5;i++){
        game.getItemLoc(temp, i);
        mathVecAdd(temp,temp,zone,3);
        if ((mathVecMagnitude(temp,3)<0.17f)&(game.hasItemBeenPickedUp(i))&(game.hasItem(i)==0))
            return i;
    }
    return 10;
}


bool    CheckDockingConditions(short id){
    if ((distance_from_destination<0.015f)){
        if (game.isFacingCorrectItemSide(id) ){
            if (game.dockItem(id))
                return  true;  
        }
    }
    return false;
}

short    ClosestItem(short id1, short id2){
    float   temp[3], dmin=10.0f, d;
    short   i, imin=-1;
    
    for (i=id1;i<=id2;i++){
        game.getItemLoc(temp, i);
        if ((game.hasItem(i)==0)&((!game.itemInZone(i))|(!game.hasItemBeenPickedUp(i)))){
            //DEBUG(("Item = %d", i));
            mathVecSubtract(temp, cpos, temp, 3);
            d = mathVecMagnitude(temp,3);
            if (d<dmin){
                dmin = d;
                imin = i;
            }
        }
    }
	return imin;
}


short    ClosestItem2(float *dmin, short id1, short id2){
    float   tempState[12], d;
    short   i, imin=-1;
    
    *dmin=10.0f;
    for (i=id1;i<=id2;i++){
        game.getItemZRState(tempState, i);
        if ((game.hasItem(i)==0)&(mathVecMagnitude((float*)&tempState[3],3)==0)&((!game.itemInZone(i))|(!game.hasItemBeenPickedUp(i)))){
            //DEBUG(("Item = %d", i));
            LinearComb(tempState, (float *)&tempState, (float *)&tempState[6], 1.0, 0.15f);
            mathVecSubtract(tempState, cpos, tempState, 3);
            d = mathVecMagnitude(tempState,3);
            if (d<*dmin){
                *dmin = d;
                imin = i;
            }
        }
    }
	return imin;
}




void   LinearComb(float *r, float  *a, float *b,  float x, float y){
    short i;
    
    for(i=0;i<3;i++)
        r[i] = a[i]*x + b[i]*y;
}


float   max(float a, float b){
    return a>b ? a : b;
}

float   min(float a, float b){
    return a<b ? a : b;
}

float sign(float a){
    return a>=0 ? 1.0f : -1.0f;
}


void   Large_Rotate(float *new_vector, float *vector, float *axes, float angle){
    float   a,b,c, R[3][3];
    short   i;
    
    a = sinf(angle);
    b = cosf(angle);
    c = 1.0f - b;
    
    //Rodrigues formula for rotation
    R[0][0] = b + axes[0]*axes[0]*c;
    R[0][1] = axes[0]*axes[1]*c - axes[2]*a;
    R[0][2] = axes[0]*axes[2]*c + axes[1]*a;
    
    R[1][0] = axes[1]*axes[0]*c+axes[2]*a;
    R[1][1] = b + axes[1]*axes[1]*c;
    R[1][2] = axes[1]*axes[2]*c - axes[0]*a;
    
    R[2][0] = axes[2]*axes[0]*c - axes[1]*a;
    R[2][1] = axes[2]*axes[1]*c + axes[0]*a;
    R[2][2] = b + axes[2]*axes[2]*c;
    
    //mathMatVecMult( new_vector, (float*) &R, vector, 3, 3 );
    new_vector[0] = R[0][0]*vector[0] + R[0][1]*vector[1] +R[0][2]*vector[2];
    new_vector[1] = R[1][0]*vector[0] + R[1][1]*vector[1] +R[1][2]*vector[2];
    new_vector[2] = R[2][0]*vector[0] + R[2][1]*vector[1] +R[2][2]*vector[2];
    
    new_vector[1] = (color==BLUE) ? new_vector[1]+0.15f : new_vector[1]-0.15f;
}



void   Rotate(float *new_vector, float *vector, float *axes, float angle){
    float   a,b,c;
    short   i;
    
    a = sinf(angle);
    b = cosf(angle);
    c = 1.0f - b;
    
    //Rodrigues formula for rotation
    
    
    //mathMatVecMult( new_vector, (float*) &R, vector, 3, 3 );
    new_vector[0] = (b + axes[0]*axes[0]*c)*vector[0] + (axes[0]*axes[1]*c - axes[2]*a)*vector[1] + (axes[0]*axes[2]*c + axes[1]*a)*vector[2];
    new_vector[1] = (axes[1]*axes[0]*c+axes[2]*a)*vector[0] + (b + axes[1]*axes[1]*c)*vector[1] +(axes[1]*axes[2]*c - axes[0]*a)*vector[2];
    new_vector[2] = (axes[2]*axes[0]*c - axes[1]*a)*vector[0] + (axes[2]*axes[1]*c + axes[0]*a)*vector[1] + (b + axes[2]*axes[2]*c)*vector[2];
    
    mathVecAdd(new_vector, new_vector, cpos, 3);
}


void   Rotate2(float *new_vector, float *vector, float *axes, float angle){
    float   U[9], R[9], temp;
    short   i;
    
    
    U[0] = 0;
    U[1] = -axes[2];
    U[2] = axes[1];
    U[3] = axes[2];
    U[4] = 0;
    U[5] = -axes[0];
    U[6] = -axes[1];
    U[7] = axes[0];
    U[8] = 0;
    temp = sinf(angle);
    for(i=0;i<9;i++)
        U[i]*=temp;
    
    for(i=0;i<9;i++)
        R[i] = 0;
    temp = cosf(angle);
    R[0] = temp;
    R[4] = temp;
    R[8] = temp;
    mathMatAdd(R, R, U, 3, 3);
    
    temp = 1-temp;
    mathMatMatTransposeMult(U, axes, axes, 3, 1, 3 );
    for(i=0;i<9;i++)
        U[i]*=temp;
    
    mathMatAdd(R, R, U, 3, 3);
    mathMatVecMult( new_vector, R, vector, 3, 3);
    mathVecAdd(new_vector, new_vector, cpos,3);
}



void    CutOffCoordinates(float *dest){
    
    if (dest[1]>0.78f)
        dest[1] = 0.78f;
    if (dest[1]<-0.78f)
        dest[1] = -0.78f;
    if (dest[0]>0.62f)
        dest[0] = 0.62f;
    if (dest[0]<-0.62f)
        dest[0] = -0.62f;
    if (dest[2]>0.62f)
        dest[2] = 0.62f;
    if (dest[2]<-0.62f)
        dest[2] = -0.62f;
}

//End page main

                        
                      
download
                        
                        //Begin page _UTIL
#define SQUARE(x) ((x) * (x))
#define RISK .075f

float distance(float p1[], float p2[]){
    
    mathVecSubtract(temp, p1, p2, 3);
    return mathVecMagnitude(temp, 3);
    
}

void scale(float scaleFactor, float vec[3]){
    
    vec[0] *= scaleFactor;
    vec[1] *= scaleFactor;
    vec[2] *= scaleFactor;
    
}

void copy(float target[], float src[]){
    
    memcpy(target, src, 3 * sizeof(float));
    
}

float angleBtwn(float d1[], float d2[]){ //warning, returns cosine, not the angle.
    
    return mathVecInner(d1, d2, 3) / (mathVecMagnitude(d1, 3) * mathVecMagnitude(d2, 3));

}

void adjustZone(float from[], float dist){

    scale(1.0f - dist / distance(zoneInfo, from), temp);
    mathVecAdd(target, from, temp, 3);
    
}


void dockTarget(){
    
    game.getItemZRState(item, targetID);
    tmp = .162f - (targetID / 2) * .0135f - 0.001f;
    scale(tmp, item + 6);
    mathVecAdd(target, item, item + 6, 3);
    copy(attTarget, item + 6);
    scale(-1.0f, attTarget);
    
}

bool isWinning(){
    return game.getScore() > game.getOtherScore();
}


char findNewTarget(char cap){
    
    char champ = -3;
    tmp = 99.0f;

    
    for (n = 0; n < cap; n++){
        
        if (!game.itemInZone(n) && n != targetID) { //
        
            float worth = 1.0f;
            
            if (game.hasItemBeenPickedUp(n)){

                copy(temp, zoneInfo);
                worth -= .2f;
                
            }
            else {
                
                game.getItemZRState(item2, n);
                mathVecSubtract(temp, item2, me, 3);
                
            }
            
            if (mathVecMagnitude(temp, 3) > .25f)
                worth += 2.0f * mathVecMagnitude(temp, 3);
            
            worth += (n/2) * .45f;

            if (worth < tmp) {
                champ = n;
                tmp = worth;
            }
        }
    }
    
    return champ;
    
}

//End page _UTIL
//Begin page main
//ISS Finale


#define LIMIT .06f

#define SPS 0
#define DOCK 2
#define TOZONE 3
#define GUARD 4
#define BLOCK 5

/* GLOBALS --------------- */

float me[9];
float other[6]; //6 > 3
float item[9], item2[9]; //9 is the sweet spot...
//, item2[3];

/* Where to go/point at */
char targetID;
float target[3];

float attTarget[3];

float zoneInfo[4]; 

float sps2[3];
float sps3[3];

/* Temp/Counter */
float tmp;
float temp[3];
char n;

float itemDist;
char guardCtr;

/* State Vars */
char movestate;


void init(){

    game.dropSPS();
    
    api.getMyZRState(me);

    targetID = findNewTarget(2);
    dockTarget();
    
    
    tmp = distance(target, me);

    if (tmp > .49f){
    
        movestate = SPS;
        
        scale(mathVecInner(temp, item + 6, 3) / (tmp * tmp), temp);
        mathVecSubtract(temp, item + 6, temp, 3);
        mathVecNormalize(temp, 3);
        scale(.31f / tmp, temp); 
        mathVecAdd(sps2, me, target, 3);
        scale(0.6f, sps2);
        mathVecAdd(target, temp, sps2, 3);

    }
    
    else {
        scale(0.55f / tmp, temp);
        sps3[0] = -1.0f * temp[1];
        sps3[1] = temp[0] - temp[2];
        sps3[2] = temp[1];
    
        mathVecNormalize(sps3, 3);
        scale(0.61f, sps3);
        
        mathVecAdd(sps2, me, temp, 3);
        mathVecAdd(temp, sps2, me, 3);
        scale(0.5f, temp);
        mathVecAdd(sps3, sps3, temp, 3);

        movestate = DOCK;
    
    }
   
    DEBUG(("To the stars! - ProgNaughtical Finale"));

}

void loop(){
	
//UPDATE GAME VARIABLES
	api.getMyZRState(me);
    api.getOtherZRState(other);
    game.getItemZRState(item, targetID);

//MOVE TO START	
	tmp = distance(target, me);

    if (tmp < SQUARE(mathVecMagnitude(me + 3, 3))/ .01315f + RISK ||
        angleBtwn(me + 3, temp) < .975f || 
        (movestate == DOCK &&
        fabsf(angleBtwn(temp, item+6) - .075f) < .075f)
        ){
        api.setPositionTarget(target);
    }
    
    else{
        scale(LIMIT / tmp, temp);
        api.setVelocityTarget(temp);
    }
//MOVE TO END
	
	api.setAttitudeTarget(attTarget);
    
    switch (movestate){
        
        case SPS:

            if (distance(me, target) < 0.135f){
                
                game.dropSPS();
                
                if (game.hasItem(targetID) == 1)
                    if (game.getNumSPSHeld())
                        copy(target, sps3);
                    else
                        movestate = TOZONE;
                else
                    movestate = DOCK;
                    
            }
        
        break;
        
        case DOCK:
        
            if (mathVecMagnitude(item + 3, 3) < 0.0075f) //good to grab
                dockTarget();
            
            if (game.hasItemBeenPickedUp(targetID)){
                    if (guardCtr < 5 && distance(other, item) < .15f && mathVecMagnitude(other + 3, 3) < .02f){
                        guardCtr += 1;
                    }
                    else {
                        if (guardCtr >= 5){
                            targetID = findNewTarget(6);
                        }
                        guardCtr = 0;
                    }
                }
            
            if(fabsf((itemDist = distance(item, me)) - (.162 - (targetID/2) * .0135f)) < 0.0105f
            && game.isFacingCorrectItemSide(targetID)
            && mathVecMagnitude(me + 3, 3) < 0.01f
            && game.dockItem(targetID)){
                
                if (game.getNumSPSHeld() == 2){
                    movestate = SPS;
                    copy(target, sps2);
                }
                
                else {
                    
                    game.dropSPS();
                    
                    scale(-1.0f, zoneInfo);
                    
                    if (distance(item, zoneInfo) < .15f) movestate = BLOCK;
                    else movestate = TOZONE;
                    
                }
            }
        
        break;
        
        case TOZONE:
        
            game.getZone(zoneInfo);
            
            mathVecSubtract(attTarget, zoneInfo, me, 3);
            if (mathVecMagnitude(attTarget, 3) > .4f){
                adjustZone(me, -.02f);
            }
            else
                adjustZone(me, itemDist);
            
            if (distance(item, zoneInfo) < 0.042f){
                game.dropItem();

                targetID = findNewTarget(6);
                movestate = GUARD;
                
                if (isWinning())
                    guardCtr = 0;
                else
                    guardCtr = 8;
            }
        
        break;
        
        case GUARD:
            
            if (!game.hasItemBeenPickedUp(targetID)){
                movestate = DOCK;
            }

            if (guardCtr < 16 && distance(me, other) > .3f && angleBtwn(temp, other + 3) < .75f){
                guardCtr += 1;
            }
            else{
                if (guardCtr >= 16 
                || (angleBtwn(temp, other + 3) < .6f && mathVecMagnitude(other + 3, 3) > .15f)) 
                    movestate = DOCK;
                guardCtr = 0;
            }
            
            adjustZone(other, 0.08f);
            
        break;
        
        case BLOCK:
        
            adjustZone(other, .165f);
            mathVecSubtract(attTarget, me, zoneInfo, 3);
            
            if (distance(item, zoneInfo) > .16f)
                game.dropItem();
            
        break;
        
    }
	
}

//End page main

                        
                      
download
                        
                        //Begin page functions
void inFrontOfObject(float targetPoint[3],float targetItemAtt[3],float targetItemPos[3],float factor) {
    float A[3];
    mathVecScale(A,targetItemAtt,factor,false);
    mathVecAdd(targetPoint,A,targetItemPos,3);
} 

void makeTriangle(float A[],float B[],float vertex3[],float attitude[3]) {
    float D[3];
    float G[3];
    mathVecSubtract(vertex3,B,A,3); //vector between B and A (A = SPHERES, B = item)

    mathVecCross(G,vertex3,attitude); //vector BA crossed with the attitude of the object
    mathVecCross(D,G,vertex3); //the cross product from the previous line crossed with the BA vector in order to get the vector
    //that is perpendicular to BA but in the same plane as BA and the attitude vector, such that the attitude vector is facing it
    
    float length = distanceBetween(A,B); 
    mathVecScale(D,D,0.25f/length,true); //scale D to the length that will create a triangle with area 0.125 (since D is the base)
    mathVecScale(vertex3,vertex3,0.5f*length,true); //scale BA to 1/2 BA so we can add the height to get isosceles triangle
    mathVecAdd(vertex3,vertex3,D,3); //Add vector D to BA
    mathVecAdd(vertex3,vertex3,A,3); //Add vector A to BA + D in order to get the point of the vertice in space
}

bool lineIntersectsSphere2(float offset2[3], float sphereCenter[3],float sphereRadius, float lineStart[3],float lineEnd[3]) {
    float segmentV[3];
    mathVecSubtract(segmentV,lineEnd,lineStart,3); //segment from line end to line start
    float pointV[3];
    mathVecSubtract(pointV,sphereCenter,lineStart,3); //segment from the center of the sphere to the start
    mathVecNormalize(segmentV,3); //normalize the segment from line end to start
    float magnitudeProjV = mathVecInner(pointV,segmentV,3); //get mag of the projection of the line from the center of the sphere to the start on the line of motion
    float projectionV[3];
    mathVecScale(projectionV,segmentV,magnitudeProjV,false); //scale the unit vector of the line of travel to the projection
    mathVecAdd(offset2,projectionV,lineStart,3); //add the projection to the start of the line to get the point in space of the projection
    mathVecSubtract(offset2,offset2,sphereCenter,3); //find the vector from the point of closest approach to the center of the sphere
    if (mathVecMagnitude(offset2,3)<sphereRadius) { //if the magnitude of the vector is less than the sphere radius
        mathVecScale(offset2,offset2,sphereRadius,true); //scale the vector from the point of closest approach to the radius of the sphere
        return true;
    }
    else {
        return false;
    }
}

int bestItem(float myState[], float otherState[], float otherVel, float zone[4]) {
    int id = 0;
    float debrisDistance;
    float currentSmallest = 0.0f;
    int initID = 0;
    float zone1[3];
    mathVecScale(zone1,zone,-1.0f,false);
    bool enemyStayingInZone = (distanceBetween(otherState,zone1) < 0.25f && otherVel < 0.005f) ? true:false;
    for(int i=initID;i<6;i++) {
        int hasItem = game.hasItem(i);
        float itemPos[12];
        game.getItemZRState(itemPos,i);   
        debrisDistance = distanceBetween(myState,itemPos);
        if (i == 4 || i == 5) debrisDistance += 1.0f; //add distance to small items
        if (i == 2 || i == 3) debrisDistance -= 0.5f; //subtract distance from medium items
        if (i == 0 || i == 1) {
            debrisDistance -= 0.9f; //subtract more distance from large items
            if (distanceBetween(itemPos,zone) < 0.3f) debrisDistance = -0.5f;//if the large item is close to the zone, subtract even more distance
        }
        float distanceToOtherZone = distanceBetween(zone1,itemPos);
        bool hasItemBeenPickedUp = game.hasItemBeenPickedUp(i);
        if(distanceToOtherZone < (0.19f + zone[3]) && hasItemBeenPickedUp) debrisDistance -= 0.8f; //if in enemy zone and picked up, subtract more distance
        if ((((fabsf(itemPos[3]) > 0.0f) || (enemyStayingInZone && distanceToOtherZone < 0.25f)) && hasItem != 2) || game.itemInZone(i)) {
            debrisDistance = 150.0f; //if moving or enemy staying in it's zone and the item is close to the zone or the item is in our zone don't go for it
        }
        if (debrisDistance < currentSmallest || i == initID) {
            currentSmallest = debrisDistance;
            id = i;
        }
    }
    return id;
}

void mathVecScale(float res[3], float src[3], float mag, bool norm) {
    memcpy(res,src,12.0f); 
    if(norm) mathVecNormalize(res,3);
    for (int i=0;i<3;i++) res[i]*=mag;
} 

float distanceBetween(float myState[3], float otherState[3]) {
    float diff[3];
    mathVecSubtract(diff, myState, otherState,3);
    return mathVecMagnitude(diff,3);
}

void goTo (float myState[12],float targetPoint[3], float accel, float currentVel) {
    float targetVel[3];
    float velocity;
    float distanceToTarget = distanceBetween(myState,targetPoint);
    if (distanceToTarget <= currentVel*currentVel/accel) velocity = 0.0f;
    else {
        float limit = 0.05f;  
        velocity = 0.06f;
        if (accel < 0.0165f) { //if carrying an object, go to a lower velocity so that less fuel is used
            velocity = 0.055f;
            limit = 0.046f;
        }
        if (currentVel <= limit) velocity = 0.15f; //accelerate as quickly as possible if at a low velocity
        if (game.getFuelRemaining() < 9.0f) velocity = 0.045f; //lower maximum velocity if low on fuel
    }
    if (distanceToTarget>0.32f) {
        mathVecSubtract(targetVel,targetPoint,myState,3);
        mathVecScale(targetVel,targetVel,velocity,true);
        api.setVelocityTarget(targetVel);
    }
    else api.setPositionTarget(targetPoint); 
}
//End page functions
//Begin page main
#define COSINE_OF_ONE_FOURTH 0.9689124217f
#define BASE_ACCEL 0.0165f
    
int targetItem;
bool hasAnItem;
float start[3];
int firstItem;
float sps2Position[3];
float firstItemPos[12];
bool itemSecond;
float distanceToZone;


void init() {
	hasAnItem = false;
	targetItem = 0; //random target item
    firstItem = 2; //medium item for the blue SPHERES
    itemSecond = false; //whether to go for the item first or second
    distanceToZone = 0.0f; //distance of object to zone of the previous second
}

void loop() {
    int time = game.getCurrentTime();
    
    if (time == 0) DEBUG(("We Have Hope. Rebellions Are Built On Hope!"));
    if (time == 0) DEBUG(("My lord, Director Krennic has arrived."));
    if (time == 1) DEBUG(("Director Krennic."));
    if (time == 2) DEBUG(("Lord Vader."));
    if (time == 3) DEBUG(("You seem unsettled."));
    if (time == 4) DEBUG(("No just... pressed for time. There's a great many things to attend to."));
    if (time == 5) DEBUG(("My apologies. You do have a great many things to explain."));
    if (time == 6) DEBUG(("I delivered the weapon the Emperor requested. I deserve an audience to make certain he understands its remarkable... potential."));
    if (time == 7) DEBUG(("It's power to create problems has certainly been confirmed. A city destroyed, an imperial facility openly attacked."));
    if (time == 8) DEBUG(("It was Governor Tarkin who suggested the test."));
    if (time == 9) DEBUG(("You were not summoned here to grovel Director Krennic."));
    if (time == 10) DEBUG(("No it's-"));
    if (time == 11) DEBUG(("There is no Death Star! The Senate has been informed that Jedha was destroyed in a mining disaster."));
    if (time == 12) DEBUG(("Yes my lord."));
    if (time == 13) DEBUG(("I expect you not to rest until you can assure the Emperor that Galen Erso has not compromised this weapon in any way."));
    if (time == 14) DEBUG(("So I'm still in command? You'll speak to the Emperor about-"));
    if (time == 15) DEBUG(("*choking noises*"));
    if (time == 16) DEBUG(("Be careful not to choke on your aspirations Director. "));
    if (time == 17) DEBUG(("We must scatter the fleets! We have no recourse but to surrender."));
    if (time == 17) DEBUG(("\nAre we really talking about disbanding something that we worked so hard to create?"));
    if (time == 17) DEBUG(("\nWe can't just give in! We joined an alliance, not a suicide pact! We've only now managed to gather our forces."));
    if (time == 17) DEBUG(("\n Gather our forces?! General Draven has already blown up an Imperial base!"));
    if (time == 17) DEBUG(("\nA decision needed to be made!"));
    if (time == 17) DEBUG(("\nIf it's war you want you'll fight alone."));
    if (time == 17) DEBUG(("\nIf that's how it's going, why have an alliance at all?"));
    if (time == 17) DEBUG(("\nIf she's telling the truth, we need to act now!"));
    if (time == 18) DEBUG(("Councilors please!"));
    if (time == 19) DEBUG(("It is simple. The Empire has the means of mass destruction, the Rebellion does not."));
    if (time == 19) DEBUG(("\nThe Death Star, this is nonsense!"));
    if (time == 20) DEBUG(("What reason would my father have to lie? What benefit would it bring him?"));
    if (time == 21) DEBUG(("To lure our forces into a final battle to destroy us once and for all!"));
    if (time == 22) DEBUG(("Risk everything based on what? The testimony of a criminal. The dying words of her father, an Imperial scientist."));
    if (time == 22) DEBUG(("\nBut don't forget the Imperial pilot."));
    if (time == 23) DEBUG(("My father gave his life so that we might have a chance to defeat this!"));
    if (time == 24) DEBUG(("So you've told us."));
    if (time == 24) DEBUG(("\nIf the Empire has this kind of power what chance do we have?"));
    if (time == 25) DEBUG(("What chance do we have? The question is what choice?! Run! Hide! Plead for mercy! Scatter your forces! You give way to an enemy this evil with this much power and you condemn the galaxy to an eternity of submission! The time to fight is now!"));
    if (time == 26) DEBUG(("Yes!"));
    if (time == 27) DEBUG(("Every moment you wait is another step closer to the ashes of Jedha! "));
    if (time == 28) DEBUG(("What is she proposing? Let the girl speak!"));
    if (time == 29) DEBUG(("Send your best troops to Scarif. Send the Rebel Fleet if you have too. You need to capture the Death Star plans if there is any hope of destroying it."));
    if (time == 30) DEBUG(("You are asking us to attack an Imperial installation based on nothing but hope."));
    if (time == 31) DEBUG(("Rebellions are built on hope. "));
    if (time == 32) DEBUG(("There is no hope!"));
    if (time == 32) DEBUG(("\nI say we fight!"));
    if (time == 32) DEBUG(("\n I say the Rebellion is finished!"));
    if (time == 32) DEBUG(("\n I'm sorry Jyn. Without the full support of the council, the odds are too great."));
    if (time == 33) DEBUG(("General Syndulla, please report to the briefing room. "));
    if (time == 34) DEBUG(("You don't look happy!"));
    if (time == 34) DEBUG(("\nThey'd prefer to surrender."));
    if (time == 34) DEBUG(("\nAnd you?"));
    if (time == 34) DEBUG(("\nShe wants to fight."));
    if (time == 35) DEBUG(("So do I. We all do."));
    if (time == 36) DEBUG(("The Force is strong. "));
    if (time == 37) DEBUG(("I'm not sure four of us is quite enough."));
    if (time == 38) DEBUG(("How many do we need?"));
    if (time == 39) DEBUG(("What are you talking about?"));
    if (time == 40) DEBUG(("They were never going to believe you. "));
    if (time == 41) DEBUG(("I appreciate the support. "));
    if (time == 42) DEBUG(("But I do. I believe you. We’d like to volunteer. Some of us... well most of us have all done terrible things on behalf of the Rebellion. Spies. Saboteurs. Assassins. Everything I did, I did for the Rebellion."));
    if (time == 42) DEBUG(("\nAnd every time I walked away from something I wanted to forget, I told myself it was for a cause I believed in. A cause that was worth it. Without that, we're lost. Everything we've done would have been for nothing."));    
    if (time == 42) DEBUG(("\nI couldn't face myself if I gave up now. None of us could."));
    if (time == 43) DEBUG(("It won't be comfortable. It'll be cramped but we'll all fit. We can go. "));
    if (time == 44) DEBUG(("Okay. Gear up! Grab anything that's not nailed down. Go go go!"));
    if (time == 45) DEBUG(("Jyn, I'll be there for you. Cassian said I had to."));
    if (time == 46) DEBUG(("I'm not used to people sticking around when things go bad."));
    if (time == 47) DEBUG(("Welcome home."));
    if (time == 48) DEBUG(("May the Force be with us."));
    if (time == 49) DEBUG(("Cargo shuttle, readback please. What's going on out there? That ship is off limits, you're not supposed to be on board until further instructions."));
    if (time == 50) DEBUG(("Yes, yes we are! Affirmative."));
    if (time == 51) DEBUG(("That's an impounded Imperial ship! What's your callsign pilot?!"));
    if (time == 51) DEBUG(("\nWe have to go."));
    if (time == 51) DEBUG(("\nUm, it's um... "));
    if (time == 51) DEBUG(("\nSay something. Come on!"));
    if (time == 51) DEBUG(("\nRogue? Rogue One!"));
    if (time == 51) DEBUG(("\nRogue One?! There is no Rogue One!"));
    if (time == 51) DEBUG(("\nWell there is now!"));
    if (time == 51) DEBUG(("\nRogue One, pulling away."));
    if (time == 52) DEBUG(("Despite what the others say, war is inevitable."));
    if (time == 53) DEBUG(("Yes I agree. I must return to Alderaan to inform my people that there will be no peace. We will need every advantage."));
    if (time == 53) DEBUG(("\nYour friend. The Jedi."));
    if (time == 53) DEBUG(("\nHe served me well during the Clone Wars and has lived in hiding since the Emperor's purge. Yes I will send for him."));
    if (time == 53) DEBUG(("\nYou will need someone you can trust."));
    if (time == 53) DEBUG(("\nI would trust her with my life. "));
    if (time == 54) DEBUG(("Okay. We're coming in. There's a planet wide defensive shield that has a single main entry gate. This shuttle should be equipped with an access code that allows us through."));
    if (time == 54) DEBUG(("\nAssuming the Empire hasn't logged it as overdue."));
    if (time == 54) DEBUG(("\nAnd if they have?"));
    if (time == 55) DEBUG(("Then they shut the gate. And we're all annihilated in the cold dark vacuum of space."));
    if (time == 55) DEBUG(("\nNot me. I can survive in space."));
    if (time == 56) DEBUG(("Okay, here it goes. Cargo shuttle SW-0608 requesting a landing pad. "));
    if (time == 57) DEBUG(("Cargo shuttle SW-0608 you're not listed on the arrival schedule. "));
    if (time == 58) DEBUG(("Acknowledged, gate control. We were rerouted from Eadu flight station. Transmitting clearance code now."));
    if (time == 59) DEBUG((" Transmitting."));
    if (time == 60) DEBUG(("Cargo shuttle SW-0608... you are cleared for entry. "));
    if (time == 61) DEBUG(("Yes!"));
    if (time == 61) DEBUG(("\nImpressive."));
    if (time == 61) DEBUG(("\nI'll tell the others."));
    if (time == 62) DEBUG(("SW-0608, cleared for landing pad nine. Acknowledge please. "));
    if (time == 63) DEBUG(("SW-0608 proceeding to LP9 as instructed. "));
    if (time == 64) DEBUG(("The main building down there, what is it?"));
    if (time == 64) DEBUG(("\nThat's the Citadel tower. They keep all of the Imperial structural archives up there. If the plans are anywhere they'll be there."));
    if (time == 64) DEBUG(("\nAnd the dish at the top? What's it for?"));
    if (time == 64) DEBUG(("\nI guess it takes a lot of signal to send those data files out. "));
    if (time == 65) DEBUG(("Landing track engaged. Landing track locked. We're landing. We're coming in!"));
    if (time == 66) DEBUG(("Saw Gerrera used to say that one fighter with a sharp stick and nothing left to lose could take the day. They have no idea we're coming. They have no reason to expect us."));
    if (time == 66) DEBUG(("\nIf we can make it to the ground we'll take the next chance and the next, on and on until we win... or the chances are spent. The Death Star plans are down there. Cassian, Kaytoo, and I will find them. We'll find a way to find them."));
    if (time == 67) DEBUG(("Melshi, Pao, Baze, Chirrut, you'll take the main squad, move east and get wide of the ship. Find a position between here and the tower. Once you get to the best spot, light the place up. Make ten men feel like a hundred."));
    if (time == 68) DEBUG(("All right."));
    if (time == 68) DEBUG(("\nAnd get those troopers away from us."));
    if (time == 69) DEBUG(("What should I do?"));
    if (time == 70) DEBUG(("Keep the engine running. You're our only way out of here."));
    if (time == 71) DEBUG(("Cargo shuttle SW-0608, be prepared to receive inspection team."));
    if (time == 72) DEBUG(("Ready?"));
    if (time == 72) DEBUG(("\nHello."));
    if (time == 72) DEBUG(("\nHey, you're probably looking for a manifest."));
    if (time == 72) DEBUG(("\nThat would be helpful."));
    if (time == 72) DEBUG(("\nIt's just down here."));
    if (time == 73) DEBUG(("Director Krennic, we are entering the Scarif shield gate. General Ramda has been informed of your arrival."));
    if (time == 74) DEBUG(("Good luck, little sister."));
    if (time == 75) DEBUG(("Go! Go now, you're clear!"));
    if (time == 75) DEBUG(("\nAll right, let's go. Come on. Move, move, move."));
    if (time == 76) DEBUG(("I have got a bad feeling about"));
    if (time == 77) DEBUG(("Kay!"));
    if (time == 77) DEBUG(("\nQuiet."));
    if (time == 78) DEBUG(("What?"));
    if (time == 78) DEBUG(("\nWe need a map."));
    if (time == 79) DEBUG(("Well, I'm sure there's one just lying about."));
    if (time == 80) DEBUG(("You know what you have to do."));
    if (time == 81) DEBUG(("One per pad. Pick your spot. We want to draw them out. I'll call the timing. Go!"));
    if (time == 82) DEBUG(("Hey, did you hear the rumors?"));
    if (time == 82) DEBUG(("\nYeah, the T-15s have been marked obsolete. Oh boy, it's about time for that."));
    if (time == 83) DEBUG(("Hey."));
    if (time == 83) DEBUG(("\nWatch out."));
    if (time == 83) DEBUG(("\nWhat the...?"));
    if (time == 84) DEBUG(("Director, what brings you to Scarif?"));
    if (time == 84) DEBUG(("\nGalen Erso, I want every dispatch, every transmission he has ever sent called up for inspection."));
    if (time == 84) DEBUG(("\nAll of them?"));
    if (time == 84) DEBUG(("\nYes. All of them. Get started."));
    if (time == 85) DEBUG(("Kay? "));
    if (time == 85) DEBUG(("\nOur optimal route to the data vault places only 89 stormtroopers in our path. We will make it no more than 33% of the way before we are killed."));
    if (time == 85) DEBUG(("\nMelshi, talk to me."));
    if (time == 85) DEBUG(("\n Ready, ready. Standing by."));
    if (time == 85) DEBUG(("\nLight it up."));
    if (time == 86) DEBUG(("Are we blind? Deploy the garrison! Move!"));
    if (time == 87) DEBUG(("Troopers! Troopers. Troopers on the left!"));
    if (time == 88) DEBUG(("Sir? Scarif base, they're reporting a rebel incursion."));
    if (time == 88) DEBUG(("\nI want to speak to Director Krennic."));
    if (time == 88) DEBUG(("\nHe's there, sir. On Scarif."));
    if (time == 88) DEBUG(("\nThe original plans for this station are kept there, are they not?"));
    if (time == 88) DEBUG(("\nThey are."));
    if (time == 88) DEBUG(("\nPrepare for the jump to hyperspace and inform Lord Vader."));
    if (time == 89) DEBUG(("Can I help you?"));
    if (time == 89) DEBUG(("\nThat won't be necessary."));
    if (time == 89) DEBUG(("\nSenator! Senator."));
    if (time == 89) DEBUG(("\nStop right there, Private."));
    if (time == 89) DEBUG(("\nLet him speak."));
    if (time == 89) DEBUG(("\nIntercepted Imperial transmission, ma'am. Rebels on Scarif."));
    if (time == 89) DEBUG(("\nI need to speak with Admiral Raddus."));
    if (time == 89) DEBUG(("\nHe's returned to his ship."));
    if (time == 89) DEBUG(("\nHe's going to fight."));
    if (time == 89) DEBUG(("\nNo!"));
    if (time == 90) DEBUG(("Attention all flight personnel, please report to your commanders immediately. All flight personnel, please report to your commanders immediately. We have been redirected to Scarif. Pilots, you'll be briefed by you squadron leaders on route."));
    if (time == 90) DEBUG(("\nMay the Force be with you."));
    if (time == 90) DEBUG(("\nScarif? They're going to Scarif? Why does nobody ever tell me anything, Artoo?"));
    if (time == 91) DEBUG(("Fall back! Fall back!"));
    if (time == 91) DEBUG(("\nKeep drawing them out!"));
    if (time == 91) DEBUG(("\nGo. Go. Move. Move. Come on, move up!"));
    if (time == 91) DEBUG(("\nPad 12 report to ground. Coming at our flank, we're outnumbered."));
    if (time == 91) DEBUG(("\nPad 12 team, what's going on down there. Status, please. All Pads report in immediately."));
    if (time == 91) DEBUG(("\nUnknown, we have rebels everywhere. We're taking fire, Pad 12, Pad 14 request assistance."));
    if (time == 91) DEBUG(("\nThis is Pad 2, Pad 2. I spot 40 rebels heading west on Pad 2. Get on there, tell him pinned down by rebels on Pad 5."));
    if (time == 91) DEBUG(("\nThis is Pad 5, we're being overrun. Help us!"));
    if (time == 91) DEBUG(("\nPad 12, Pad 10, Pad 8, please confirm. Confirm and report. Get reinforcements down to Pad 5 immediately!"));
    if (time == 92) DEBUG(("This is not working, Kay."));
    if (time == 92) DEBUG(("\nRight hand."));
    if (time == 93) DEBUG(("Baze! Baze! What?"));
    if (time == 93) DEBUG(("\nRun!"));
    if (time == 93) DEBUG(("\nRetreat. Retreat!"));
    if (time == 94) DEBUG(("This is Admiral Raddus of the Rebel Alliance. All Squadron leaders report in."));
    if (time == 94) DEBUG(("\nAdmiral, this is Blue Leader standing by."));
    if (time == 94) DEBUG(("\nThis is Gold Leader, standing by."));
    if (time == 94) DEBUG(("\nThis is Red Leader, standing by."));
    if (time == 95) DEBUG(("Sir, those are rebel ships."));
    if (time == 95) DEBUG(("\nGet Admiral Gorin, immediately."));
    if (time == 95) DEBUG(("\nDirector, the rebel fleet, it's amassing outside the shield."));
    if (time == 95) DEBUG(("\nLock down the base. Lock down."));
    if (time == 95) DEBUG(("\nAnd close the shield?"));
    if (time == 95) DEBUG(("\nDo it!"));
    if (time == 96) DEBUG(("Red and Gold squadron, attack formations. Defend the fleet. Blue squadron, get to the surface before they closed that gate."));
    if (time == 96) DEBUG(("\nCopy that Admiral. Blue squadron, on me."));
    if (time == 96) DEBUG(("\nCopy, Blue Leader."));
    if (time == 96) DEBUG(("\nCopy, Blue Leader."));
    if (time == 96) DEBUG(("\nWe won't have long."));
    if (time == 96) DEBUG(("\nClose the field. Close it!"));
    if (time == 96) DEBUG(("\nCome on, come on, come on!"));
    if (time == 96) DEBUG(("\nPull up!"));
    if (time == 97) DEBUG(("The Rebel fleet has arrived."));
    if (time == 97) DEBUG(("\nWhat?"));
    if (time == 97) DEBUG(("\nThere's fighting on the beach, they've locked down the base, they've closed the shield gate."));
    if (time == 97) DEBUG(("\nWhat does that mean?"));
    if (time == 97) DEBUG(("\nWe're trapped?"));
    if (time == 97) DEBUG(("\nWe could transmit the plans to the rebel fleet. We'd have to get a signal out to tell them it's coming. It's the size of the data files, that's the problem. They'll never get through.Someone has to take that shield gate down."));
    if (time == 97) DEBUG(("\nBodhi. Bodhi, can you hear me? Bodhi, tell me you're out there. Bodhi!"));
    if (time == 97) DEBUG(("\nHello, I'm here. We're standing by. They've started fighting, the base is on lockdown!"));
    if (time == 97) DEBUG(("\nI know, listen to me!"));
    if (time == 97) DEBUG(("\nThe rebel fleet is up there. You've got to tell them to blow a hole in the shield gate so we can transmit the plans."));
    if (time == 97) DEBUG(("\nWait, I can't. I'm not hooked into the comm tower. We're not tied in."));
    if (time == 97) DEBUG(("\nIt's the only way we're getting them out of here. Find a way!"));
    if (time == 97) DEBUG(("\nCover our backs."));
    if (time == 98) DEBUG(("You'll need this. You wanted one, right?"));
    if (time == 98) DEBUG(("\nYour behavior, Jyn Erso, is continually unexpected."));
    if (time == 98) DEBUG(("\nJyn. Come on."));
    if (time == 99) DEBUG(("Fire on my command."));
    if (time == 99) DEBUG(("\nLook!"));
    if (time == 100) DEBUG(("Get ready. We're going to have to go out there."));
    if (time == 100) DEBUG(("\nWhat're you doing?"));
    if (time == 100) DEBUG(("\nThey closed the shield gate, we're stuck here. But, the rebel fleet is pulling in. We just have to get a signal strong enough to get through to them and let them know we're trapped down here."));
    if (time == 100) DEBUG(("\nFor that, we have to connect to the communications tower."));
    if (time == 100) DEBUG(("\n Now, I can patch us in over here, the landing pad but you have to get on the radio, get one of the guys out there to find a master switch. Get them to activate the connection between us and that comms tower. Okay. Then go!"));
    if (time == 101) DEBUG(("What's going on down there, Lieutenant?"));
    if (time == 101) DEBUG(("\nUnknown, sir. We can't raise them. All rebel frequencies are blocked."));
    if (time == 101) DEBUG(("\nKeep trying. Engage those Star Destroyers and let's start probing that shield."));
    if (time == 101) DEBUG(("\nYes, sir."));
    if (time == 101) DEBUG(("\nWe're going in."));
    if (time == 102) DEBUG(("Stay with me."));
    if (time == 102) DEBUG(("\nSchematics bank, Data tower two."));
    if (time == 102) DEBUG(("\nHow do I find that?"));
    if (time == 102) DEBUG(("\nSearching. I can locate the tape, but you'll need the handles for extraction."));
    if (time == 102) DEBUG(("\nWhat are we supposed to do with these? Whoa."));
    if (time == 102) DEBUG(("\nWell, finally."));
    if (time == 102) DEBUG(("\nWait, stop."));
    if (time == 102) DEBUG(("\nThe rebels! They went over there."));
    if (time == 102) DEBUG(("\nThere's one. Oh well."));
    if (time == 102) DEBUG(("\nCome on!"));
    if (time == 103) DEBUG(("Red Leader, this is Gold Leader, we're starting our attack run on the shield gate. Keep it tight and watch out for those towers."));
    if (time == 103) DEBUG(("\nMore fighters. Form up, at 65."));
    if (time == 103) DEBUG(("\nSir, enemy fighters coming in."));
    if (time == 104) DEBUG(("Melshi. Melshi, come in, are you there? Bodhi will send a signal from here. He's patching us in. But you guys have to open up a line for the tower."));
    if (time == 104) DEBUG(("\nHow? Please advise."));
    if (time == 104) DEBUG(("\nThere's a master switch to the base of the Comm tower"));
    if (time == 104) DEBUG(("\nMaster switch, describe, what are we looking for? What does it look like, the master switch?"));
    if (time == 104) DEBUG(("\nWhere is it."));
    if (time == 105) DEBUG(("U-wings, reinforce those troops on the beach. All fighters, on me. We have to shield them from air attack."));
    if (time == 106) DEBUG(("Troop reinforcements. Here we go."));
    if (time == 107) DEBUG(("For Jedha! Jedha!"));
    if (time == 108) DEBUG(("We are having no effect on that shield."));
    if (time == 108) DEBUG(("\nStick close, Red 5, where are you going?"));
    if (time == 108) DEBUG(("\nI'm trying to lose them. This is Red 5, I need help!"));
    if (time == 109) DEBUG(("Hyperspace Tracking, Navigational Systems. Two screens down. Structural Engineering, open that one!"));
    if (time == 110) DEBUG(("I got one on my tail."));
    if (time == 110) DEBUG(("\nI'm on it."));
    if (time == 111) DEBUG(("Project code names: Stellarsphere. Mark Omega. Pax Aurora. War-Mantle. Cluster-Prism. Black-Saber."));
    if (time == 111) DEBUG(("\nWhat?"));
    if (time == 111) DEBUG(("\nStardust. That's it."));
    if (time == 111) DEBUG(("\nHow do you know that?"));
    if (time == 111) DEBUG(("\nI know because it's me."));
    if (time == 112) DEBUG(("Hey, you. Identify yourself."));
    if (time == 112) DEBUG(("\nI can explain."));
    if (time == 113) DEBUG(("Incoming fire, take cover!"));
    if (time == 114) DEBUG(("Kay, we need the file for Stardust!"));
    if (time == 114) DEBUG(("\nStardust."));
    if (time == 114) DEBUG(("\nThat's it."));
    if (time == 114) DEBUG(("\nKay."));
    if (time == 114) DEBUG(("\nClimb. Climb. You can still send the plans to the fleet. If they open the shield gate, you can broadcast from the tower! Locking the bolt door now."));
    if (time == 114) DEBUG(("\nKay. Kay!"));
    if (time == 114) DEBUG(("\nGood bye."));
    if (time == 115) DEBUG(("Kay!"));
    if (time == 116) DEBUG(("Sir, unauthorized access at the data vault."));
    if (time == 116) DEBUG(("\nWhat?"));
    if (time == 116) DEBUG(("\nIt's just come in, sir."));
    if (time == 117) DEBUG(("Send my guard squadron into battle! Two men with me now! Get that beach under control!"));
    if (time == 118) DEBUG(("Bodhi? Are you there? Have you got the switch?"));
    if (time == 118) DEBUG(("\nI can't get to the shuttle, I can't plug in!"));
    if (time == 119) DEBUG(("You have to! They have to hit that gate. If the shield's open, we can send the plans!"));
    if (time == 120) DEBUG(("General, behind you!"));
    if (time == 120) DEBUG(("\nTake them out."));
    if (time == 121) DEBUG(("Step back."));
    if (time == 122) DEBUG(("We're going down!"));
    if (time == 123) DEBUG(("Come on! Look out."));
    if (time == 124) DEBUG(("\nHang on. The master switch, it's over at that console."));
    if (time == 125) DEBUG(("I'm going!"));
    if (time == 126) DEBUG(("I've got it."));
    if (time == 127) DEBUG(("Careful! You okay? Jyn!"));
    if (time == 128) DEBUG(("I'm one with the Force and the Force is with me. I'm one with the Force and the Force is with me."));
    if (time == 128) DEBUG(("\nChirrut!"));
    if (time == 129) DEBUG(("I'm one with the Force and the Force is with me. I'm one with the Force and the Force is with me. I'm one with the Force and the Force is with me."));
    if (time == 129) DEBUG(("\nI'm one with the Force and the Force is with me. I'm one with the Force and the Force is with me. I'm one with the Force and the Force is with me."));
    if (time == 130) DEBUG(("Chirrut, come back!"));
    if (time == 131) DEBUG(("I'm one with the Force and the Force is with me."));
    if (time == 132) DEBUG(("Yes!"));
    if (time == 133) DEBUG(("Keep going. Keep going!"));
    if (time == 134) DEBUG(("Cassian."));
    if (time == 135) DEBUG(("Chirrut, come! Come with me! Chirrut! Chirrut, don't go. Don't go. I'm here. I'm here."));
    if (time == 135) DEBUG(("\nIt's okay. It's okay. Look for the Force-"));
    if (time == 136) DEBUG(("Chirrut"));
    if (time == 137) DEBUG(("-and you will always find me."));
    if (time == 138) DEBUG(("The Force is with me. I'm one with the Force. The Force is with me. I'm one with the Force. The Force is with me. And I'm one with the Force."));
    if (time == 139) DEBUG(("Deflector shield's down to 50%, we're sustaining heavy damage."));
    if (time == 140) DEBUG(("There's no turning back now, we have to press the attack."));
    if (time == 140) DEBUG(("\nHit that opening."));
    if (time == 141) DEBUG(("We see it, Gold Leader. Ion torpedoes away."));
    if (time == 142) DEBUG(("What's happening?"));
    if (time == 143) DEBUG(("We've lost engine power, sir."));
    if (time == 144) DEBUG(("That Star Destroyer is disabled."));
    if (time == 145) DEBUG(("This is Rogue One, calling any Alliance ships that can hear me! Is there anybody up there? This is Rogue One! Come in, over."));
    if (time == 146) DEBUG(("This is Admiral Raddus, Rogue One, we hear you!"));
    if (time == 147) DEBUG(("We have the plans! They found the Death Star plans. They have to transmit them from the communications tower! You have to take down the shield gate. It's the only way to going to get them through!"));
    if (time == 148) DEBUG(("Call up a Hammerhead corvette. I have an idea. Standby, Rogue One, we're on it."));
    if (time == 149) DEBUG(("This is for you, Galen."));
    if (time == 150) DEBUG(("The Force is with me. I'm one with the Force."));
    if (time == 151) DEBUG(("Corvette 5, locked on target. Prepare for impact. Sublight thrusters, full power!"));
    if (time == 152) DEBUG(("Reset antenna alignment. Reset antenna alignment. Reset antenna alignment. Reset antenna alignment."));
    if (time == 153) DEBUG(("Reverse engines, full power!"));
    if (time == 154) DEBUG(("Antenna aligned. Ready to transmit."));
    if (time == 155) DEBUG(("Who are you?"));
    if (time == 156) DEBUG(("You know who I am. I'm Jyn Erso. Daughter of Galen and Lyra. You've lost."));
    if (time == 157) DEBUG(("Oh, I have, have I?"));
    if (time == 158) DEBUG(("My father's revenge. He built a flaw in the Death Star. He put a fuse in the middle of your machine and I've just told the entire galaxy how to light it."));
    if (time == 159) DEBUG(("The shield is up, your signal will never reach the rebel base. All your ships in here will be destroyed. I lose nothing but time. YOU on the other hand die with the Rebellion."));
    if (time == 160) DEBUG(("Transmitting. Transmitting."));
    if (time == 161) DEBUG(("Admiral, receiving transmission from Scarif."));
    if (time == 162) DEBUG(("Hey, leave it. Leave it. That's it. That's it. Let's go."));
    if (time == 163) DEBUG(("Transmission received."));
    if (time == 164) DEBUG(("Admiral, we have the plans!"));
    if (time == 165) DEBUG(("She did it."));
    if (time == 166) DEBUG(("Do you think, anybody is listening?"));
    if (time == 167) DEBUG(("I do. Someone's out there."));
    if (time == 168) DEBUG(("Sir, we're detecting a massive object emerging from hyperspace."));
    if (time == 169) DEBUG(("Sir, shall I begin targeting their fleet?"));
    if (time == 170) DEBUG(("Lord Vader will handle the fleet. Target the base at Scarif. Single reactor ignition"));
    if (time == 170) DEBUG(("\nYes, sir."));
    if (time == 171) DEBUG(("You may fire when ready."));
    if (time == 172) DEBUG(("Commence primary ignition."));
    if (time == 173) DEBUG(("Rogue One, may the Force be with you. All ships, prepare for jump to hyperspace!"));
    if (time == 174) DEBUG(("You father would have been proud of you, Jyn."));
    if (time == 175) DEBUG(("The Rebel flagship is disabled, my lord, but has received transmission from the surface."));
    if (time == 176) DEBUG(("Prepare a boarding party."));
    if (time == 177) DEBUG(("Yes, my lord."));
    if (time == 178) DEBUG(("Hurry!"));
    if (time == 178) DEBUG(("\nLet's go!")); 
    if (time == 178) DEBUG(("\nWe got to go now."));
    if (time == 178) DEBUG(("\nHurry, let's go."));
    if (time == 178) DEBUG(("\nIt's stuck. Hey, help!"));
    if (time == 178) DEBUG(("\nOpen fire!"));
    if (time == 178) DEBUG(("\nHelp us!"));
    if (time == 178) DEBUG(("\nIt's jammed."));
    if (time == 178) DEBUG(("\nPush!"));
    if (time == 178) DEBUG(("\nHere. Here. Take it! Take it."));
    if (time == 178) DEBUG(("\nLAUNCH!"));
    if (time == 179) DEBUG(("Make sure you secure the airlock. And prepare the escape pods. Your Highness, the transmission we received. What is it they've sent us?"));
    if (time == 180) DEBUG(("Hope."));

    float myState[12];
    float otherState[12];
    float zone[4];
    float targetPoint[4];
    targetPoint[0] = 0.0f;
    targetPoint[1] = 0.0f;
    targetPoint[2] = 0.0f;

    float factor; 
	float targetItemPos[12];
    float accel = BASE_ACCEL;
    api.getMyZRState(myState);
    float currentVel = mathVecMagnitude(&myState[3],3);
    api.getOtherZRState(otherState);
    float otherVel = mathVecMagnitude(&otherState[3],3);
    float targetAtt[3];
    memcpy(targetAtt,&myState[6],12.0f); 
    
    if (time == 0) {
	    float itemsPos[6][12];
        for (int i = 0; i<6;i++) game.getItemZRState(itemsPos[i],i);
	    game.dropSPS();
        memcpy(start,myState,12.0f); 
        int secondItem = 0; //large item for blue
	    if (myState[1] < 0.0f) { //if red
	        firstItem = 3; //medium item for red
	        secondItem = 1; //large item for red
	    }
	    factor = 0.146f; //distance from center of sphere to medium item
	    inFrontOfObject(targetPoint,&itemsPos[firstItem][6],itemsPos[firstItem],0.146f);
	    if (distanceBetween(start,targetPoint) <= 0.42f && distanceBetween(myState,itemsPos[secondItem]) > 0.42f) {
	        firstItem = secondItem;
	        factor = 0.173f; //distance from center of sphere to large item
	    }
	    memcpy(firstItemPos,itemsPos[firstItem],48.0f);
	    inFrontOfObject(targetPoint,&itemsPos[firstItem][6],itemsPos[firstItem],factor);
	    zone[3] = distanceBetween(myState,targetPoint);
	    if (zone[3] > distanceBetween(myState,itemsPos[firstItem]) && zone[3] >= 0.41f) { //if the item is facing away from us
	        itemSecond = true;
	        memcpy(sps2Position,targetPoint,12.0f); 
	    }
	}

    int sps = game.getNumSPSHeld();
    
	float distanceToStart = distanceBetween(myState,start);
	
    if ((sps == 1 && !itemSecond) || (itemSecond && sps == 2)) {
        makeTriangle(start,sps2Position,targetPoint,&firstItemPos[6]);
        if (distanceBetween(myState,targetPoint) < 0.02f || (distanceToStart > distanceBetween(start,targetPoint) && itemSecond) || (distanceBetween(sps2Position,myState) > distanceBetween(targetPoint,sps2Position) && !itemSecond)) {
            game.dropSPS(); //if distance to the start is greater than the distance from the start to the
            //target point and we're going for the item second, then drop, 
            //or if the distance from sps2position to us is greater than distance between
            //sps2position and the targetpoint and we're going for the item first, drop
            sps -= 1;
        }
    }
    
    game.getZone(zone);
    
	if (((sps == 2 && !itemSecond) || sps == 0 || (sps == 1 && itemSecond)) && !hasAnItem) {
	    targetItem = bestItem(myState,otherState,otherVel,zone);
        if (sps > 0 && game.hasItem(firstItem) != 2) targetItem = firstItem;
        
	    game.getItemZRState(targetItemPos,targetItem);
        factor = (targetItem <= 1) ?  0.162f : ((targetItem <= 3) ? 0.149f : 0.135f);
        //distances from the center of the SPHERES to the large, medium, and small items respectively
        
        inFrontOfObject(targetPoint,&targetItemPos[6],targetItemPos,factor);
	    mathVecSubtract(targetAtt,targetItemPos,targetPoint,3);
	    //DEBUG(("fabsf(targetItemPos[3]): %f",fabsf(targetItemPos[3])));

        if ((targetItemPos[3]) != 0.0f) {
            inFrontOfObject(targetPoint,&firstItemPos[6],firstItemPos,factor);
            mathVecSubtract(targetAtt,firstItemPos,targetPoint,3);
        }
        
        float vectorBetween[3];
        mathVecSubtract(vectorBetween,targetItemPos,myState,3);
        mathVecNormalize(vectorBetween,3);
        float angleBetween = mathVecInner(vectorBetween,&myState[6],3);
        
        float distanceToItemCenter = distanceBetween(myState,targetItemPos);
	    if ((currentVel < 0.01f) && (fabsf(distanceToItemCenter-factor) < (0.011f)) && angleBetween > COSINE_OF_ONE_FOURTH) {
            //max error for distanace is 0.011f, max velocity is 0.01f m/s
	        game.dockItem(targetItem);
	    } 
	    
	    if (game.hasItem(targetItem) == 1) {
	        hasAnItem = true;
	        if (distanceToStart >= 0.41f) {  //distance for base of isosceles triangles to make a reasonable height
    	       game.dropSPS();
               sps -= 1;
               memcpy(sps2Position,myState,12.0f); 
	        }
	    }
	    
        //factor = (targetItem <= 1) ? ((0.04125f + 0.11f)*sqrt(2.0f)) :((0.0275f + 0.11f)*sqrt(2.0f));
        factor = (targetItem <= 1) ? (0.2138998013f) : ((targetItem <= 3) ? (0.1944543648f) : 0.1750089283f);
        //factor is the distance from the center of the item to the corner of the item plus sphere radius, so (side of item) * sqrt(2)
        float offset1[3];
        if (angleBetween <= (COSINE_OF_ONE_FOURTH - 0.214f) && lineIntersectsSphere2(offset1,targetItemPos,factor,myState,targetPoint) && distanceBetween(myState,targetPoint) > distanceToItemCenter) {
	        mathVecAdd(targetPoint,targetItemPos,offset1,3);
        }
    } 
    
    if (sps == 2 && hasAnItem) {
        mathVecSubtract(targetPoint,myState,start,3);
        mathVecScale(targetPoint,targetPoint,0.5372849659f,true); 
        //equilateral distance of one leg of an equilateral triangle with area 0.125 or so error
        mathVecAdd(targetPoint,targetPoint,start,3);
	    //0.02 off from equilateral distance for a small error
        if (distanceToStart > 0.5172849659f) { 
            game.dropSPS();
            memcpy(sps2Position,myState,12.0f); 
        }        
    }
    
    game.getItemZRState(targetItemPos,targetItem);
    float currentDistanceToZone = distanceBetween(targetItemPos,zone);
    
	if (hasAnItem && sps == 0) {
        //accel = (targetItem <= 1) ?  (0.0176f*8.0f/11.0f) : ((targetItem <= 3) ? (0.0176f*4.0f/5.0f) : (0.0176f*8.0f/9.0f));
        //changed base accel to 0.0165f
        //base accel * 8/11 for a large item, base accel * 4/5 for a medium or small item
        accel = (targetItem <= 1) ?  (0.012f) : (0.0132f);
        
    	mathVecSubtract(targetPoint,zone,myState,3);
	    mathVecScale(targetPoint,targetPoint,((distanceBetween(myState,zone)-distanceBetween(myState,targetItemPos))),true);
        mathVecAdd(targetPoint,targetPoint,myState,3);
        
        mathVecSubtract(targetAtt,zone,myState,3);
	    if (currentDistanceToZone < (0.1f-zone[3]) || (game.getFuelRemaining() == 0.0f && currentDistanceToZone > distanceToZone)) {
	        //if item is definitel in zone or if fuel is running out and distance from item to zone increases
	        game.dropItem();
	        hasAnItem = false;
	    }
	    distanceToZone = currentDistanceToZone;
	}
	
    if (sps == 2) {
        accel = 0.011f;// 0.0165f*4.0f/6.0f
        if (hasAnItem) accel = 0.0101538462;//0.0165f*8.0f/13.0f
    }
    if (sps == 1) accel = 0.011f;//0.0165f*8.0f/12.0f

	if (targetPoint[0] == 0.0f) game.dropSPS();
	
    goTo(myState,targetPoint,accel,currentVel);
    api.setAttitudeTarget(targetAtt);
}

//End page main

                        
                      
download
                        
                        //Begin page main
/*
@author ZERO WORK ETHIC - QUANTUM ENTANGLEMENT - 99.95 ROBOTICS
*/

#define ITEM_TYPE_ALL                                   -1
#define SPS_SPEED                                   0.060f
#define DEFAULT_SPEED                               0.060f
#define MIN_RADIAL_DIST                              0.50f
#define RADIAL_OFFSET                                0.10f
#define STOPPING_DIST                                0.24f
#define SPHERE_RADIUS                                0.11f
#define ITEM_NEAR_ZONE_DIST                          0.35f
#define FALLBACK_DIST                                0.20f

struct {
    float st[12];
    float ost[12];
    
    float vel;

    float dest[3];
    float attDest[3];
    
    float zone[3];
    float ozone[3];
    
    float initPos[3];
    float initSPS[3];
    float initDir[3];
    int initItemId;
    bool initItemFar;
    
    float initItemPositions[NUM_ITEMS][3];
} data;
    
float MIN_ITEM_RANGES[3];
float MAX_ITEM_RANGES[3];

void init() {
    // Initial state
    api.getMyZRState(data.st);
    api.getOtherZRState(data.ost);
    
    // Update items
    for (int i = 0; i < NUM_ITEMS; i++) {
        float tmp[3];
        game.getItemLoc(tmp, i);
        memcpy(data.initItemPositions[i], tmp, 3 * sizeof(float));
    }
    
    // Find SPS positions
    memcpy(data.initPos, data.st, 3 * sizeof(float));
    float firstState[12];
    data.initItemId = item(data.st, ITEM_TYPE_LARGE, 1e10);
    game.getItemZRState(firstState, data.initItemId);
    float origDist = distance(firstState, data.initPos);
    mathVecScale(&firstState[6], &firstState[6], 0.162f, 3);
    mathVecAdd(data.initSPS, firstState, &firstState[6], 3);
    float initPosToSPS[3];
    mathVecSubtract(initPosToSPS, data.initSPS, data.st, 3);
    float spsDist = mathVecMagnitude(initPosToSPS, 3);
    float offset = 0.50f - spsDist + RADIAL_OFFSET;
    data.initItemFar = spsDist > 0.50f || spsDist > origDist;
    mathVecScale(initPosToSPS, initPosToSPS, (offset < 0.15f ? 0.0f : offset) / spsDist, 3);
    mathVecAdd(data.initSPS, data.initSPS, initPosToSPS, 3);
    mathVecAdd(data.initDir, data.initSPS, &firstState[6], 3);
    
    // Set up range constants
    MIN_ITEM_RANGES[ITEM_TYPE_LARGE] = 0.151f;
    MIN_ITEM_RANGES[ITEM_TYPE_MEDIUM] = 0.138f;
    MIN_ITEM_RANGES[ITEM_TYPE_SMALL] = 0.124f;
    MAX_ITEM_RANGES[ITEM_TYPE_LARGE] = 0.173f;
    MAX_ITEM_RANGES[ITEM_TYPE_MEDIUM] = 0.160f;
    MAX_ITEM_RANGES[ITEM_TYPE_SMALL] = 0.146f;
    
    game.dropSPS();
}

void loop() {
    // Update states
    api.getMyZRState(data.st);
    api.getOtherZRState(data.ost);
    data.vel = mathVecMagnitude(&data.st[3], 3);
    
    // Update position logic
    float destSpeed = move(); 
    float destDist = distance(data.st, data.dest);
    
    // Adjust for close quarters
    if (destDist <= STOPPING_DIST) {
        // MUST ADJUST IF STOPPING_DIST > 0.22
        destSpeed = -0.75f * destDist * destDist + 0.3f * destDist;
        if (data.vel - destSpeed > 0.01f) {
            destSpeed -= 0.02f + 0.5f * (data.vel - destSpeed);
        }
    } 
    
    float velocity[3];
    mathVecSubtract(velocity, data.dest, data.st, 3);
    mathVecNormalize(velocity, 3);
    mathVecScale(velocity, velocity, destSpeed, 3);
    api.setVelocityTarget(velocity);
    
    // Turning
    api.setAttitudeTarget(data.attDest);
}

float move() {
    
    int sps = game.getNumSPSHeld();
    bool holdingItem = game.hasAnyItem();
    float zoneDist = distance(data.st, data.zone);
    
    bool radialCondition = data.initItemFar ? sps == 1 : sps == 2 && holdingItem;
    bool medialCondition = data.initItemFar ? sps == 2 : sps == 1;
    if (radialCondition || medialCondition) {
        if (radialCondition) {
            memcpy(data.dest, data.initSPS, 3 * sizeof(float));
            face(data.initSPS, data.initDir);
        }
        if (medialCondition) {
            float halfway[3];
            mathVecAdd(halfway, data.initPos, data.initSPS, 3);
            mathVecScale(halfway, halfway, 0.5f, 3);
            mathVecPerpendicular(data.dest, data.initSPS, data.initPos, data.initDir);
            mathVecScale(data.dest, data.dest, 0.27f / distance(data.initSPS, data.initPos) + RADIAL_OFFSET, 3);
            mathVecAdd(data.dest, halfway, data.dest, 3);
            memcpy(data.attDest, &data.st[6], 3 * sizeof(float));
        }
        if (distance(data.st, data.dest) <= 0.10f) {
            game.dropSPS();
            sps--;
            game.getZone(data.zone);
            mathVecScale(data.ozone, data.zone, -1, 3);
        }
        else {
            return DEFAULT_SPEED;
        }
    }
    
    for (int i = 0; i < NUM_ITEMS; i++) {
        if (game.itemInZone(i))
            data.initItemId = -1;
    }
    
    int itemId = data.initItemId;
    
    bool olowFuel = api.getTime() > 120;
    if (olowFuel) { 
        itemId = item(data.ozone, ITEM_TYPE_ALL, 0.1f);
        if (itemId == -1)
            itemId = item(data.st, ITEM_TYPE_ALL, 1e10);
    }
    
    // Locate any nearby items (they have higher priority)
    int nearbyItemId = item(data.zone, ITEM_TYPE_ALL, ITEM_NEAR_ZONE_DIST);
    if (nearbyItemId != -1 && data.initItemId == -1) {
        float nearbyItemPos[3];
        getSafeItemLoc(nearbyItemPos, nearbyItemId);
        if (olowFuel || distance(nearbyItemPos, data.st) + zoneDist < distance(data.ost, data.zone)) {
            itemId = nearbyItemId;
        }
    }
    
    // Stay at our zone if we are out of either items or fuel.
    bool lowFuel = game.getFuelRemaining() <= 10.0f;
    if (!holdingItem &&
        zoneDist <= FALLBACK_DIST &&
        (itemId == -1 || lowFuel)) 
    {
        memcpy(data.attDest, &data.st[6], 3 * sizeof(float));
        float toOther[3];
        mathVecSubtract(toOther, data.ost, data.zone, 3 * sizeof(float));
        mathVecNormalize(toOther, 3);
        mathVecScale(toOther, toOther, lowFuel ? 0.0f : 0.025f, 3);
        mathVecAdd(data.dest, data.zone, toOther, 3);
        return DEFAULT_SPEED;
    }
    
    // Contains item's actual position, velocity, etc.
    float itemState[12];
    game.getItemZRState(itemState, itemId);
    
    // Contains the position of the item when it was not moving
    float safeItemPos[3];
    getSafeItemLoc(safeItemPos, itemId);
    int owner = game.hasItem(itemId); 
    if (owner) {
        memcpy(safeItemPos, owner == 2 ? data.ozone : itemState, 3 * sizeof(float));
    }
    
    int itemType = game.getItemType(itemId);
    
    float safeItemCorrectSide[3];
    float separation = (MIN_ITEM_RANGES[itemType] + MAX_ITEM_RANGES[itemType]) * 0.5f;
    mathVecScale(safeItemCorrectSide, &itemState[6], separation, 3);
    mathVecAdd(safeItemCorrectSide, safeItemPos, safeItemCorrectSide, 3);
    
    mathVecScale(data.attDest, &itemState[6], -1, 3);
    
    // Actual item distance vs. safe item distance
    float itemDist = distance(itemState, data.st);
    
    if (!holdingItem && itemId != -1) {
        // Collison imminent if ...
        float safeItemDist = distance(data.st, safeItemPos);
        if (safeItemDist <= STOPPING_DIST && distance(data.st, safeItemCorrectSide) > safeItemDist - 0.65f * separation) {
            float v[3];
            mathVecSubtract(v, data.st, safeItemPos, 3);
            mathVecNormalize(v, 3);
            float w[3];
            mathVecSubtract(w, safeItemCorrectSide, safeItemPos, 3);
            mathVecNormalize(w, 3);
            float h[3];
            mathVecAdd(h, v, w, 3);
            mathVecScale(h, h, separation, 3);
            mathVecAdd(data.dest, safeItemPos, h, 3);
        }
        // Dock item as normal
        else {
            memcpy(data.dest, safeItemCorrectSide, 3 * sizeof(float));
            if (!mathVecMagnitude(&itemState[3], 3) &&
                data.vel <= 0.01f && 
                itemDist >= MIN_ITEM_RANGES[itemType] && 
                itemDist <= MAX_ITEM_RANGES[itemType] &&
                mathVecInner(&data.st[6], data.attDest, 3) >= MAX_FACING_ANGLE) 
            {
                holdingItem = game.dockItem(itemId);
            }
        }
    }
    
    if (holdingItem) {
        face(data.zone, data.st);
        
        mathVecSubtract(data.dest, data.zone, data.st, 3);
        mathVecNormalize(data.dest, 3);
        mathVecScale(data.dest, data.dest, 0.13f, 3);
        mathVecSubtract(data.dest, data.zone, data.dest, 3);
        
        if (distance(itemState, data.zone) <= 0.05f) {
            game.dropItem();        
        }
    }
            
    return DEFAULT_SPEED;
}

void face(float towards[3], float from[3]) {
    mathVecSubtract(data.attDest, towards, from, 3);
    mathVecNormalize(data.attDest, 3);
}

int item(float pos[3], int itemType, float maxDist) {
    float dist = 1e10;
    int itemId = -1;
    
    for (int i = 0; i < NUM_ITEMS; i++) {
        float tmp[3];
        getSafeItemLoc(tmp, i);
        int hasItem = game.hasItem(i);
        if (hasItem == 1) {
            return i;
        }
        int actualType = game.getItemType(i);
        if (!game.itemInZone(i) && (itemType == actualType || (itemType == ITEM_TYPE_ALL && actualType != ITEM_TYPE_SPECIAL && actualType != ITEM_TYPE_ADAPTER))) {
            float origDist = distance(pos, tmp);
            float score = origDist + 0.1f * actualType;
            if (origDist <= maxDist && score < dist) {
                dist = score;
                itemId = i;
            }
        }
    }
    return itemId;
}

float distance(float *a, float *b) {
    float sub[3];
    mathVecSubtract(sub, a, b, 3);
    return mathVecMagnitude(sub,3);
}

void mathVecScale(float *buf, float *vec, float scalar, int n) {
    for (int i = 0; i < n; i++) {
        buf[i] = vec[i] * scalar;
    }
}

void getSafeItemLoc(float *buf, int i) {
    float itemState[12];
    game.getItemZRState(itemState, i);
    if (mathVecMagnitude(&itemState[3], 3) > 0.0f) {
        memcpy(buf, data.initItemPositions[i], 3 * sizeof(float));
    }
    else {
        memcpy(buf, itemState, 3 * sizeof(float));
    }
}

// See http://math.stackexchange.com/questions/137362/how-to-find-perpendicular-vector-to-another-vector
void mathVecPerpendicular(float *buf, float *to, float *from, float *dest) {
    float v[3], d[3], dot;
    mathVecSubtract(v, dest, from, 3);
    memcpy(d, to, 3 * sizeof(float));
    mathVecNormalize(d, 3);
    dot = mathVecInner(v, d, 3);
    mathVecScale(buf, d, dot, 3);
    mathVecSubtract(buf, v, buf, 3);
    mathVecNormalize(buf, 3);
}
//End page main

                        
                      
download
                        
                        //Begin page gameLib
#define DITEM 0.022f

short selectContinuation(){
    if(gameStep==350){
       if((alertAttack || oppSpeed <0.02f || !oppExitFromYourZone) && deltaScore>0.0f)return 350;
       if((statusItem[2]==2 ||statusItem[3] == 2)&& latestItemDropped<2 )return 350;
    }
     if(goOnAttack)return 400;
     if(idItemTarget>=0 && calculateDistance(itemInfo,myState)>0.20f && oppExitFromYourZone){
         selectItem(idItemTarget);
         goOnAttack=true;
     }
     return 400;
 }

void selectItemStarting(){
    float d[2],vt[6],vtg[6],zz[3];
    short k=0,j;
    for(int i=0;i<2;i++){
      j=3*i;    
      game.getItemZRState(itemInfo,idItem+2*i);
      mathVecMult(&vt[j],&itemInfo[6],0.16f,true);
      mathVecAdd(&vt[j],&vt[j],itemInfo,3);
      mathVecSubtract(&vt[j],&vt[j],myState,3);
      mathVecMult(&vtg[j],&itemInfo[6],0.50f,true);
      mathVecCross(zz,&vt[j],&vtg[j]);
      d[i]=0.02f/mathVecMagnitude(zz,3); 
      if(d[i]>0.099f){//
          memcpy(&vtg[j],&itemInfo[6],sizeof(float)*3);
          vtg[1+3*i]=-(vtg[j]*itemInfo[6]+vtg[2+j]*itemInfo[8])/itemInfo[7];
          mathVecMult(&vtg[j],&vtg[j],0.50f,true);
          mathVecCross(zz,&vt[j],&vtg[j]);
          d[i]=0.02f/mathVecMagnitude(zz,3);
      }
    }
    if(d[0]>0.099f && d[0]>d[1]){
        k=3;Ix=1;idItem=idItem+2;
    }
    mathVecAdd(vTarget,myState,&vtg[k],3);
}


void selectItem(short idt){
    idItem=idt;if(idt<0)idItem=idSel;
    Ix=game.getItemType(idItem);
}

void pickItem(){
    float ps,b[3],w[3],v[3];
    float d3=Inf[Ix]+DITEM*0.5f;
    game.getItemZRState(itemInfo,idItem);
    mathVecSubtract(v,myState,itemInfo,3);
    mathVecMult(w,v,1.0f,true);
    ps=mathVecInner(w,&itemInfo[6],3);
    mathVecMult(b,&itemInfo[6],d3,true);
    if(ps<0.65f){//0.70
          mathVecAdd(b,w,&itemInfo[6],3);
          mathVecMult(b,b,d3+0.005f,true);
      }
    mathVecAdd(vTarget,itemInfo,b,3);
    mathVecMult(vAttTarget,&itemInfo[6],-1.0f,true);
}

bool myDockItem(){
    float pp3=Inf[Ix]+DITEM,d=calculateDistance(itemInfo,myState);
    if(d>Inf[Ix] && d<pp3){
        if(mySpeed>0.01f){
                float v[3];
                mathVecMult(v,&myState[3],-0.048f,true);
                api.setForces(v);
            } else if(game.isFacingCorrectItemSide(idItem))return game.dockItem(idItem);
   }

    return false;
}

void getCloserItem(){
    short m,idsel=-1;
    float tempInfo[12];
    float d0=10.0f,d,sc[3]={0.20,0.15f,0.10f};
    idSel=10;deltaScore=0.0f;
    idItemTarget=-1;
    for(int i=0;i<6;i++){
        m=game.hasItem(i); 
        if(m==0){
           if(statusItem[i]==1 ||statusItem[i]==2){
              statusItem[i]=statusItem[i]+2;
           }
        }else {
           statusItem[i]=m; 
        }
          
        
       game.getItemZRState(tempInfo,i); 
       if(statusItem[i]==4 && idItemTarget<0)idItemTarget=i; 
       if(statusItem[i]==0 && mathVecMagnitude(&tempInfo[3],3)<0.01f){
         d=calculateDistance(tempInfo,myState);  
         if(d<=d0){
             idsel=i;d0=d;
         }
       }
       if(idsel>=0){
           if(i==1)idSel=idsel;
           if(idSel>1)idSel=idsel;
       }
       m=game.getItemType(i);
      if(statusItem[i]==3)deltaScore=deltaScore+sc[m];
      if(statusItem[i]==4)deltaScore=deltaScore-sc[m];
    }
}
//End page gameLib
//Begin page main
//ISS_SUBMISSION_FINAL_QUATTRO
//VERSIONE DEL 04-01-2017 H 17:00
// 100% codesize usage. used/allocated = 1795/1800. 
//SUBMISSION 04-01-2017 H: 17.00
bool alertAttack;
bool goOnAttack;
bool zoneOn;
bool evalueZoneDrop;
bool oppExitFromYourZone;
short time;
short idItem;
short idSel;
short idItemTarget;
short Ix;
short latestItemDropped;
short gameStep;
float deltaScore;
float distFromItem;
float distFromOpp;
float oppFromMyZone;
float oppSpeed;
float mySpeed;
short statusItem[6];
float vTarget[3];
float vAttTarget[3];
float Inf[3];
float zoneInfo[4];
float itemInfo[12];
state_vector myState,oppState;

void init(){
  alertAttack=false;goOnAttack=false;
  gameStep=100;idItem=0;Ix=0;
  memset(statusItem,0,sizeof(statusItem));
  Inf[0]=0.151f;Inf[1]=0.138f;Inf[2]=0.124f;
}

void loop(){
  float r,v[3],w[3];   
  time=api.getTime();
  api.getMyZRState(myState);api.getOtherZRState(oppState);
  oppSpeed=mathVecMagnitude(&oppState[3],3);
  mySpeed=mathVecMagnitude(&myState[3],3);
  distFromOpp=calculateDistance(myState,oppState);
  
 if(time==0){
      if(myState[1]<0)idItem=1;
      selectItemStarting();
      game.dropSPS();
  }
game.getItemZRState(itemInfo, idItem);
if(gameStep==100){
    r=moveToFaster(vTarget,1.55f,0.15f,0.055f);//1.475
      if(r<0.04f){
            game.dropSPS();
            selectItem(idItem);
            gameStep=400;
      }
  }
 zoneOn=game.getZone(zoneInfo); 
 if(zoneOn){
    mathVecMult(w,zoneInfo,-1.0f,false);
    mathVecSubtract(v,oppState,w,3);
    mathVecNormalize(v,3);
    r=mathVecInner(v,&oppState[3],3);
    oppExitFromYourZone=r>0 && oppSpeed>0.01f;
    oppFromMyZone=calculateDistance(zoneInfo,oppState);
    mathVecSubtract(v,zoneInfo,oppState,3);
    r=mathVecInner(v,&oppState[3],3)/(oppFromMyZone*oppSpeed);
    alertAttack=oppFromMyZone<0.45f || r>0.915f;//0.35
    }
 

if(gameStep==300){
    if(evalueZoneDrop){
    mathVecSubtract(w,myState,zoneInfo,3);mathVecMult(w,w,calculateDistance(itemInfo,myState),true);
    mathVecAdd(vTarget,zoneInfo,w,3);mathVecMult(vAttTarget,w,-1.0f,true);}
    r=calculateDistance(itemInfo,zoneInfo);evalueZoneDrop=r<0.30f;
    moveToFaster(vTarget,1.55f,0.30f,0.05f);//moveToFaster(vTarget,1.475f,0.30f,0.05f);
    if(r<0.04f ){//if(r<0.04f )
      game.dropItem();gameStep=350;
      goOnAttack=false;
      latestItemDropped=idItem;
      selectItem(-1);
     
    }
  }

 getCloserItem();

 if(gameStep==350){
     gameStep=selectContinuation();
     mathVecSubtract(v,oppState,zoneInfo,3);
     mathVecMult(v,v,0.08f,true);mathVecAdd(v,v,zoneInfo,3);
     moveToFaster(v,1.3f,2.0f,0.05f); 
  }
 
 
  
  if(gameStep==400){
      if(mathVecMagnitude(&itemInfo[3],3)>0.01f){
        selectItem(-1); 
      }
      selectContinuation();
      pickItem();
      moveToFaster(vTarget,1.55f,0.30f,0.055f);//moveToFaster(vTarget,1.475f,0.30f,0.055f);//1.475 
      if(myDockItem()){
            game.dropSPS();
            gameStep=300;
            evalueZoneDrop=true;
        }
  }
 
 
 if(gameStep>100)api.setAttitudeTarget(vAttTarget);
 
}

//End page main
//Begin page moveLib
float moveToFaster(float *target,float k,float dist,float tempSpeed){
    float r,v[3];
    mathVecSubtract(v,target,myState,3);
    r=mathVecMagnitude(v,3);
    if(r>dist && k>1.0f)k=tempSpeed;
    //if(distFromOpp<0.30f && time>10)k=oppSpeed*0.85f;//0.45-0.75
    if(k<1.0f){
        if(r>dist){
              mathVecMult(v,v,k,true);
              api.setVelocityTarget(v);   
              return r;
            }
       k=1.0f;     
    }
    mathVecMult(v,v,k,false);
    mathVecAdd(v,v,myState,3);
    api.setPositionTarget(v);
    return r;
}




float calculateDistance(float *v1, float *v2){
    float v[3];
    mathVecSubtract(v,v1,v2,3);
    return mathVecMagnitude(v,3);
}

void mathVecMult(float res[3],float src[3],float mag,bool norm){
    memcpy(res,src,sizeof(float)*3);
    if(norm)mathVecNormalize(res,3);
    res[0]*=mag; res[1]*=mag; res[2]*=mag;
}
//End page moveLib

                        
                      
download
                        
                        //Begin page main
/*Changes needed:
glitches:
myZone is not identified
Occasional out of bounds
*/



//State variables
float myState[12];
float enState[12];//Making this six instead of 12 saves us 48 words... But breaks the code
// macros that point to parts of state array
// ( "&" returns the pointer)
#define myPos (&myState[0])
#define myVel (&myState[3])
#define myAtt (&myState[6])
#define myRot (&myState[9])
#define enPos (&enState[0])
#define enVel (&enState[3])
#define enAtt (&enState[6])
#define enRot (&enState[9])

#define ROUND_LENGTH 180
int targetItem;

float myScore;
float enScore;
float positionTarget[3];

bool carryingItem;
// shorthand for printing vectors
#define PRINTVEC(str, vec) DEBUG(("%s %f %f %f", str, vec[0], vec[1], vec[2]))
#define PRINTBOOL(str, bool) DEBUG(("%s is %s",str, bool?"true":"false"))
// float pickUpDist;
// //                                   large            small    medium
// #define DOCK_OUTER_DIST(id) ( id<2 ? 0.173f : (id>3 ? 0.146f : 0.16f))
// #define DOCK_INNER_DIST(id) ( id<2 ? 0.151f : (id>3 ? 0.124f : 0.138f))
// #define DOCK_DIST(id) ( id<2 ? 0.162f : (id>3 ? 0.135f : 0.149f))
//                                   large      medium
#define DOCK_OUTER_DIST(id) ( id<2 ? 0.173f : 0.16f)
#define DOCK_INNER_DIST(id) ( id<2 ? 0.151f : 0.138f)
#define DOCK_DIST(id) ( id<2 ? 0.162f : 0.149f)


//float zeroVec[3];
float vcoef;
// bool justPickedUp;
bool enAttacking;

#define targetItemAtt (&itemLoc[6])
float enToMyZone[3];

float startingPos[3];
// float distFromEnToMyZone;
bool switchBestItem;
bool itemOnThirdLeg;
float accel;
float secondSPS[3];
float itemLoc[12];
void init() {
    //accel = .0152f;////////////////////////Was .014f
    DEBUG(("Clone 1.8.3B"));

    
    // adjust positionTarget PID gains
    #define SPEEDCONST 0.45f//.45 or .55 is 37 secs
    #define DERIVCONST 2.8f//2.8 or 3.3 is 37 secs
    api.setPosGains(SPEEDCONST,0.1f,DERIVCONST);
    //#define ATTCONST .4
    //api.setAttGains(ATTCONST,.1,8*ATTCONST);
    api.getMyZRState(myState);
    
    
    
    //carryingItem=false;
	//memset(zeroVec, 0.0f, 12);
	memcpy(startingPos, myPos, 12);
	//myScore = 0.0f;
	//enScore = 0.0f;
	vcoef = .08f; //alternately, 2.5*0.07 (which isn't .154)
	//justPickedUp = false;
	enAttacking=false;
    //memset(enZone, 100.0f, 12);
    switchBestItem = false;
    float maxPoints = -100000;
    for (int i=0; i<4; i++) {
        float loc[12]; //using itemLoc instead does not save space
        #define att (&loc[6])
        game.getItemZRState(loc, i);
        float dockPos[3];
        float usToPickup[3];
        for (int j=0;j<3;j++){
            dockPos[j]=att[j]*DOCK_DIST(i)+loc[j];
            usToPickup[j]=dockPos[j]-myPos[j];
        }
        float distToPickupMinus;
        distToPickupMinus = dist(myPos,dockPos)-.58f;
        float points;
        points = (distToPickupMinus>0?-distToPickupMinus:distToPickupMinus) * (1+1000*(distToPickupMinus<-.05f))//////////////////////////////////////////////////////////////////////////////////////////////////////////
         + 0.1f*(game.getItemType(i)==ITEM_TYPE_LARGE);///////////////////////////////////////////////////////////////////////
        DEBUG(("points %i %f", i, points));
        if (points>maxPoints) {
            maxPoints=points;
            targetItem=i;
            itemOnThirdLeg=angle(usToPickup,att)<2.3f;
        }
        //DEBUG(("Points %f Best: %i", points, i));
    }
    game.dropSPS();
    //itemLoc[5]=0.0f;
    carryingItem = false;

}


void loop(){
    float myDeltaScore;
    float enDeltaScore;
    float fvector[3], flocal;
    //DEBUG(("3rdleg %d",itemOnThirdLeg));
    int numSPSHeld = game.getNumSPSHeld();
    // justPickedUp = false;
    myDeltaScore = (myScore<.01f)?0:(game.getScore()-myScore);
    enDeltaScore = game.getOtherScore()-enScore;
    //Finds current score increase
    float myZone[4];
    
    
    
    myScore = game.getScore();
    enScore = game.getOtherScore();
	api.getMyZRState(myState);
    api.getOtherZRState(enState);
    mathVecAdd(fvector,enPos,myZone,3);//Same as subtracting their Zone
    flocal = (numSPSHeld==0)?mathVecMagnitude(fvector,3):10;//Used Later
    //refresh bestItem if the opponent has picked up our item
    bool currentItemMoving;
    int currentTime = api.getTime();
    currentItemMoving = itemLoc[3]>0.001f or itemLoc[3]<-0.001f;

    if (switchBestItem or game.hasItem(targetItem)==2 or (flocal>.2f and flocal<.25f) or  ((currentItemMoving or dist(enPos,itemLoc)<.04f) and currentTime>3)){
        float maxPoints = -3000;
        int oldTarget;
        oldTarget = targetItem;
        targetItem=-1;
        DEBUG(("picking new item"));
        for (int i=0; i<4; i++) {
            float loc[3]; //using itemLoc instead does not save space
            game.getItemLoc(loc, i);
            
            //if (points>5) points=5;//So this takes no codesize, apparently...
            //This is changed to reflect SANTA's habit of getting a medium item first
            mathVecSubtract(fvector,loc,enPos,3);
            float points=1/dist(myPos,loc);
            points += 300*((game.getItemType(i)==ITEM_TYPE_LARGE and angle(fvector,enVel)>2))
            + 350*(dist(myPos,myZone)<.4f and dist(loc,myZone)<(mathVecMagnitude(myZone,3)/2.7f))
            + -2000*(dist(enPos,loc)<.2f or (targetItem==oldTarget && currentItemMoving)) 
            + -6000 * (game.itemInZone(i)/* and game.hasItem(i)!=1*/)
            + 4000*(game.hasItem(i)==1);
            //                                      using fabsf takes more space
            if (points>maxPoints) {
                maxPoints=points;
                //currentTargetItem=i;
                targetItem=i;
            }
            //targetItem = currentTargetItem;
            //DEBUG(("Points %f Best: %i", points, i));
        }
        
        DEBUG(("Target item changed to %d", targetItem));
        switchBestItem=false;
        DEBUG(("MaxPoints: %f", maxPoints));
    }
    DEBUG(("targetItem %d",targetItem));
    if (numSPSHeld==0 or !carryingItem){
        game.getItemZRState(itemLoc, targetItem);
    }



    if (numSPSHeld > 0){

        
        if ((numSPSHeld!=2) ^ itemOnThirdLeg){
            //First SPS: try to get futher from targetItem
            accel = .0155f;
            float perpendicularVector[3];
            float startingPosToItem[3];
            for(int i=0;i<3;i++){
                startingPosToItem[i]=(itemOnThirdLeg?(itemLoc[i]+itemLoc[i+6]*DOCK_DIST(targetItem)):secondSPS[i])-startingPos[i];
            }
            float tempPerp[3];
            mathVecCross(tempPerp,targetItemAtt,startingPosToItem);
            // DEBUG(("Vector dot of perpVec and startingToItem after forst xp: %f",mathVecInner(perpendicularVector,startingPosToItem,3)));
            mathVecCross(perpendicularVector,startingPosToItem,tempPerp);
            DEBUG(("Vector dot of perpVec and startingToItem after second xp: %f",mathVecInner(perpendicularVector,startingPosToItem,3)));
            // PRINTVEC("perpVec: ",perpendicularVector);
            // PRINTVEC("tempPerp: ",tempPerp);
            scale(perpendicularVector,0.35f/mathVecMagnitude(startingPosToItem,3)/mathVecMagnitude(perpendicularVector,3));//.3 = .125*2+ epsilon
            DEBUG(("altitude: %f",mathVecMagnitude(perpendicularVector,3)));
            scale(startingPosToItem, 0.5f);
            mathVecAdd(positionTarget,perpendicularVector,startingPosToItem,3);
            mathVecAdd(positionTarget,startingPos,positionTarget,3);
            
            // for (int j=0;j<3;j++){
            //     positionTarget[j]=startingPos[j]+perpendicularVector[j]+startingPosToItem[j]/2;
            // }
            if (dist(myPos,positionTarget) < 0.08f){
                game.dropSPS();
            }
        }
        else{
            if ( getItem() ){
                
                game.dropSPS();
                memcpy(secondSPS,myPos,12);
            }
        }
    }
    ////////////////STANDARD OPERATION
    else {
        game.getZone(myZone);
        flocal = dist(itemLoc, myZone);
        //PRINTBOOL("enAttacking", enAttacking);
        mathVecSubtract(enToMyZone,myZone,enPos,3);
        // distFromEnToMyZone = mathVecMagnitude(enToMyZone,3);
        float angleOfAttack = angle(enVel, enToMyZone);
        
        if (dist(enPos,myZone)>0.6f and currentTime<165)
            enAttacking = false;
        float toZone[3];
        mathVecSubtract(toZone, myZone, myPos, 3);
        float distToZone = mathVecNormalize(toZone,3);
        if (carryingItem){
            vcoef = 0.08f;
            
            
            api.setAttitudeTarget(toZone);
            scale(toZone, distToZone - DOCK_DIST(targetItem));
            mathVecAdd(positionTarget, toZone, myPos, 3);
            //DEBUG(("Rad: %f", ((.1f-myZone[3]>.01f) ? (.1f-myZone[3]) : .01f)));
            DEBUG(("dist to zone %f", dist(fvector, myZone)));
            PRINTVEC("itemLoc",itemLoc);
            if (flocal<((.1f-myZone[3]>.018f) ? (.1f-myZone[3]) : .018f)) {
                //Drops item in zone when close enough
                game.dropItem();
                carryingItem=false;
                switchBestItem=true;
            }
        }
        else if (enAttacking || (myDeltaScore>=(enDeltaScore+((myScore<enScore)?.01f:0))
            && dist(enPos,myZone)>distToZone && (angleOfAttack<0.37f or myDeltaScore>.34f))){//.6f needs testing
            enAttacking = true;
            //mathVecSubtract(positionTarget, enPos, myZone, 3);
            //scale(positionTarget, 0.05f/mathVecMagnitude(myZone,3));

            memcpy(positionTarget,enToMyZone,12);
            scale(positionTarget, -0.05f/mathVecMagnitude(myZone,3));
            
            mathVecAdd(positionTarget, positionTarget, myZone, 3);
            DEBUG(("GUARD"));
        }
        else if (game.hasItem(targetItem)!=1){
            DEBUG(("myDeltaScore: %i  enDeltaScore: %i",myDeltaScore,enDeltaScore));
            vcoef = (myDeltaScore>.19f and enDeltaScore<.01f)?0:0.06f;
            getItem();
            
        }
        
    }
    /////////////////////moving script
    float distance;  //Can be as high as .0175
    mathVecSubtract(fvector, positionTarget, myPos, 3);
    distance = mathVecNormalize(fvector, 3);
    if (distance > 0.05f) {
        flocal = vcoef;
        float max;
        max = sqrtf((distance-.04f)*accel);//Was .4
        if (flocal>max){
            flocal = max;
        }
        float aPlusVel = mathVecMagnitude(myVel,3)+accel;
            // (acceleration + velocity)
        if (flocal>aPlusVel){
            flocal = aPlusVel;
        }
        scale(fvector, flocal);
        api.setVelocityTarget(fvector);
        //PRINTVEC("positionTarget", positionTarget);
        //PRINTVEC("vtar",fvector);
        //DEBUG(("MV: %f",vcoef));
        //DEBUG(("vmag: %f", flocal));

    }
    else {
        api.setPositionTarget(positionTarget);
    }
}   

bool getItem() {
    //Prerequisites:
    /*
    float myState[12];
    api.getMyZRState(myState);
    #define DOCK_OUTER_DIST(id) ( id<2 ? 0.173f : (id>3 ? 0.146f : 0.16f))
    #define DOCK_INNER_DIST(id) ( id<2 ? 0.151f : (id>3 ? 0.124f : 0.138f))
    The scale function
    Our (BACON's) movement algorithm is verified to work well with this
    */
    float toItem[3];
    float itemState[12];
    game.getItemZRState(itemState,targetItem);
    #define itemAtt (&itemState[6])
    accel = .0154f;

    float itemPickupDist = DOCK_INNER_DIST(targetItem);//+0.011f;
    mathVecSubtract(toItem, itemState, myPos, 3);
    float dynamicVector[3];//Will eventually become positionTarget
    memcpy(dynamicVector,toItem,12);
    float itemDist = mathVecMagnitude(toItem, 3);
    float targetDist = itemPickupDist+((itemDist<itemPickupDist+.011f)?0.022f:0.004f);
    

    scale(dynamicVector,(targetDist*-.875f)/itemDist -.125f);// now contains the target vector based from the item, unrotated
    float perpendicularVector[3]; // axis
    mathVecCross(perpendicularVector,dynamicVector,itemAtt);
    mathVecNormalize(perpendicularVector,3);

    float angleOfChange = sqrtf(angle(dynamicVector,itemAtt)/3);
    // #define angleOfChange 0.15f
    float orthoKK[3];
    mathVecCross(orthoKK,perpendicularVector,dynamicVector);
    for (int i=0;i<3;i++){
        positionTarget[i]=dynamicVector[i]*cosf(angleOfChange)+orthoKK[i]*sinf(angleOfChange)
        + perpendicularVector[i] * mathVecInner(perpendicularVector,dynamicVector,3) * (1 - cosf(angleOfChange)) + itemState[i];
    }
    
    
    //rotateAround(positionTarget,perpendicularVector,,positionTarget);
    //mathVecAdd(positionTarget,positionTarget,itemState,3);

    //DEBUG(("Who has it?: %d", game.hasItem(targetItem)));
    //memcpy(positionTarget, targetPos, 12);
    //Our code plugs positionTarget into our movement algorithm - modify as needed 
    //mathVecSubtract(toItem, itemPos, myPos, 3);
    scale(itemAtt,-1); // changes itemState
    api.setAttitudeTarget(itemAtt);//Points in the correct docking direction
    if(mathVecMagnitude(myVel,3)<0.01f && itemDist<itemPickupDist+.022f
    && itemDist>itemPickupDist && angle(myAtt,toItem)<0.25f
     && game.isFacingCorrectItemSide(targetItem)) {//Conditions for docking
        //DEBUG(("attempting to dock"));
        carryingItem = game.dockItem(targetItem);
        // pickUpDist = itemDist;
        // game.getItemLoc(itemLoc, targetItem);
        // justPickedUp = carryingItem;
        return carryingItem;
    }
    return false;
}

void scale (float* vec, float scale) {
    for (int i=0; i<3; i++) {
        vec[i] *= scale;
    }
}

// magnitude of difference between two vectors.
// with position vectors, it is the distance between the points
float dist(float* vec1, float* vec2) {
    float diff[3];
    mathVecSubtract(diff, vec1, vec2, 3);
    return mathVecMagnitude(diff,3);
}

// use dot product to compute the angle between 
// two vectors in radians
float angle(float* vec1, float* vec2) {
    return acosf(mathVecInner(vec1,vec2,3)/(mathVecMagnitude(vec1,3)*mathVecMagnitude(vec2,3)));
}

//End page main

                        
                      
download
                        
                        //Begin page main
float myState[12];
float initState[12];
#define myVel &myState[3]
#define myAtt &myState[6]
float zone[4];
float vecZone[3];
float item[9][12];
float initItem[9][12];
float vec[9][3];
float distance[9];
float SPSpos[3];
char step;
char ID;
char firstItem;
char OOB;
int t;

void init() {
    step = t = 0;
    api.getMyZRState(initState);
    game.dropSPS();
}

void loop() {
    api.getMyZRState(myState);
    for (char i = 0; i < 9; i++) {
        if (t == 0) {
            game.getItemZRState(initItem[i], i);
        }
        game.getItemZRState(item[i], i);
        mathVecSubtract(vec[i], item[i], myState, 3);
        distance[i] = mathVecMagnitude(vec[i], 3);
    }
    if (game.getNumSPSHeld() == 0) {
        game.getZone(zone);
        mathVecSubtract(vecZone, zone, myState, 3);
    }
    if (t == 0) {
        ID = firstItem = closestItem(2, 3);
    }
    calcSPS(initItem[ID]);
    OOB = 0;
    if (fabsf(SPSpos[0]) > 0.64f || fabsf(SPSpos[1]) > 0.8f || fabsf(SPSpos[2]) > 0.64f) {
        if (t == 0) {
            ID = firstItem = closestItem(0, 1);
        }
        OOB = 1;
    }
    if (game.hasItem(ID) == 2) {
        ID = firstItem = closestItem(0, 3);
    }
    
    if (step == 0) {
        placeSPS();
    }
    else if (step == 1) {
        pickItem();
    }
    else {
        defendZone();
    }
    t++;
}

void placeSPS() {
    float newTarget[3];

    mathVecSubtract(newTarget, item[ID], initState, 3);
    multVec(newTarget, 0.5f, 1);
    mathVecAdd(newTarget, newTarget, initState, 3);
    if (OOB == 1) {
        calcSPS(newTarget);
    }
    
    api.setAttitudeTarget(vec[ID]);
    if (game.getNumSPSHeld() == 2) {
        moveCapVelocity(SPSpos);
        if (dist(myState, SPSpos) < 0.05f) {
            game.dropSPS();
        }
    } 
    else if (OOB == 1 && game.getNumSPSHeld() == 1) {
        moveCapVelocity(newTarget);
        if (dist(myState, newTarget) < 0.05f) {
            game.dropSPS();
        }
    }
    else { 
        pickItem();
        if (game.hasItem(ID) == 1) {
            game.dropSPS();
        }
    }
}

void pickItemOld() {
    float dmax;
    float dmin;
    float vecAtt[3];
    float a[3];
    float target[3];
    float A[3];
    float cosalpha = 0.0f;

    if (ID <= 1) {
        dmax = 0.173f;
        dmin = 0.151f;
    }
    else if (ID <= 3) {
        dmax = 0.160f;
        dmin = 0.138f;
    }
    else {
        dmax = 0.146f;
        dmin = 0.124f;
    }
    
    if (game.hasItem(ID) != 1) {
        memcpy(vecAtt, &item[ID][6], 3*sizeof(float));
        for (int i = 0; i < 3; i++) {
            vecAtt[i] *= (dmin+dmax)/2.0f;
            a[i] = -vec[ID][i];
            target[i] = (a[i]+vecAtt[i])/2.0f;
        }
        mathVecAdd(vecAtt, vecAtt, item[ID], 3);
        mathVecSubtract(A, vecAtt, myState, 3);
        mathVecNormalize(target, 3);
        for (int i = 0; i < 3; i++) {
            target[i] *= ((dmax+dmin)/2.0f)+0.05f;
        }
        mathVecAdd(target, target, item[ID], 3);
        if (mathVecInner(vec[ID], A, 3) < mathVecMagnitude(A, 3)*mathVecMagnitude(A, 3)) {
            moveCapVelocity(target);
        } else {
            moveCapVelocity(vecAtt);
        }
        api.setAttitudeTarget(vec[ID]);
        cosalpha = mathVecInner(myAtt, vec[ID], 3)/(mathVecMagnitude(myAtt, 3)*mathVecMagnitude(vec[ID], 3));
        if (distance[ID] < dmax && mathVecMagnitude(myVel, 3) < 0.01f && cosalpha > 0.97f && game.isFacingCorrectItemSide(ID)) {
            game.dockItem(ID);
        }
    } else if (step == 1) {
        sbocciaItem();
    } else {
        reachZone();
    }
}

void pickItem() {
    float dmax;
    float dmin;
    float targetPos[3];
    float vecToTarget[3];
    float itemAtt[3];
    float orthagComponent[3];

    //dmax = dimn = 0 if ID == 7 || ID == 8
    if (ID <= 1) {
        dmax = 0.173f;
        dmin = 0.151f;
    }
    else if (ID <= 3) {
        dmax = 0.160f;
        dmin = 0.138f;
    }
    else {
        dmax = 0.146f;
        dmin = 0.124f;
    }

    if (game.hasItem(ID) != 1) {
        
        for (int i = 0; i < 3; i++) {
            targetPos[i] = itemAtt[i] = orthagComponent[i] = item[ID][i+6];
        }
        
        multVec(targetPos, dmin+(dmax-dmin)*0.3f, 0);
        mathVecAdd(targetPos, targetPos, item[ID],3);
        mathVecSubtract(vecToTarget, myState, targetPos,3);
    
        //this value if is positive if we need to go around the item
        //distance between position and plane of correct face of item
        float D = mathVecInner(itemAtt,targetPos,3);
        float pointPlaneDist = mathVecInner(itemAtt,myState,3)-D;

    
        if (pointPlaneDist < -0.015f) {
            float target[3];
            //projection of the distance vec (sattelite -> item) onto plane of correct face of item
            //http://maplecloud.maplesoft.com/application.jsp?appId=5641983852806144
            float projVecPlane[3];
            multVec(orthagComponent, mathVecInner(itemAtt,vecToTarget,3)/(mathVecMagnitude(itemAtt,3)), 0);
            mathVecSubtract(projVecPlane,vecToTarget,orthagComponent,3);
            multVec(projVecPlane, dmin * 1.15f, 1);
            mathVecAdd(target,targetPos,projVecPlane,3);
            moveCapVelocity(target);
        }
        else {
            moveCapVelocity(targetPos);
        }
        multVec(itemAtt, -1.0f, 0);
        api.setAttitudeTarget(itemAtt);
    
        float cosalpha = mathVecInner(myAtt, vec[ID], 3)/(mathVecMagnitude(myAtt, 3)*distance[ID]);
        if (distance[ID] < dmax && mathVecMagnitude(myVel, 3) < 0.01f && cosalpha > 0.97f && game.isFacingCorrectItemSide(ID)) {
            game.dockItem(ID);
        }
    } else if (step == 1){
        sbocciaItem();
    } else {
        reachZone();
    }
}

void reachZone() {
    float vecZoneToMe[3];
    mathVecSubtract(vecZoneToMe, myState, zone, 3);
    mathVecNormalize(vecZoneToMe, 3);
    for (int i = 0; i < 3; i++){
        vecZoneToMe[i] *= dist(item[ID], myState);
    }
    mathVecAdd(vecZoneToMe, vecZoneToMe, zone, 3);
    moveCapVelocity(vecZoneToMe);
    api.setAttitudeTarget(vecZone);
    if (dist(item[ID], zone) < 0.04f) {
        game.dropItem();
        step++;
        if (step == 1) {
            for (int i = 5; i >= 0; i--) {
                if (game.hasItemBeenPickedUp(i) == 1 && i != firstItem) {
                    ID = i;
                }
            }
            if (ID == firstItem) {
                ID = closestItem(0, 1);
            }
        }
    }
}

void defendZone() {
    float otherState[12];
    float vecD[3];
    
    api.getOtherZRState(otherState);
    mathVecSubtract(vecD, otherState, zone, 3);
    multVec(vecD, 0.1f, 1);
    mathVecAdd(vecD, vecD, zone, 3);
    moveCapVelocity(vecD);
}

int closestItem(int x, int y) {
    char id = -1;
    float d = 20.0f;

    do {
        for (int i = y; i >= x; i--) {
            if (distance[i] < d && game.itemInZone(i) == 0 && game.hasItem(i) != 2) {
                d = distance[i];
                id = i;
            }
        }
        x = 0;
        y = 5;
        d = 20.0f;
    } while (id == -1);
   
    return id;
}

void moveCapVelocity(float *whereTo){
    if (dist(myState, whereTo) < 0.15f){
        api.setPositionTarget(whereTo);
    }
    else{
        float v[3];
        mathVecSubtract(v, whereTo, myState, 3);
        multVec(v, 0.041f, 1);
        api.setVelocityTarget(v);
    }
}

float dist(float *a, float *b) {
    float sub[3];
    mathVecSubtract(sub, a, b, 3);
    return mathVecMagnitude(sub,3);
}

void multVec(float* vec, float mult, bool pl) {
    if(pl == 1){
        mathVecNormalize(vec,3);
    }
    vec[0] *= mult;
    vec[1] *= mult;
    vec[2] *= mult;
}

void calcSPS(float target[]) {
    float startDist;
    float vecAtt[3];
    float vecy[3];
    float vecm[3];
    
    mathVecSubtract(vecm, target, initState, 3);
    startDist = mathVecMagnitude(vecm, 3);
    /*for (int i = 0; i < 3; i++) {
        vecm[i] = vecm[i]/2.0f;
        vecAtt[i] = item[ID][i+6];
    }*/
    multVec(vecm, 0.5f, 0);
    memcpy(vecAtt, &item[ID][6], sizeof(vecAtt));
    mathVecCross(vecy, vecAtt, vecm);
    mathVecCross(SPSpos, vecm, vecy);
    mathVecAdd(vecAtt, vecAtt, item[ID], 3);
    mathVecAdd(vecm, vecm, initState, 3);
    multVec(SPSpos, 0.18f/startDist + 0.2f, 1);
    mathVecAdd(SPSpos, SPSpos, vecm, 3);
}

void sbocciaItem () {
    float vecOtherZone[3];
    float otherZone[3];
    
    memcpy(otherZone, zone, 3*sizeof(float));
    multVec(otherZone, -1.0f, 0);
    if (dist(item[0], otherZone)+dist(item[1], otherZone) < 0.3f && t > 130) {
        mathVecSubtract(vecOtherZone, myState, otherZone, 3);
        api.setAttitudeTarget(vecOtherZone);
        if (dist(item[ID], otherZone) > 0.13f) {
            game.dropItem();
            ID == 0 ? ID = 1 : ID = 0;
        }
    } else {
        reachZone();
    }    
}
//End page main

                        
                      
download
                        
                        //Begin page main
//WGTH
//BA BA BA
//BBBBEEEEENEEEEE
//OK CIAO GRAZIE
//TOGLIERE TUTTOOOOOOO

#define Z_DUE 0.2f
#define Z_CINQUE 0.5f
#define Z_SC 0.65f

float err[2][3], item[2][3], dist, area[4], num, min, max, zritem[12],zritem1[3], itemloc[4][3], oldscoreAvv, oldscore, scorefineNoi, scorefineAvv, score, scoreOP;
float reference[3];
float vel_OP; 
short int stato, c, t, cont;
bool b, warning, pass,en;

ZRState mystate, OP;

void init()
{
    b  = warning = pass = en = false;
    stato = 1;
    
    game.dropSPS();

    api.getMyZRState(mystate);
    
    for(int i=0; i<4; i++)  game.getItemLoc(itemloc[i], i);
    
    searchitem_VAR(1,2);
    game.getItemZRState(zritem,c);
   
    snm(err[0], zritem, mystate);
    
    if(mathVecInner(&zritem[6], err[0], 3) > -Z_DUE) 
    {
        stato=0; 
        pass=1;
    }
    
    scale_add(zritem1,err[1], Z_DUE, &zritem[6], zritem);
    
    if( snm(err[0], zritem1, mystate) < 0.6f)  
    {
        warning=1;
        scale_add(zritem1,err[0], 0.6f, err[0], mystate);
    } 

    tria();
    DEBUG(("GIORGIO SALI!!!"));
    
}

void loop()
{
    api.getMyZRState(mystate);
    api.getOtherZRState(OP);
    
    game.getZone(area);
    
    t = game.getCurrentTime();
    
    score = game.getScore();
    scoreOP = game.getOtherScore();
    vel_OP = mathVecMagnitude(&OP[3], 3);
    
    scorefineNoi = (score - oldscore) * (180 - t) + score;
    scorefineAvv = (scoreOP - oldscoreAvv) * (180 - t) + scoreOP;
    
    DEBUG(("STATO :::::::::: %d",stato));
    switch(stato)
    {
        
        case 2:

            scale_add(item[1],err[1], -0.15f, err[1], &area[0]);
            POS(item[1],3);
        
            dist = calcoli_e_attitude(&area[0], 0);
            if (dist < 0.18f && dist > 0.09f && mathVecInner(err[1], &mystate[6], 3) > 0.98f)
            {
                b = 0;
                game.dropItem();
                en = 1;
                cont=0;
                memcpy(reference, area, sizeof(float) * 3);
                stato=10;
            }

        break;
        
        case 0:
        
            POS(item[0],2.3f);
            
            if(snm(err[0], item[0], mystate) < 0.15f) 
            {   
                if(pass)
                { 
                    stato = 1; 
                    if(warning) stato = 100; 
                }
                else stato = 2;
                game.dropSPS();
            }
            
        break;
        
        case 100:
        
            POS(zritem1,2.3f);
            
            calcoli_e_attitude(zritem, 1);
            
            if( snm(err[0], zritem1, mystate) <0.2f) 
            {
                if(pass)stato=1; 
                else stato=0; 
                warning=0; 
                game.dropSPS();
            }
            
        break;
        
        case 1:
        
        if(en)  searchitem_VAR(0,1);
        //DEBUG(("  c ==== %d ",c));
        
        game.getItemZRState(zritem, c);
        score = snm(err[0], zritem, mystate); 
        
        if (mathVecMagnitude(&zritem[3], 3) > 0) memcpy(zritem, itemloc[c], sizeof(float) * 3);
        
        if (c < 2)  {num = 0.163f; max = 0.173f; min = 0.151f;} 
        else        {num = 0.147f; max = 0.157f; min = 0.138f;}
        
        if (score < max && score > min && mathVecMagnitude(&mystate[3], 3) < 0.009f && game.isFacingCorrectItemSide(c)) b=game.dockItem(c);
    
        scale_add(item[1],err[1], num, &zritem[6], zritem);
    
        calcoli_e_attitude(zritem, 1);

        if (mathVecInner(err[1], &zritem[6], 3) > -Z_DUE)
        {
            for (int i = 0; i < 2; i++) item[2][i] = Z_CINQUE / (zritem[6 + i]);
            item[2][2] = -1 / (zritem[8]);

            snm(err[0], mystate, zritem);
            
            mathVecNormalize(item[2], 3);

            if (mathVecInner(item[2], err[0], 3) < 0) mathVecScale(item[2], item[2], -1);

            scale_add(item[1],item[2], Z_DUE, item[2], item[1]);
         }

        POS(item[1],2.8f);
            
        if(b)  
        {
            if(!warning) game.dropSPS();
            if (pass || en) stato=2;
            else
            {
                if(warning)  stato=100; 
                else  stato=0;
            }
        }
            
        
    break;
    
    case 10:
    
        POS(&area[0],5.1f);
        
        if(t%5 == 0 && t>105)
        {
            if(vel_OP > num-0.0007f && vel_OP < num && scorefineNoi<scorefineAvv) {stato=1; mathVecScale(reference, area, -1);}
            num = vel_OP; 
        }
        
        min = mathVecNormalize(&OP[3],3);         
        
        if(min<0.004f) cont++;
        else cont=0;
        
        DEBUG((" cont == %d ",cont));
        if(scoreOP > oldscoreAvv && snm(err[0], &area[0], OP)>Z_CINQUE && ((mathVecInner(&OP[3],err[0] , 3)<Z_DUE  && min>0.025f) || (cont>7 && scorefineNoi<scorefineAvv))) 
            stato=1;
               
    break;
    
    }

    oldscoreAvv = scoreOP;
    oldscore = score;

}


//funzioni---------------------------------------------------------------------------
void mathVecScale(float res[3], float src[3], float mag)
{
    for (int i = 0; i < 3; i++)  res[i] = src[i] * mag;
}

void POS(float ret[3],float rt)
{
    api.setPosGains(0.4f,0,rt);
    api.setPositionTarget(ret);
    
}

float calcoli_e_attitude(float v[3], bool inv)
{
    float w;
    float mystateQ[13];
    float Quat[4];
    api.getMySphState(mystateQ);
    w = snm(err[1], v, mystate);
    mathVecScale(err[0], &zritem[6], -1);
    api.attVec2Quat(&mystate[6], err[!inv], &mystateQ[6], Quat);
    api.setQuatTarget(Quat);
    return w;
}

void searchitem_VAR(bool wq,int rey)
{
    float db,dbm, al[3], bound[3];
    short int n;
    
    bound[0]=bound[2]=Z_CINQUE;
    bound[1]=Z_SC;
    
    if(wq)
    {
        if(mystate[1]>0) n = 2; 
        else             n = 3;   
    }
    else n=3;
    
    dbm=100;
    
    for (int i = n; i >= 0; i-=rey)
    {
        game.getItemZRState(zritem,i); 
        
        if(wq)
        {
            scale_add(item[1],al, Z_DUE, &zritem[6], zritem);
        
            if(fabs(item[1][0])>Z_CINQUE || fabs(item[1][1])>0.65f || fabs(item[1][2])>Z_CINQUE)  db = snm(err[0], item[1], bound);
            else db = 0;
        
        }
        else db = snm(err[0], zritem, reference);
        
        if(!game.hasItem(i) && !game.itemInZone(i) && db <= dbm)
        {
            dbm = db;
            c = i;
        }
    
    }
    
    
}

float snm(float o3[3], float o1[3], float o2[3])
{
    mathVecSubtract(o3, o1, o2, 3);
    return mathVecNormalize(o3, 3);
}

void scale_add(float resu[3],float e[3], float w, float k[3], float q[3])
{
    mathVecScale(e, k, w);
    mathVecAdd(resu, e, q, 3);
}

void tria()
{
    float CC[3],scal,startP[3],RIS[3];
    
    startP[0]=zritem[7]/3;  
    startP[1]=mystate[1]+zritem[8]/3; 
    startP[2]=-zritem[6]/4;
    
    snm(item[1], zritem1, mystate);
    
    scal = mathVecInner(&zritem[6], item[1], 3);
    
    if(fabs(scal)>0.9f) snm(item[1], zritem1, startP);
    
    mathVecCross(CC,item[1], &zritem[6]); 
    
    mathVecCross(RIS,CC,item[1]);
    
    mathVecAdd(item[0],RIS, mystate, 3);  
    mathVecNormalize(item[0], 3);
    
    mathVecScale(item[0], item[0], Z_SC);
    
    if(item[0][1]>Z_SC)  item[0][1]=Z_SC;
    if(item[0][1]<-Z_SC) item[0][1]=-Z_SC;
    
    for(int i=0;i<3;i+=2)
    {
        if(item[0][i]>Z_CINQUE) item[0][i]=0.49f;
        if(item[0][i]<-Z_CINQUE) item[0][i]=-0.49f;
    }
    
}
//End page main

                        
                      
download
                        
                        //Begin page Functions
#define TIME_DIST_COEFFICIENT   31.0f//30.481f old pids //27.4 old - increased to account for angle factor

float dist(float target[3], float pos[3], bool time){
    float output[3];
    mathVecSubtract(output,pos,target,3);
    output[0] = mathVecMagnitude(output,3);
    if(time){
        if(output[0] < 0.171f) output[0] = 0.0f;
        output[0] = TIME_DIST_COEFFICIENT * sqrtf(output[0]);//cbrtf(output[0]);
    } 
    return output[0];
}

/*void clip(float * input, float range){
    if(*input > range) *input = range;
    range *= -1.0f;
    if(*input < range) *input = range;
}*/

float points_factor(int item){
   // return 0.1f + ((2 - (item/2)) * 0.05f);
   //return (-0.05f * (item/2)) + 0.2f;
    if(item < 2) return 0.2f;//item/2 //2,1,0
    else if(item < 4) return  0.15f;//-0.05x + 0.2
    else return 0.1f;
}

/*float dist_factor(int item){
    if(item < 2) return 0.162f;
    else if(item < 4) return 0.149f;
    else return 0.135f;
}*/

void assign(float * output, float * input){
    *output++ = *input++;
    *output++ = *input++;
    *output = *input;
}

void vecScalerMult(float * input, float factor){
    *input++ *= factor;
    *input++ *= factor;
    *input *= factor;
}

float get_angle(float vec_1[3], float vec_2[3]){
    return (mathVecInner(vec_1,vec_2,3))/(mathVecMagnitude(vec_1,3) * mathVecMagnitude(vec_2,3));
}
//End page Functions
//Begin page main
#define TWO_TENTHS_RADIAN               0.9800f 
#define QUARTER_RADIAN                  0.9689f     
#define SOME_RADIANS                    0.9850f
#define PI_OVER_TWO_RADIAN              0.0f

#define ARRIVE_DIST                     0.01f               //less than 50 seconds, special item might be beneficial
#define DROP_ANGLE                      SOME_RADIANS        //maybe only do special angle on first item, or alwasy taret pos but not angle or something
#define PICKUP_ANGLE                    QUARTER_RADIAN      //FIX issue where sometimes we get penalty for incorrect docking procedure
#define BEGIN_SPIN_DIST                 1.0f     

#define ST_CALC_TRI                     0
#define ST_SIDE_2                       1
#define ST_SCORE_ITEM                   3
#define ST_PICK_UP_ITEM                 2

bool block;
bool oppo_zone;
bool item_held;

int side;
int state;
int item;
int in_count;

float tri_target[2][3];
float zone[2][3];
float last_score[2];

void init(){
    //state = ST_CALC_TRI;
    //in_count = 0;
    side = block = oppo_zone = in_count = state = false;
}

void loop(){

    ZRState mystate, oppostate, itemstate;
    
    api.getMyZRState(mystate);
    api.getOtherZRState(oppostate);
    
    float target[3];
    float zero[3];
    float angular_target[3];
    
    float score[2];
    float d_score[2];
    
    score[0] = game.getScore();
    score[1] = game.getOtherScore();
    
    zero[0] = zero[1] = zero[2] = 0.0f;
    
    int time_left = 180 - game.getCurrentTime();
    
   /* for(int i = 0; i < 2; i++){
        d_score[i] = score[i] - last_score[i];
        last_score[i] = score[i];
        if(d_score[i] > 0.75f) d_score[i] = 0.0f;
        score[i] += d_score[i] * time_left;
    }*/
    
    //score[0] = game.getScore();
    //score[1] = game.getOtherScore();
    d_score[0] = score[0] - last_score[0];
    d_score[1] = score[1] - last_score[1];
    
    last_score[0] = score[0];
    last_score[1] = score[1];
    
    //bool rotate = false;

    
    if(d_score[0] > 0.75f) d_score[0] = 0.0f;
   // if(d_o_score > 0.75f) d_o_score = 0.0f;
    score[0] += d_score[0] * time_left;
    score[1] += d_score[1] * time_left;
    
    for(int i = 0; i < 3; i++){
    
    switch(state){
            
        case ST_CALC_TRI:{
            
            float a[3];
            float pos[2][3];
            float dist_a;
    
            game.getItemLoc(pos[0], 0);
            game.getItemLoc(pos[1], 1);

            //item = dist(&mystate[0], pos[0], false) > dist(&mystate[0], pos[1], false);
            
            if(dist(&mystate[0], pos[0], false) < dist(&mystate[0], pos[1], false)) item = 0;
            else item = 1;
          
            game.getItemZRState(itemstate, item);
            assign(pos[0], &itemstate[6]);
            mathVecNormalize(pos[0], 3);

            vecScalerMult(&pos[0][0], 0.162f);
            mathVecAdd(pos[0], &itemstate[0], pos[0], 3);
            
            
            assign(&a[0], pos[0]);
            mathVecSubtract(pos[1], &mystate[0], pos[0], 3);
            dist_a = mathVecMagnitude(pos[1], 3);
            mathVecCross(pos[0], pos[1], &itemstate[6]);
            mathVecNormalize(pos[0], 3);
            
            if(dist_a  < 0.25f){
                mathVecNormalize(pos[1],3);
                vecScalerMult(&pos[1][0], 0.5f - dist_a);
                dist_a = 0.5f;
                mathVecAdd(tri_target[0], pos[1], &mystate[0],3);
            } 
            else{
                assign(tri_target[0],&mystate[0]);
            }
            
            float factor = 0.25f/dist_a;
            //if(factor > 0.6f) factor = 0.6f;
            vecScalerMult(&pos[0][0], factor);
            
            mathVecAdd(tri_target[1],pos[0], a, 3);
            state = ST_SIDE_2;
    
            for(int j = 0; j < 2; j++){
                for(int i = 0; i < 3; i++){
                    if(tri_target[j][i] > 0.63f){
                      tri_target[j][i] = 0.63f;
                      //DEBUG(("CLIP"));
                    } 
                    if(tri_target[j][i] < -0.63f){
                        tri_target[j][i] = -0.63f;
                        //DEBUG(("CLIP"));
                    } 
                } 
            }
        }
        break;

        
        case ST_SIDE_2:{
            
            assign(target, tri_target[side]);
            
            if(dist(&mystate[0], tri_target[side], false) < 0.05f){
                game.dropSPS();
                if(!side) side = 1;
                else state = ST_PICK_UP_ITEM;
            }
        }
        break;
            
    
        case ST_PICK_UP_ITEM:{
            
            float factor = 0.135f; //0.135
            
            game.getItemZRState(itemstate, item);
        
            mathVecSubtract(target, &mystate[0], &itemstate[0], 3);
    
            float angle = get_angle(target, &itemstate[6]);        
        
            if(item < 2) factor = 0.162f;  //0.162
            else if(item < 4) factor = 0.149f; //0.149
        
            float vector[3];
        
            if(angle < 0.0f){
                
                float cross[3];
                
                mathVecCross(vector, target, &itemstate[6]);    //rodrigues rotation thingy formula
                mathVecNormalize(vector, 3);
                mathVecCross(cross, vector, target);
                
                float dot = mathVecInner(vector, target, 3);
                vecScalerMult(&vector[0], dot);
                mathVecAdd(target, vector, cross, 3);
                factor *= 2.5f;
            }
            else assign(target, &itemstate[6]);    
        
            mathVecNormalize(target, 3);
        
            vecScalerMult(&target[0], factor);
        
            mathVecAdd(target, &itemstate[0], target, 3);
            

            angular_target[0] = itemstate[6] * -1.0f;
            angular_target[1] = itemstate[7] * -1.0f;
            angular_target[2] = itemstate[8] * -1.0f;

        
            angle = get_angle(angular_target, &mystate[6]);
        
            //mathVecNormalize(angular_target, 3);
            
            float dist_to_item = fabs(dist(&mystate[0], &itemstate[0], false) - factor);
            float speed = mathVecMagnitude(&mystate[3],3);
        
            //rotate = true;

            if(angle > PICKUP_ANGLE && game.isFacingCorrectItemSide(item) && dist_to_item < 0.01f && speed < 0.01f){
                
                if(game.dockItem(item)){
                    
                   // item_held = true;
                
                float d_minus = points_factor(item);
                
                game.dropSPS();
                
                    
                    oppo_zone = block = false;
                   // block = false;
                   /* DEBUG(("d_score: %f", d_score));
                    DEBUG(("my_score: %f", my_score));
                    DEBUG(("oppostate: %f, %f, %f", oppostate[0], oppostate[1], oppostate[2]));
                    DEBUG(("time: %f", dist(&oppostate[0], zone[0], true)));
                    DEBUG(("time
                    left: %d", time_left));*/
                   //// float vector[3];
                   // game.getItemLoc(vector, !item);
                   // mathVecSubtract(vector, &oppostate[0],zone[0],3);
                   // DEBUG(("dist_oppo: %f,%f", mathVecMagnitude(vector,3), dist(&oppostate[0], zone[0],false)));
                    time_left -= 1;
                   //DEBUG(("score: %f", score[0] - ((time_left - dist(&oppostate[0], zone[0], true)) * d_score[0])));
                    //DEBUG(("oppo_score: %f", score[1] - ((time_left) * d_minus)));
                    //DEBUG(("d_minus: %f", d_minus));
                    float score_me = score[0] - ((time_left - dist(&oppostate[0], zone[0], true)) * d_score[0]);
                    float score_oppo = score[1] - ((time_left) * d_minus);
                    DEBUG(("Dist: %f", dist(&mystate[0], zone[1], false)));
                    if(dist(&mystate[0], zone[1],false) < 0.3f && score_me > score_oppo){
                        oppo_zone = block = true;
                    }
                    
                    state = ST_SCORE_ITEM;
                }
            }
        }
        break;
            
            
        case ST_SCORE_ITEM:{
            
            float factor = 0.138f;
            
            game.getZone(zone[0]);
            
            for(int i = 0; i < 3; i++){
                zone[1][i] = zone[0][i] * -1.0f;
            }
            
            if(item < 2) factor = 0.165f;  //0.162
            else if(item < 4) factor = 0.152f; //0.149
            
            if(side == 1){
                mathVecSubtract(target, zone[0], zone[1], 3);
            }
            else{
                mathVecSubtract(target, &mystate[0], zone[0], 3);
            }
            
            angular_target[0] = target[0] * -1.0f;
            angular_target[1] = target[1] * -1.0f;
            angular_target[2] = target[2] * -1.0f;
            
            //mathVecSubtract(angular_target, zone[0], &mystate[0], 3);
          // mathVecSubtract(angular_target, zone[1], zone[0], 3);
            //assign(angular_target, target);
            //vecScalerMult(angular_target, -1.0f);
            //rotate = true;
            
            if(block){
                mathVecSubtract(target, &oppostate[0], zone[oppo_zone], 3);
                mathVecSubtract(angular_target, &oppostate[0], &mystate[0], 3);
                factor = 0.195f;
            }
            
            mathVecNormalize(target, 3);
            
            vecScalerMult(&target[0], factor);
            
            mathVecAdd(target, target, zone[oppo_zone], 3);
            
            //mathVecNormalize(angular_target, 3); //might not need
            
            float angle = get_angle(angular_target, &mystate[6]);
            //DEBUG(("angle: %f", angle));
            if((dist(&mystate[0], target, false) < 0.025f && angle > DROP_ANGLE) || block){
                
                if(!block){
                    game.dropItem();
                    score[0] += time_left * points_factor(item);
                }  
                
                side = 2;
                
                int has_item = -1;
                float best_score = 0.0f;
                
                for(int i = 0; i < 6; i++){
                    if(game.hasItem(i) == 2) has_item = i;
                    float pos[3];
                    game.getItemLoc(pos,i);
                    float dist_4 =dist(&oppostate[0], pos, true) + dist(pos, zone[1], true);
                    float score = (time_left - dist_4) * points_factor(i);
                    if(score > best_score) best_score = score;
                }
                
                //DEBUG(("best_score: %f", best_score));
                //DEBUG(("has_item: %d", has_item));
                if(has_item >= 0) score[1] += (time_left - (dist(&oppostate[0], zone[1],true))) * points_factor(has_item); 
                
                score[1] += best_score;
               // DEBUG(("my_score: %f", score[0]));
               // DEBUG(("oppo_score: %f", score[1]));
                
                //oppo_zone = false;
                //block = true;
                float dist_oppo_zone = dist(&oppostate[0], zone[1], false);
                if(dist_oppo_zone < 0.225f) in_count++;
                else in_count = 0;

                if(score[0] > score[1] || (in_count <= 60 && in_count != 0) || (block && oppo_zone)){ //only block if oppo coming near and not picking up other stuff to win
                    if(!block) oppo_zone = false;
                    block = true;
                }
                else{
                    float best_item = 0.0f;
                    int item_choice = -1;
                    //bool big_item = false;
                    
                    for(int i = 0; i < 6; i++){
                        
                        game.getItemZRState(itemstate, i);
                        
                        bool close = false;
                        
                        int has_item = game.hasItem(i);
                        //DEBUG(("Dist: %f", dist(&oppostate[0], zone[1], false)));
                        if(dist_oppo_zone < 0.65f && has_item == 2){
                            assign(&itemstate[0], zone[1]);    
                            close = true;
                        } 
                       // DEBUG(("i: %d", i));
                        //DEBUG(("points_factor: %f", points_factor(i)));
                       // DEBUG(("dist: %f", dist(&mystate[0], &itemstate[0], false)));
                        float score = (points_factor(i) * 100.0f) - dist(&mystate[0], &itemstate[0],false);
                        
                        if(dist(zone[1], &itemstate[0],false) < 0.15f && i < 4 ){
                            score *= 1000.0f;  
                            //if(i < 2) big_item = true;
                            //DEBUG(("ONE"));
                        } 
                        
                       /* if(dist(zone[0], &itemstate[0],false) < 0.3f && big_item){
                            score *= 1000.0f; 
                            //DEBUG(("TWO"));
                        }  */
                        

                        if(game.itemInZone(i) || ((has_item == 2 || (dist(&oppostate[0], &itemstate[0], false) < 0.25f && in_count > 60)) && !close) || mathVecMagnitude(&itemstate[3],3) > 0.005f){
                            score = 0.0f;
                            //DEBUG(("NULL"));
                        } 
                       // DEBUG(("score: %f", score));
                       // DEBUG(("best_item: %f", best_item));
                        if(score > best_item){
                            best_item = score;
                            item_choice = i;
                        }
                    }
                
                    
                    //if(!block){
                    block = oppo_zone = false;
                    state = ST_PICK_UP_ITEM;
                    item = item_choice;
                    //}
                }
            }
            
        }
        break;
    } 
    /*DEBUG(("state: %d", state));
    DEBUG(("item: %d", item));
    DEBUG(("block: %d", block));
    DEBUG(("oppo_zone: %d", oppo_zone));
    DEBUG(("in_count: %d", in_count));*/
    }
    
    api.setPositionTarget(target);
    
    if(/*side && */dist(&mystate[0], target,false) < BEGIN_SPIN_DIST && state > 1/* && state > 1 && (!block || game.hasItem(item) == 1)*/) api.setAttitudeTarget(angular_target);
    else api.setAttRateTarget(zero);
    
   // last_score[0] = game.getScore();
    //last_score[1] = game.getOtherScore();
    
    //DEBUG(("mystate: %f, %f, %f", &mystate[0], &mystate[1], &mystate[2]));
   
    
   /* DEBUG(("block: %d", block));
    DEBUG(("oppo_zone: %d", oppo_zone));
    DEBUG(("gains: %d", gains));
    DEBUG(("tri_done: %d", tri_done));*/
}

//End page main

                        
                      
download
                        
                        //Begin page main
// main.c

float myState[12];
float itemState[12];
float startY;
float zoneInfo[4];
unsigned int itemNumber;
unsigned int state;
float vertex_b[3];
float vertex_c[3];
float finalLoc[3];
float finalItemPos[3];
#define START 0
#define SPS2 1
#define SPS3 2
#define DECIDE 3
#define DOCK 4
#define DELIVER 5

#define NONSENSE_VALUE 250

#define MAX_TRIANGLE_AREA 0.132f // 0.136f

#define MAX_ERROR 0.12f // Should really be 0.1, but, that took too long to deliver.

#define NUMBER_OF_ITEMS 6

#define TIME_IN_GAME 180

// The pretend sphere we check item movement towards to simplify things.
#define GAME_RADIUS 0.92398675482166f

#define PI_CONST 3.141592653589793f

void init() {
    state = START;
    DEBUG(("CosmicSparTech Alliance ISS Finals Code"));
}

void loop() {
    api.getMyZRState(myState);
    DEBUG(("%i", state));
    switch(state) {
        case START:
            game.dropSPS();
            startY=myState[1];
            itemNumber = selectTarget();
            DEBUG(("Aiming for %d", itemNumber));
            float distVec[3];
            game.getItemZRState(itemState, itemNumber);
            if (isCloseEnough(myState, itemState, 0.35f, 3.0f)) {
                DEBUG((" NOT Extending Vertex B"));
                memcpy(vertex_b, itemState, 3*sizeof(float));
            }
            else {
                DEBUG((" Extending Vertex B"));
                mathVecSubtract(distVec, itemState, myState, 3);
                setMagnitudeOfVector(distVec, 0.35f, true);
                mathVecAdd(vertex_b,&myState[0], distVec, 3);
            }
            state=DOCK;
            break;

        case SPS2:
            
            //update vertex_b
            moveByVelocity(vertex_b,0.185f, false);
            //memcpy(vertex_b,&myState[0],3*sizeof(float));
            //DEBUG(("Target item : %d ",itemNumber));
            if (isCloseEnough(myState,vertex_b,0.0f,0.08f))
            {
                game.dropSPS();
                state = SPS3;
                DEBUG(("dropped 2nd SPS"));
            }
            
            break;

        case SPS3: {
            vertex_c[0]= -0.5f*sign(vertex_b[0]);
            vertex_c[1]= startY*4.333333f; //0.65f/0.15f
            vertex_c[2]=0.5f*sign(vertex_b[2]);
            float max_Area=0.136f;    //0.136f;
            float init_Area;
            float vect_AB[3];
            float vect_BC[3];
            float vec_Cross[3];
            float startPos[3];
            startPos[0]=0.0f;
            startPos[1]=startY;
            startPos[2]=0.0f;
            mathVecSubtract(vect_BC,vertex_c,vertex_b,3);
            mathVecSubtract(vect_AB,vertex_b,startPos,3);
            mathVecCross(vec_Cross,vect_AB,vect_BC);
            init_Area=mathVecMagnitude(vec_Cross,3)/2;
            //DEBUG(("Area est = %4.3f",init_Area));
            if (init_Area < 0.01f/MAX_ERROR)//triangle would be too small
            {
              vertex_c[1]*=-1.0f;  
            }
            //if (init_Area > max_Area) {
                setMagnitudeOfVector(vect_BC,(max_Area/init_Area),false);
                //vect_BC[0]=(max_Area/init_Area)*vect_BC[0];
                //vect_BC[1]=(max_Area/init_Area)*vect_BC[1];
                //vect_BC[2]=(max_Area/init_Area)*vect_BC[2];
                mathVecAdd(vertex_c,vect_BC,vertex_b,3);
            //}
            //DEBUG(("Vertex_C %4.3f %4.3f %4.3f",vertex_c[0],vertex_c[1],vertex_c[2]));
            moveByVelocity(vertex_c,0.18f, false);//0.16f

            if (isCloseEnough(myState,vertex_c,0,0.08f)) {
                game.dropSPS();
                game.getZone(zoneInfo);
                DEBUG(("dropped 3rd SPS"));
                state=DELIVER;
            }
            break;
        }

        case DOCK:
            // itemNumber is a global that should always be set before getting
            // here.
            dockObject();
            break;

        case DELIVER:
            // itemNumber is a global that should always be set before getting
            // here.
            deliverObject();
            break;
    }
}


// movement/faceToFrom.c

void faceToFrom(float toPos[3], float fromPos[3]) {
    //orients the satellite so that when it reaches fromPos, it will be facing toPos
    float displacement[3]; // Of toPos from fromPos
    mathVecSubtract(displacement,toPos,fromPos,3);
    mathVecNormalize(displacement,3);
    api.setAttitudeTarget(displacement);
}


// movement/moveByVelocity.c

void moveByVelocity(float TargetPos[3],float factor, bool absolute) {
    float Diff[3];
    //api.getMyZRState(myState);
    mathVecSubtract(Diff,TargetPos,myState,3);
    if (!mathVecMagnitude(Diff, 3)) { 
        // If the target is the current position, set to zero. 
        // Doing this here because normalising a magnitude 0 vector makes it 
        // undefined (because of a zero division error). 
        api.setVelocityTarget(Diff); 
        return; 
    } 
    if (absolute) {
        mathVecNormalize(Diff, 3);
    }
    // limit vel mag demand to 0.05 m/s
    if (mathVecMagnitude(Diff,3)*factor > 0.0525f)
        factor=0.0525f/mathVecMagnitude(Diff,3);
    setMagnitudeOfVector(Diff,factor,false);// limits the velocity mag to 0.05f
    // if (mathVecMagnitude(Diff,3) > 0.05f)
    // {
    //     factor=mathVecMagnitude(Diff,3)
    // setMagnitudeOfVector(Diff,0.05f,true);
    // }
    //DEBUG(("vel demanded: %4.3f %4.3f %4.3f",Diff[0],Diff[1],Diff[2]));
    api.setVelocityTarget(Diff);
}


// info/closestPointInIntervalToPoint.c

void closestPointInIntervalToPoint(float pointToFill[3], float point[3], float lineStart[3], float lineEnd[3]) {
	// See maths/collision_detection to see what I am doing here.
    //    n = pointToFill
    //    a = lineStart
    //    b = lineEnd
    //    c = point
	//  fill temp with the normal vector of the plane, the
	// line passing through lineStart and lineEnd.
    float temp[3];
	mathVecSubtract(temp, lineEnd, lineStart, 3);
	// You really need to read the documentation for this (and the stuff below the if statement):
    float t = (
        (
            mathVecInner(temp, point, 3)
            - mathVecInner(temp, lineStart, 3)
        ) / (
            mathVecMagnitude(temp, 3)
            * mathVecMagnitude(temp, 3)
        )
    );
	DEBUG(("t: %f", t));
    if (t >= 1) {
        memcpy(pointToFill,lineEnd, 3*sizeof(float));
		DEBUG(("Closest at end."));
    } else {
    	// But, if the point is between them...
    	setMagnitudeOfVector(temp, t, false);
        mathVecAdd(pointToFill, lineStart, temp, 3);
    }
}


// info/getTimeBeforeReset.c

float getTimeBeforeReset(int itemNumber) {
    game.getItemZRState(itemState, itemNumber);
    float speed = mathVecMagnitude(&itemState[3], 3);
    //if (speed == 0) {
     //   return 0;
    //}
    return (speed == 0) ? 0  : ((GAME_RADIUS - mathVecMagnitude(itemState, 3)) / speed);
}


// info/isCloseEnough.c

bool isCloseEnough(float *myState, float *Target, float minDist, float maxDist) {
    bool result = false;
    float diff[3];
    mathVecSubtract(diff,myState,Target,3);
    float dist = mathVecMagnitude(diff,3);
    return (dist < maxDist) && (dist > minDist);
}


// info/isInZone.c

bool isInZone(int itemNumber, bool other) {
    // If other is true it will check for opponents zone.
    /*float error = zoneInfo[3];
    if (error == -1) {
        return false;
    }
    if (error >= MAX_ERROR) {
        // If your triangle placement failed, you are just going to have to
        // hope you got lucky
        error = 0;
    }
    game.getItemZRState(itemState, itemNumber);*/

    float oppZone[3];
    for(int i = 0; i < 3; i++) {
        oppZone[i] = (other) ? -zoneInfo[i] : zoneInfo[i];
    }

    return isCloseEnough(itemState, oppZone, 0, 0.045f);
    // radius of zone is 0.1 and error
    // i'm just making it a lil smaller to be safe
}


// math/getAngleBetweenVectors.c

float getAngleBetweenVectors(float* vector1, float* vector2) {
    /* a . b = |a|*|b|*cos(theta), where theta is the angle between.
     * Therefore cos(theta) = (a . b) / (|a|*|b|)
     */
    return acosf(
        mathVecInner(vector1, vector2, 3) /
        (mathVecMagnitude(vector1, 3) * mathVecMagnitude(vector2, 3))
        );
}


// math/setMagnitudeOfVector.c

void setMagnitudeOfVector(float vector[3], float magnitude, bool normalise) {
    /* Normalising a vector preserves its direction, but makes its magnitude
     * one. If you then multiply it by a particular amount, that amount will
     * become the magnitude.
     * If you want to simply scale the vector, use normalise = false.
     */
    if (normalise) {
        mathVecNormalize(vector, 3);
    }
    for (int i = 0; i < 3; i++) {
        vector[i] = vector[i] * magnitude;
    }
}


// math/sign.c

int sign(float x) {
	return (x >= 0) ? 1 : -1;
}


// items/deliverObject.c

void deliverObject() {
    float temp1[3];
    // Filling temp1 with the position of the item.
    game.getItemLoc(temp1, itemNumber);
    float temp2[3];
    // Filling temp2 with the displacement of the item from the sphere.
    mathVecSubtract(temp2, temp1, myState, 3);
    // Filling temp1 with the displacment of the zone from the sphere.
    mathVecSubtract(temp1, zoneInfo, myState, 3);
    // Scaling back temp1 but the length of temp2.
    setMagnitudeOfVector(
        temp1,
        mathVecMagnitude(temp1, 3) - mathVecMagnitude(temp2, 3),
        true
        );
    // Setting temp2 to the desired item drop position.
    mathVecAdd(temp2, temp1, myState, 3);
    moveByVelocity(temp2, 0.16f, false);
    faceToFrom(zoneInfo, temp2);
    game.getItemZRState(itemState, itemNumber);
    if (
        isInZone(itemNumber, false)
        && mathVecMagnitude(&itemState[3], 3) <= 0.02f
        ) {
        game.dropItem();
        state=DOCK;
    }
}


// items/dockObject.c

void dockObject() {
    if (selectTarget() != itemNumber) {
        DEBUG(("Target no longer viable."));
        finalLoc[0] = NONSENSE_VALUE;
        itemNumber = selectTarget();
    }
    float dockDistMean[6] = {0.162f, 0.162f, 0.149f, 0.149f, 0.135f, 0.135f};
    // The item radii have been increased above what I thought they should be
    // because we were still colliding with them.
    float itemRadii[6] = {
        0.196f, //0.18219744050247993f,
        0.196f, //0.18219744050247993f,
        0.184f, //0.17180607827429067f,
        0.184f, //0.17180607827429067f,
        0.166f, //0.15699252209515996f,
        0.166f, //0.15699252209515996f,
    };
    // float itemRadii[6] = {
    //     0.186f, //0.18219744050247993f,
    //     0.186f, //0.18219744050247993f,
    //     0.175f, //0.17180607827429067f,
    //     0.175f, //0.17180607827429067f,
    //     0.160f, //0.15699252209515996f,
    //     0.160f, //0.15699252209515996f,
    // };
    game.getItemZRState(itemState,itemNumber);
    if (mathVecMagnitude(&itemState[3], 3) == 0) {
        finalLoc[0] = NONSENSE_VALUE;
    }
    if(finalLoc[0] == NONSENSE_VALUE) {
        game.getItemZRState(itemState,itemNumber);

        for (int i = 0; i < 3; i++) {
            // Separate because finalItemPos may be persistent.
            finalItemPos[i] = itemState[i];
            finalLoc[i] = finalItemPos[i] + (itemState[i+6] * 0.985f*dockDistMean[itemNumber]);
        }
    }
    faceToFrom(finalItemPos, finalLoc);
    //<collision-avoidance>
    float temp[3];
    // Filling temp with the displacement of the item from the sphere.
    mathVecSubtract(temp, itemState, myState, 3);
    bool avoid = (
        getAngleBetweenVectors(temp, &itemState[6])
        < 3 * PI_CONST / 5 // was 3/4*PI
    );
    bool isWayPoint = false;
    if (avoid) {
        DEBUG(("finalLoc: [%f, %f, %f]", finalLoc[0], finalLoc[1], finalLoc[2]));
        DEBUG(("itemState: [%f, %f, %f]", itemState[0], itemState[1], itemState[2]));
        closestPointInIntervalToPoint(finalLoc, itemState, myState, finalLoc);
        DEBUG(("Next finalLoc: [%f, %f, %f]", finalLoc[0], finalLoc[1], finalLoc[2]));
        // But, if it is inside the raidus, it needs to be adjusted.
        int x = 0;
        // Filling temp with the displacement of the final location to be in
        // from the item.
        mathVecSubtract(temp, finalLoc, itemState, 3);
        while (
                mathVecMagnitude(temp, 3) < itemRadii[itemNumber]
                && x < 5
        ) {
            isWayPoint = true;
            DEBUG(("LOOP"));
            x ++;
            setMagnitudeOfVector(temp, itemRadii[itemNumber], true);
            mathVecAdd(finalLoc, temp, itemState, 3);
            // Setting to check if there is a point along this line.
            closestPointInIntervalToPoint(finalLoc, itemState, myState, finalLoc);
            DEBUG(("finalLoc: [%f, %f, %f]", finalLoc[0], finalLoc[1], finalLoc[2]));
            mathVecSubtract(temp, finalLoc, itemState, 3);
        }
    }
    //</collision-avoidance>
    DEBUG(("aa finalLoc: [%f, %f, %f]", finalLoc[0], finalLoc[1], finalLoc[2]));
    DEBUG(("aa itemState: [%f, %f, %f]", itemState[0], itemState[1], itemState[2]));
    moveByVelocity(finalLoc, .16f, isWayPoint);
    
    if (
        isCloseEnough(
            myState,
            finalItemPos,
            0,
            0.09f
            ) && !isWayPoint)
        api.setPositionTarget(finalLoc);
    
    if (
        isCloseEnough(
            myState,
            finalItemPos,
            dockDistMean[itemNumber] - 0.011f,
            dockDistMean[itemNumber] + 0.011f
            )
        && mathVecMagnitude(&myState[3],3) < 0.01f
        ) {
        DEBUG(("All but facing"));
        if (
                game.isFacingCorrectItemSide(itemNumber)
                && game.dockItem(itemNumber)
        ) {
            DEBUG(("Attempting to dock item %i", itemNumber));
            state = (game.getNumSPSHeld() > 0) ? SPS2 : DELIVER;
        }
    }
}


// decisions.c

#define STEALING_COEFFICIENT 3
#define SCORE_COEFFICIENT 4
#define DISTANCE_COEFFICIENT 16
#define MOVING_COEFFICIENT 0.75f

#define INFINITE_COST 99999999

unsigned int selectTarget() {
    float itemScores[6] = {0.2f, 0.2f, 0.15f, 0.15f, 0.1f, 0.1f};
    if (game.getNumSPSHeld() > 0)
    {
        itemScores[0] = 0.15f;
        itemScores[1] = 0.15f;
    }
    float minCost = 4.f * INFINITE_COST;
    int minCostItem = 0;
    for (int i = 0; i < NUMBER_OF_ITEMS; i++) {
        float displacementToItem[3];
        // Temporarily fill displacement with the item position.
        game.getItemZRState(itemState, i);
        mathVecSubtract(
            displacementToItem,
            itemState,
            myState,
            3
            );
        int stealingCoefficient = isInZone(i, true) ? STEALING_COEFFICIENT : 1;
        int itemTakenModifier = (
            (game.itemInZone(i) && game.hasItemBeenPickedUp(i))
            || game.hasItem(i) == 2
        ) ? INFINITE_COST : 0;

        float currentCost = (
            +(DISTANCE_COEFFICIENT * mathVecMagnitude(displacementToItem, 3))
            - (
                stealingCoefficient
                * SCORE_COEFFICIENT
                * itemScores[i]
                * (TIME_IN_GAME - game.getCurrentTime())
                )
            + (itemTakenModifier)
            + (
                MOVING_COEFFICIENT
                * getTimeBeforeReset(i)
                )
            );

        if (currentCost < minCost) {
            minCostItem = i;
            minCost = currentCost;
        }
    }
    DEBUG(("Item: %i", minCostItem));
    return minCostItem;
}
//End page main

                        
                      
download
                        
                        //Begin page main
float myZRState[12];
float otherState[12];
int currentState;
float itemDockOffsets[3];

float dropZone[6];
float otherZone[4];
float startZRState[12];
float SPS2Loc[3];

float finalSPS[3];
int bestItem;
int heldItem;

int enemyTarget;

#define OPPONENT_AVERAGE 2

float oppVel[OPPONENT_AVERAGE][3];

#define triangleArea 0.33f

#define SPS_ITEM 0
#define SPS_PLACE 1
    
#define DROP 2
#define MOVE_MIDDLE 3
    
#define BLOCK_OUR 4
    
#define STEAL 5
#define BLOCK_ENEMY 6
#define GET 7

void init(){
    itemDockOffsets[0] = 0.173f;
    itemDockOffsets[1] = 0.160f;
    itemDockOffsets[2] = 0.146f;
    currentState = SPS_ITEM;
    game.dropSPS();
    api.setPosGains(0.42f, 0.74f, 3.44f);

    api.getMyZRState(startZRState);
};

static float angle(float* a, float* b) {
    return mathVecInner(a, b, 3) / (mathVecMagnitude(a, 3) * mathVecMagnitude(b, 3));
}

static float distance(float a[3], float b[3]) {
    float disVec[3];
    mathVecSubtract(disVec, a, b, 3);
    return mathVecMagnitude(disVec, 3);
}

void SFMove(float target[3]) {
    api.setPositionTarget(target);
    if (distance(myZRState,target)<0.2f) {
        float accel[3];
        for (int i = 0; i < 3; i++) {
            accel[i] = (2.0f * target[i] - myZRState[i]) / (1.0f);
        }
        api.setPositionTarget(accel);
    }
}

void getItemMoveLoc(float* loc, int itemID) {
    game.getItemZRState(loc, itemID);

    for (int i = 0; i < 3; i++) {
        loc[i] +=  (loc[6 + i] *= (itemDockOffsets[itemID/2] - 0.012f));
    }
}

bool itemPickup(int itemID) {
    float movePos[12];
    float itemState[12];
    float toVec[3];
    
    if(game.hasItem(itemID) == 1){
        return false;
    }
    
    game.getItemZRState(itemState, itemID);
    getItemMoveLoc(movePos, itemID);

    mathVecSubtract(toVec, movePos, myZRState, 3);
    mathVecNormalize(toVec, 3);

    //DEBUG(("AngleTo: %f", mathVecInner(toVec, & itemState[6], 3)));
    if (mathVecInner(toVec, &itemState[6], 3) > -0.184f) {
        DEBUG(("Going Around"));
        mathVecCross(&itemState[9],toVec,&itemState[6]);
        //DEBUG(("MovePos0: %f,%f,%f", inter[0],inter[1],inter[2]));
        mathVecCross(movePos,&itemState[9],toVec);
        mathVecNormalize(movePos,3);
        for (int i = 0; i < 3; i++) {
            movePos[i] = (itemDockOffsets[itemID / 2] + 0.032f) * (movePos[i] + toVec[i]*0.16f) + itemState[i];
        }
    }


    for (int i = 0; i < 3; i++) {
        itemState[6 + i] *= -1;
        finalSPS[i] = SPS2Loc[i];
    }
    SFMove(movePos);
    api.setAttitudeTarget(&itemState[6]);
    
    float d = distance(itemState, myZRState);
    mathVecSubtract(toVec, itemState, myZRState, 3);

    bool dockable = angle(&myZRState[6], toVec) > 0.968f &&
            mathVecMagnitude(&myZRState[3], 3) < .009f &&
            game.isFacingCorrectItemSide(itemID) &&
            d < itemDockOffsets[itemID / 2] &&
            d > itemDockOffsets[itemID / 2] - 0.022f;

    if (dockable && game.dockItem(itemID)) {
        heldItem = itemID;
        game.dropSPS();
        return true;
    }
    return false;
}
bool SPS2(int itemID) {
    float itemLoc[12];
    getItemMoveLoc(itemLoc, itemID);
    float toItemVec[3];

    mathVecSubtract(toItemVec, itemLoc, startZRState, 3);

    mathVecCross(SPS2Loc,toItemVec, itemLoc);
    mathVecCross(SPS2Loc, SPS2Loc, itemLoc);
    mathVecNormalize(SPS2Loc,3);

    float scalar = -triangleArea / mathVecMagnitude(toItemVec, 3);
    for (int i = 0; i < 3; i++) {
        SPS2Loc[i] = scalar*SPS2Loc[i] + (startZRState[i] + itemLoc[i]) / 2.0f;
    }

    //return (mathVecMagnitude(toItemVec, 3) > 0.1f);
    //return verifyBounds(SPS2Loc);
    return 0.55f > fabs(SPS2Loc[0]) && 0.7f > fabs(SPS2Loc[1]) && 0.55f > fabs(SPS2Loc[2]);
}

void middle(){
    float itemLoc[12];
    float target[6];
    
    int held = 0;
    int item = false;
    
    for(int i = 5; i >= 0; i--){
        target[i] = dropZone[i]/2;
        bool g = game.hasItemBeenPickedUp(i);
        
        held += (game.hasItem(i) == 2);
        
        game.getItemLoc(itemLoc, i);
        if(distance(itemLoc, otherZone) < 0.2f && g){
            item = true;
        }
    }
    
    SFMove(target);
    currentState = MOVE_MIDDLE;
    DEBUG(("WTF: %d, %d", enemyTarget, item));
    getItemMoveLoc(itemLoc, heldItem);
    if(enemyTarget == -1 || distance(otherState, itemLoc) < 0.4f){
        currentState = BLOCK_OUR;
    }else if((held || enemyTarget > -1 || enemyTarget == -2) && item){
        game.getItemLoc(itemLoc, enemyTarget);
        //float d1 = distance(myZRState, otherZone);
        currentState = STEAL;
    }
}

void block(float* zone){
    float dirVec[3];
    
    if(distance(dropZone, otherState) > 0.3f && currentState != BLOCK_ENEMY){
        middle();
    }
    
    mathVecSubtract(dirVec, otherState, zone, 3);
    mathVecNormalize(dirVec, 3);
    for(int i = 0; i < 3; i++){
        dirVec[i] = dirVec[i]*0.25f*(zone==otherZone) + zone[i];
    }
    SFMove(dirVec);
}

void loop(){
    
    api.getMyZRState(myZRState);
    api.getOtherZRState(otherState);
    int timeRemaining = game.getCurrentTime();
    int SPSNum = game.getNumSPSHeld();
    memcpy(oppVel[timeRemaining%OPPONENT_AVERAGE], &otherState[3], 12);
    
    float total[3];
    float itemLoc[12];
    float dirVec[3];
    float bestDistance = 10000;
    mathVecAdd(total, oppVel[0], oppVel[1], 3);
    
    enemyTarget = -2;
    bool cont = true;
    
    for(int i = 0; i < 6; i++){
        getItemMoveLoc(itemLoc, i);
        mathVecSubtract(dirVec, itemLoc, otherState, 3);
        if(angle(dirVec, total) > 0.82f && timeRemaining > 2 && cont) {//check if angle is > 10deg (cos(10deg) = 0.984807753)
            enemyTarget = i;
            game.getItemZRState(itemLoc, i);
            if(distance(itemLoc, dropZone) < 0.1f){
                enemyTarget = -1;
                cont = false;
            }
        }
        float d1 = distance(itemLoc, myZRState);
        if(SPS2(i) && !game.hasItemBeenPickedUp(i) && mathVecMagnitude(&itemLoc[3], 3) < 0.001f && (d1 < distance(itemLoc, otherState) + 0.15f || i != enemyTarget) && (timeRemaining%2 == 0 || timeRemaining < 3)){
            mathVecSubtract(dirVec, itemLoc, myZRState, 3);
            float d = (angle(dirVec, &itemLoc[6]) > -0.184f ? 0.15f : 0) + (distance(SPS2Loc, startZRState) + distance(SPS2Loc, itemLoc) + d1*0.5f) + (i/2)*100;
            
            if(d < bestDistance){
                bestDistance = d;
                bestItem = i;
            }
        }
    }
    
    DEBUG(("STATE: %d", currentState));
    DEBUG(("BEST: %d", bestItem));
    
    switch (currentState){
        case SPS_ITEM:
        {
            SPS2(bestItem);
            currentState+= itemPickup(bestItem);
            break;
        }
        case SPS_PLACE:
        {
            SFMove(finalSPS);
            if (distance(finalSPS, myZRState) < 0.05f) {
                game.dropSPS();
                game.getZone(dropZone);
                for (int i = 0; i < 3; i++) {
                    otherZone[i] = -dropZone[i];
                }
                currentState++;
            }
            break;
        }
        case DROP:
        {
            game.getItemZRState(itemLoc, heldItem);
            if (distance(itemLoc, dropZone) < 0.038f){
                game.dropItem();
                currentState++;
            } else {
                float offset[3];
                //float fuzz = itemDockOffsets[heldItem/2] - 0.022f;
                //Determine the direction vector between us and the target
                mathVecSubtract(offset, dropZone, myZRState, 3);
                //Make it a unit vector for simplicity
                mathVecNormalize(offset, 3);
                //Rotate toward it
                api.setAttitudeTarget(offset);
        
                //Multiply the unit vector by the fuzz
                for (int i = 0; i < 3; i++) {
                    offset[i] *= itemDockOffsets[heldItem/2] - 0.022f;
                }
                /*
                offset[0] *= fuzz;
                offset[1] *= fuzz;
                offset[2] *= fuzz;*/
        
                //Determine the position to move to
                mathVecSubtract(offset, dropZone, offset, 3);
        
                //Move there
                SFMove(offset);
            }            
            break;
        }
        case MOVE_MIDDLE:
        {
            middle();
            break;
        }
        case STEAL:
        {
            middle();
            int i = -1;
            while(i < 6){
                game.getItemLoc(itemLoc, i);
                if(distance(otherZone, itemLoc) < 0.2f && game.hasItemBeenPickedUp(i) && game.hasItem(i) == 0){
                    currentState += itemPickup(i);
                    break;
                }
                i++;
            }
            break;
        }
        
        case BLOCK_OUR:
        {
            getItemMoveLoc(itemLoc, heldItem);
            block(itemLoc);
            break;
        }
        case BLOCK_ENEMY:
        {
            block(otherZone);
            break;
        }
        /*
        case GET:
        {
            middle();
            currentState -= itemPickup(bestItem)*5;
            break;
        }*/
   }
};

//End page main

                        
                      

Virtual Alliance

Code

download
                        
                        //Begin page Utils
void mathVecScale(float res[], float src[], float scl, bool norm) {
    memcpy(res, src, 3 * sizeof(float));
    if(norm && mathVecMagnitude(src,3)!=0)
    {
        mathVecNormalize(res, 3);
    }
    for(int i = 0; i < 3; i++) res[i] *= scl;
}

/*float cosine(float a[3],float b[3])
{
    return mathVecInner(a,b,3)/mathVecMagnitude(a,3)/mathVecMagnitude(b,3);
}*/

float targetArea;//put this somewhere
int finalId;

void subtractFromMyState(float res[], float a[]) {
    mathVecSubtract(res, a, myState, 3);
} 

void getTriangleTarget()
{
    //targetArea = .01/0.07;
    //targetArea = 0.030344958/0.07;
    float targetArea = 0.158f;//0.158f;//0.19
    //assume called in first loop and that initial pos is myState
    float maxScore=-1000000.0f;
    //float selectedArea = 0;
    float vecA[3],vecB[3];
    float temp[3];
    float area;
    for(int i = 2; i < 4; i++) // for blue i is 0,2,4 and for red is 1,3,5
    {
        subtractFromMyState(vecA,itemDockLocs[i]);
        for(int j=9;j<134;j++)
        {
            //Generate arb points
            
            /*if(j>=9)
            {
                itemLocations[j][0] = (j/5)*(1.28/5*0.9) - 0.64*0.9;
                itemLocations[j][1] = ((j%25)/5)*(1.6/5*0.9) - 0.8*0.9;
                itemLocations[j][2] = (j%5)*(1.28/5*0.9) - 0.64*0.9;
            }*/
            
            //triangle points
            subtractFromMyState(vecB,itemDockLocs[j]);
            //calculate area
            mathVecCross(temp,vecA,vecB);
            area=mathVecMagnitude(temp, 3)/2;
            if(area<targetArea ||  area>0.18f)
            {
                continue;
            }
            //DEBUG(("DA  %f   %d   %d", area, area<targetArea, area>0.18f));
            /*if(i<7&&j<7)
            {
                continue;
            }*/
            int score=
            
            //-1000*(area<targetArea)
            // for the whole routine, might have problems because triangles rely on item positions, not dock positions
            +(targetArea-area) //area is not too big
            //2. one is 0-5, other not 0-5       //have one item at least
                                            //don't have two items
            //+100 * (((i < 6 && i>-1) || (j<6 && j>-1)) && ((i<6 && j>6) || (j<6 && i>6)))
            //-1000 * (i == 6)                        // first is not spec item
            //-1000 * ((i != 7 && i !=8) && (j == 6)) // if first is not adapter, second isnt spec item|| j == 6?
            //+ 100-(sphere/2)*50
            //+ (j<7)*(10-j/2)
            
            /*+10 * ((i/2==0) || (j/2==0)) // large item priority
            +9  * ((i/2==1) || (j/2==1)) // medium item priority
            +8  * (j==6)                             // spec item priority
            +7  * ((i/2==2) || (j/2==2)) // last priority small blues*/
            
            
            -distance(myState, itemDockLocs[i])
            -distance(itemDockLocs[i], itemDockLocs[j])
            -distance(itemDockLocs[j],myState)                                    // 5. no long legs but small area
            
            
            // should also probably consider the velocity effects of different items
            //3. 1st 7-8, then 6 //don't go to special item without adapter
            //4. 0,1,  > 2,3 > specitem > 4,5 //big items better than smaller ones
            //what about ??
            //if(i<6 && i > -1 && j >6) || (i>6 && j < 6 && j > -1)) ?
            ;
            if(score>maxScore)
            {
                //DEBUG(("AREA  %f",area));
                //selectedArea = area;
                maxScore=score;
                finalId = j;
                id = i;
            }
        }
    }
    //DEBUG(("ID: %d Final ID: %d", id, finalId));
}

bool iCanPickup()
{
    float errorVec[3];
    float errorMag;
    subtractFromMyState(errorVec,itemLocations[id]);//error is from us to item
    errorMag=mathVecMagnitude(errorVec,3);//mag of error
   ////DEBUG(("they %d have it,the distance is %f,we are traveling at %f,our cos is %f, is facing correct side %d",
    //game.hasItem(id)==2,errorMag,mathVecMagnitude(&myState[3],3),
    //mathVecInner(errorVec,&myState[6],3)/errorMag,game.isFacingCorrectItemSide(id)));
    return 
        game.hasItem(id)==0&&//if they don't have it
        fabsf(errorMag-satDistID[id])<0.0109f&&//and we are within 0.01 of tgt distance
        myV<0.01f&&//and we are slower than 0.01 m/s
        game.isFacingCorrectItemSide(id);   
        //mathVecInner(errorVec,&myState[6],3)/errorMag>0.9767f;//and we are pointed at it

}

bool iCanDrop()
{
    //DEBUG(("Distance =  %f", distance(itemLocations[holdingItem],zone)));
    float dx[3],dist,normvel[3];
    mathVecSubtract(dx,zone,itemLocations[holdingItem],3);
    dist=mathVecMagnitude(dx,3);
    //dist = distance(itemLocations[holdingItem], zone);
    if(dist>0)
        mathVecNormalize(dx,3);
    mathVecScale(normvel,&myState[3],1,true);
    //return (distance(itemLocations[holdingItem],zone)<(.11f-zone[3])) 
    return (dist)<(.11f-zone[3])
    || (dist < 0.2 && mathVecInner(normvel,dx,3) < 0.01f);
}

float distance(float pos1[3], float pos2[3])
{
    float temp[3];
    mathVecSubtract(temp, pos2, pos1, 3);
    return mathVecMagnitude(temp, 3);
}

void getDockItem() //Docks with an item on one side
{
    //id is the item we need to dock with
    //float itemState[12];
    //game.getItemZRState(itemState,id);//get the item state
    //mathVecScale(tgtAtt,&itemState[6],satDist[game.getItemType(id)],true);   //subtract the docking distance
    //mathVecAdd(tgtPos,itemState,tgtAtt,3);                                      //add to the original position
    memcpy(tgtPos, &itemDockLocs[id], sizeof(float)*3);
    mathVecScale(tgtAtt,&itemLocations[id][6],-1,false); 
    ////DEBUG(("%f %f %f", itemState[6], itemState[7], itemState[8]));
    
}

/*void subtractScale(float res[3],float vecA[3],float vecB[3],float len)
{
    mathVecSubtract(res,vecA,vecB,3);
    mathVecScale(res,res,len,true);
}*/

void goToZone()
{
    mathVecSubtract(tgtAtt,myState,zone, 3);                             //find vector from pack to us
    mathVecScale(tgtAtt,tgtAtt,satDistID[holdingItem],true);   //subtract the docking distance
    //subtractScale(tgtAtt,myState,zone,satDist[game.getItemType(holdingItem)]);
    mathVecAdd(tgtPos,zone,tgtAtt,3);                                      //add to the original position
    mathVecScale(tgtAtt,tgtAtt,-1,false);
}

/*inline void cDrive()
{
    float parDS[3],vel[3],dist;
    
        
    dist=distance(myState,tgtPos);//mathVecMagnitude(ds,3);
    mathVecSubtract(parDS,tgtPos,myState, 3);
    mathVecScale(vel,parDS,maxVelocity,true);
    //mathVecSubtract(ds,tgtPos,myState, 3);
   
    
    if(!avoid())
    {
        if(swd == 0)
        {
            swd = 1;
            maindistance = dist*0.7;
        }
        if(dist > maindistance && maindistance > 0.4 && dist > 0.15) api.setVelocityTarget(vel);
        else api.setPositionTarget(tgtPos);
    }
    else
    {
        //DEBUG(("DA"));
        if(/*state==GUARD||dist<myV*myV/2/0.007f*8/9+myV*1.5f)
        {
            api.setPositionTarget(tgtPos);
        ////DEBUG(("POSITION"));
        }
        else if(distance(vel,&myState[3])>0.02f)
        {
            api.setVelocityTarget(vel);
        ////DEBUG(("VELOCITY"));
        }
    }
    //subtractScale(vel,tgtPos,myState,maxVelocity);
    /*if(/*state==GUARD||dist<myV*myV/2/0.007f*8/9+myV*1.5f)
    {
        api.setPositionTarget(tgtPos);
        ////DEBUG(("POSITION"));
    }
    else if(distance(vel,&myState[3])>0.01f)
    {
        api.setVelocityTarget(vel);
        ////DEBUG(("VELOCITY"));
    }
    ////DEBUG(("C"));
}*/

void avoid()
{
    if(holdingItem > -1)return /*0*/;
    //tgtPos is target position
    ////DEBUG(("TGTPOS%f,%f,%f",tgtPos[0],tgtPos[1],tgtPos[2]));
    
    float ds[3],itemVec[3],fishyVec[3],dodgeVec[3],toDockLoc[3];
    subtractFromMyState(ds,tgtPos);
    float dist,distToDockLoc;//,lowestDist=127;
    subtractFromMyState(toDockLoc, tgtPos);
    distToDockLoc = mathVecMagnitude(toDockLoc, 3);
    /*for(int i=0;i<9;i++)
    {*/
        
        //location is itemLocations[i]
        subtractFromMyState(itemVec,itemLocations[id]);
        dist=mathVecMagnitude(itemVec,3);
        //if(i == id) //DEBUG(("Failure conditions: %d, %d", 
        //mathVecInner(itemVec,ds,3)>0,
        //dist<distToDockLoc));
 /*       if(mathVecInner(itemVec,ds,3)>0)
            if(dist<lowestDist&&game.hasItem(id)==0)
                if(dist-0.2f<distToDockLoc) //TODO: THIS DOENS'T REALLY WORK
                {
                    lowestDist=dist;
                } */
    //}
    
   // //DEBUG(("Closest: %d", closest));
    //if(closest < 0) return 0;
    if(distToDockLoc < dist) return /*0*/;
    float avoidDistance = satDistID[id] + 0.05f;
    mathVecScale(fishyVec,ds,mathVecInner(ds,itemVec, 3) / distToDockLoc,true);//Fishey vec is the closest point to the item
    mathVecSubtract(dodgeVec,fishyVec,itemVec, 3);//dodge vec is the vector perpendicular to our target vector through the item
    if(mathVecMagnitude(dodgeVec,3)>avoidDistance)return /*0*/;//If we don't need to avoid then return
    mathVecScale(dodgeVec,dodgeVec,avoidDistance,true);//scale dodge vec to the dodge distance
    mathVecAdd(tgtPos,dodgeVec,itemLocations[id],3);//return to the correct referance plane
    //swd = 0;
    //DEBUG(("ro=FEREA BA!!! en=AVOID"));
    //return 1;
    ////DEBUG(("Fishy: %f, %f, %f", fishyVec[0], fishyVec[1], fishyVec[2]));
   // //DEBUG(("DodgeVec: %f, %f, %f", dodgeVec[0], dodgeVec[1], dodgeVec[2]));
}

inline void generateArbPoints() {
    memset(itemLocations, 0, sizeof(float)*134*12);
    //Arbitrary points
    int arbIndex = 9;//9
    int indexArray[3];
	for(indexArray[0] = 0; indexArray[0] < 5; indexArray[0]++) {
	    for(indexArray[1] = 0; indexArray[1] < 5; indexArray[1]++) {
	        for(indexArray[2] = 0; indexArray[2] < 5; indexArray[2]++) {
	            for(int l = 0; l < 3; l++) {
	                itemLocations[arbIndex][l] = indexArray[l]*(1.28f/5*0.85f) - 0.64f*0.85f;
	                //itemLocations[arbIndex][l] = indexArray[l]*(1.28f/5*0.9f) - 0.64f*0.9f;
	            }
	            arbIndex++;
	            /*itemLocations[arbIndex][0] = i*(1.28f/5*0.9f) - 0.64f*0.9f;
	            itemLocations[arbIndex][1] = j*(1.28f/5*0.9f) - 0.64f*0.9f;
	            itemLocations[arbIndex][2] = k*(1.28f/5*0.9f) - 0.64f*0.9f;*/
	            //arbIndex++;
	        }
	    }
	}
	memcpy(itemDockLocs, itemLocations, sizeof(float)*134*12);
}
//End page Utils
//Begin page main
//PWP JOS

//Declare any variables shared between functions here
//int time;//NEED
int state;//NEED
int sphere; //NEED
//bool swd;
//float maindistance;
int oldState;
//blue = 1, red = -1 PRODUCTIVITY 
int id;     //NEED
int newID;
//int satState[6];//NEED
float itemLocations[134][12];//NEED

//int closest;
//float tmpAtt[3];

float tgtPos[3];//NEED
float tgtAtt[3];//NEED
//float triMid[3];//NEED
//float triPoints[2][3];//NEED
bool otherHasOne;//NEED

int hasItem;
int holdingItem;
int otherHoldingItem;

int closeToZone;

float myState[12],
otherState[12],
zone[4],//USE THIS ONE WHEN YOU NEED IT
theirZone[3],
//satDist[5],
satDistID[9],
itemDockLocs[134][12];

float maxVelocity;
float myV;

bool newItemHeld;

//enum states {DRAWTRIANGLE, GETSPART, TOZONE,/*GUARD,*/ SECONDLEG}; //used to select which state we are in

void init() {
    DEBUG(("CYS B"));
    
    //time = 0; //initializes time to zero
    //memset(satState,0,sizeof(int)*3);
    
    maxVelocity=0.04f;//0.06
    
    //arbPoints = 5;
    
    memset(zone,0,sizeof(float)*4);
    
    /*satDist[ITEM_TYPE_ADAPTER] = 0.0f;
    satDist[ITEM_TYPE_SMALL] = 0.135f; //Small (+- 0.011)
    satDist[ITEM_TYPE_SPECIAL] = 0.135f; //Small (+- 0.011)
    satDist[ITEM_TYPE_MEDIUM] = 0.149f;  //Medium max (+- 0.011)
    satDist[ITEM_TYPE_LARGE] = 0.162f; //Large max (+- 0.011)
    */
    
    
    satDistID[0] = 0.162f;
    satDistID[1] = 0.162f;
    satDistID[2] = 0.149f;
    satDistID[3] = 0.149f;
    satDistID[4] = 0.135f;
    satDistID[5] = 0.135f;
    satDistID[6] = 0.135f;
    satDistID[7] = 0.0f;
    satDistID[8] = 0.0f;
    
    state = 0;
    
    //id=myState[1]>0?1:0;
    
    //-0.558958,0.385189,-0.045561 
    //triMid[0]=0;
    //triMid[1]=0.5*sphere;
    //triMid[2]=-0.5*sphere;
    game.dropSPS();
    //swd = 0;
    /*if(sphere==1){
        triPoints[0][0] = 0.23+0.11455;
        triPoints[0][1] = 0.23+0.11455;
        triPoints[0][2] = 0.23;
        triPoints[1][0] = -0.23;
        triPoints[1][1] = -0.23;
        triPoints[1][2] = -0.23-0.162;
    }

    else {
        triPoints[0][0] = 0.23;
        triPoints[0][1] = 0.23;
        triPoints[0][2] = 0.23+0.162;
        triPoints[1][0] = -0.23-0.11455;
        triPoints[1][1] = -0.23-0.11455;
        triPoints[1][2] = -0.23;
    }*/
    
    //tempAtt[0]=1;
    //tempAtt[1]=0;
    //tempAtt[2]=0;
    generateArbPoints();
    getStates();
    sphere = (myState[1] > 0)?0:1;//0=blue,1=red
    
    newItemHeld = false;
    
    id=sphere+2;//#3
    finalId=128;//#3
    
    newID = -1;
    
    getTriangleTarget();
    DEBUG(("SPHERE %d",sphere));
    DEBUG(("id=%d,final=%d",id,finalId));
    //for(int i = 9;i <= 133; i++) DEBUG(("%f   %f  %f   %d", itemLocations[i][0], itemLocations[i][1], itemLocations[i][2],i));
}

void getStates()
{
    
    api.getMyZRState(myState); //Initialy gets ZR state to determine team color
    api.getOtherZRState(otherState);
    getItemInfo();
}

void loop() 
{
    DEBUG(("STATE   : %d",state));
    int loops = 0;
 	do
	{
	    //DEBUG(("Holding Item: %d targetId: %d", holdingItem, id));
	    getStates();
    
        //dScore=game.getScore()-score;
        //dOtherScore=game.getOtherScore()-otherScore;
        ////DEBUG(("dScore: %f",dScore));
        
        //score = game.getScore();
        //otherScore = game.getOtherScore();
        
        game.getZone(zone);
        mathVecScale(theirZone,zone,-1,false);
    	
    	
    	//Figure out states of sats
    	/*
    	0: no one has picked it up
    	1: we have it
    	2: they are currently holding it
    	-1: they have held it and have put it down
    	*/
    	getDockItem();
    	
    	myV = mathVecMagnitude(&myState[3],3);

	    oldState=state;
    	switch(state) {
    	    case 1:
    	        id = finalId;
    	    case 0:
                {
                    //DEBUG(("id%d,finalID%d", id, finalId));
                    //DEBUG(("final ID %f,%f,%f",itemDockLocs[finalId][0],itemDockLocs[finalId][1],itemDockLocs[finalId][2]));
                    //DEBUG(("STATE %d",state));
                    if(distance(myState, itemDockLocs[id]) <= 0.04f) 
                    {
                        if(id < 7) 
                        {
                            pickupAndChangeState();
                        } 
                        else 
                        {
                            if(id < 9)
                            {
                                if(myV<0.01f)
                                {
                                    id=newID;
                                    nextState();
                                }
                            } 
                            else 
                            {
                                nextState();
                            }
                        }
                    }
                }
    	        break;
    	    case 2:     //Go to spart WE NEED TO GO PICK UP ADAPTER
                {
                    /*if (iCanPickup())
                    {
                        if(game.dockItem(id))  //Checks if we have successfully docked with the SPart
                        {
                            holdingItem = id;
                            game.dropSPS(); 
                            state = 3; //Changes state and head to the zone.
                        }
                    }*/
                    pickupAndChangeState();
                    game.dropSPS();
                }
    	        break;
    	        
    	    case 3:     //GO TO ZONE Start moving towards zone point
    	        {
    	            game.dropSPS();
    	            goToZone(); //Finds ideal position and attitude to drop off SPart at zone
    	            
                    if(iCanDrop()) //Checks if we are close enough to SPart drop off at zone
    	            {   
    	                //inZone[id] = true;
    	                game.dropItem(); //Drops SPart at zone
    	                getItemInfo();
    	                holdingItem = -1;
    	                maxVelocity = 0.055f;
    	                id = newID;
    	                state = 2; //Changes state to go get another SPart
    	                
    	            }
    	        }
    	        break;
    	}
    	DEBUG(("State after switch: %d", state));
    	loops++;
	}
	while(oldState!=state && loops < 5);
	DEBUG(("Loops: %d", loops));
	if(loops > 4) {
	    DEBUG(("Stuck in state %d", state));
	}
    api.setAttitudeTarget(tgtAtt);
    //cDrive();
    avoid();
    api.setPositionTarget(tgtPos);
}

inline void guard()
{
    mathVecSubtract(tgtPos,otherState,zone,3);
    mathVecScale(tgtPos,tgtPos,.12f,true);
    mathVecAdd(tgtPos,zone,tgtPos,3);
}

void pickupAndChangeState()
{
    getDockItem();
    if(iCanPickup())
    {
        if(game.dockItem(id)) 
        {
            holdingItem = id;
            newItemHeld = false;
            nextState();
        }
    }
}

void nextState(){
    //swd = 0;
    //if(api.getTime()>50)
    {
        if(state>0)
        {
            if(holdingItem!=-1)
            {
                state=2;//really state 3
            }
            else
            {
                id=newID;
            }
        }
        state++;
        game.dropSPS();
    }
}

/*void getTgtSat()
{
    
    id=newID;
    
    /*
    //float diff[3];
    float score;
    float maxScore=-1000000000.0f;
    /*if(holdingItem>-1){
        return;
    }*//*
    
    for(int i=0;i<6;i++) 
    {
        //mathVecSubtract(diff,otherState,itemLocations[i], 3);
        //if(i==5 && !game.hasAdapter())continue;
        score=//Scores are in decreasing order of importance
        + 150*  ((0.45f>=distance(itemDockLocs[i],zone)) //it is close
                &&(.45f>=distance(myState,zone))) //and we are close
                
        + 4 *  (otherHasOne)*(
            distance(itemDockLocs[i], myState)-
            distance(itemDockLocs[i], otherState))
                /*&&mathVecInner(&otherState[3],diff,3)>0.75f*//* //One which is further from them
               
        + 40 *  (3-i/2) //GO for the bigger one
        //+ 10 * (i==6)
        
        //+ 20 * distance(itemLocations[i],theirZone)<0.1
        
        //-  5 *  ((game.hasItem(i)==2)?1:0) //Not if they have it
        
        -       distance(itemDockLocs[i], myState) //Split ties based on which is closest
        
        ;//Don't delete this semicolon!
        ////DEBUG(("%d has a score of %f and %d is inzone",i,score,game.itemInZone(i)));
        if(score>maxScore) 
        {
            //if(satState[i]!=1) {
            //if(game.hasItem(i)==0) {
                if(!game.itemInZone(i)) {
                    id=i;
                    maxScore=score;
                }
            //}
            //}
        }
    }*/
    ////DEBUG(("I chose id: %d",id));
//}

void getItemInfo() {
    holdingItem = -1;
    otherHoldingItem = -1;
    //closest = 0;
    otherHasOne = false;
    //DEBUG(("Item in zone: %d",game.itemInZone(0)));
    float lowestDist = 1000.0f;
    float dist;
    int closestItem = -1;
    for(int i=0;i<9;i++)
	{
	    ////DEBUG(("SATDIST%d,%f,%d",i,satDist[game.getItemType(i)],game.getItemType(i)));
	    game.getItemZRState(itemLocations[i],i);
	    //MIGHT BREAK SOMETHING
	    mathVecAdd(itemLocations[i],itemLocations[i],&itemLocations[i][3],3);
	    //MIGHT BREAK EVERYTHING
	    mathVecScale(itemDockLocs[i], &(itemLocations[i][6]), satDistID[i], true);
	    mathVecAdd(itemDockLocs[i], itemDockLocs[i], itemLocations[i], 3);


        //Go for close ones
        if(i<6)
        {
            if(!game.itemInZone(i))
            {
                dist=distance(zone,itemDockLocs[i]);
                if(dist<lowestDist)
                {
                    closestItem=i;
                    lowestDist=dist;
                }
            }
        }

	    hasItem=game.hasItem(i);
	    
	    if(hasItem==1 && i < 7)
	    {
	            holdingItem = i;

	    }
	    if(hasItem==2 && i < 7)
	    {
	            otherHasOne=true;
	            otherHoldingItem = i;
	            newID = i;
	            newItemHeld = true;
	    }
	    
	}
	
	if(!newItemHeld) {
	    newID = closestItem;
	}
	
}


//End page main

                        
                      
download
                        
                        //Begin page main
float myState[12], myPos[3], myVel[3], myAtt[3];
float itemState[3], itemPos[3], itemAtt[3]; 
float itemPos1[3], itemPos2[3];
float zoneInfo[4], zonePos[3], dir[3];
float SPS2[3], SPS3[3], perp[3], base[3], bet[3], negBet[3], dest[3]; float minArea;
float minRad, maxRad, minRadL, maxRadL, minRadM, maxRadM; 
int step, pack;
#define SPS game.getNumSPSHeld()
void init() {
    minArea = 0.090f;
    minRadL = 0.151f; maxRadL = 0.173f;
    minRadM = 0.138f; maxRadM = 0.160f; 
    step = 0; 
}

void loop() {
    api.getMyZRState(myState);
    for (int i = 0; i < 3; i++) {
        myPos[i] = myState[i];
        myVel[i] = myState[i+3];
        myAtt[i] = myState[i+6];
    }
        if (pack == 0 || pack == 1) {
                minRad = minRadL; 
                maxRad = maxRadL;
            }
            else if (pack == 2 || pack == 3) {
                minRad = minRadM; 
                maxRad = maxRadM;
            }
    switch (SPS){
        case 3:
        game.dropSPS();
        pack = closestItem(); 
        game.getItemZRState(itemState, pack);
        for (int i = 0; i < 3; i++) {
            itemPos[i] = itemState[i];
            itemAtt[i] = itemState[i+6];
        }
        for (int i = 0; i < 3; i++) {
            SPS3[i] = itemAtt[i];
        }
        setLength(SPS3, (maxRadL + minRadL)/2);
        mathVecAdd(SPS3, itemPos, SPS3, 3);
        
        mathVecCross(perp, myPos, SPS3);
        mathVecSubtract(base, SPS3, myPos, 3);
        mathVecCross(SPS2, base, perp);
        setLength(SPS2, 2*minArea/mathVecMagnitude(base, 3) + 0.05f);
        mathVecAdd(SPS2, SPS2, myPos, 3);
        multVec(base, 0.5f);
        mathVecAdd(SPS2, SPS2, base, 3);
        break;
    case 2:
        moveFaster(SPS2);
        if (dist(myPos, SPS2) < 0.02f || myPos[1]>0.6 || myPos[1]<-0.6) {
            game.dropSPS();
        }
        break;
    case 1: 
            if (pickUp(pack)) {    
                game.dropSPS();         
                game.getZone(zoneInfo);
                for (int i = 0; i < 3; i++) {
                    zonePos[i] = zoneInfo[i];
                }
        }
        else {                        
            moveFaster(SPS3);             
            if (dist(myPos, SPS3) < 0.025f) { 
                game.dropSPS();
                game.getZone(zoneInfo);   
                for (int i = 0; i < 3; i++) {
                    zonePos[i] = zoneInfo[i];
                }
            }
        }
        break;
    case 0:
        if (game.hasItem(pack)==1) {
            bringItemBack(pack);
            DEBUG(("bringing"));
            step=0;
        }
        else {
            pack = -1;
            // Check if a large or medium item is in opponent zone 
            for (int i = 0; i < 4; i++) {
                if (game.hasItem(i) == 2 && step==0) {
                    pack = i;
                    break;
                }
            }
            // If opponent zone is empty then find an item based on:
            // size primarily and distance if both items of that size open
            if (pack == -1) {
                for (int i = 0; i <= 1; i++) {
                    if (!game.itemInZone(i)) {
                        if (pack != -1) {
                            game.getItemLoc(itemPos1, pack); game.getItemLoc(itemPos2, i);
                            if (dist(myPos, itemPos2) < dist(myPos, itemPos1))
                                pack = i;
                        }
                        else
                            pack = i;
                    }
                }
                if (pack == -1) {
                    for (int i = 2; i <= 3; i++) {
                        if (!game.itemInZone(i)) {
                            if (pack != -1) {
                                game.getItemLoc(itemPos1, pack); game.getItemLoc(itemPos2, i);
                                if (dist(myPos, itemPos2) < dist(myPos, itemPos1))
                                    pack = i;
                            }
                            else
                                pack = i;
                        }
                    }
                }
            }
            
            pickUp(pack);
            step++;
        }
        
        break;
    }
}

int closestItem() {
    float a[3], b[3], c[3], d[3];
    game.getItemLoc(a, 0);
    game.getItemLoc(b, 1);
    game.getItemLoc(c, 2);
    game.getItemLoc(d, 3);
    if (dist(myPos, a) < dist(myPos, b) && dist(myPos, a)>0.3f)
    return 0;
    else if(dist(myPos,b)>0.30f && dist(myPos, a) > dist(myPos, b))
    return 1;
    else if (dist(myPos, c) < dist(myPos, d)) 
    return 2;
    else if (dist(myPos, c) > dist(myPos, d))
    return 3;
}

bool canPickUp(int id) {
    return (game.hasItem(id) == 0 &&
            !game.itemInZone(id));
}

// might need to set maxRad and minRad values before using
bool pickUp(int id) {
    game.getItemZRState(itemState, id);
    for (int i = 0; i < 3; i++) {
        itemPos[i] = itemState[i];
        itemAtt[i] = itemState[i+6];
    }
    mathVecSubtract(bet, itemPos, myPos, 3);
    for (int i = 0; i < 3; i++) {
        negBet[i] = -bet[i];
    }
    for (int i = 0; i < 3; i++) {
        dest[i] = negBet[i] + itemAtt[i];   
    }
    if (angleCos(itemAtt, negBet) < 0.0f) 
        setLength(dest, maxRad * 1.5f);
    else
        setLength(dest, (minRad + maxRad)/2); 
    mathVecAdd(dest, dest, itemPos, 3);
    
    moveFaster(dest);
    if(dist(myPos,dest)<0.0357f && mathVecMagnitude(myVel,3)>0.02f)
    api.setPositionTarget(myPos);
    
    api.setAttitudeTarget(bet);
        if (game.hasItem(id) == 0 && 
            !game.itemInZone(id) && 
            mathVecMagnitude(myVel, 3) <= 0.01f && 
            dist(myPos, itemPos) >= minRad && 
            dist(myPos, itemPos) <= maxRad && 
            game.isFacingCorrectItemSide(id))               
        return game.dockItem(id); 
    return false;
}

// set dir before 
bool bringItemBack(int item) {         
    game.getItemLoc(itemPos, item);
    mathVecSubtract(dest, zonePos, myPos, 3);
    api.setAttitudeTarget(dest);
    setLength(dest, mathVecMagnitude(dest, 3) - dist(myPos, itemPos));
    mathVecAdd(dest, dest, myPos, 3);
        moveFaster(dest);
    DEBUG(("%f", dist(itemPos, zonePos)));
    if (dist(itemPos, zonePos) < 0.04f) {
        game.dropItem();
        return true;
    }
    return false;
}
float dist(float *a, float *b) {
	float v[3];
	mathVecSubtract(v,a,b,3);
	return mathVecMagnitude(v,3);
}

void multVec(float* vec, float mult) {
    for (int i = 0; i < 3; i++) {
        vec[i] *= mult; 
    }
}

float angleCos(float a[3], float b[3]){ //return the cos of the angle
    return mathVecInner(a, b, 3) / (mathVecMagnitude(a, 3) * mathVecMagnitude(b, 3));
}

/*
// Courtesy of our SFF friends
void moveFaster(float pos[3]) {
    float bet[3];
    mathVecSubtract(bet, pos, myPos, 3);
    float dis = mathVecMagnitude(bet, 3);
    if (dis <= 0.2f)
        api.setPositionTarget(pos);
    else {
        multVec(bet, dis * dis * 0.5);
        api.setVelocityTarget(bet);
    }
}
*/
void moveFaster(float *dest) {
    float v[3];
    mathVecSubtract(v, dest, myState, 3);
    multVec(v, 1.57f);
    mathVecAdd(v, myState, v, 3);
    api.setPositionTarget(v);
}

void setLength(float* vec, float mult) {
    mathVecNormalize(vec, 3);
    multVec(vec, mult);
}


//End page main

                        
                      
Back to Top