Zero Robotics High School Tournament 2015 (USA & ESA)

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1024 : public ZRUser
{

//Begin page gameLib
float selectNearItem(short start,short end,bool flag,short di){
    float minDist=100.0f,d,w[3],zMin=100.0f,z;
    idItemSel=-1;
    for(int i=start;i<=end;i=i+di){
      if(game.hasItem(i)==-1){
        game.getItemLoc(w,i);z=w[2];
        mathVecSubtract(w,w,myState,3);
        d=mathVecMagnitude(w,3);
        if(flag){
          if(d=vLimit){
      phase=450;vTarget[1]=myState[1];
      return;
    }
}





void upLoadPicture(){
    #define angLim 0.906307787f
    #define angInUpLo 0.64278761f
    float w[3]={0.0f,0.0f,1.0f};
    float omega=mathVecMagnitude(&myState[9],3);
    if(flagUpload || myEnergy<1.0f){
        flagUpload=false;
        angInitUpload=myState[8];
    }
    if(myState[8]>angLim && angInitUpload0.06f){
      mathVecMult(w,&myState[9],-1.0,false);
      api.setAttRateTarget(w);
    }else api.setAttitudeTarget(w);
   if(phase==450)vTarget[1]=-0.15f;//0.1f
    if(myState[8]>0.96895f && omega <0.05f){
        game.uploadPics();forcedUpLoad=false;flagUpload=true;

    }
}






//End page gameLib
//Begin page main
//CRAB_ALL_CANDIDATE_FINAL_CANDIDATE_SLOW
//V. 18-12-2015 H:16.00

bool activeUpLoad,forcedUpLoad,activeAligne;
bool flagUpload,flagIntoLight,mirrorOn;
short time;
short phase;
short side;
short myMemoryFill;
short ixItem;
short vLimit;
int idItemSel;
int impulseDelay;
int oppZone;
int myZone;
float angInitUpload;
float beginLight;
float distFromOpp;
float vSpeed;
float myEnergy;
float dist;
float myYRel;
float zetaItem;
float _dLim,_d;

float vTarget[3];
state_vector myState,oppState;

void init(){
    phase=0;impulseDelay=30;side=0;ixItem=0;vLimit=3;_dLim=0.45f,_d=0.01f;
    activeUpLoad=false;forcedUpLoad=false;flagUpload=true;
    dist=0.15f;flagIntoLight=false;
}

 void loop(){


  float v[3];
 time=api.getTime();
 api.getMyZRState(myState);api.getOtherZRState(oppState);
 myZone=game.posInArea(myState);

if(time==0){
        if(myState[0]<0){
            side=1;
        }
       v[0]=selectNearItem(3+side,5+side,false,2);zetaItem=vTarget[2];
       //evalueItemSel();
       v[1]=(vTarget[1]-game.getLightInterfacePosition()+0.07f)/0.026f;
       vSpeed=v[0]/v[1];
}
    beginLight=game.getLightInterfacePosition()-0.80f;if(beginLight<-0.80f)beginLight=beginLight+1.60f;
    myEnergy=game.getEnergy();
    myMemoryFill=game.getMemoryFilled();
    myYRel=myState[1]-beginLight;
    mathVecSubtract(v,oppState,myState,3);
    distFromOpp=mathVecMagnitude(v,3);
    oppZone=game.posInArea(oppState);
    activeUpLoad=(myMemoryFill==2 && (myZone==1 ||(myZone!=1 && myEnergy>2.8f)))||(myMemoryFill>0 && (time>150 || forcedUpLoad));
    activeAligne=time>=20;
    if(activeAligne){
       if(!activeUpLoad){
           mathVecNormalize(v,3);
           api.setAttitudeTarget(v);
           if(myEnergy>1.1f){
              if(game.isFacingOther()&&oppZone!=-1){
                 if(game.isCameraOn()&&(myMemoryFill<2)){
                    if(game.getPicPoints()>0.0f)game.takePic();
                    }
                 }
            }
       }
    }

   if(phase<2 && game.hasItem(idItemSel)!=-1){
        ixItem++;phase=1;
        _dLim=0.25f,_d=dist;
        evalueItemSel();
    }
    if(myZone!=1 && (myYRel>-0.08f && myYRel<=0.08f)){
         if(flagIntoLight)game.useMirror();
    }
    mirrorOn=game.getMirrorTimeRemaining()>0;
    if(myYRel>-0.20f && myYRel<-0.081f)flagIntoLight=true;
    if(phase==450){
      vSpeed=0.03f;
      if((myZone==-1 && flagIntoLight)||mirrorOn){
        selectNearItem(3,6,true,1);
        if(idItemSel<0)phase=500;
      }
      if(myZone==-1&&myMemoryFill==1&&oppZone==1){
          vSpeed=-0.01f;
          mathVecMult(vTarget,myState,1.0,false);phase=500;
      }
    }

    if(phase==500){
            if(mirrorOn){
                vSpeed=0.03f;
                vTarget[1]=beginLight-0.3f;
                phase=510;forcedUpLoad=myMemoryFill>0;
            }
    }
    if(phase==510){
      if(myZone==-1){
          forcedUpLoad=((myMemoryFill==2)||(oppZone==-1 && myMemoryFill>0)||forcedUpLoad);
          if(!mirrorOn && myMemoryFill==0){
              vTarget[1]=0.50f;vSpeed=0.02f;selectNearItem(3,6,true,1);
          }
          if(myMemoryFill==2){
             vSpeed=-0.01f;
             mathVecMult(vTarget,myState,1.0,false);phase=500;
           }
      }
    }


    if(activeUpLoad){
     upLoadPicture();

    }

    float mv=mathVecMagnitude(&myState[3],3),vo=mathVecMagnitude(&oppState[3],3);
    if(myEnergy<=0.65f){moveToFaster(vTarget,-0.01f,0.0f,1);DEBUG(("time > %i;myEnergy % f\n",time, myEnergy));}
      else{
        if(distFromOpp<_dLim && mv>=(vo*0.5f)){
            moveToFaster(vTarget,0.0f,_d,1);
        }else moveToFaster(vTarget,vSpeed,dist,impulseDelay);
      }

/*     mathVecMult(v,myState,-0.048f,true);
    if(fabs(myState[0])>0.6f ||fabs(myState[1])>0.75f || fabs(myState[2])>0.6f)
       api.setForces(v);*/

    //   DEBUG(("%i->FASE = %i\n",time,phase));
}



//End page main
//Begin page moveLib
float moveToFaster(float *target,float k,float dist,int coff){
    float r,v[3],vv[3];
    bool correzione;
    mathVecSubtract(v,target,myState,3);
    r=mathVecMagnitude(v,3);
    mathVecMult(vv,&myState[3],r,true);
    mathVecAdd(vv,vv,myState,3);mathVecSubtract(vv,vv,vTarget,3);
    correzione=mathVecMagnitude(vv,3)>0.035f;
    if(k<1.0f){
        if(r>dist){
            mathVecMult(v,v,(k+0.01f),true);
            if(mathVecMagnitude(&myState[3],3) < fabs(k) || time % coff == 0 || correzione)api.setVelocityTarget(v);
            return r;
         }
       k=1.0f;
    }
    mathVecMult(v,v,k,false);
    mathVecAdd(v,v,myState,3);
    api.setPositionTarget(v);
    return r;
}

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

};

ZRUser *zruser7 = new ZRUser1024;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1029 : public ZRUser
{

//Begin page main
#define DARK -1
#define LIGHT 1

//Rare that these should be here
bool otherHasMirror;
bool dive;
bool uploading;
float speedLimit;
float velocity;
float myEnergy;
float bestDistance;     //the distance between the closest item and you 
float distanceBetweenUs;//the distance between you and the other SPHERE
int memory;
int getNumMirrorsHeld;
int getMirrorTimeRemaining;
int bestID;  
int zone;
int otherZone;
int action;
int data; int mirror; int otherMirror; int freeData;
int counter;            // time variable
float attitude[3];      //the official setter for your desired attitude (where you're facing)
float position[3];      //the official setter for your desired position
int itemStatis[9];      // put it right here
float myState[12];      //all information about your SPHERE
float otherState[12];   //all information about opponent SPHERE
float items[9][3];      //contains the position for all 9 items
float fuel;

void init(){
  action = 1;
    dive = false;
  //origin[0] = origin[1] = origin[2] = 0.0f;
  DEBUG(("Greetings from Cookie Source Invaders."));
  DEBUG(("Please ignore the extravegent assortment of comments to come..."));
} //end of init

void loop(){
  // Updates
  float temp[3];          //literally a temp array, used for calculations 
    api.getMyZRState(myState);
  api.getOtherZRState(otherState);
  data = mirror = otherMirror = freeData = 0;
  for (int a = 0; a < 9; a++){
      game.getItemLoc(items[a], a);//Item location to items[] array
      itemStatis[a] = game.hasItem(a);
      if (a >= 3 && a <= 6){
            if (itemStatis[a] == 0) data++;//If we have these items, add to amount of score packs we have
            if (itemStatis[a] == -1) freeData++;
      }
      if (a >= 7){
            if (itemStatis[a] == 0) mirror++;//If we have a mirror
            if (itemStatis[a] == 1) otherMirror++;//If the other people have a mirror
      }
    }// end of for loop
    
  zone = game.posInArea(myState);
  otherZone = game.posInArea(otherState);
  myEnergy = game.getEnergy();
  getMirrorTimeRemaining = game.getMirrorTimeRemaining();
  velocity = mathVecMagnitude(&myState[3], 3);
  counter= game.getCurrentTime();
  memory = game.getMemoryFilled();
  speedLimit = 0.5f;
  getNumMirrorsHeld = game.getNumMirrorsHeld();
  fuel = game.getFuelRemaining();

  DEBUG(("zone=%d otherZone=%d myEnergy=%f MirTimeRemaining=%d velocity=%f counter =%d memory=%d speedLimit =%f getNumMirrorsHeld=%d fuel=%f",
  zone,
  otherZone,
  myEnergy,
  getMirrorTimeRemaining,
  velocity,
  counter,
  memory,
  speedLimit,
  getNumMirrorsHeld,
  fuel
  ));
  
  if (memory == 0) uploading = false;
  
  takePictures(); //function description below. 
  upload();       //function description below.
  
  // Decision Making
  // ALL YOUR CODE GOES HERE!!! WRITE EVERYTHING HERE!!! 
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    memcpy(position, myState, sizeof(float)*3);
    switch (action){
        case 1:
        
            DEBUG(("Case 1"));
            grabClosestItems(3,6,true); 
            DEBUG(("Grabbing data"));
           
            if (    data > 0 && (
                    (data == 1 && items[bestID][2] > 0.0f && bestDistance > 0.2f) || 
                    zone != -1 ||
                    data >= 2)
                    )
            {
                action = 2;
            }
            // editted 12/19
            else { 
                break;
            }
          
        case 2:
            DEBUG(("Case 2"));
            grabClosestItems(7,8,true);
           
            if ( bestID != -1 && bestDistance > distance(temp,items[bestID],otherState) && otherMirror == 0 ){
                if ( bestID == 7) bestID = 8; else bestID = 7;//grab closest mirror
                if (itemStatis[bestID] == -1) {
                    memcpy(position,items[bestID],sizeof(float)*3) ;
                } else 
                    bestID = -1; 
            }
            
            DEBUG(("Grabbing mirrors"));
            /*If we have a mirror or bestID is not good*/
            if (mirror == 1 || bestID == -1 ){
                action = 3;
            }
            //editted 12/19
            else {
                break;
            }
            
         case 3:
            DEBUG(("Case 3"));
            
            if ( zone != DARK && ! (getMirrorTimeRemaining > 18 && dive) )  {
                if (myState[1] > -0.45f) {
                     position[1] -= 0.35f;
                }
            } 
            else {
                if ( freeData > 0 && mirror > 0 ) dive=true ; else dive = false;
                if (dive) { 
                    grabClosestItems(3,6,true);
                    DEBUG(("Grabbing data"));
                    dive = true;
                    speedLimit = 0.35f;
                } 
                else { 
                    if (zone == DARK) { 
                        DEBUG(("Running away from light")); 
                        if (myState[2] > -0.33f) position[2] -= 0.2f ;else position[2] = -0.53f;
                        if (myState[1] < 0.45f) position[1] += 0.22f; else position[1] = 0.65f;    
                    }
                }
            }
          break;
    } //end of switch statements
  
  if(action > 2 && getMirrorTimeRemaining == 0){
      grabClosestItems(3,6, false);
      DEBUG(("BEST DISTANCE: %f . Best ID: %d", bestDistance, bestID));
      if (bestDistance < 0.2f && velocity < 0.15f) {
          grabClosestItems(3,6, true);
          DEBUG(("DETOUR"));
      }
  }
  
  mathVecSubtract(temp, myState, otherState, 3);
  mathVecNormalize(temp, 3);
  float pseudoAngle = distance(temp, temp, &otherState[6]);
  
  memcpy(temp, myState, sizeof(float)*3); 
  temp[1] -= 0.075f; 
  
  if (getNumMirrorsHeld > 0 && 
      pseudoAngle < 0.55f &&
      (game.getOtherEnergy() > 0.5f || otherZone == LIGHT) &&
      distanceBetweenUs > 0.45f && 
      !(otherZone != DARK && otherState[2] - myState[2] > 0.5f) && 
      (game.posInDark(temp) == false || zone != DARK) && 
      counter > 90
      ){
          game.useMirror();
      }

    if (otherHasMirror == false && 
        ((myEnergy < 1.0f && zone == -1) || (counter > 170 && memory ==0 ) || 
        (counter > 150 && myState[1] > 0.5f && memory == 0 && freeData == 0))){
        game.takePic();
    }
  
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  
  // Always Do
  goTo();
  api.setAttitudeTarget(attitude);
} //end of loop

void goTo(){
    float vTgt[3];
    float scaledTgt[3];
    float dToTgt ;

    if (
        fuel < 5 || 
        (myEnergy < 1.0f && zone != 1) || 
        (distanceBetweenUs < 0.3f && velocity > 0.025f)
        ) {
        memcpy(position, myState, sizeof(float)*3); 
    }
    //x and z = .55 y = 0.7
    else if (myState[0] < -0.55f || myState[0] > 0.55f || myState[1] < -0.7f || myState[1] > 0.7f || myState[2] < -0.55f || myState[2] > 0.55f) {
        position[0]=position[1]=position[2]=0.0f;
    }

    dToTgt = distance(vTgt, position, myState);
    if ( dToTgt < speedLimit) speedLimit = dToTgt ;
    if ( dToTgt > 0 ) mathVecNormalize(vTgt, 3);
    for (int i=0; i<3; i++)  vTgt[i] *= speedLimit ;  
    mathVecAdd(scaledTgt, vTgt, myState, 3);
    api.setPositionTarget(scaledTgt);
}

//takePictures: always sets your attitude to face opponent. Note: overidden by functions called below in loop (i.e. upload) 
//takePictures: takes pictures under the neccessary condition. 
void takePictures(){
    otherHasMirror = false;
    distanceBetweenUs = distance(attitude, otherState, myState);
    mathVecNormalize(attitude, 3);
    
    if (uploading == false &&
        distanceBetweenUs > 0.5f && //there is enough distance 
        otherZone != DARK && //they are not in the dark
        game.isFacingOther() == true && //I am facing them
        memory != 2 && //my memory is not full
        game.isCameraOn() == true && //my Camera is On
        ((zone == 1 && myEnergy > 1.0f ) || (zone != 1 && myEnergy > 2.1f)) && 
        getMirrorTimeRemaining == 0  && //I do not have a mirror deployed
        // (memory > 0 && counter < 30) == false && 
        action != 1
        ){
        //if they have never picked up an item
        if (otherMirror > 0) {
            // If they have a mirror deployed
            if (game.getPicPoints() < 0.0f){ 
                otherHasMirror = true;
            } 
        }
        //If not deployed, take picture. 
        if (otherHasMirror == false){
            game.takePic();
        }
    } // first set of requirements
} //end of takePictures

//upload: faces earth and uploads pictures if memory is full or if games is about to end. 
void upload(){
    if (memory == 2 || 
        (memory > 0 && counter > 140) ||
        ((otherHasMirror || getMirrorTimeRemaining > 1) && zone == 1 && memory > 0) || 
        (otherZone == DARK && myEnergy > 2.5f && memory > 0)
        ){ 
        attitude[0] = attitude[1] = 0.0f; attitude[2] = 1.0f;
        uploading = true;
        if (
            myState[8] > 0.968f && 
            mathVecMagnitude(&myState[9], 3) < 0.05f &&
            (((zone == 1 && myEnergy > 1.0f ) || (zone != 1 && myEnergy > 2.0f)) || (myEnergy > 1.0f && counter > 145) )
            ){
            game.uploadPics();
        } // if I am facing earth, upload
    } //if my memory is full or the game is about to end and I have pic
} //end of upload


//safety: conditions are listed within function. 
// NO LONGER CALLED
/*
void safety(){
    // if my fuel is low, energy is low, or I am going to fast, stop my motion

    if (
        game.getFuelRemaining() < 5 || 
        myEnergy < 1.0f  || 
        (distanceBetweenUs < 0.3f && velocity > 0.025f)
        ) {
        memcpy(position, myState, sizeof(float)*3); 
    }
    // if my fuel is lower or  my energy is lower, stop attitude
    
    if (game.getFuelRemaining() < 2.5f || myEnergy < 0.5f){
        memcpy(attitude, &myState[6], sizeof(float)*3); 
    }
    
    // if I am out of bounds or nearing out of bounds, then move towards the origin
    // origin is arbitrary but works :)
    if (myState[0] < -0.57f || myState[0] > 0.61f || myState[1] < -0.76f || myState[1] > 0.76f || myState[2] < -0.60f || myState[2] > 0.60f) {
        position[0]=position[1]=position[2]=0.0f;
        //mathVecSubtract(position, origin, origin, 3);    
    }
}
*/
//end of safety

//grabClosestItems: grabs items closest to you based on their ID (argument of function)
void grabClosestItems(int start, int end, bool set){
/*
  energy: start = 0, end = 2.
  data: start = 3, end = 6.
  energy & data: start = 0, end = 6.
  mirror: start = 7, end = 8.
  if set is true, then actually move to grab the items. 
  if set is false, just calculate the closest one. 
*/
  float temp[3];
  bestDistance = 100.0f; bestID = -1;
  // The variable "float bestDistan ce = 100.f;" is made global for the purpose of loop()
  for (int a = start; a <= end; a ++){    
    if (itemStatis[a] == -1 ){       // if enough space, then make sure other sphere is going towards item (so include angle) before not going to it
        float myDist = distance(temp, items[a], myState);
        //mathVecSubtract(temp, items[a], otherState, 3);
        //float otherDist = mathVecMagnitude(temp, 3);
        //if (myDist < bestDistance && (myDist < otherDist || otherDist > 0.15f))  {
        if (myDist < bestDistance) {
          bestDistance = myDist;
          bestID = a;
        }
    }
  }
  
  if (data == 0 && ! (items[3][2] < 0.15f && items[5][2] < 0.15f) ){
        if ( items[3][2] < items[5][2] ){
           if (bestID > 4 ) bestID -=2 ;
        } else {
           if (bestID < 5 ) bestID +=2 ;
        }
  }
  
  if (bestID != -1 && set){
    memcpy(position, items[bestID], sizeof(float)*3);       
  }
} //end of grabClosestItems

float distance(float store[3], float one[3], float two[3]){
    mathVecSubtract(store, one, two, 3);
    return mathVecMagnitude(store, 3); 
}
//End page main

};

ZRUser *zruser7 = new ZRUser1029;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1034 : public ZRUser
{

//Begin page Function
#define LIFECHANGINGDECISION        0
#define SCOREPACKDECISION           1
#define TAKEPIC                     2
#define TELEPORT                    3

float thefunction(int option){
    ZRState mystate, oppostate;
    api.getMyZRState(mystate);
    api.getOtherZRState(oppostate);
    
    float energy = game.getEnergy();
    float nummirror = game.getNumMirrorsHeld();
    
    switch(option){
        case LIFECHANGINGDECISION:{
            int count = 0;
            for(int i = 0; i < 6; i++){
                game.getItemLoc(item[i],i+3);
                if(item[i][2] >= 0.0f){
                    count++;
                }
            }
            DEBUG(("count: %d\n",count));

            if(count == 4){
                choice = 4;
            }else{
                if(item[0][2] <= item[2][2]){
                    choice = 0;
                }else{
                    choice = 2;
                }
            }
            
            if(mystate[0] > 0.0f){
                //color = BLUE;
                if(choice == 0){
                    itemID = 0;
                }else if(choice == 2){
                    itemID = 2;
                }else{
                    itemID = 4;
                }
            }else{
                //color = RED;
                if(choice == 0){
                    itemID = 1;
                }else if(choice == 2){
                    itemID = 3;
                }else{
                    itemID = 5;
                }
            }
            break;
        }
        
        case SCOREPACKDECISION:{
            float dis;
            float disO;
            float min = 5.0f;
            int choice = 6;
            for(int i = 0; i < 6; i++){
                if(game.hasItem(i + 3) == -1 && !(nummirror >= 1 && (i == 4 || i == 5))){
                    dis = distance(&mystate[0],item[i]);
                    disO = distance(&oppostate[0],item[i]);
                    if(min > dis && dis < disO && !pack && item[i][2] < 0.0f){
                        min = dis;
                        choice = i;
                    }else if(pack && i != 4 && i != 5 && dis <= min){
                        min = dis;
                        choice = i;
                    }
                }
            }
            return choice;
        }
        break;
        
        case TAKEPIC:{
            float target[3];
            float bound = (game.getLightInterfacePosition() - 0.75f);
            if(bound < -0.8f){
                bound = bound + 1.6f;
            }
            float difference = oppostate[1] - bound;
            float dist = distance(&mystate[0],&oppostate[0]);
            int oppolightpos = game.posInArea(&oppostate[0]);
            int mirrortime = game.getMirrorTimeRemaining();
            int memoryfilled = game.getMemoryFilled();
            
            mathVecSubtract(target,&oppostate[0],&mystate[0],3);
            
            if(game.isFacingOther() && game.isCameraOn() && energy > 1.7f && /*distance(&mystate[0],&oppostate[0])*/dist >= 0.5f && oppolightpos != -1 && mirrortime == 0 && (curtime < 120 || energy > 3.0f)){
                if(game.getPicPoints() > 0.0f){
                    game.takePic();
                }
            }
            
            if((memoryfilled == 2) || (memoryfilled == 1 && ((oppolightpos == -1 && energy >= 2.5f) || mirrortime > 0))){
                target[0] = 0.0f;
                target[1] = 0.0f;
                target[2] = 1.0f;
                if(angle(&mystate[6],target) <= 0.25f && mathVecMagnitude(&mystate[9],3) <= 0.05f && energy > 1.8f){
                    game.uploadPics();
                }
            }
            
            if((mirrortime > 16 || dist < 0.35f) && game.posInLight(&mystate[0]) && memoryfilled == 0){
                game.takePic();
            }
            
            if(((difference < 0.28f && difference > 0.0f) || oppolightpos != -1 || memoryfilled != 0)){
                api.setAttitudeTarget(target);
            }
        }
        break;
        
        case TELEPORT:{
            float disVec[3];
            mathVecSubtract(disVec,item[itemID],&mystate[0],3);
            for(int i = 0; i < 3; i++){
                disVec[i] = disVec[i] / (40.0f);
            }
            api.setVelocityTarget(disVec);
        }
        break;
    }
}
//End page Function
//Begin page angle-distance
float angle(float Att[3], float target[3]){
    return acosf(mathVecInner(Att,target,3) / (mathVecMagnitude(Att,3) * mathVecMagnitude(target,3)));
}

float distance(float target[3],float myPos[3]){
    float disVec[3];
    mathVecSubtract(disVec,target,myPos,3);
    return mathVecMagnitude(disVec,3);
}
//End page angle-distance
//Begin page main
#define ST_PICKITEM      0
#define ST_CHECKITEM     1
#define ST_LIGHTTELEPORT 2
#define ST_DARKTELEPORT  3
#define ST_FACINGDOWN    4

#define LIFECHANGINGDECISION        0
#define SCOREPACKDECISION           1
#define TAKEPIC                     2
#define TELEPORT                    3

#define HP 2
#define HI 2
#define HD 13
#define LP 0.13//0.258
#define LI 0.0//0.0086
#define LD 1.5//1.935
#define SP 0.12
#define SI 0
#define SD 1.55

float item[7][3];

int itemID;
int state;
int choice;
int curtime;

int zero;
int pack;
//bool situation;

void init(){
    zero = FALSE;
    pack = FALSE;
    thefunction(LIFECHANGINGDECISION);
    state = ST_PICKITEM;
    curtime = -1;
}

void loop(){
    ZRState myState, otherState;
    api.getMyZRState(myState);
    api.getOtherZRState(otherState);

    float DGBound = game.getLightInterfacePosition() - 0.05f;
    float LGBound = DGBound - 0.7f;
    float myEnergy = game.getEnergy();
    int numMirror = game.getNumMirrorsHeld();//
    int count = 0;
    bool myDark = game.posInDark(&myState[0]);
    bool otherLight = game.posInLight(&otherState[0]);
    
    curtime++;
    
    api.setPosGains(0.12f,0.0f,1.55f);

    if(DGBound < -0.8f){
        DGBound = DGBound + 1.6f;
    }
    if(LGBound < -0.8f){
        LGBound = LGBound + 1.6f;
    }
    
    DEBUG(("state: %d\n", state));
    //DEBUG(("dif: %f\n", myState[2] - otherState[2]));
    //DEBUG(("mdifference: %f\n",mdifference));
    //DEBUG(("mirrortime: %d\n",game.getMirrorTimeRemaining()));
    //DEBUG(("itemID: %d\n", itemID));
    //DEBUG(("difference: %f\n", DGBound - otherState[1]));
    //DEBUG(("DGBound: %f\n", DGBound));
    //DEBUG(("LGBound: %f\n", LGBound));
    //DEBUG(("dis01: %f\n",distance(item[0],item[1])));
    //DEBUG(("dis23: %f\n",distance(item[2],item[3])));
    
    if(myEnergy < 1.0f){
        game.takePic();
    }
    
    thefunction(TAKEPIC);
    
    float mdifference = myState[1] - LGBound;
    
    DEBUG(("mdifference: %f\n",mdifference));
    
    if((mdifference <= 0.06f || mdifference >= 0.8f) && (myState[2] - otherState[2] >= 0.45f || curtime >= 95)){
        game.useMirror();
        //numMirror = game.getNumMirrorsHeld();
    }
    
    if(myEnergy < 1.2f && myDark && curtime < 165 && !zero && (mdifference >= 0.05f || mdifference < - 0.08f)){
        thefunction(TELEPORT);
        DEBUG(("break"));
        return;
    }
    
    for(int i = 0; i < 3; i++){
        item[6][i] = myState[i];
    }

    switch(state){
        case ST_PICKITEM:{
            pickitem:
            if(count > 3){
                break;
            }
            count++;
            if(!(game.hasItem(itemID + 3) == -1)){
                state = ST_CHECKITEM;
                goto checkitem;
            }
            if((mdifference <= 0.05f || !myDark) && curtime >= 110 || (myState[1] - otherState[1] > 0.05f && !myDark && curtime >= 50)){
                itemID = 6;
                state = ST_LIGHTTELEPORT;
                goto lightteleport;
            }
            break;
        }
        
        case ST_CHECKITEM:{
            checkitem:
            if(count > 3){
                break;
            }
            count++;
            itemID = thefunction(SCOREPACKDECISION);
            if(itemID == 6 && (otherLight && otherState[2] - myState[2]  >= 0.4f || choice == 4) && curtime < 110){
                state = ST_FACINGDOWN;
                goto facingdown;
            }else if(itemID == 6 && myDark){
                state = ST_DARKTELEPORT;
                goto darkteleport;
            }else if(itemID == 6 && !myDark){
                state = ST_LIGHTTELEPORT;
                goto lightteleport;
            }else{
                state = ST_PICKITEM;
                goto pickitem;
            }
            //return;
        }
        
        case ST_FACINGDOWN:{
            facingdown:
            if(count > 3){
                break;
            }
            count++;
            item[6][0] = otherState[0];
            item[6][1] = otherState[1] - 0.1f;
            item[6][2] = -0.55f;
            if(otherState[5] < -0.15f || otherState[2] < -0.1f){
                state = ST_CHECKITEM;
                choice = 1;
                goto checkitem;
            }
            if(numMirror == 0){
                zero = FALSE;
                choice = 1;
                state = ST_LIGHTTELEPORT;
                goto lightteleport;
                //return;
            }
            break;
        }
        
        case ST_LIGHTTELEPORT:{
            lightteleport:
            if(count > 3){
                break;
            }
            count++;
            if(myEnergy < 3.3f){
                if(DGBound < 0.4f){
                    item[6][1] = DGBound + 0.3f;
                }else{
                    item[6][1] = -0.7f;
                }
            }else{
                item[6][1] = DGBound - 0.2f;
            }
            
            if(numMirror >= 1 || curtime > 110){
                if(myState[2] <= 0.1f){
                    item[6][2] = 0.1f;
                }else{
                    item[6][2] = myState[2];
                }
            }else{
                item[6][2] = item[4][2];
            }
            
            if(myState[1] < DGBound){
                state = ST_CHECKITEM;
                goto checkitem;
            }
            break;
        }
        
        case ST_DARKTELEPORT:{
            darkteleport:
            if(count > 3){
                break;
            }
            count++;
            pack = TRUE;
            int score = 0;
            for(int i = 0; i < 4; i++){
            if(game.hasItem(i + 3) == -1){
                score++;
                }
            }
            if(score > 0){
                state = ST_CHECKITEM;
                goto checkitem;
                return;
            }
            if(curtime < 130 && numMirror >= 1 && game.getMemoryFilled() == 0 && !otherLight){
                zero = TRUE;
                game.takePic();
            }else if(curtime > 130){
                item[6][1] = 0.6f;
                if(myState[1] >= 0.5f){
                    game.takePic();
                }
            }
            if(mdifference <= 0.05f || !myDark){
                zero = FALSE;
                state = ST_LIGHTTELEPORT;
                goto lightteleport;
            }
            break;
        }

    }
    
    api.setPositionTarget(item[itemID]);

    //yes....
    //yes.... -Mr.P
    
}
//End page main

};

ZRUser *zruser1 = new ZRUser1034;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1023 : public ZRUser
{

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

#define  tol  0.05f
#define  MIN_ANGLE  0.79f
#define  GRAY_AREA -1
#define  DARK_AREA   0
#define  LIGHT_AREA  1
#define  MINIM_FUEL    5.0f
#define  VEC_SIZE    12
#define  LIMIT   0.62f

//Declare any variables shared between functions here
short               photos_taken, step, TIME; 
bool                must_upload_pictures, oponent_in_dark,
                    oponent_close, slow_down, light, pack_detected, hugme;
                    // can_take_a_picture, oponent_converges, can_take_a_picture_no_mirror;
short               score_item, mirror_item/*, old_score, old_energy*/, tt, mirrors;
float               destination[3], *cpos, *cvel, *catt, *crot, *oatt,
                    o_distance, velocity, rot_vel, fuel, energy,
                    oponent_direction[3], State[12], oState[12], *opos, *ovel,
                    desired_attitude[3], distance_to_light, distance_to_dark, distance,
                    dark_border, light_border, ovelocity;

    
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 = 6;
  mirrors=0;
  mirror_item=-1;
//  old_score = 100;
  //old_energy = 100;
  must_upload_pictures = 0;
  hugme = false;
  api.setAttGains(1.0f,.0f,1.0f);
}



void loop(){
  //This function is called once per second.  Use it to control the satellite.
  
  float           flocal, fvector[3], score_location[3], oenergy, vcoef;
  
//  bool            /*dummy = false,*/ sphere_is_getting_into_light;
  
  
  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
  
  rot_vel = mathVecMagnitude(crot,3);             //rotational velocity
  velocity = mathVecMagnitude(cvel,3);            //velocity
  photos_taken = game.getMemoryFilled();          //how many photos into memory
  //moving_outwards = (mathVecInner(cpos,cvel,3)>0);  //true if we are moving outwards, false otherwise
  //fuel = game.getFuelRemaining();             //fuel 
    energy = game.getEnergy();                  //energy
    oenergy = game.getOtherEnergy();          //oponent energy
  
  

    //Sets desired attitude. We want to face the oponent in order to take pictures
    mathVecSubtract(oponent_direction,opos,cpos,3);
    //flocal = o_distance;
    o_distance=mathVecNormalize(oponent_direction,3);
    if (((mathVecInner(ovel,oponent_direction,3)<-0.997f*ovelocity)&(ovelocity>.01f)))hugme=true;
    //check if oponent is close or converges
    //oponent_converges = ((o_distance<0.4f)&&(flocal > o_distance)) ? true : false;
    //#ifdef MOV_DEBUG_MESSAGES2
    //DEBUG(("Converges %d.", oponent_converges));
    //#endif

    //oponent_close = (o_distance<0.3f) ? true : false;
    //#ifdef MOV_DEBUG_MESSAGES2
    //DEBUG(("Antipalos konta 3: %d", oponent_close));
    //#endif*/
    
    //additional information
    //Sets the light variable to 1 (if we are in the light zone), 0 (if we are in grey zone) or -1
    //if we are in dark zone
    //light = (game.posInLight(cpos)) ? LIGHT_AREA : game.posInDark(cpos) ? DARK_AREA : GRAY_AREA;
    // light = (game.posInLight(cpos)) ? LIGHT_AREA : DARK_AREA;
  light = game.posInLight(cpos);
  oponent_in_dark = game.posInDark(opos);
  light_border = game.getLightInterfacePosition();
  distance_to_light = cpos[1] + cvel[1] - light_border;
  dark_border = (light_border>0) ? light_border - 0.8f : light_border + 0.8f;
  distance_to_dark = cpos[1] + cvel[1] - dark_border;
  
//  sphere_is_getting_into_light = ((dark_border>light_border)^(distance_to_light<0.1f)) //changed last num
//                          &(distance_to_dark<0.08f);
  
//  can_take_a_picture_no_mirror = ((tt==0)&(energy>action_energy)&(photos_taken<=1)
             // &(game.isFacingOther())&(!oponent_in_dark)&(game.isCameraOn()));

//  can_take_a_picture = false;
//  if (can_take_a_picture_no_mirror)
     // can_take_a_picture = (game.getPicPoints()>0);
    // can_take_a_picture= ((tt==0)&(energy>action_energy)&(photos_taken<=1)
             // &(game.isFacingOther())&(!oponent_in_dark)&(game.isCameraOn()))
             // && (game.getPicPoints()>0); //boolean and for short-circuiting
  //determine the velocity (variable: vcoef) for our movement
  //If the Sphere is in light, then move fast. Otherwise move slower.
  //Also, determine the required energy to take photo and upload (action_energy)
  //action_energy = ((light!=DARK_AREA)&&(distance_to_light > 0.15f)) ? 1.5f  : 2.7f;

//  #define action_energy 1.3f
  
  //DEBUG(("Action energy : %f", action_energy));
  
  must_upload_pictures = (photos_taken>1)
                      |((((((energy<1.1f)&(distance_to_dark>0.2f))|(tt>0)|(TIME>=163)) )
                      |(must_upload_pictures))&(photos_taken>0));
  
    
  tt = game.getMirrorTimeRemaining();
  
  
    //Messages (printed only if DEBUG_MESSAGES is defined)
  
  if ((mirror_item!=-1)&(game.hasItem(mirror_item)==0)) mirrors++;
  //best suited items (-1 if no items are present)
  mirror_item = ItemSelection(7,8);
  score_item = ItemSelection(3,6);
  game.getItemLoc(score_location,score_item);

  
  vcoef = (light) ? 0.55f : 0.28f;
  
  
  //stop, no more energy...
//  flocal = (velocity<0.02f) ? 0.70f : (velocity<0.03f) ? 0.9f : 1.3f;
  
  
    // DEBUG(("vcoef:%f",vcoef));
    //-----------------------------------------------------------------
  //Strategic Steps (step is initialized at 0)
  switch(step){
  
  case 4:
        if (score_item>=0){
            memcpy(destination,score_location, VEC_SIZE);
            //if dark is close or if the oponent is moving towards the dark zone
            // DEBUG(("needed speed: %f",(tt>0?distance_to_light/tt:-1)));
            //previously included (distance_to_light<0.4f)&(distance_to_light>-0.05f)
            if ( ((tt>0) bitand (distance_to_light>0.025f*(tt-1))) | ((tt<8)&(distance_to_light>-0.1f)&(o_distance>0.4f))
                        |((opos[1]-cpos[1]<-0.1f)&(ovel[1]<-0.15f)) )
                destination[1] = light_border - 0.2f;
        }
        else {
            step = 8;
        }
    break;


    /*case 5:
    label5:
        memcpy(destination,score_location, VEC_SIZE);
        vcoef = 0.5f;
        if (light){
            step = 4;
        }
    break;*/



    case 6:
        // mirrors = game.getNumMirrorsHeld();
        DEBUG(("Number of mirrors: %d",mirrors));
        if (mirror_item!=-1){
            game.getItemLoc(destination,mirror_item);
            vcoef=0.9f;
            if (o_distance<0.3f)
                vcoef = .4f;
                
            float theirDist = DistanceBetweenPoints(destination,opos);
            float ourDist = DistanceBetweenPoints(destination,cpos);
            mathVecSubtract(fvector,destination,opos,3);
            mathVecNormalize(fvector,3);
            if ((theirDist>.02f) & mathVecInner(fvector,ovel,3)>.93f*ovelocity
            & (ourDist>.03f) & (mirrors==1) & ((theirDist>ourDist-.2f)) )// MAJOR CHANGE !!!!!!!
                vcoef = .27f*ourDist/theirDist;
                
            
        }
        else{
            if (mirrors == 2){
                step = 8;
            }
            else{
                step = 4;
            }
        }
    break;
    
    
    case 8://go up for pictures
        if ((TIME<75)&(light))
            vcoef = 1.428f;
        destination[2] = -0.35f;
        destination[0] = cpos[0]*0.85f;
        if (hugme bitand (TIME<130)){
            vcoef = 1.0f;
            destination[1]=-0.72f;
            if (/*(cpos[0]<-.5f) bitand */ (o_distance<.42f)){
                destination[2]=-.1f;
            }
        }
        else{
            destination[1] = light ? max(-0.78f, dark_border-1.2f) 
                                                     : ( ((TIME<100)&(energy>1)) bitor tt>0? min(-0.2f, dark_border+0.2f) : -0.1f );
            if (hugme){
                destination[1]=-0.2f;
                vcoef = 0.15f;
            }
            else if (TIME>100){
                if (photos_taken==0){
                    vcoef = 0.5f;
                }
                //if we couldn't take photos on a downstream movement, go for scores.
                if ((score_item>=0)&(energy>3)){
                    step = 4;
                }
            }
            else if ((!hugme)&(score_item>=0)&(
                                (opos[1](0.9f))) { // hardstop-esque logic
        mathVecSubtract(fvector, destination, cpos, 3);
        distance = mathVecNormalize(fvector, 3);
        if (distance > 0.40f) {
            flocal = vcoef*0.07f;
            if ( ( fabs(flocal - velocity) > 0.008f)|(step!=6)|(light==LIGHT_AREA)){
                //DEBUG(("VELOCITY CONTROL : vel = %f  des_vel = %f", velocity, flocal));
                vrescale(fvector, flocal);
                api.setVelocityTarget(fvector);
            }
        }
        else{
            api.setPositionTarget(destination);
        }
  }
  else {
      memset(fvector,0.0f,VEC_SIZE);
      api.setVelocityTarget(fvector);
      DEBUG(("stopping"));
  }
    
    
    
    //DEBUG(("ACTION ENERGY %f", action_energy));
    //picture taking stuff
    //Normal Picture
    if ((((tt==0)&(energy>1.2f)&(photos_taken<=1)
          &(game.isFacingOther())&(!oponent_in_dark)&(game.isCameraOn()))
          && (game.getPicPoints()>0))
          |((energy<1) | ((TIME>150)&(photos_taken==0))|((tt>8)&(o_distance<0.35f)))) {
        game.takePic();
    }
    DEBUG(("%f   %f",distance_to_light,distance_to_dark));
    DEBUG(("HugMe=%d",hugme));
    //mirror code
    // if ( !(hugme|TIME<45)/*&(mathVecInner(oatt,oponent_direction,3)<-0.80f)*/&((o_distance>0.4f) bitor step==8)&(tt<=1)&((light_border>dark_border)?(distance_to_light<.2f & distance_to_light>-.6f):((distance_to_light<.2f) bitor (distance_to_dark>-.2f)))){
    if ( !(hugme)
        &(((step == 8)|
        ((mathVecInner(oatt,oponent_direction,3)< -0.80f)&(o_distance>0.4f)))
        &(tt<=1) & ((distance_to_dark>.7f) bitor (distance_to_dark<.2f bitand (distance_to_light> (TIME<68?.4f:-.15f) bitor distance_to_light<-.65f)))) ) {
            game.useMirror();
            DEBUG(("USING MIRROR"));
    }
    //if memory is full we need to upload.
    // DEBUG(("must_upload_pictures:%d",must_upload_pictures));
    vrescale(oponent_direction, o_distance);
    mathVecAdd(desired_attitude, oponent_direction, ovel, 3);
    mathVecSubtract(desired_attitude, desired_attitude, cvel, 3);
    if (must_upload_pictures/*&&!huggingNow*/){
        //uploading stuff
        if ( (catt[2]>0.969f)&(rot_vel<0.05f)&(energy>((TIME>165)?1:1.2f)) ){
          game.uploadPics();
        }
        memcpy(desired_attitude, EARTH, VEC_SIZE);
        // mathVecAdd(desired_attitude, (float *) EARTH, cvel, 3);
    }
    //mathVecNormalize(desired_attitude,3);
    if ( (TIME>50)&(energy>0.5f)){
        DEBUG(("Rotating to %s",must_upload_pictures?"upload":"take pic"));
        api.setAttitudeTarget(desired_attitude);
        //DEBUG(("ROTATE"));
    }
    
}












//--------------------------------------------------------------
//------------- Our own functions ------------------------------

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

float  min(float a, float b){
    return (a l*a
void    vrescale(float *vector, float local)
{
    vector[0] *=local;
    vector[1] *=local;
    vector[2] *=local;
}


//--------------------------------------------------
//Stabilization function - Hard breaks




//--------------------------------------------------------
//find the closest (free) item with item id between i_start and i_stop.
//The function disregards the items that are closer to the oponent
//returns -1 if there are no other free items.
short    ItemSelection(short i_start, short i_stop){
    float       pos1[3],d1,d2,d0;
    short       i,i0=-1;
    
    d0 = 100;
    for (i=i_start;i<=i_stop;i++){
        if (game.hasItem(i)==-1){
            game.getItemLoc(pos1,i);
            d1 = DistanceBetweenPoints(pos1,cpos);
            d2 = DistanceBetweenPoints(pos1,opos);
            if ((d1<=d0)&((d2>0.02f)|(ovelocity>0.02f))){ //just happen to be the same
                d0=d1;
                i0=i;
            }
        }
    }
    
    return i0;    
}

float   DistanceBetweenPoints(float  *A,  float *B){
    return (A[0]-B[0])*(A[0]-B[0])+(A[1]-B[1])*(A[1]-B[1])+(A[2]-B[2])*(A[2]-B[2]);
}
//End page main

};

ZRUser *zruser1 = new ZRUser1023;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1026 : public ZRUser
{

//Begin page functions
int choseScoreNearMirror(float mirror[3],float myState[12],float score)
{
    float lastDistance = 50.0f;
    int choosenIndex = -1;
    int i=3;
    while(i<7)
    {
        float actualPosition[3];
        game.getItemLoc(actualPosition,i);
        float nowDistance = getDistance(mirror,actualPosition);//Prendo la distanza dallo score attuale
        if(score>2.9)
            nowDistance = getDistance(myState,actualPosition);
        
        if(nowDistance0.8f)?1.65f:0.00f;
    float mirror[3];
    short picInsideMemory = game.getMemoryFilled(),
                            myZone = game.posInArea(myState),
                            enemyZone = game.posInArea(enemyState);
    if(((picInsideMemory<2 &&actualProcedure!=8)||(actualProcedure==8&&picInsideMemory<1))&& conditionToTakePic(watchingEnemy,myZone,enemyZone,distanceToEnemy,energy) /*&& takePics*/)
    {
        if(game.isCameraOn() && game.getMirrorTimeRemaining()==0)
        {
            (game.getPicPoints()>=0)?game.takePic():0;
        }
    }
    if(((picInsideMemory>=1 && actualFrame>168)
        || (actualProcedure>7 && energy<3.4f && picInsideMemory>0)
        || (picInsideMemory==1 && myZone==-1 && enemyZone==-1 && energy>2.3f) 
        || (myZone==1 && game.getMirrorTimeRemaining()>0 && picInsideMemory>0)
        || (picInsideMemory>1)))
    {
        attitudeToEarth=true;
    }
    
    /*if(takePics)
    {*/
        if(!attitudeToEarth)
        {
            mathVecSubtract(whereAttitude, enemyState, myState, 3);
        }
        else
        {
            float earth[3]={myState[0],myState[1],myState[2]+0.07f};
            mathVecSubtract(whereAttitude,earth,myState,3);
            /*float myAttProject[3],myPointProject[3];
            memcpy(myAttProject,myState+6,sizeof(float)*3);
            mathVecNormalize(myAttProject,3);
            for(int i=0;i<3;i++){
                myPointProject[i]=myState[i]+(myAttProject[i]*0.68413680834*0.05);
            }
            myPointProject[2]=myState[2]+0.05f;
            mathVecSubtract(whereAttitude,myPointProject,myState,3);*/
            if(myState[8]>0.96891242171f) //cosf(0.25)
            {
                if((energy>=1.55f) || (energy>=1.00f && actualFrame>140))
                {
                    if(mathVecMagnitude(myState+9,3)<=0.05f&&picInsideMemory>0)
                    {
                        game.uploadPics();
                        if(energy>2.2f)
                            attitudeToEarth=false;
                    }
                    
                }
            }
        }
    
        mathVecNormalize(whereAttitude,3);
        if(actualProcedure!=6 || actualProcedure==6 && (!attitudeToEarth||myZone!=-1||energy>3)){
            api.setAttitudeTarget(whereAttitude);
        }
    
    
    game.getItemLoc(mirror,8-Color);

    /*if(score>1.4f&&enemyState[1]<=(mirror[1]+0.02f)){
        allScore = true;
        takeTwoScore=false;
    }
    else if(actualFrame<43){
         allScore = false;
         if(score>1.4f&&game.hasItem(7+Color)==1)
            takeTwoScore = true;
    }*/
    
    short availableEnergy = 3;
    
    for(int i=0;i<3;i++){
       if(game.hasItem(i)!=-1)
            availableEnergy--;
    }
        
    DEBUG(("allScore=%d",allScore));
   
    if(actualProcedure==0)
    {
        choseScoreNearMirror(mirror,myState,score);
        float distanceToNextScore = getDistance(myState,whereToGo);
        if(score>1.4f&&enemyState[1]<=(mirror[1]+0.01f)){
            allScore = true;
            takeTwoScore=false;
        }
        else if(actualFrame<43){
             allScore = false;
            if(score>1.4f&&game.hasItem(7+Color)==1||availableEnergy<3)
                takeTwoScore = true;
        }
        else if(distanceToEnemy<0.2f&&actualFrame>75){
            takeTwoScore = false;
        }
        if((choseScoreNearMirror(mirror,myState,score)==-1)||(/*(actualFrontInterface+0.3)>myState[1]
        &&*/score>1.4f && (distanceToNextScore>0.1f||whereToGo[2]>-0.03f)//whereToGo[2]>-0.1f
        &&(!allScore)&&(!takeTwoScore))||(takeTwoScore&&score>2.9f)){
            if((actualFrontInterface+0.34f)>myState[1]||actualFrame>50){
                actualProcedure=5;
            }
            else{
                whereToGo[0]=myState[0];
                whereToGo[1]=myState[1]-0.2f;
                if(myState[2]>-0.3)
                    whereToGo[2]=-0.35f;
            }
        }
         DEBUG(("distance:%f",distanceToNextScore));    
        if(!allScore&&!takeTwoScore){
            kVel = 0.0965077148161220323f;
           }
        else if(availableEnergy<3){
            kVel = 0.062f;
        }
        else{
            kVel = 0.058f;
        }
    }
    
    DEBUG(("twoscore:%d",takeTwoScore));
    if(actualProcedure==5)
    {
        if(game.hasItem(8-Color)==-1)
        {
            game.getItemLoc(whereToGo,8-Color);
        }else if ((game.getScore()>4.7f) //||(myState[8]>0.975f)//prova.. eliminare se nn buono
        ||(game.hasItem(8-Color)==1)||(actualFrame>95)||(actualFrame>70&&myZone==-1)
        ||(enemyState[1]<(myState[1]-0.015f))){
            actualProcedure=6;
        }
        
        /*if(myState[8]>0.9f)
            kVel = 0.08f;
        else*/
        if(availableEnergy<3)
            kVel = 0.06f;
        else
            kVel = 0.05f;
            //kVel = 0.0507148161220323f;
        
    }
    
    if(actualProcedure==6)
    {
        
        if(actualFrame>42&&actualFrame<99&&myZone==1){
            if(enemyState[1]<(myState[1]-0.01f))
                kVel = 0.05f;
            else
                kVel=0.115f;
            
        }
        else if(myZone==1 && energy>1.5f){
            kVel=0.15f;
        }
        else{
            kVel=0.1f;
        }
        
        if((myState[1]-actualFrontInterface)<0.035f /*&& distanceToEnemy>=0.48f*/ && actualFrame>75)
        {
            game.useMirror();
        }
        
    
        if(choseScoreNearMirror(mirror,myState,score)==-1||actualFrame>130
        ||myState[1]>(actualBackInterface-0.1f)&&actualFrame<105
        ||(myState[1]-actualFrontInterface)<-0.05f
        ||getDistance(enemyState,whereToGo)+0.2f105)
                whereToGo[1]=actualBackInterface-0.3f;
            else
                whereToGo[1] = 0.1f;
            whereToGo[2]=myState[2]+0.05f;
            
        }
        
        
        if(actualBackInterface>myState[1] && myZone==-1 && actualFrame>114)
        {
            actualProcedure=8;
        }
        //if(score>game.getOtherScore() && (myState[1]-0.1)105)
            //takePics = false;
    }
    
    
    
    if(actualProcedure==8)
    {
       
        if(choseScoreNearMirror(mirror,myState,score)==-1){
            whereToGo[0]=(Color==1)?0.4f:-0.4f;
            whereToGo[1]=0.65f;
            kVel = 0.17f;
        }
        else if(picInsideMemory<2)
        {
            kVel = 0.11f;
        }
        else{
            kVel = 0.16f;
        }
        
    }
    DEBUG(("VEL:%f",kVel));
    if(energy<1) game.takePic();
    
    float distanceToWhereGo = getDistance(myState, whereToGo);
    DEBUG(("ORA:%f",distanceToWhereGo));
    velocity = (((distanceToWhereGo*0.0068f)/kVel));
    if(actualProcedure==6&&energy<0.3f)
        velocity = 0;
       
    DEBUG(("fase:%d",actualProcedure));
    float velocityVector[3];
  mathVecSubtract(velocityVector, whereToGo, myState, 3);
  mathVecNormalize(velocityVector, 3);
  for(int i=0;i<3;i++)
    velocityVector[i]*=velocity;
  api.setVelocityTarget(velocityVector);
  
}
//End page main
//Begin page takePic
bool conditionToTakePic(bool myAttitudeToEnemy,short myZone,short enemyZone,float myState_enemyState,float energy)
{
    return (myAttitudeToEnemy && ((myZone != 1) ? ((energy>=1.55 && actualProcedure!=0) || (energy>2.45f && actualProcedure==0)) : energy>=1.00) && enemyZone!=-1 && myState_enemyState>=0.5f); 
}
//End page takePic

};

ZRUser *zruser3 = new ZRUser1026;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1025 : public ZRUser
{

//Begin page Functions
void checkPhoto() {
    if (memoryFilled < 2 && time<181)
    {
        /*faceOther=true;
        if (faceOther)*/ face(otherState);
            if (game.isFacingOther() && game.posInArea(otherState)!= -1
            && (energy >1.3f || (posInArea==1 && energy>1.0f))
            && distanceBetween(myState, otherState)>0.4f)


                if (game.getMirrorTimeRemaining()==0 && !hasMirrorActive())
                if(game.getPicPoints()>0.0f) game.takePic();
    }
        uploadPics();

      //GUADAGNAMO 0.01 QUANDO SIAMO IN LUCE
      if(posInArea==1 && energy>4.3f && !hasMirrorActive())
         game.takePic();
}

void uploadPics() {
    if (memoryFilled== 2 && energy>1.5f || memoryFilled!=0 && (time>168||goal==4)) {
        //faceOther=false;
        api.setAttitudeTarget(earthAtt);
        if (mathVecInner(&myState[6],earthAtt,3)>0.968912f && mathVecMagnitude(&myState[9],3)<=0.05f)
            game.uploadPics();
    }
}

/*void checkEnergy() {
   if(game.posInLight(myState)==true && game.getEnergy() > minEnergy) copyPos=false;

    if (game.getEnergy() <= minEnergy && game.posInLight(myState)==false)
    {
         if (copyPos==false) {
            memcpy(myPos,myState, 3*sizeof(float));
            copyPos=true;
           }
         if(copyPos==true) {
           api.setPositionTarget(myPos); }   //DEBUG(("ok"));
           }
}*/

bool hasMirrorActive () {
    if(game.hasItem(8-color)==1)
    {
        if(game.getPicPoints()<0.0f)
        {
            mirrCount= time;
            return true;
            if(time - mirrCount<24) return true;
            else return false;
        }
    }
     return false;
}

bool isFacingUs() {
    // MAX_FACING_ANGLE =  is cosf(0.25f)
    float relativePos[3];
    mathVecSubtract(relativePos, myState, otherState, 3);
    mathVecNormalize(relativePos, 3);
    return (mathVecInner(&otherState[6], relativePos, 3) > 0.968912f); //C.C.
}

float DistFromObj(float *target,float *who) {
    float dif[3];
    mathVecSubtract (dif, target,who, 3);
    return mathVecMagnitude (dif,3);
}

void face(float* pos) {
    float vectorBetween[3];
    mathVecSubtract(vectorBetween, otherState, myState,3);
    mathVecNormalize(vectorBetween,3);
    api.setAttitudeTarget(vectorBetween);
}

bool isCloseTo( float* who, float* target , float d ) {
    float dif[3] ;
    mathVecSubtract(dif , target , who , 3 ) ;
    return (mathVecMagnitude( dif , 3 ) < d) ;
}

void mathVecScale(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;
}

bool isEnemyOutOfBounds() {
    return (fabs(otherState[0])>0.64f || fabs(otherState[1])>0.8f || fabs(otherState[2])>0.64f);
}

bool outOfBounds() {
    return (fabs(myState[0])>0.64f || fabs(myState[1])>0.8f || fabs(myState[2])>0.64f);
}

int closestPoint() {
    int min;
    float minDistance,actualDistance;
    minDistance=999;
    min=-999;
    for(int i=0;i<4;i++) {
        actualDistance=distanceBetween(myState,item[i+3]);
        if(game.hasItem(i+3)==-1&&actualDistance=100) {game.useMirror(); goal=4;}
    if(time>130) goal=5;
}

void setSecureTarget(float point[3]) {
    if(distanceBetween(myState, otherState)>0.22f||energy>0.2f) api.setPositionTarget(point);
}
//End page Functions
//Begin page Init
ZRState myState,otherState;
//---------------Variables that are used for the strategy-----------//

//------------------Common And General Variables------------------//
float  item[9][3] ;

float earthAtt[3], maxUploadAngVel, myPos[3];

bool neverUsedEnemy, copyPos;
//float minEnergy;
float energy;

  int time, mirrCount, ID, takenScore;
  char colorSign;  // 1 blue,      -1 if red
  char color;      // 0 if blue     1 if red
  char posInArea; // otherPosInArea;

int goal;
int outCount;
int firstItem;
int memoryFilled;

void init()
{
    earthAtt[0] = .0f;
    earthAtt[1] = .0f ;
    earthAtt[2] = 1.0f;

    //minEnergy = 1.0f;

    // IF BLUE
  colorSign = 1;
  color = 0;

  time                    = 0;
    ID          = 0;
    takenScore              = 0;


    copyPos                 = false;
    neverUsedEnemy          = true;


    goal = 0;
    outCount=0;

   for (int t=0; t<9 ; t++)
        game.getItemLoc(item[t], t);
}

//End page Init
//Begin page Not Used Functions
//Attention: this function needs the global float array oldPos set to 0
//And conta set to 0
/*
bool notCollision()
{
    if (conta==0) memcpy(oldPos,otherState,3*sizeof(float));
    conta++;

    float vecBetween[3], relativeP[3];
    if (conta>3)
    {
        mathVecSubtract(vecBetween,otherState,oldPos,3 );
        mathVecNormalize(vecBetween,3);

        mathVecSubtract(relativeP, myState, otherState, 3);
        mathVecNormalize(relativeP, 3);

        bool face = mathVecInner(vecBetween, relativeP, 3) > 0.955336f ; //0.30 rad

        conta=0;
        return face;
    }
}
*/

/*

void avoidOutOfBounds()
{
  float ToWard[3] = {0.0f, 0.0f, 0.0f};
  float forceNoOut[3];

  mathVecSubtract(forceNoOut, ToWard, myState, 3);

  if ((fabs(myState[0])>0.60f && fabs(myState[3])>0.01f) || 

    (fabs(myState[1])>0.75f && fabs(myState[4])>0.01f) ||

    (fabs(myState[2])>0.60f && fabs(myState[5])>0.01f))
  {
      DEBUG(("Danger!"));
    api.setForces(forceNoOut);
  }
}
*/




//Attention: this function needs the global float variable threshold inizialized to 0
/*
void moveFaster(float * Point, int counter)
{
    float Dist = DistFromObj(myState,Point);



    if (counter==0) threshold = Dist*0.2f;//0.21f
    if(Dist > threshold) setConstantVelocityTo(Point, 0.05f);
    else api.setPositionTarget(Point);
}
*/

/*bool isFacingEarth() //C.C. (non serve, era usata una sola volta)
{
    return (mathVecInner(&myState[6], earthAtt, 3) > 0.968912f);
}*/


//------------------------------Motion Functions----------------//

/*
void PowerForce(char power, float *target) // follow the target using a velocity that varies exponentially with the distance myState/target
{

  float k = mathVecMagnitude(&target[3], 3) * powf(DistFromObj(myState, target) + 0.83f, power);
  setConstantVelocityTo(target, k);
  if (DistFromObj(myState, target) < 0.4f)
  {
    setConstantVelocityTo(target, 0.01f*k);//0.05
  }
}
*/


/*
void setConstantVelocityTo(float point[], float k) // goes to a point with a constant velocity
{
  float velocityVector[3];
  mathVecSubtract(velocityVector, point, myState, 3);
  mathVecNormalize(velocityVector, 3);

  velocityVector[0] *= k;
  velocityVector[1] *= k;
  velocityVector[2] *= k;

  api.setVelocityTarget(velocityVector);
}
*/

//End page Not Used Functions
//Begin page main
void loop()
{
    api.getMyZRState(myState);
    api.getOtherZRState(otherState);

    if (time== 0) {color = myState[0]<0.0f; if(color==1) colorSign=-1;}

    float safePos[3];
    safePos[0]=0.4f*colorSign;

    energy          =      game.getEnergy();
    posInArea       =      game.posInArea(myState);
    memoryFilled    =      game.getMemoryFilled();

        if(isEnemyOutOfBounds()) goal=2;
        if(goal!=5)checkPhoto();
        if (game.hasItem(ID)== 0) takenScore++;
            ID = closestPoint();

        if (goal==0) {
       //(1)ANDIAMO A PRENDERE PRIMA L'ITEM NOSTRO PIU' IN ALTO E POI QUELLO PIU'VICINO
        if(distanceBetween(item[5+color], item[3+color])>0.25f) {
          if (item[5+color][2]< item[3+color][2]) firstItem=5+color;
           else firstItem=3+color;
             if(time<10 || time>15) setSecureTarget(item[firstItem]); // primo time <9
             if(game.hasItem(firstItem)!=-1) {setSecureTarget(item[ID]); takenScore==1;}
       }
        else {
        //(2)ANDIAMO A PRENDERE L'ITEM PIU' VICINO (tempo ancora da definire perfettamente)
         if(time<10 || time>15) setSecureTarget(item[ID]);

        }
          if(takenScore>=1 || energy<0.5f)  goal=1;
    }

    if (goal==1) {//GET MIRROR
        api.setPositionTarget(item[7+color]);
        if(game.hasItem(7+color)!=-1) { goal=2;
        }
    }

    if (goal==2) {//AVVICINATI ALL'ASSE X E PREPARATI PER ATTRAVERSARE LA LUCE

        safePos[1]=0.0f;
        if(posInArea!=-1)safePos[2]=-0.58f;
        else safePos[2]=0.0f;
        api.setPositionTarget(safePos);

         if(game.hasItem(7+color)==0) {
         NextGoal();
        }
    }

    if (goal==4) { //OUT-LIGHT ESCI DALLA LUCE
      if(energy>0.1f) outCount++;
      if(memoryFilled!=0 && energy>2.5) uploadPics();

         safePos[1]=-0.6f;
         safePos[2]=0.0f;

      if(outCount<4) api.setPositionTarget (safePos);
      if(posInArea!=1 && outCount>=10) {goal=5;outCount=0;}
    }

    if (goal==5) { //PRENDI ENERGY PACK
        face(otherState);
        if(energy>2.5f) checkPhoto();
        outCount++;

        if(outCount< 6) api.setPositionTarget(item[2]);
        if (distanceBetween(myState,item[2])<0.15f || outCount>16 )  //outCount>15
        api.setPositionTarget(item[2]);

        if(game.hasItem(2)!=-1 || energy>3.5f) {goal=6; outCount=0;}
    }

    if (goal==6) { //IN-DARK

         safePos[1]=0.6f;
         safePos[2]=otherState[2];

         //SE QUALCUNO SI E' SCORDATO UN ITEM CE LO ANDIAMO A PRENDERE NOI
         if(ID!=-999)api.setPositionTarget(item[ID]);
         //ALTRIMENTI ANDIAMO IN DARK
         else api.setPositionTarget(safePos);

         //CHECK ENERGY USATA SOLO QUI
         /* if (energy <= 1.0f && posInArea!=1) { //minEnergy=1.0f;
              if(outCount++==0) memcpy(myPos,myState, 3*sizeof(float));
              api.setPositionTarget(myPos);
             }  */
    }

    DEBUG(("Ephemeris=%d", goal));
    DEBUG(("distance=%f", distanceBetween(myState, otherState)));
    time++;
}
//End page main

};

ZRUser *zruser4 = new ZRUser1025;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1035 : public ZRUser
{

//Begin page functions
void Facepos(float topoint[3]) { 
    float attTarget[3];
    mathVecSubtract(attTarget,topoint,myState,3);
    mathVecNormalize(attTarget,3);
    api.setAttitudeTarget(attTarget);
}
    
float distance(float vec1[3], float vec2[3]) {
    float diffvec[3];
    mathVecSubtract(diffvec, vec1, vec2, 3);
    return mathVecMagnitude(diffvec, 3);
}
void goToNearestMirror(){
game.getItemLoc(target,7);
game.getItemLoc(camp,8);
if(distance(myPos,target) MAX_FACING_ANGLE;
}

bool canTakePics() {
    return (game.posInArea(otherPos)!=-1) && (distance(otherPos,myPos)> 0.5) && game.isFacingOther() && game.getEnergy()>1.5 && game.isCameraOn();
}

bool OutOfBounds(float pos[3]) {
    return pos[0]<-ZONE_pX   || pos[0]>ZONE_pX   || pos[1]<-ZONE_pY   || pos[1]>ZONE_pY  || pos[2]<-ZONE_pZ   || pos[2]>ZONE_pZ  ;    
}



//End page functions
//Begin page main
    ZRState myState;
    float myPos[3];
    
    ZRState otherState;
    float otherPos[3];
    
    bool x;
    float front;
    float back;
    float EARTH[3];
    float speed;

    int itemID;
    int mirrorID;
    float target[3],camp[3];
    int counter;
     short int time;
    float minDistance;
    bool usingMirror;
    int lastcounter;
    bool itemsdown;
    void init() 
    {
        EARTH[0]=0.0f;
        EARTH[1]=0.0f;
        EARTH[2]=1.0f;
        counter=0;
        usingMirror=false;
        time=0;
        x=false;
        itemsdown=false;
        itemID=-1;
    } 
    
    void loop()
    {
        api.getMyZRState(myState);
        api.getOtherZRState(otherState);
        for (int i = 0; i < 3; i++)
        {
           myPos[i] = myState[i]; 
           otherPos[i] = otherState[i];
        }
        DEBUG(("counter:%d",counter));
        back=game.getLightInterfacePosition();//back of light zone
        front=back<-0.1 ? back+0.9 : (back-0.7);//front of light zone
        if(game.posInArea(myPos)!=1 && game.getEnergy()<1 && counter!=5)counter=6;//if low energy stay in ur pos and wait light
       /*if(distance(myPos,otherPos)<0.5f){//if distance between spheres < 0.5 take pics
         if(OutOfBounds(myPos) || game.posInLight(myPos))game.takePic();
         else if(game.getEnergy()>1.8)game.takePic();
        }*/
        for(int i=3;i<7;i++){game.getItemLoc(target,i);if(target[2]>0)itemsdown=true;else{itemsdown=false;break;}}
        if(counter==0){goToNearestMirror();goToNearestItem(0);game.getItemLoc(target,itemID);if(itemID!=-1 && (target[0]<0.3 && target[0]>-0.3 ))counter=1;else{
            target[0]=myPos[0];target[1]=myPos[1];target[2]=-0.55;api.setPositionTarget(target);if(game.getCurrentTime()>20){if(itemsdown)counter=7;else counter++;}}}
        if(counter==1){
            lastcounter=1;
           goToNearestItem(0);
           counter++;
            }
        if(counter==2){
            lastcounter=2;
              game.getItemLoc(target,itemID);
             api.setPositionTarget(target);
             if(game.hasItem(itemID)!=-1 || minDistance==100 || distance(target,otherPos)<0.3)counter++;
        }
        if(counter==7){
            if(otherPos[2]>-0.01){
            target[0]=otherPos[0];
            target[1]=otherPos[1];
            target[2]=-0.55;
                api.setPositionTarget(target);
            }
            else counter=3;
        }
        if(counter==3) {
            lastcounter=3;
            game.getItemLoc(target,mirrorID);
             api.setPositionTarget(target);
           if(game.hasItem(mirrorID)!=-1 && game.posInDark(myPos))x=true;
         
           if(x){
           goToNearestItem(1);
           game.getItemLoc(target,itemID);
           if(minDistance!=100)
            api.setPositionTarget(target);
           }
           if(usingMirror && game.getCurrentTime()>60){
            
            counter=5;}
        }
        if(counter==5){
         goToNearestItem(1);
         lastcounter=5;
         if(usingMirror){
         target[0]=myPos[0];
         target[1]=back-0.1;
         target[2]=myPos[2];}
         else if(back>myPos[1]){game.getItemLoc(target,itemID);if(distance(target,myPos)>0.3)target[1]=back-0.1;}
         else{game.getItemLoc(target,itemID);}
         api.setPositionTarget(target);
        }
        if(counter==6){
        api.setPositionTarget(myPos);
        if(game.posInLight(myPos) && game.getEnergy()>1)counter=lastcounter;
        }
        if(game.getMemoryFilled()<2 && usingMirror==false ){
            Facepos(otherState);
        if(canTakePics()){
        if(game.hasItem(7)==1 || game.hasItem(8)==1){
        if(game.getPicPoints()>0){
           game.takePic();}
        }
        else game.takePic();
        }
        }
         if((game.getMemoryFilled()>1  || (counter==5 && game.getMemoryFilled()>0) ) && game.getEnergy()>2) {
         
         api.setAttitudeTarget(EARTH);
        speed = mathVecMagnitude(&myState[9],3);
        if(speedmyPos[1]-0.01 && x  && game.getNumMirrorsHeld()>0){ game.useMirror();usingMirror=true;time=game.getCurrentTime();}
        if(usingMirror==true && game.getCurrentTime()>time+23)usingMirror=false;
    }

//End page main

};

ZRUser *zruser4 = new ZRUser1035;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1030 : public ZRUser
{

//Begin page main
                                              
#define UPLOAD_PRIORITY_START           MAX_GAME_TIME - 15
#define UPLOAD_PRIORITY_END             MAX_GAME_TIME -  7
#define PREDICT_TIME                                     2
#define LIGHT_ARRIVAL_DARK_ZONE                          8
#define LIGHT_ARRIVAL_DURING_MIRROR                     10
#define LIGHT_ARRIVAL_SPAM_ZONE                         12
#define MIN_ENERGY                                   1.20f
#define NON_GLIDING_DIST                             0.22f
#define COLLISON_DIST                                0.40f         
#define ITEM_RANGE                                   0.15f
#define ITEM_RANGE_CLOSE                             0.12f
#define DEFAULT_SPEED                               0.050f
#define SCORE_SPEED                                 0.030f
#define MIRROR_ACTIVATION_Y                        -0.020f
#define RESTING_Y                                   0.500f

struct data {
    float st[12]; 
    float ost[12];
    float botDistance;
        
    float scoreItem[3];
    float scoreDist;
    float mirrorItem[3];
    float mirrorDist;
    
    float energy;
    float oenergy;
    
    int zone;
    int ozone;
    
    int totalTime;
    int mirrorTime;
    int memFilled;
    int mirrors;
    int lightArrival;
    int olightArrival;
    int initLightArrival;
    
    bool noMirror;
    bool noScoreItem;
    bool itemPhase;

    float lastPictureTime;
    
    float lightTailBoundary;
    float cyclePoint;
    int cycles;
} data;

void init() {
    data.cycles = 0;
    
    DEBUG(("The greatest victory is that which requires no battle."));
}

void loop() {
    api.getMyZRState(data.st);
    api.getOtherZRState(data.ost);
    
    data.botDistance = distance(data.st, data.ost);
    
    data.energy = game.getEnergy();
    data.oenergy = game.getOtherEnergy();
    
    data.zone = game.posInArea(data.st);
    data.ozone = game.posInArea(data.ost);
    
    data.totalTime = game.getCurrentTime();
    data.mirrorTime = game.getMirrorTimeRemaining();
    data.memFilled = game.getMemoryFilled();
    data.mirrors = game.getNumMirrorsHeld();
    data.lightArrival = getLightArrivalTime(data.st[1] + PREDICT_TIME * data.st[4], true);
    data.olightArrival = getLightArrivalTime(data.ost[1] + PREDICT_TIME * data.ost[4], true);
    
    getItemWeighted(data.scoreItem, ITEM_TYPE_ADD_SCORE);
    data.scoreDist = distance(data.st, data.scoreItem);
    getItemWeighted(data.mirrorItem, ITEM_TYPE_MIRROR);
    data.mirrorDist = distance(data.st, data.mirrorItem);
    if (data.totalTime == 0) {
        data.cyclePoint = data.mirrorItem[1];
    }
    
    float newLightTailBoundary = game.getLightGreyBoundary() - 0.05f;
    if (data.lightTailBoundary <= data.cyclePoint && newLightTailBoundary > data.cyclePoint) {
        data.cycles++;
    }
    data.lightTailBoundary = newLightTailBoundary;
    
    float dest[3];
    executeCameraTask();
    float destTime = executeMovementTask(dest);
    
    float destDist = distance(data.st, dest);
    if (!destTime) {
        destTime = destDist  / DEFAULT_SPEED;
    }

    if (destDist <= NON_GLIDING_DIST ||
        data.zone == 1 ||
        (destDist <= COLLISON_DIST && data.botDistance <= COLLISON_DIST)) 
    {
        api.setPositionTarget(dest);
    }
    else {
        float velocity[3];
        for (int i = 0; i < 3; i++)
            velocity[i] = (dest[i] - data.st[i]) / destTime;
        api.setVelocityTarget(velocity);
    }
}

float executeMovementTask(float dest[3]) {
    float destTime = 0.0f;
    bool someEnergy = data.energy > 0.6f;
    bool scoreItemPresent = data.scoreDist > 0.0f; 
    
    memcpy(dest, data.st, 3*sizeof(float));
    
    if (data.lightArrival <= LIGHT_ARRIVAL_DARK_ZONE && data.cycles >= 2) {
        game.useMirror();
        data.mirrorTime = game.getMirrorTimeRemaining();
    }
    
    if (scoreItemPresent && (data.noScoreItem || data.scoreDist <= ITEM_RANGE_CLOSE)) {
        memcpy(dest, data.scoreItem, 3*sizeof(float));
        destTime = 2.0f + (data.scoreItem[1] - data.lightTailBoundary) / 0.025f;
        return destTime;
    }
    
    if (data.noMirror && data.mirrorDist) {
        memcpy(dest, data.mirrorItem, 3*sizeof(float));
        return destTime;
    }
    
    if (data.mirrors) {
        if (data.cycles < 2) {
            if (data.zone == 1 || (data.zone == 0 && data.st[1] > data.ost[1])) {
                dest[1] -= 0.17f;
                if (data.ost[1] - data.st[1] < 0.2f && data.zone == 1) {
                    dest[1] -= 0.05f;
                }
            }
        }
        else {
            if (data.ozone == -1 || data.st[1] > data.ost[1]) {
                if (dest[1] > MIRROR_ACTIVATION_Y && someEnergy) {
                    dest[1] -= 0.15f;
                }
                if (data.scoreDist && (data.scoreDist <= NON_GLIDING_DIST || someEnergy)) {
                    memcpy(dest, data.scoreItem, 3*sizeof(float));
                    destTime = data.scoreDist / SCORE_SPEED;
                }
            }  
        }
        return destTime;
    }
    
    if (data.mirrorTime) {
        dest[1] = data.lightTailBoundary + LIGHT_SPEED * (data.mirrorTime - LIGHT_ARRIVAL_DURING_MIRROR);
        if (dest[1] > ZONE_pY) {
            dest[1] = ZONE_nY + (dest[1] - ZONE_pY);
        }
        return destTime;
    }
    
    if (scoreItemPresent) {
        memcpy(dest, data.scoreItem, 3*sizeof(float));
        destTime = data.scoreDist / SCORE_SPEED;
        return destTime;
    }
    
    data.itemPhase = false;
    
    if (dest[2] > -0.30f && data.energy > 2.0f) {
        dest[2] -= 0.15f;
    }
    if (dest[1] < RESTING_Y) {
        dest[1] += 0.19f;
    }
    return destTime;
}

int getLightArrivalTime(float posY, bool allowZero) {
    float lzFrontNonWrapped = data.lightTailBoundary + LIGHT_WIDTH - LIGHT_GREY_WIDTH / 2.0f;
    bool needsWrapping = lzFrontNonWrapped > ZONE_pY;
    float lzFront = needsWrapping ? ZONE_nY + (lzFrontNonWrapped - ZONE_pY) : lzFrontNonWrapped;
    if (allowZero && ((posY < lzFrontNonWrapped && posY > data.lightTailBoundary) || (needsWrapping && posY < lzFront))) {
        return 0;
    }
    if (posY < lzFront) {
        lzFront = ZONE_nY - (ZONE_pY - lzFront);
    }
    return 1.0f + (posY - lzFront) / LIGHT_SPEED; // '1 +' same as calling 'ceil'
}

void getItemWeighted(float buf[3], int itemType) {
    memcpy(buf, data.st, 3*sizeof(float));
    
    float dist = 100.0f;
    float tmp[3];
    
    for (int i = 0; i < NUM_ITEMS; i++) {
        game.getItemLoc(tmp, i);
        int hasItem = game.hasItem(i);
        int actualType = game.getItemType(i);
        if (hasItem == -1 && actualType  == itemType) {
            float onewDist = distance(data.ost, tmp);
            float newDist = distance(data.st, tmp) - onewDist / 2.0f + tmp[2] / 3.0f;   
            if (newDist < dist && onewDist > ITEM_RANGE) {
                dist = newDist;
                memcpy(buf, tmp, sizeof(tmp));
            }
        }
        else if (hasItem == 0) {
            if (actualType == ITEM_TYPE_MIRROR)
                data.noMirror = false;
            if (actualType == ITEM_TYPE_ADD_SCORE)
                data.noScoreItem = false;
        }
    }
}

void executeCameraTask() {
    float att[6];
    memcpy(att, &data.st[6], 6*sizeof(float));
    #define attRate     &att[3]
    
    float earth[3];
    earth[0] = 0.0f;
    earth[1] = 0.0f;
    earth[2] = 1.0f;
        
    bool facingEarth = mathVecInner(att, earth, 3) > MAX_FACING_ANGLE;
    bool finalPhase = !data.mirrorTime && !data.mirrors && data.zone == -1;
    bool onlyOnePhoto = data.noScoreItem || finalPhase;
    
    bool convenientUpload = data.memFilled && 
        data.lastPictureTime <= UPLOAD_PRIORITY_END && 
        data.cycles > 1 &&
        (data.cycles != 2 || finalPhase || data.mirrorTime || data.energy > 3.10f) &&
        (data.mirrorTime ||
         data.ozone == -1 ||
         data.totalTime >= UPLOAD_PRIORITY_START ||
         data.botDistance <= PHOTO_MIN_DISTANCE ||
         facingEarth);
                             
    bool useCamera = game.isFacingOther() &&
        game.isCameraOn() &&
        data.memFilled < CAMERA_DEFAULT_MEMORY &&
        data.ozone >= 0 &&
        data.botDistance >= PHOTO_MIN_DISTANCE &&
        data.energy > 1.10f &&
        (data.memFilled == 0 || !onlyOnePhoto);
        
    if (!useCamera) {
        if ((data.energy < 1.00f && (data.lightArrival > LIGHT_ARRIVAL_SPAM_ZONE || !data.itemPhase))) { 
            game.takePic();
        }
    }
                        
    if (data.memFilled == CAMERA_DEFAULT_MEMORY || convenientUpload) {
        float minEnergy = data.scoreDist ? 1.60f : 1.00f;
        if (facingEarth && mathVecMagnitude(attRate, 3) <= 0.05f && data.energy >  minEnergy) {
            game.uploadPics();
            data.memFilled = 0;
        }
        else {
            api.setAttitudeTarget(earth);
            return;
        }
    }
    
    if (useCamera && game.getPicPoints() > 0.0f) {
        game.takePic();
        data.lastPictureTime = data.totalTime;
    }
    
    float pointAt[3];
    mathVecSubtract(pointAt, data.ost, data.st, 3);
    api.setAttitudeTarget(pointAt);
}

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

//End page main

};

ZRUser *zruser2 = new ZRUser1030;

          
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1047 : public ZRUser
{

//Begin page Distance
float Distance(float postogo[3])
{
    float vectorB[3];
    mathVecSubtract(vectorB, postogo, hisPos, 3);
    return mathVecMagnitude(vectorB, 3);
}
//End page Distance
//Begin page Picture
void Picture()
{
    float targetAtt[3];
    bool light = game.posInLight(myPos);
    int mirror = game.getMirrorTimeRemaining();
    mathVecSubtract(targetAtt,hisPos,myPos,3);
    mathVecNormalize(targetAtt,3);
    api.setAttitudeTarget(targetAtt);
    if(Time > 165 && game.getEnergy() > 2) 
        if(game.getPicPoints() >= 0) game.takePic();
    if(light && game.getPicPoints() >= 0 && Distance(myPos) < 0.4) game.takePic();
    if(light && game.getPicPoints() >= 0 && mirror > 5) game.takePic();
    if (game.posInArea(hisPos) > -1 && game.isFacingOther() == true && game.getEnergy() > 2 && mirror == 0 && game.getPicPoints() > 0){
        game.takePic();
    }
}
//End page Picture
//Begin page goUp
void goUp()
{
    float posToGo[3], speed;
    speed = mathVecMagnitude(hisSpeed,3);
    if(speed > 0.01 && !dark) posToGo[1] = hisPos[1] - 0.3;
    else posToGo[1] = hisPos[1] + 0.1;
    posToGo[2] = -0.55;
    if(!dark || game.getScore() > game.getOtherScore() || (myPos[0] > 0 && hisPos[0] > 0) || (myPos[0] < 0 && hisPos[0] < 0) || game.getEnergy() <= 2.5)
    {
        posToGo[0] = myPos[0];
    }
    else
    {
        if(myPos[0] > 0) posToGo[0] = 0.5;
        else posToGo[0] = -0.5;
    }
    api.setPositionTarget(posToGo);
}
//End page goUp
//Begin page gotoItems
void gotoItems(int a, int b)
{
    float distance, finalCoords[3], finalDistance = 300, vectorB[3];
    for(int i = a; i <= b; i++)
    {
        if(game.hasItem(i) == -1 && Distance(Items[i+1]) > 0.1)
        {
            mathVecSubtract(vectorB, Items[i+1], myPos, 3);
            distance = mathVecMagnitude(vectorB, 3);
            if(distance < finalDistance)
            {
                for(int j = 0; j <= 2; j++) finalCoords[j] = Items[i+1][j];
                finalDistance = distance;
            }
        }
    }
    api.setPositionTarget(finalCoords);
}
//End page gotoItems
//Begin page gotoOrigin
void gotoOrigin()
{
    float gotoPos[3];
    gotoPos[1] = 0.0;
    gotoPos[0] = myPos[0];
    gotoPos[2] = -0.5;
    api.setPositionTarget(gotoPos);
}
//End page gotoOrigin
//Begin page hasEnergy
bool hasEnergy()
{
    for(int i = 0; i <= 2; i++)
        if(game.hasItem(i) == 0) return 1;
    return 0;
}
//End page hasEnergy
//Begin page main
float myState[12], hisState[12], myPos[3], hisPos[2], Items[15][3], hisAtt[3];
float mySpeed[3], hisSpeed[3], myAtt[3];
bool energy, dark;
int memory, Time;

void init(){
    for(int i = 1; i <= 9; i++)
        game.getItemLoc(Items[i],i-1);
    energy = 0;
}

void loop(){
    api.getMyZRState(myState);
    api.getOtherZRState(hisState);
    for(int i = 0; i <= 2; i++)
    {
        myPos[i] = myState[i];
        hisPos[i] = hisState[i];
        hisAtt[i] = hisState[6+i];
        myAtt[i] = myState[9+i];
        mySpeed[i] = myState[3+i];
        hisSpeed[i] = hisState[3+i];
    }
    dark = game.posInDark(myPos);
    memory = game.getMemoryFilled();
    Time = game.getCurrentTime();
    if(game.posInLight(myPos)) energy = 1;
    if(game.getEnergy() <= 1) api.setPositionTarget(myPos);
    else
    {
        if(Time < 163 || memory == 0)
        {
            if(!hasEnergy() && energy == 0) gotoItems(0,2);
            else
            {
                if(pointsTaken() < 4) gotoItems(3,6);
                else
                {
                    if(myMirrorTaken() < 1) gotoItems(7,8);
                    else goUp();
                }
            }
        }
    }
    if (Time >= 163 && memory > 0){ 
        uploadPictures();
        gotoOrigin();
    }
    else if (game.posInDark(hisPos) && memory > 0)
            uploadPictures();
        else
        {
            if (memory < 2)  Picture();
            else uploadPictures();
        }
    if (game.posInArea(myPos) > -1 && watchingUs() && memory == 2 && myPos[2] > hisPos[2]) { 
        game.useMirror();
    }
    if(fabs(game.getDarkGreyBoundary() - 0.8 - myPos[1]) < 0.35 && game.getEnergy() < 2) game.useMirror();
    if(hisPos[1] < 0 && !dark && game.posInGrey(hisPos)) game.useMirror();
}

//End page main
//Begin page myMirrorTaken
bool myMirrorTaken()
{
    int mirror7 = game.hasItem(7), mirror8 = game.hasItem(8);
    if(mirror7 == 0 || mirror8 == 0) return 1;
    if(mirror7 == 1 && mirror8 == 1) return 1;
    else return 0;
}
//End page myMirrorTaken
//Begin page pointsTaken
int pointsTaken()
{
    int points = 0;
    for(int i = 3; i <=6; i++) 
        if(game.hasItem(i) != -1 || Distance(Items[i+1]) < 0.1) points++;
    return points;
}
//End page pointsTaken
//Begin page setArray
void setArray(float array[3],float toThis[3]){
    /*set one array to the other one*/
    for(int a=0;a<3;a++){
        array[a]=toThis[a];
    }
}
//End page setArray
//Begin page uploadPictures
void uploadPictures()
{
    //faces earth and uploads pictures when we can.
    float earth[3], speed;
    float attRate, targetAtt[3];
    earth[0] = myPos[0];
    earth[1] = myPos[1];
    earth[2] = 1;
    attRate = fabs(mathVecMagnitude(myAtt,3));
    mathVecSubtract(targetAtt,earth,myPos,3);
    speed = mathVecMagnitude(mySpeed,3);
    mathVecNormalize(targetAtt,3);
    api.setAttitudeTarget(targetAtt);
    if (attRate < 0.05f && (game.getEnergy() > 2 || speed < 0.01)){
        game.uploadPics();
    }
    
}
//End page uploadPictures
//Begin page watchingUs
bool watchingUs(){
    //returns true if hisAtt is facing us, false otherwise
    float pos[3];
    float att[3];
    float targetAtt[3];
    float Meg1;
    float Meg2;
    float angle;
    float tolerance;
    tolerance=0.25f;
    
    setArray(pos,hisPos);
    setArray(att,hisAtt);
        
    mathVecSubtract(targetAtt,myPos,pos,3);
    mathVecNormalize(targetAtt,3);
    
    Meg1=mathVecMagnitude(targetAtt,3);
    Meg2=mathVecMagnitude(att,3);
    
    angle=acosf((att[0]*targetAtt[0]+att[1]*targetAtt[1]+att[2]*targetAtt[2])/(Meg1*Meg2));
    
    if(angle

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1041 : public ZRUser
{

//Begin page Photos
#define SPHERE_ANG   6
#define SPHERE_ANG_X 6
#define SPHERE_ANG_Y 7
#define SPHERE_ANG_Z 8
#define SPHERE_ROT   9
#define SPHERE_ROT_X 9
#define SPHERE_ROT_Y 10
#define SPHERE_ROT_Z 11

/*void photoTakeUpload ()
{
    if (uploadCondition()) 
        stratUploadPhotos();
    else 
        stratTakePhotos();
}*/

bool uploadCondition()
{
    if (memFilled == 0) return false;
    if (memFilled == 2 || timp > 157) return true;
    if (otherInDark)
    {
        if (usInLight) return true;
        else if (e > 1.2) return true;
    }
    if (mirrorTime > 0) return true;
    return false;
}
bool photoCondition(float newAtt[3])
{
    //float newAtt[3];
    //mathVecSubtract(newAtt, sphereOther, sphereMe, 3);
    float mag = mathVecNormalize(newAtt, 3);
    //if (mirrorTime != 0) return false;
    if (!game.isFacingOther() || !game.isCameraOn() || otherInDark || mag < 0.51) return false;
    /*if (!game.isCameraOn()) return false; 
    if (otherInDark) return false;
    if (mag < 0.51) return false; */
    if ((usInLight && e < 1.3) ||
        (!usInLight && e < 21*mathVecMagnitude(&sphereMe[3],3) + 1.3)) return false;
    if (game.getPicPoints() < 0) 
        return false;
    return true;
}

void stratTakePhotos() {
    float newAtt[3];
    mathVecSubtract(newAtt, sphereOther, sphereMe, 3);
    //if (!game.isFacingOther())
    api.setAttitudeTarget(newAtt);
    if (photoCondition(newAtt) || e < 0.9)
        game.takePic();    
}

// Upload photos.
void stratUploadPhotos()
{
    api.setAttitudeTarget(toEarth);
    //DEBUG(("%f", sphereMe[8]));
    if ((e > 1.5 && sphereMe[8] > 0.95 &&
        mathVecMagnitude(sphereMe + SPHERE_ROT, 3) < 0.049)
        || (timp > 179 && memFilled >= 1)) 
            game.uploadPics();
}
//End page Photos
//Begin page functions
float pointDistance(float * p1, float * p2) 
{
    float diff[3];
    mathVecSubtract(diff, p1, p2, 3);
    return mathVecMagnitude(diff, 3);
}


//End page functions
//Begin page go_to_the_point

void get_point (int target)
{
        float pos[3];
        game.getItemLoc(pos, target);
        setPosition(pos);
}
//End page go_to_the_point
//Begin page main
#define GAME_LIMIT_X 0.64f
#define GAME_LIMIT_Y 0.8f
#define GAME_LIMIT_Z 0.64f

float toEarth[3];
float zeroArray[3], back[3], front[3];
float sphereMe[12], sphereOther[12];
int timp, wave; //the current time and the wave we are in (1/2)
float item_pos[3], light, e,  memFilled, lightFront, otherScore, otherScoreLast, distThemToUs;
int energy, mirrors, mirror_taken, otherMem, mirrorTime; 
bool usInDark, usInLight, otherInDark, sign, min_point;


void init()
{
    api.getMyZRState(sphereMe);
    sign = (sphereMe[0] < 0)?(0):(1);
    for (int i = 0; i<3; i++)
    {
        zeroArray[i] = 0;
    }
    /*zeroArray[0] = 0;
    zeroArray[1] = 0;
    zeroArray[2] = 0;*/
    toEarth[0] = 0;
    toEarth[1] = 0;
    toEarth[2] = 1;
    otherScore = 0;
    //min_point = -2;
    float p3[3];
    float p5[3];
    game.getItemLoc(p3, 3);
    game.getItemLoc(p5, 5);
    min_point =  (pointDistance(p3, p5) < 0.3 && (p3[0] < 0.15 || p5[0] < 0.15)) ? (true):(false);
}

void loop()
{
    //state();
    //--------------------------------------------------------------------
     api.getMyZRState(sphereMe);
    api.getOtherZRState(sphereOther);
    timp = api.getTime();
    e = game.getEnergy();
    mirrorTime = game.getMirrorTimeRemaining();
    memFilled = game.getMemoryFilled();
    usInDark = game.posInDark(sphereMe);
    usInLight = game.posInLight(sphereMe);
    otherInDark = game.posInDark(sphereOther);
    light = game.getLightInterfacePosition();//tail zone centre of grey
    //lightFront = (light > 0) ? (light - 0.8) : (light + 0.8) ;
    // for (int i = 3; i<7; i++) 
    //     if (game.hasItem(i) == 0) points_taken ++
    mirror_taken = 0;
    if (game.hasItem(7) != -1) mirror_taken++;
    if (game.hasItem(8) != -1) mirror_taken++;
    otherScoreLast = otherScore;
    otherScore = game.getOtherScore();
    if (otherScore - otherScoreLast > 0.105 && otherScore - otherScoreLast < 0.115)
        otherMem ++;
    else if (otherScore - otherScoreLast > 1.7)
        otherMem = 0;
    //photoCanTake = photoCondition();
    distThemToUs = pointDistance(sphereOther, sphereMe);
    //------------------------------------------------------------------------
    //------------------------------------------------------------------------
    //photoTakeUpload();
    //------------------------------------------------------------------------
    /*
    if (memFilled == 0) return false;
    if (memFilled == 2 || timp > 157) return true;
    if (otherInDark)
    {
        if (usInLight) return true;
        else if (e > 1.2) return true;
    }
    if (mirrorTime > 0) return true;
    return false;
    */
    if (!memFilled == 0 && ((memFilled == 2 || timp > 157) ||
        otherInDark && (usInLight || e>1.2) || mirrorTime > 0))
        stratUploadPhotos();
    else 
        stratTakePhotos();
    //------------------------------------------------------------------------
    //mirror();
    //--------------------------------------------
    float otherEnergy = game.getOtherEnergy();
    if (!usInDark && !(sphereMe[1] < light + 0.4) &&
        otherEnergy > 1) //&& sphereOther[2] < 0 &&
        //game.getScore() > otherScore + otherMem*3)
      {
        //float distanceY = sphereOther[1] - sphereMe[1]; // 1 pos, 5 neg
        if (sphereOther[2] < 0 && (
        (otherInDark && (sphereOther[1] - sphereMe[1] < 0 || otherEnergy > 2.2)) ||
        (!otherInDark && game.getScore() > otherScore + otherMem*3)))
            game.useMirror();
      }
    //--------------------------------------------
    //move();
    //----------------------------------------------------------
    float pos[3];
    int point = -1;
  //  bool min_point = false;
    float best = 100;
        for (int i = 3; i <= 6; i++) 
            if (game.hasItem(i) == -1)
            {
                //pos[3]
                game.getItemLoc(pos, i);
                float distance = pointDistance(pos, sphereMe);
                //float theirDist = pointDistance(pos, sphereOther);
                if (distance < best && (pos[2] < 0 || distance < 0.3)) 
                {
                    point = i;
                    best = distance;
                }
            }
    //DEBUG(("Best: %d", point));
    pos[0] = sphereMe[0];
    pos[1] = sphereMe[1];
    if (timp < 20)
    {
        //pos[0] = sphereMe[0];
        //pos[1] = sphereMe[1];
        pos[2] = -0.6;
        setPosition(pos);
    }
    else if (point!=-1 && min_point){
        get_point(point);
    }
    else if (mirror_taken == 0)
        get_point((sign)?(8):(7));
    else if (sphereOther[2] > 0)
    {
        pos[0] = sphereOther[0];
        pos[1] = sphereOther[1];
        pos[2] = -0.6;
        setPosition(pos);
    }
    else if (mirror_taken == 1) //&& points_taken == 0)// && !very_close(point)
        {
            game.getItemLoc(pos, ((sign)?(7):(8)));
            (distThemToUs > 0.1)?(setPosition(pos)):(api.setVelocityTarget(zeroArray));
        }
    else if (point!=-1)
        get_point(point);
    else
    {
            //pos[0] = sphereMe[0];
            pos[2] = sphereMe[2];
            bool idk = (sphereMe[1] < light + 0.1 && sphereMe[1] > light-0.1);
            if (mirrorTime > 0)
            {
                pos[1] += ((light - sphereMe[1])/mirrorTime - 0.025 > 0.03)
                ? (0.25) : (-0.25);
            }
            else if (distThemToUs < 0.4)
                pos[1] += (sphereMe[1] > -0.3) ? (-0.4):(0.4);
            else
            {
                pos[1] += (idk || usInDark)?(0.1):(-0.2);
                if (idk || !usInDark)
                    pos[2] = -0.6;
            }
            setPosition(pos);
    }
    //----------------------------------------------------------
}

//End page main
//Begin page mirror

//End page mirror
//Begin page move
void move()
{
    float pos[3];
    int point = -1;
  //  bool min_point = false;
    float best = 100;
        for (int i = 3; i <= 6; i++) 
            if (game.hasItem(i) == -1)
            {
                //pos[3]
                game.getItemLoc(pos, i);
                float distance = pointDistance(pos, sphereMe);
                //float theirDist = pointDistance(pos, sphereOther);
                if (distance < best && (pos[2] < 0 || distance < 0.2)) 
                {
                    point = i;
                    best = distance;
                }
            }
    //DEBUG(("Best: %d", point));
    if (timp > 20)
    if (point!=-1 && min_point){
        get_point(point);
    }
    else if (mirror_taken == 0)
        get_point((sign)?(8):(7));
    else if (sphereOther[2] > 0)
    {
        pos[0] = sphereOther[0];
        pos[1] = sphereOther[1];
        pos[2] = -0.6;
        setPosition(pos);
    }
    else if (mirror_taken == 1) //&& points_taken == 0)// && !very_close(point)
        {
            game.getItemLoc(pos, ((sign)?(7):(8)));
            (distThemToUs > 0.1)?(setPosition(pos)):(api.setVelocityTarget(zeroArray));
        }
    else if (point != -1)
        get_point(point);
    else
    {
            pos[0] = sphereMe[0];
            pos[2] = sphereMe[2];
            bool idk = (sphereMe[1] < light + 0.1 && sphereMe[1] > light-0.1);
            if (mirrorTime > 0)
            {
                pos[1] = sphereMe[1] +
                ((light - sphereMe[1])/mirrorTime - 0.025 > 0.03)
                ? (0.25) : (-0.25);
            }
            else if (distThemToUs < 0.4)
                pos[1] = sphereMe[1] + (sphereMe[1] > -0.3) ? (-0.4):(0.4);
            else
            {
                pos[1] = sphereMe[1] + (idk || usInDark)?(0.1):(-0.2);
                if (idk || !usInDark)
                    pos[2] = -0.6;
            }
            setPosition(pos);
    }
    else
    {
        pos[0] = sphereMe[0];
        pos[1] = sphereMe[1];
        pos[2] = -0.6;
        setPosition(pos);
    }
}
//End page move
//Begin page setPosition
void setPosition(float target[3]) 
{
   //if (timp < 20 && light - sphereMe[1] < 0.126)
   //     target[1] = sphereMe[1];
    if (e < 21*mathVecMagnitude(&sphereMe[3],3) + 0.2 || game.getFuelRemaining() < 5 ||
    distThemToUs < 0.2)
        {
            api.setVelocityTarget(zeroArray);
          
        }
    else
    {
        target[0] = clamp(target[0], -GAME_LIMIT_X + 0.05, GAME_LIMIT_X - 0.05);
        target[1] = clamp(target[1], -GAME_LIMIT_Y + 0.05, GAME_LIMIT_Y - 0.05);
        target[2] = clamp(target[2], -GAME_LIMIT_Z + 0.05, GAME_LIMIT_Z - 0.05);
        api.setPositionTarget(target);
    }
}
//End page setPosition
//Begin page state
/*void state()
{
    api.getMyZRState(sphereMe);
    api.getOtherZRState(sphereOther);
    timp = api.getTime();
    e = game.getEnergy();
    mirrorTime = game.getMirrorTimeRemaining();
    memFilled = game.getMemoryFilled();
    usInDark = game.posInDark(sphereMe);
    usInLight = game.posInLight(sphereMe);
    otherInDark = game.posInDark(sphereOther);
    light = game.getLightInterfacePosition();//tail zone centre of grey
    //lightFront = (light > 0) ? (light - 0.8) : (light + 0.8) ;
    // for (int i = 3; i<7; i++) 
    //     if (game.hasItem(i) == 0) points_taken ++
    mirror_taken = 0;
    if (game.hasItem(7) != -1) mirror_taken++;
    if (game.hasItem(8) != -1) mirror_taken++;
    otherScoreLast = otherScore;
    otherScore = game.getOtherScore();
    if (otherScore - otherScoreLast > 0.105 && otherScore - otherScoreLast < 0.115)
        otherMem ++;
    else if (otherScore - otherScoreLast > 1.7)
        otherMem = 0;
    photoCanTake = photoCondition();
    distThemToUs = pointDistance(sphereOther, sphereMe);
}*/

float clamp(float val, float min, float max) {
    if (val < min) return min;
    if (val > max) return max;
    return val;
}
//End page state

};

ZRUser *zruser6 = new ZRUser1041;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1038 : public ZRUser
{

//Begin page main
/*******************************************************************************
 * SHA-2468 Alliance Competitor -- Advanced Rush Bot                           *
 *                                                                             *
 * Authors                                                                     *
 *  Yicheng Wang + Young Kim                                                   *
 *                                                                             *
 * Description                                                                 *
 *  Grab 2 items and rush the opponent!                                        *
 *                                                                             *
 ******************************************************************************/

/* TODO
 *  Better checkPhoto, everything regarding when to spam and when not to spam is
 *   stored there -- curently very duct-tapy
 *  general improvements
 *  Testing!
 */

/* Dev Log
 *  Project Created: 2015-11-26 21:45 - Yicheng W.
 */

/* INIT ***********************************************************************/
/* MOVE STATES */
#define STOP -1
#define GET_SCORE_PACKS 0
#define END_GAME 1
#define TO_ORIGIN 2

/* ATT STATES */
#define FACE_OTHER 3
#define UPLOAD 4

#define GIVE_UP_THRESHOLD 0.3
#define ENERGY_SHUTDOWN_THRESHOLD 0.7

ZRState me;
ZRState other;

float items[9][3];
int itemBool[9];

float target[3];

float temp[3];

float origin[3];
float facing[3];

int itemID;

int i;

int move_state;
int att_state;

void init() {
    for (int i = 0 ; i < 9 ; i++) {
        game.getItemLoc(items[i], i);
    }
    updateGameState();
    
    origin[0] = origin[1] = origin[2] = 0.0f;

    move_state = GET_SCORE_PACKS;
    att_state = FACE_OTHER;
    itemID = recalibrate(3, 6);

    DEBUG(("Hello from SHA-2468!"));
}

/* MAIN ***********************************************************************/
void loop() {
    blah();
    updateGameState();
    changeState();

    if (game.getEnergy() < ENERGY_SHUTDOWN_THRESHOLD) {
        api.setVelocityTarget(origin);
        return;
    }

    switch (att_state) {
        case FACE_OTHER:
            mathVecSubtract(temp, other, me, 3);
            api.setAttitudeTarget(temp);
            if (checkPhoto()) {
                game.takePic();
            }
            break;

        case UPLOAD:
            temp[0] = 0.0f;
            temp[1] = 0.0f;
            temp[2] = 1.0f;
            api.setAttitudeTarget(temp);
            if (checkUpload()) {
                game.uploadPics();
            }
            break;
    }

    switch (move_state) {
        case GET_SCORE_PACKS:
            if (game.getScore() == 0) {
                mathVecAdd(temp, me, me + 3, 3); // where we will be
                if (!game.posInDark(temp)) {
                    api.setVelocityTarget(origin); // steady...
                }
                else {
                    temp[0] = game.getCurrentTime();
                    if (temp[0] < 4 || temp[0] > 17) {
                        api.setPositionTarget(items[itemID]);
                    }
                }
            }
            else {
                api.setPositionTarget(items[itemID]);
            }
            break;

        case END_GAME:
            memcpy(temp, other, 3 * sizeof(float));
            mathVecNormalize(temp, 3);
            mathVecMultiply(temp, -0.3, 3);
            mathVecAdd(target, temp, other, 3);
            api.setPositionTarget(target);            
            break;

        case TO_ORIGIN:
            api.setPositionTarget(origin);
            break;
    }
}
/* CHANGE *********************************************************************/
void changeState() {
    if (game.getMemoryFilled() == 2) {
        att_state = UPLOAD;
    }

    if (att_state == UPLOAD && game.getMemoryFilled() == 0) {
        att_state = FACE_OTHER;
    }

    if (game.getCurrentTime() > 167 && game.getMemoryFilled() > 0) {
        att_state = UPLOAD;
    }

    if (move_state == GET_SCORE_PACKS) {
        if (game.getScore() > 3) {
            itemID = recalibrate(3,6);
            if (itemID == -1 || distance(me, items[itemID]) - distance(other, items[itemID]) > GIVE_UP_THRESHOLD) {
                move_state = END_GAME;
            }
        }
        else if (itemBool[itemID] != -1) {
            itemID = recalibrate(3, 6);
            if (itemID == -1) {
                move_state = END_GAME;
            }
        }
    }
    else {
        if (!inBounds(other)) {
            move_state = TO_ORIGIN;
        }
        else {
            move_state = END_GAME;
        }
    }
}

/* UTILS **********************************************************************/
void updateGameState() {
    api.getMyZRState(me);
    api.getOtherZRState(other);
    for (i = 0 ; i < 9 ; i++) {
        itemBool[i] = game.hasItem(i);
    }
}

int recalibrate(int start, int end){ //gets index of closest item in item range[start:end] 
    float shortdist = 100.0f;
    int index = -1;
    for(int i = start; i <= end; i++){
        if(itemBool[i] == -1 &&
           distance(me,items[i]) < shortdist &&
           (game.getScore() < 3 || distance(me, items[i]) < distance(other, items[i]))){
            shortdist = distance(me,items[i]);
            index = i;
        }
    }
    return index;
}
float distance(float p1[], float p2[]){
    mathVecSubtract(temp,p1,p2,3);
    return mathVecMagnitude(temp,3);
}
void mathVecMultiply(float vec[], float multipler, int dimensions) {
    for (i = 0 ; i < dimensions ; i++) {
        vec[i] = vec[i] * multipler;
    }
}

bool checkPhoto() {
    float energy = game.getEnergy();
    if (game.getScore() > 0) {
        if (energy > 1 || game.posInLight(me)) {
            bool goodPhoto = (game.posInArea(other) >= 0 &&
                      game.isFacingOther() &&
                      game.getPicPoints() > 0);

            if (goodPhoto || (move_state == END_GAME && distance(me, other) < 0.5)) {
                return true;
            }
        }
        else if (energy > 2 || game.posInLight(me)){
            bool goodPhoto = (game.posInArea(other) >= 0 &&
                      game.isFacingOther() &&
                      game.getPicPoints() > 0);
            if (goodPhoto || distance(me, other) < 0.5) {
                return true;
            }
        }
    }
    return false;
}

bool checkUpload(){
    if( ( me[8] > 0.9689f ) && (mathVecMagnitude(me+9,3) < 0.05f) ) return true;
    return false;
}

bool inBounds(float loc[]) {
    return (abs(loc[0]) < 0.64 &&
            abs(loc[1]) < 0.8 &&
            abs(loc[2]) < 0.64);
}

void blah() {
    DEBUG(("[FIB, 0, TIME, %d]\n", time));
DEBUG(("[FIB, 1, FRED, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f]\n", 
            me[0] *  -17 ,
            me[1] *  2   ,
            me[2] *  9   ,
            me[3] *  19  ,
            me[4] *  -27 ,
            me[5] *  -8  ,
            me[6] *  29  ,
            me[7] *  3   ,
            me[8] *  -13 ,
            me[9] *  -21 ,
            me[10] *  4   ,
            me[11] *  12));
DEBUG(("[FIB, 2, GEORGE, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f]\n",
            other[0] *  17 ,
            other[1] *  -2   ,
            other[2] *  -9   ,
            other[3] *  -19  ,
            other[4] *  27 ,
            other[5] *  8  ,
            other[6] *  -29  ,
            other[7] *  -3   ,
            other[8] *  13 ,
            other[9] *  21 ,
            other[10] *  -4   ,
            other[11] *  -12));
}


//End page main

};

ZRUser *zruser2 = new ZRUser1038;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1022 : public ZRUser
{

//Begin page functions
//itemToRetrieve = 0 for score pack
//           1 for energy pack
//           2 for mirror
int bestItem(float myState[],float otherState[], int itemToRetrieve) {
    int id = 0;
    float debrisDistance;
    float currentSmallest = 0.0f;
    int initID = (itemToRetrieve == 0 ? 3 : (itemToRetrieve == 1 ? 0 : 7));
    for(int i=initID ; i<=(initID == 3 ? 6 : (initID == 0 ? 2 : 8)) ; i++) {
        if (game.hasItem(i) == -1) {
            float itemPos[3];
            game.getItemLoc(itemPos,i);
            debrisDistance = distanceBetween(myState,itemPos);
            if (itemToRetrieve == 0) {
                if (itemPos[2] > -0.1f) debrisDistance++;
                if (distanceBetween(otherState,itemPos) < 0.1f) debrisDistance = 100.0f;
            }
        }
        else debrisDistance = 100.0f;
        if (debrisDistance < currentSmallest || i == initID) {
            currentSmallest = debrisDistance;
            id = i;
        }
    }
    if (currentSmallest == 100.0f) id = -1;
    return id;
}


bool hasOneItem(int itemToTest) {
    bool hasOneItem2 = false;
    int initID = (itemToTest == 0 ? 3 : (itemToTest == 1 ? 0 : 7));
    for(int i=initID ; i<=(initID == 3 ? 6 : (initID == 0 ? 2 : 8)) ; i++) {
        if (game.hasItem(i) == 0) hasOneItem2 = true;
    }

    return hasOneItem2;
}

void mathVecScale(float res[3], float src[3], float mag, bool norm) {
    memcpy(res,src,sizeof(float)*3.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], int Case, bool slowDown) {
    float targetVel[3];
    float velocity = 90.0f;
    bool norm = true;

    if ((distanceBetween(myState,targetPoint) -  (mathSquare(mathVecMagnitude(&myState[3],3))/0.0165f)) <= 0.0f) velocity = 0.0f;
    else  {
        velocity = 0.04f;
        if (Case == 1) velocity = 0.05f;
        if (slowDown) velocity = 0.015f;
    }
    if (distanceBetween(myState,targetPoint)>0.22f) {
        mathVecSubtract(targetVel,targetPoint,myState,3);
        if (Case == 2 && velocity !=  0.0f) {
            norm = false;
            velocity = 0.025f/targetVel[1];
        }
        mathVecScale(targetVel,targetVel,velocity,norm);
        api.setVelocityTarget(targetVel);
    }
    else {
        api.setPositionTarget(targetPoint);

    }
}

//End page functions
//Begin page main
//Never Tell Me the Odds
bool noPic;

void init(){
    noPic = false;
}


void loop() {
    int time = game.getCurrentTime();
    float myState[12], otherState[12], vectorBetween[3];
  api.getMyZRState(myState);
  api.getOtherZRState(otherState);


    float vel = 0.0f;
    int memoryFilled = game.getMemoryFilled();
    int numMirrorsHeld = game.getNumMirrorsHeld();
    bool posInDark = game.posInDark(myState);
    bool posInLight = game.posInLight(myState);
    bool otherPosInDark = game.posInDark(otherState);
    int mirrorTimeRemaining = game.getMirrorTimeRemaining();
    float energy = game.getEnergy();
    float futurePos[3] = {myState[0],(myState[1] - 0.05f),myState[2]};
    int bestItem0 = bestItem(myState,otherState,0);
    int bestItem2 = bestItem(myState,otherState,2);
    float stoppingNRG = mathVecMagnitude(&myState[3],3)*40.0f;
    float bestScorePack[3];
    bool hasMirror =  hasOneItem(2);
    bool getScorePacks = false;
    bool enableLowNRGStop = true;
    bool EOGScorePack = false;
    bool slowDown = false;
    float darkGreyBoundary = game.getLightInterfacePosition() - 0.05f;
    bool goingForEndScores = false;
    if (posInDark && mirrorTimeRemaining == 0 && numMirrorsHeld == 0 && hasMirror) goingForEndScores = true;

  if (time == 0) DEBUG(("Never tell me the odds."));
    if (time == 1) DEBUG(("Welcome, young Skywalker. I have been expecting you."));
    if (time == 2) DEBUG(("You no longer need those."));
    if (time == 3) DEBUG(("Guards, leave us."));
    if (time == 4) DEBUG(("I'm looking forward to completing your training. In time you will call me Master."));
    if (time == 5) DEBUG(("You're gravely mistaken. You won't convert me as you did my father."));
    if (time == 6) DEBUG(("Oh, no, my young Jedi. You will find that it is you who are mistaken...about a great many things."));
    if (time == 7) DEBUG(("His lightsaber."));
    if (time == 8) DEBUG(("Ah, yes, a Jedi's weapon. Much like your father's. By now you must know your father can never be turned from the dark side. So will it be with you."));
    if (time == 9) DEBUG(("You're wrong. Soon I'll be dead...and you with me."));
    if (time == 10) DEBUG(("Perhaps you refer to the imminent attack of your Rebel fleet."));
    if (time == 11) DEBUG(("Yes...I assure you we are quite safe from your friends here."));
    if (time == 12) DEBUG(("Your overconfidence is your weakness."));
    if (time == 13) DEBUG(("Your faith in your friends is yours."));
    if (time == 14) DEBUG(("It is pointless to resist, my son."));
  if (time == 15) DEBUG(("Everything that has transpired has done so according to my design. Your friends up there on the Sanctuary Moon are walking into a trap. As is your Rebel fleet! It was I who allowed the Alliance to know the location of the shield generator. It is quite safe from your pitiful little band. An entire legion of my best troops awaits them."));
  if (time == 16) DEBUG(("Oh...I'm afraid the deflector shield will be quite operational when your friends arrive."));
  if (time == 17) DEBUG(("All right! Up! Move! Come on! Quickly! Quickly, Chewie."));
    if (time == 18) DEBUG(("Han! Hurry! The fleet will be here any moment."));
  if (time == 19) DEBUG(("Charges! Come on, come on!"));
  if (time == 20) DEBUG(("Oh, my! They'll be captured!"));
    if (time == 21) DEBUG(("Wa-wait! Wait, come back! Artoo, stay with me."));
    if (time == 22) DEBUG(("Freeze! You Rebel scum."));
    if (time == 23) DEBUG(("All wings report in."));
    if (time == 24) DEBUG(("Red Leader standing by."));
    if (time == 25) DEBUG(("Gray Leader standing by."));
    if (time == 26) DEBUG(("Green Leader standing by."));
    if (time == 27) DEBUG(("Lock S-foils in attack positions."));
    if (time == 28) DEBUG(("May the Force be with us."));
    if (time == 29) DEBUG(("We've got to be able to get some kind of a reading on that shield, up or down. Well, how could they be jamming us if they don't know if we're coming."));
    if (time == 30) DEBUG(("Break off the attack! The shield is still up."));
    if (time == 31) DEBUG(("I get no reading. Are you sure?"));
    if (time == 32) DEBUG(("Pull up! All craft pull up!"));
    if (time == 33) DEBUG(("Take evasive action! Green Group, stick close to holding sector MV-7."));
    if (time == 34) DEBUG(("Admiral, we have enemy ships in sector 47."));
    if (time == 35) DEBUG(("It's a trap!"));
    if (time == 36) DEBUG(("Fighters coming in."));
    if (time == 37) DEBUG(("There's too many of them!"));
    if (time == 38) DEBUG(("Accelerate to attack speed! Draw their fire away from the cruisers."));
    if (time == 39) DEBUG(("Copy, Gold Leader."));
    if (time == 40) DEBUG(("Come, boy. See for yourself."));
    if (time == 41) DEBUG(("From here you will witness the final destruction of the Alliance, and the end of your insignificant Rebellion."));
    if (time == 42) DEBUG(("You want this, don't you? The hate is swelling in you now. Take your Jedi weapon. Use it. I am unarmed. Strike me down with it. Give in to your anger. With each passing moment, you make yourself more my servant."));
    if (time == 43) DEBUG(("No!"));
    if (time == 44) DEBUG(("It is unavoidable. It is your destiny. You, like your father, are now mine!"));
    if (time == 45) DEBUG(("All right, move it! I said move it! Go on!"));
    if (time == 46) DEBUG(("Hello! I say, over there! Were you looking for me?"));
    if (time == 47) DEBUG(("Bring those two down here!"));
    if (time == 48) DEBUG(("Let's go!"));
    if (time == 49) DEBUG(("Well, they're on their way. Artoo, are you sure this was a good idea?"));
    if (time == 50) DEBUG(("Freeze! Don't move!"));
    if (time == 51) DEBUG(("We surrender."));
    if (time == 52) DEBUG(("Ohhh! Stand back, Artoo."));
    if (time == 53) DEBUG(("The code's changed. We need Artoo!"));
    if (time == 54) DEBUG(("Here's the terminal."));
    if (time == 55) DEBUG(("Artoo, where are you? We need you at the bunker right away."));
    if (time == 56) DEBUG(("Going? What do you mean, you're going. But-- but going where, Artoo? No, what! Artoo! Oh, this is no time for heroics. Come back!"));
    if (time == 57) DEBUG(("Watch yourself, Wedge! Three from above!"));
    if (time == 58) DEBUG(("Red Three, Red Two, pull in!"));
    if (time == 59) DEBUG(("Got it!"));
    if (time == 60) DEBUG(("Three of them coming in, twenty degrees!"));
    if (time == 61) DEBUG(("Cut to the left! I'll take the leader! They're heading for the medical frigate."));
    if (time == 62) DEBUG(("Pressure's steady."));
    if (time == 63) DEBUG(("Only the fighters are attacking.  I wonder what those Star Destroyers are waiting for."));
    if (time == 64) DEBUG(("We're in attack position now, sir."));
    if (time == 65) DEBUG(("Hold here."));
    if (time == 66) DEBUG(("We're not going to attack?"));
    if (time == 67) DEBUG(("I have my orders from the Emperor himself. He has something special planned for them. We only need to keep them from escaping."));
    if (time == 68) DEBUG(("As you can see, my young apprentice, your friends have failed. Now witness the firepower of this fully armed and operational battle station. Fire at will, Commander."));
    if (time == 69) DEBUG(("Fire!"));
    if (time == 70) DEBUG(("That blast came from the Death Star! That thing's operational! Home One, this is Gold Leader."));
    if (time == 71) DEBUG(("We saw it. All craft prepare to retreat."));
    if (time == 72) DEBUG(("You won't get another chance at this, Admiral."));
    if (time == 73) DEBUG(("We have no choice, General Calrissian. Our cruisers can't repel firepower of that magnitude."));
    if (time == 74) DEBUG(("Han will have that shield down. We've got to give him more time."));
    if (time == 75) DEBUG(("We're coming!"));
    if (time == 76) DEBUG(("Come on! Come on!"));
    if (time == 77) DEBUG(("Oh, Artoo, hurry!"));
    if (time == 78) DEBUG(("My goodness! Artoo, why did you have to be so brave?"));
    if (time == 79) DEBUG(("I'll cover you."));
    if (time == 80) DEBUG(("Yes! I said closer! Move as close as you can and engage those Star Destroyers at point-blank range."));
    if (time == 81) DEBUG(("At that close range, we won't last long against those Star Destroyers."));
    if (time == 82) DEBUG(("We'll last longer then we will against that Death Star...and we might just take a few of them with us."));
    if (time == 83) DEBUG(("Your fleet has lost. And your friends on the Endor moon will not survive. There is no escape, my young apprentice. The Alliance will die...as will your friends."));
    if (time == 84) DEBUG(("Good. I can feel your anger. I am defenseless.  Take your weapon! Strike me down with all your hatred, and your journey towards the dark side will be complete."));
    if (time == 85) DEBUG(("I think I got it. I got it!"));
    if (time == 86) DEBUG(("Oh, Princess Leia, are you all right?"));
    if (time == 87) DEBUG(("Let's see."));
    if (time == 88) DEBUG(("It's not bad."));
    if (time == 89) DEBUG(("Freeze!"));
    if (time == 90) DEBUG(("Oh, dear."));
    if (time == 91) DEBUG(("Don't move!"));
    if (time == 92) DEBUG(("I love you."));
    if (time == 93) DEBUG(("I know."));
    if (time == 94) DEBUG(("Hands up! Stand up!"));
    if (time == 95) DEBUG(("Stay back."));
    if (time == 96) DEBUG(("Chewie!  Get down here!  She's wounded!  No, wait....  I got an idea."));
    if (time == 97) DEBUG(("Good.  Use your aggressive feelings, boy!  Let the hate flow through you."));
    if (time == 98) DEBUG(("Obi-Wan has taught you well."));
    if (time == 99) DEBUG(("I will not fight you, father."));
    if (time == 100) DEBUG(("You are unwise to lower your defenses."));
    if (time == 101) DEBUG(("Your thoughts betray you, father. I feel the good in you...the conflict."));
    if (time == 102) DEBUG(("There is no conflict."));
    if (time == 103) DEBUG(("You couldn't bring yourself to kill me before, and I don't believe you'll destroy me now."));
    if (time == 104) DEBUG(("You underestimate the power of the dark side. If you will not fight, then you will meet your destiny."));
    if (time == 105) DEBUG(("Good. Good."));
    if (time == 106) DEBUG(("Watch out. Squad at .06."));
    if (time == 107) DEBUG(("I'm on it, Gold Leader."));
    if (time == 108) DEBUG(("Good shot, Red Two."));
    if (time == 109) DEBUG(("Now...come on, Han, old buddy. Don't let me down."));
    if (time == 110) DEBUG(("It's over, Commander. The Rebels have been routed. They're fleeing into the woods. We need reinforcements to continue the pursuit."));
    if (time == 111) DEBUG(("Send three squads to help. Open the back door."));
    if (time == 112) DEBUG(("Yes, sir."));
    if (time == 113) DEBUG(("Throw me another charge."));
    if (time == 114) DEBUG(("You cannot hide forever, Luke."));
    if (time == 115) DEBUG(("I will not fight you."));
    if (time == 116) DEBUG(("Give yourself to the dark side. It is the only way you can save your friends."));
    if (time == 117) DEBUG(("Yes, your thoughts betray you. Your feelings for them are strong. Especially for... "));
    if (time == 118) DEBUG(("Sister! So...you have a twin sister."));
    if (time == 119) DEBUG(("Your feelings have now betrayed her, too. Obi-Wan was wise to hide her from me.  "));
    if (time == 120) DEBUG(("Now his failure is complete. If you will not turn to the dark side, then perhaps she will."));
    if (time == 121) DEBUG(("Never-r-r!"));
    if (time == 122) DEBUG(("Good! Your hate has made you powerful. Now, fulfill your destiny and take your father's place at my side!"));
    if (time == 123) DEBUG(("Never! I'll never turn to the dark side. You've failed, Your Highness. I am a Jedi,like my father before me. "));
    if (time == 124) DEBUG(("So be it...Jedi."));
    if (time == 125) DEBUG(("Move! Move!"));
    if (time == 126) DEBUG(("The shield is down! Commence attack on theDeath Star's main reactor."));
    if (time == 127) DEBUG(("We're on our way. Red Group, Gold Group, all fighters follow me."));
    if (time == 128) DEBUG(("Told you they'd do it!"));
    if (time == 129) DEBUG(("If you will not be turned, you will be destroyed."));
    if (time == 130) DEBUG(("Young fool...only now, at the end, do you understand."));
  if (time == 131) DEBUG(("Your feeble skills are no match for the power of the dark side. You have paid the price for your lack of vision."));
    if (time == 132) DEBUG(("Father, please. Help me."));
    if (time == 133) DEBUG(("Now, young Skywalker...you will die."));
    if (time == 134) DEBUG(("I'm going in."));
    if (time == 135) DEBUG(("Here goes nothing."));
    if (time == 136) DEBUG(("Now lock onto the strongest power source. It should be the power generator."));
    if (time == 137) DEBUG(("Form up. And stay alert. We could run out of space real fast."));
    if (time == 138) DEBUG(("Copy, Gold Leader."));
    if (time == 139) DEBUG(("That was too close."));
    if (time == 140) DEBUG(("Sir, we've lost our bridge deflector shield."));
    if (time == 141) DEBUG(("Intensify the forward batteries. I don't want anything to get through."));
    if (time == 142) DEBUG(("Intensify forward firepower!"));
    if (time == 143) DEBUG(("It's too late!"));
    if (time == 144) DEBUG(("Luke, help me take this mask off."));
    if (time == 145) DEBUG(("But you'll die."));
    if (time == 146) DEBUG(("Nothing can stop that now. Just for once... let me look on you with my own eyes."));
    if (time == 147) DEBUG(("Now...go, my son. Leave me."));
    if (time == 148) DEBUG(("No. You're coming with me. I can't leave you here. I've got to save you."));
    if (time == 149) DEBUG(("You already have, Luke. You were right about me. Tell your sister...you were right."));
    if (time == 150) DEBUG(("Father...I won't leave you."));
    if (time == 151) DEBUG(("There it is! "));
    if (time == 152) DEBUG(("All right, Wedge. Go for the power regulator on the north tower."));
    if (time == 153) DEBUG(("Copy, Gold Leader. I'm already on my way out."));
    if (time == 154) DEBUG(("Move the fleet away from the Death Star."));
    if (time == 155) DEBUG(("They did it!"));
    if (time == 156) DEBUG(("I'm sure Luke wasn't on that thing when it blew."));
    if (time == 157) DEBUG(("He wasn't. I can feel it."));
    if (time == 158) DEBUG(("You love him, don't you?"));
    if (time == 159) DEBUG(("Yes."));
    if (time == 160) DEBUG(("All right. I understand. Fine. When he comes back, I won't get in the way."));
    if (time == 161) DEBUG(("Oh. No, it's not like that at all. He's my brother."));
    if (time == 162) DEBUG(("Yub nub, eee chop yub nub;"));
    if (time == 163) DEBUG(("Ah toe meet toe peechee keene,"));
    if (time == 164) DEBUG(("G'noop dock fling oh ah."));
    if (time == 165) DEBUG(("Coatee cha tu yub nub;"));
    if (time == 166) DEBUG(("Coatee cha tu yahwah;"));
    if (time == 167) DEBUG(("Coatee cha tu glowah;"));
    if (time == 168) DEBUG(("Yahwah, eee chop yahwah;"));
    if (time == 169) DEBUG(("G'noop dock fling oh ah."));
    if (time == 170) DEBUG(("Coatee cha tu yub nub;"));
    if (time == 171) DEBUG(("Glowah, eee chop glowah"));
    if (time == 172) DEBUG(("Ya glowah pee chu nee foom,"));
    if (time == 173) DEBUG(("Ah toot dee awe goon daa."));
    if (time == 174) DEBUG(("Coatee cha tu goo; (Yub nub!)"));
    if (time == 175) DEBUG(("Coatee cha tu doo; (Yahwah!)"));
    if (time == 176) DEBUG(("Coatee cha tu too; (Ya chaa!)"));
    if (time == 177) DEBUG(("Allay loo ta nuv,"));
    if (time == 178) DEBUG(("Allay loo ta nuv,"));
    if (time == 179) DEBUG(("Allay loo ta nuv,"));
    if (time == 180) DEBUG(("Yub Nub!"));


    if (energy<1.5f && (!posInLight)) slowDown = true;

    if (bestItem0 != -1) {
        game.getItemLoc(vectorBetween, bestItem0);
        float distanceBetween2 = distanceBetween(myState,vectorBetween);
        if (((goingForEndScores)) || (distanceBetween2 < 0.3f && energy>0.7f && (posInDark || distanceBetween2 < 0.15f) && mirrorTimeRemaining == 0)) getScorePacks = true;
        EOGScorePack = true;
    }
    bool EOGBehaviorTrue = false;
    if (hasMirror && numMirrorsHeld == 0 && mirrorTimeRemaining== 0 && !EOGScorePack) EOGBehaviorTrue = true;


    if (!noPic &&  distanceBetween(myState,otherState) >= 0.5f && game.isCameraOn() && game.isFacingOther() && memoryFilled != 2 && (energy > 1.1f && (posInLight || (fabs(myState[1]-(darkGreyBoundary+0.9f)) < 0.2f)  || hasMirror || (stoppingNRG + 1.0f < energy))) && mirrorTimeRemaining == 0 && (!otherPosInDark)) {
        if (game.getPicPoints() > 0.0f){
        game.takePic();
        }
    }
    if ((!getScorePacks && ((((0.015f - myState[4]) <= 0.003f && EOGBehaviorTrue) || (myState[1] > bestScorePack[1] && time > 170)) && (otherPosInDark || energy < 1.0f) && memoryFilled == 0)) || (game.getFuelRemaining() == 0))  {
       /* if (game.getPicPoints() == 0.0f)*/ game.takePic();
    }

    if (((!hasOneItem(0)) || getScorePacks) && bestItem0 != -1){
        int Case2 = -1;
        if (goingForEndScores && myState[1] < vectorBetween[1]) {
            Case2 = 2;
            enableLowNRGStop = false;
        }
        if ((goingForEndScores || game.getScore() < 1.5f) && memoryFilled == 1) noPic = true;
        else noPic = false;
        goTo(myState,vectorBetween,Case2,slowDown);
    }
    else if (!hasMirror && (bestItem2 != -1)) {
        noPic = false;
        enableLowNRGStop = false;
        game.getItemLoc(vectorBetween, bestItem2);
        goTo(myState,vectorBetween,1,slowDown);
    }
    else {

        if (!game.posInDark(futurePos) && posInDark && time > 65) game.useMirror();
        game.getItemLoc(bestScorePack, bestItem(myState,otherState,0));

     noPic = false;
        vectorBetween[0] = myState[0];
        vectorBetween[1] = myState[1] - 0.4f;
        vectorBetween[2] = myState[2];
        if (mirrorTimeRemaining>0 && EOGScorePack) {
            vectorBetween[2] = bestScorePack[2];
            vectorBetween[0] = bestScorePack[0];
        }
        futurePos[1] = myState[1] - mathSquare(myState[4])/0.0165f;

        if (!posInDark) {
            mathVecSubtract(vectorBetween,vectorBetween,myState,3);
            vel = -0.04f/vectorBetween[1];
            if ((darkGreyBoundary <= myState[1]) && (futurePos[1] <= (darkGreyBoundary - /*y vel of light zone divided by accel*/2.840909091f*(myState[4])))) vel = 0.000001f/vectorBetween[1];
        }
        else {
            vectorBetween[0] = myState[0];
            vectorBetween[1] = myState[1] + 0.1f;
            if (goingForEndScores || (EOGBehaviorTrue)){
                vectorBetween[2] = myState[2];
                mathVecSubtract(vectorBetween,vectorBetween,myState,3);
                vel = 0.0f;
                if (myState[1] < 0.55f) vel = 0.025f/vectorBetween[1];
                enableLowNRGStop = false;
                if (memoryFilled == 1) noPic = true;
                else noPic = false;
            }
            else {
                mathVecSubtract(vectorBetween,vectorBetween,myState,3);
                vel = 0.007f/vectorBetween[1];
            }

        }
        mathVecScale(vectorBetween,vectorBetween,vel,false);
        if (((0.025f - myState[4]) > 0.003f) || vectorBetween[1] != 0.025f) {
            api.setVelocityTarget(vectorBetween);
        }

    }

    vectorBetween[0] = 0.0f;
    vectorBetween[1] = 0.0f;

    if (((stoppingNRG) > energy) && posInDark && enableLowNRGStop) {
        vectorBetween[2] = 0.0f;
        api.setVelocityTarget(vectorBetween);
    }

  if (((mirrorTimeRemaining > 0.0f  || time > 165 || goingForEndScores) && memoryFilled > 0) || memoryFilled==2) {
        vectorBetween[2] = 1.0f;
        api.setAttitudeTarget(vectorBetween);
        if ((((stoppingNRG + 1.2f) < energy) || time > 154 || (posInLight && energy>=1.0f)) && (mathVecInner(vectorBetween,&myState[6],3)>=0.968912f && mathVecMagnitude(&myState[9],3) <= 0.05f)) game.uploadPics();
  }
    else  {
      mathVecSubtract(vectorBetween,otherState,myState,3);
      api.setAttitudeTarget(vectorBetween);
    }
}
//End page main

};

ZRUser *zruser5 = new ZRUser1022;

  
#include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1036 : public ZRUser
{

//Begin page State loop
void StateLoop()
{
  validPicture=game.getMemoryFilled();
  energy=game.getEnergy();
  energyOther=game.getOtherEnergy() ;
  api.getMyZRState(Mystate);
  api.getOtherZRState(Otherstate);
  distance=Distanza(Mystate,Otherstate);
  TMR=game.getMirrorTimeRemaining();
  LI=game.getLightInterfacePosition() - 0.05;
  LS=LI+0.9f;
  if (LS>0.8f) LS=LS-1.6f;
  LM= game.posInArea(Mystate);
  LO= game.posInArea(Otherstate);
  if(LM==1) {EUP=1.05f;  ETP=1.05f;}
  else {EUP=1.0f;  ETP=2.5f;}

  Earth[0]=0.0f;  Earth[1]=0.0f;   Earth[2]=1.0f;
  DEBUG (("%d",TMR));

  for(i=0; i<9; i++){
      game.getItemLoc (Item[i], i);
  }
  if(t==0){
      idm=8;  // my sphera is red
      xmir=-0.05;
      if(Mystate[0]>0.0f) { idm--;xmir=0.05;}   // my sphera is blue
  }
  ids=idm-4;       // score for sphera
  idss=ids+2;
  if(Item[ids][2]>Item[idss][2]){ idss=ids; ids+=2;  }

  ID[0]=15-idm;
  ID[1]=idm ;
  ID[2]=ids ;
  ID[3]=idss ;
  ID[4]=idss+1 ;
  ID[5]=ids+1 ;

DEBUG ((" Z_idmother=%f   Z_idss=%f",Item[15-idm][2],Item[idss][2]));


  //if(LO!=-1 && t>160 && Usingmir==-1 and (game.hasItem (15-idm)==1))
  //{P=0;  DEBUG (("****Allora sei utile****"));}
//DEBUG(("********NEW LOOP  my energia= %f my energia other= %f ", energy,energyOther));
//if(t==1)  DEBUG(("Distanza da raggiungere= %f",Distanza(Mystate,Item[idm])));
//DEBUG ((" zona=%d  LI=%f  LS=%f  Y=%f",LM,LI,LS,Mystate[1])); Otherstate
//DEBUG ((" Towards idm=%f",Towards(Otherstate,Item[15-idm])));
//DEBUG ((" Towards idso=%f",Towards(Otherstate,Item[idso])));
//DEBUG ((" Towards idsso=%f",Towards(Otherstate,Item[idsso])));
//DEBUG ((" Towards ids=%f",Towards(Mystate,Item[ids])));
/*
  DEBUG ((" Towards idmo=%f  idso=%f  idsso=%f  ids=%f",
  Towards(Otherstate,Item[15-idm]),
  Towards(Otherstate,Item[idso]),
  Towards(Otherstate,Item[idsso]),
  Towards(Mystate,Item[ids])
  ));
*/

}



//End page State loop
//Begin page controlli
float Distanza(float state[12],float target[3])
{
    float diff[3];
  mathVecSubtract(diff,state,target,3);
  return mathVecMagnitude(diff,3);
}

void Controlmirror(){
  i=Mystate[1]+Mystate[4];
  if(   TMR==0 // My Mirror not activated
     //&& game.getNumMirrorsHeld()>0       // My mirror exists
     && distance>0.5f
     && (   LM!=-1                       // My sphera is not dark
         || (i<=LS+0.05f && Mystate[4]<0.0f)
         || (i>=LI+0.04f)
        )
     && (   (energyOther>0.5f    && LO==1)
         || (energyOther>1.0f    && LO!=1)
         || (Otherstate[2]<-0.4f && LO==1)
        )
     && t-Usingmir>24                       // other mirror  is not activated
    )
  {
      game.useMirror();
  }
}


//End page controlli
//Begin page game
void UPLOAD1(){
    if(  Mystate[8]>=0.97f  )
        {
           if(mathVecMagnitude(&Mystate[9],3)<=0.05f && energy >EUP) game.uploadPics();
        }
    api.setAttitudeTarget( Earth);
    DEBUG(("******ATTITUDINE=  %f , velocita=  %f ",Mystate[8],mathVecMagnitude(&Mystate[9],3)));
}


//End page game
//Begin page go
void movetostop(float stop[3]) {
float diff[3];
      if (Distanza(Mystate, stop) < 0.2f){
            api.setPositionTarget(stop);
        }
        else{
            GetVecDirect(diff, stop, Mystate, fast);
            api.setVelocityTarget(diff);
        }
}

//End page go
//Begin page help
//Begin page Help

bool help3()
{
    if (   fabsf(Mystate[0]) > 0.61f
        || fabsf(Mystate[1]) > 0.75f
        || fabsf(Mystate[2]) > 0.61f

    )
    {       Earth[2]=0.0f;
        GetVecDirect(Mystate,Mystate,Earth, -0.046f);
        api.setForces(Mystate);
      DEBUG(("\n **** NOT OUT ****"));
      return true ;
    }
    return false ;
}



//End page Help

//End page help
//Begin page main
//Begin page main
float Mystate[12];
float Otherstate[12];
float direction[3];
float Item[9][3];
int   ID[6];
float Earth[3];
float xmir;
float validPicture;
float energy;
float energyOther;
float distance;
int i;  // usato come indice di cicli
int idm, ids, idss;
int t; //conta il tempo dall'inizio, t+1 il numero di loop effettuati
int TMR;
int Usingmir; //indica lo stato del mirror: -1= non utilizzato, 0= in utilizzo, 1 gia utilizzato
int P;
int step;
float LS; //coda Grey Dark
float LI; //coda Dark Grey
int LM;
int LO;
float ETP;//energia al di sotto della quale non conviene fare foto
float EUP;//energia al di sotto della quale non conviene fare upload
float fast;
void init(){
    t=0;
    Usingmir=-1;
    step=0;
    P=2;// Massimo Numero di foto consentite dalla disponibilita di energia
    fast=0.06f;
    DEBUG(("Fuel The Jet now"));
}

void loop(){

StateLoop();
TakePhoto(); //control Take photo and other's mirror
if( !help3() )
 {
  if(    (t>=165)
      || validPicture==2
      || (validPicture==1 && TMR!=0)
    )
       UPLOAD1();
  else if(step!=0)
    { //FACE DIRECTION FOR PHOTO
      GetVecDirect(direction,Otherstate, Mystate,1);
      api.setAttitudeTarget(direction);
    }
  Controlmirror();

//--------------------------------STEP-------------------------------------

  if(energy<1.8f) {fast=0.025f;};
  if(energy<0.4f && step>1) {fast=0.0f;};
  if(step==0 && fabsf(Item[15-idm][2]-Item[ids+1][2])<0.15 )
  {fast=0.05f;  DEBUG (("allora rallento"));}

  for(i=0; i<6 ; i+=1){
        ids=ID[i];
        xmir*(1-2*i);
        if(step==i){
            if(Distanza(Mystate,Item[ids])=0 && LO==1) || (Mystate[4]<0 && LO!=1))
            Mystate[1]=Otherstate[1]+0.2f;
        Mystate[0]=0.0f;
        if(t>164) {Mystate[1]=0.0f;Mystate[2]=0.0f;}
        movetostop(Mystate);
        DEBUG(("------STEP FINAL-------"));
 }
}
 t=t+1;
}


//End page main
//Begin page take
void TakePhoto(){
    if(     (  (game.hasItem (7)==1) || (game.hasItem (8)==1)  )
         && ( LO!=-1 )
         && Usingmir==-1
       )
    {
         DEBUG(("get pic points"));
         if( game.getPicPoints()<0)
         {  DEBUG(("INIZIO mirror avversario, fine =%f",t+24));
                  Usingmir =t;
         }
    }

    if( // controllo se sono pronto
       (
        (  ( TMR==0)
        && ( game.isFacingOther() )
        && ( energy >=ETP   )         // E viene fissato in ogni step
        && ( validPicture
0.5f )// controllo che non sia troppo vicino
        )
        || ( LM==1)
        || ( energy<0.5f)
        || ( game.getFuelRemaining()<0.5f)
       )
       && ( t-Usingmir>24 )
      )
    {
     {game.takePic();}
   }
}




//End page take
//Begin page vector
void GetVecDirect(float vecresultJ[3],float to[3], float from[3], float length)
{
    mathVecSubtract(vecresultJ,to,from,3);
    mathVecNormalize(vecresultJ,3);
    vecresultJ[0]*=length;
    vecresultJ[1]*=length;
    vecresultJ[2]*=length;
}



//End page vector

};

ZRUser *zruser5 = new ZRUser1036;

          
        #include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1044 : public ZRUser
{

//Begin page main
#define ZR_MAX_FACING_ANGLE     0.968912f   //Cosine of the angle at which pictures may be taken/uploaded.
#define MOVE_ENERGY             1.0f        //Reserve energy for SPHERE Movement

// This function scale a normalized or non-normalized vector by mag; depending on the value of norm (true=normalize)
void mathVecMult(float res[3], float src[3], float mag, bool norm)
{
  memcpy(res,src,sizeof(float)*3);   // copy the source vector to the result vector
  if(norm) mathVecNormalize(res,3);  // normalize the vector first if norm is true
  // scale the result vector by mag
  res[0]*=mag;
  res[1]*=mag;
  res[2]*=mag;
}

//  Calculate the cosine of the angle between two position vectors
float doCosineTheta(float targetPos[3], float myPos[3])
{
    float v1[3], v2[3];
    
    mathVecMult(v1, targetPos, 1.0f, true);  // compute the unit vector for the target position vector
    mathVecMult(v2, myPos, 1.0f,true);       // compute the unit vector for my position vector
    return mathVecInner(v1, v2, 3);          // the inner product of the two vectors is Cosine Theta
}

// compute the distance between two points
float doPointToPointDistance(float fromPointLoc[3], float toPointLoc[3]) {
  float vecFT[3];
  
  mathVecSubtract(vecFT, fromPointLoc,toPointLoc, 3);
  return mathVecMagnitude(vecFT, 3);
}

float myZRState[12];      // my SPHERE state
float otherZRState[12];     // other SPHERE state
float displacement[3];          // displacement vector between my SPHERE and opponent SPHERE
float direction[3];             // direction vector to point to opponent SPHERE
float uploadDir[3];             // direction vector to upload picture to earth (+z-axis unit vector = 0.0f,0.0f,1.0f)
float closestPointsBlockList[4][3]; // 2D array to store PointsBlock locations
float myEnergyBlock[3];         // my closest energy block
float stepCloser[3];            // move closer to the target to score more photo points, must be >0.5f
float cosineTheta;              // The cosine of the aiming angle
float picPoints;                // picture points
float myEnergy;                 // my current energy 
float opponentDistance;         // opponent's distance to my SPHERE
float opponentDirection;        // if this value is close to -1.0f then opponent is directly above you.
float leadingEdge;              // leading edge of light wave
float trailingEdge;             // trailing edge of light wave
float d1, d2;                   // temp variables for distance comparison
int i;                          // temp loop count variable
int pointsBlockIDList[4];       // array of pointsBlockIDs
int myEnergyID;                 // energy block closest to me initially
int gameTime;                   // number of seconds passed since the beginning of the game (how many time game loop is called)
int availablePointBlocks;       // PointBlocks still available to pick up
bool meInLight;                 // first time we see the light
bool opponentTooClose;          // opponent is too close for picture taking
bool opponentExposed;           // opponent is exposed
bool pointsBlockAvailable;      // at least one pointsBlock still available
int memFilled;                // how much memory is filled
bool finalUpload;

void init(){
  gameTime = 0; // initialize game time to 0
  
  // initialize upload direction unit vector to point to the positive z-axis 
  uploadDir[0] = 0.0f;
  uploadDir[1] = 0.0f;
  uploadDir[2] = 1.0f;
    trailingEdge = -0.2f;  // initialize trailing edge location to -0.2f in y axis
    leadingEdge = 0.5f;    // initialize leading edge to -0.2+0.7 = 0.5f in y axis
  api.getMyZRState(myZRState); // read my SPHERE states
  myEnergyID = 0;  // initialize ID of energy block closest to me to 0
  game.getItemLoc(myEnergyBlock, myEnergyID);  // read closest energy block location into stepCloser
  if ((myEnergyBlock[0]*myZRState[0]) < 0.0f) {
      myEnergyID = 1;  // revise ID of energy block closest to me to 1
      game.getItemLoc(myEnergyBlock, myEnergyID); // revise closest energy block location into stepCloser
  } 
  pointsBlockAvailable = true; // initialize pointsBlock Available to true
  
  // find closest points block near my SPHERE
  d2 = 2.0f; // make d2 super large initially
  pointsBlockIDList[0] = 3; // initialize closest points block ID 
  for (i=0; i<4; i++) { // find closest Points block
      game.getItemLoc(closestPointsBlockList[0], i+3); // read first point block location
      d1 = doPointToPointDistance(myEnergyBlock,closestPointsBlockList[0]);
      if (d1 < d2) { // found a points block closer to my SPHERE
          d2 = d1;  // replace d2 as the closest distance
          pointsBlockIDList[0] = i + 3;
          if (pointsBlockIDList[0] < 5) {  // find the second closest points block on my mirror side
              pointsBlockIDList[1] = pointsBlockIDList[0] + 2;
          } 
          else {
              pointsBlockIDList[1] = pointsBlockIDList[0] - 2;
          }
          if ((pointsBlockIDList[0] == 3) || (pointsBlockIDList[0] == 5)){
                  pointsBlockIDList[2] = pointsBlockIDList[1] + 1;
                  pointsBlockIDList[3] = pointsBlockIDList[0] + 1;
          }
          else {
                  pointsBlockIDList[2] = pointsBlockIDList[1] - 1;
                  pointsBlockIDList[3] = pointsBlockIDList[0] - 1;
          }
      }
  }
   for (i=0; i<4; i++) {
        game.getItemLoc(closestPointsBlockList[i], pointsBlockIDList[i]);
    }
  // set first way-point high up in -z
    finalUpload = false;
}

void loop(){
  //This function is called once per second.  Use it to control the satellite.
  api.getMyZRState(myZRState); // read my SPHERE states
  api.getOtherZRState(otherZRState); // read other SPHERE states
  meInLight = game.posInLight(myZRState);
    myEnergy = game.getEnergy();  // my current energy level
    opponentExposed = !game.posInDark(otherZRState);
    memFilled = game.getMemoryFilled();
  mathVecSubtract(displacement, otherZRState, myZRState, 3);  //find the direction vector to aim at opponent
  mathVecMult(direction, displacement, 1.0f, true);  // normalize the direction vector
  opponentDistance = mathVecMagnitude(displacement, 3);


    if(gameTime >= 165 && memFilled < 1) {
        game.takePic();
    }
    if (opponentDistance < 0.45f) { // opponent is too close
        opponentTooClose = true;
        if ((meInLight || gameTime >= 160 || myEnergy > 3.5f || myEnergy < 1.0f) && memFilled < 1)  {
            game.takePic(); // take picture anyway for 0.01 pts
        }
    }
    else {
        opponentTooClose = false;
    }
    
    if(gameTime >= 160 && memFilled > 0) {
        finalUpload = true;
    }
    
    // check if opponent is out of bound and weak in energy, if so, move to the center and take picture if we have energy
    if ((myEnergy > MOVE_ENERGY) && ((otherZRState[0] > 0.64f) || (otherZRState[0] < -0.64f) || (otherZRState[1] > 0.8f) || (otherZRState[1] < -0.8f) || (otherZRState[2] > 0.64f) || (otherZRState[2] < -0.64f))) {
       memset(stepCloser, 0.0f, 12);
     api.setPositionTarget(stepCloser); // move farther from opponent SPHERE to score more
    }
    else if (!pointsBlockAvailable) { 
        // move back so pictures can be taken
        if ((opponentExposed) && (otherZRState[1] > trailingEdge)  && (myEnergy > 3.5f) && ((otherZRState[1] - 0.55f)> -0.75)) {
            stepCloser[0] = myZRState[0];
            stepCloser[1] = otherZRState[1] - 0.55f;        // move to the dark to take picture  
        }
        
        else {
            bool driftingInGray = false;
            
            // move to trail behind opponent
            stepCloser[0] = 0.333f * myZRState[0] + 0.667f * otherZRState[0];
            stepCloser[1] = 0.333f * myZRState[1] + 0.667f * otherZRState[1];
            
            // set the y-velocity but make sure to not be stuck in gray
            if(myZRState[1] < leadingEdge + 0.1f && myZRState[4] < 0.03f && myZRState[4] > 0.02f) {
                driftingInGray = true;
            }
            if(driftingInGray) {
                // if it will be drifting in the gray, speed up
                stepCloser[1] = -1 * myZRState[1] + 2 * otherZRState[1];
            }
        }
        
        // always be on top
        stepCloser[2] = -0.45f;
        
      api.setPositionTarget(stepCloser); // move closer to opponent SPHERE to score more photo points
    }
  else if ((game.hasItem(myEnergyID) < 0) && (myEnergy > MOVE_ENERGY)){ // my mirror is still available for pickup
          api.setPositionTarget(myEnergyBlock); // move to energy block to pick it up 
  }

  else if ((pointsBlockAvailable) && (myEnergy > MOVE_ENERGY)) { // there are still points block(s) to capture
          availablePointBlocks = 0; 
          for (i=0; i<4; i++) {
              if ((game.hasItem(pointsBlockIDList[i]) < 0) && ((closestPointsBlockList[i][2] - otherZRState[2]) < 0.48f)) { // points block is still available
                  availablePointBlocks += 1;
                  if ((myEnergy > MOVE_ENERGY) && (availablePointBlocks == 1)) // check energy level before move to points block
                      api.setPositionTarget(closestPointsBlockList[i]); // move to capture closest points block
              }
          }
          if (availablePointBlocks == 0) pointsBlockAvailable = false;
  }
  else {  // stay where you are
          api.setPositionTarget(myZRState); 
  }
  if(finalUpload) {
      float noForce[3];
      memset(noForce, 0.0f, sizeof(float) * 3);
      api.setForces(noForce);
  }

    // decide where should the camera be aiming
  if ((memFilled > 1) || ((memFilled > 0) && ((gameTime > 160) || (opponentTooClose)))) { // camera film slots full
      api.setAttitudeTarget(uploadDir);  //point camera at earth for uploading picture
  }
  else { 
      api.setAttitudeTarget(direction);  //point camera at Opponent SPHERE
  }

  // check if we are in range to slow down the spinning for uploading
    cosineTheta = doCosineTheta(uploadDir, &myZRState[6]); // compute cosine theta 

    //check if we can score and it is safe to take a picture
    if ((!opponentTooClose) && game.isFacingOther() && (opponentExposed) && (myEnergy >= 1.8f)) {  // opponent SPHERE is in view for picture taking
        picPoints = game.getPicPoints();  // check how many points the picture worths
        if (game.isCameraOn() && (myEnergy >= 2.0f || (myEnergy >= 1.7f && !pointsBlockAvailable)) && (picPoints > 0.0f) && opponentExposed) { // check conditions for picture taking
            picPoints = game.takePic(); // take picture
      }
    }

    // check if camera is filled and need to upload pictures to earth
    if(cosineTheta >= ZR_MAX_FACING_ANGLE && mathVecMagnitude(&myZRState[9], 3) <= 0.05f) {
        if(memFilled > 1 && myEnergy >= 1.7f) {
            game.uploadPics();
        }
        else if(memFilled > 0 && (gameTime >= 160 || (opponentTooClose && myEnergy >= 1.5f)) && (!pointsBlockAvailable || myEnergy >= 2.0f)) {
            game.uploadPics();
        }
    }

    if (gameTime == 180) DEBUG(("ZiRconiuM_Paly_Eagles TE6C"));
    gameTime++; // increase game time value by one
    // compute leading and trailing edges of the light wave block moving at 0.025f per second
    leadingEdge += 0.025f; 
    trailingEdge += 0.025f;
    
    if (leadingEdge >= 0.8f) leadingEdge = -0.8f;  // wrap around y boundary from 0.8 to -0.8
    if (trailingEdge >= 0.8f) trailingEdge = -0.8f;  // wrap around y boundary from 0.8 to -0.8
}


//End page main

};

ZRUser *zruser8 = new ZRUser1044;

        

          
        #include "ZRGame.h"
#include "ZR_API.h"
#include "ZRUser.hpp"
#include 

static ZeroRoboticsGame &game = ZeroRoboticsGame::instance();
static ZeroRoboticsAPI &api = ZeroRoboticsAPI::instance();

//Implement your simulation code in init() and loop()
class ZRUser1028 : public ZRUser
{

//Begin page aInit
struct
{
    float myState[12], otherState[12];
    float zero[3], uploadAtt[3], nextItem[3], vecBet[3], moveDark[3];
    float scoreToUpload;
    int time, state, item;
    bool enemyHasMirror;
    float side[3];
    bool doUpload, uploading, gotPicture, inGreyE, driftingDark, usPlus, inLight;
    int spacks;
    int upStart;
    bool enableRotation;
    int bursted;
    float burst[3];
} e;//do not confuse with euler constant


void getStates()
{
    api.getMyZRState(e.myState);
    api.getOtherZRState(e.otherState);
}

void positioningHelper()
{
    e.side[0] = e.otherState[0] > 0.0f ? -0.025f:0.025f;
}

void init()
{
    memset(&e, 0, sizeof e); //safer than 175, still badass rebels
    e.uploadAtt[2] = 1.0f;
    e.moveDark[1] = 0.025f;
    e.burst[1] = -1.0f;
    getStates();
    positioningHelper();
    e.usPlus = e.myState[0] > 0.0f;
    getClosest();
    //e.doUpload = false;
    //api.setAttGains(0.8f, 0.0f, 8.0f);
    // e.item = e.myState[0] > 0? 7:8; //fetch the id of the mirror
}


//End page aInit
//Begin page bFunctions
float distance(float pos1[3], float pos2[3])
{
    mathVecSubtract(e.vecBet, pos2, pos1, 3);
    return mathVecMagnitude(e.vecBet, 3);
}

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

//Angle between two vectors
float angle(float vec1[3], float vec2[3])
{
    float v1cpy[3], v2cpy[3];
    mathVecScale(v1cpy, vec1, 1.0f, true);
    mathVecScale(v2cpy, vec2, 1.0f, true);
    return acosf(mathVecInner(v1cpy, v2cpy, 3));
}
//End page bFunctions
//Begin page eGameFunctions
bool canUpload()
{
    return (e.myState[8] > MAX_FACING_ANGLE) &&
    (mathVecMagnitude(e.myState+9, 3) < 0.05f);
}

void stayOnTop()
{
    memcpy(e.nextItem, e.otherState, 12);
    e.nextItem[2] = -0.505f;
    if(distance(e.myState, e.otherState) > .5)
        e.nextItem[1] += 2*e.otherState[4];//game.posInDark(e.otherState)? 0.05f:-0.05f;
        //Changed this because on the transition of them going to dark, we reversed direction.
        //Better to just match their velocity
    while(distance(e.nextItem, e.otherState) < 0.515f)
    {
        mathVecAdd(e.nextItem, e.nextItem, e.side, 3);
    }

    if(!e.inLight)
    {
        e.bursted = 0;
    }
    if(e.otherState[4] > e.myState[4] && e.otherState[1] < e.myState[1] &&  e.inLight && e.bursted < 2)
    {
        e.bursted++;
        api.setForces(e.burst);
        DEBUG(("BURST"));
    }

    //mathVecAdd(e.nextItem, proj, side, 3);
    DEBUG(("distance %f", distance(e.myState, e.otherState)));

    if(e.nextItem[1] > 0.75f) e.nextItem[1] = 0.75f;
    if(e.nextItem[1] < -0.75f) e.nextItem[1] = -0.75f;
    if(e.nextItem[0] > 0.55f) e.nextItem[0] = 0.55f;
    if(e.nextItem[0] < -0.55f) e.nextItem[0] = -0.55f;
    //DEBUG(("next %f %f %f",e.nextItem[0], e.nextItem[1], e.nextItem[2]));
}

void getClosest()
{
    //float pos[3];
    float spackPos[7][3];
    //float min = 10.0f;
    //float d;
    e.item = -1;
    //e.spacks = 0;
    int theirUp;
    int theirDown;
    int ourUp;
    int ourDown;
    //float vecBetEnemy[3];
    //float vecBet[3];
    for(int i = 3; i < 7; i++)
    {
        game.getItemLoc(spackPos[i], i);
    }
    if(spackPos[4][2] < spackPos[6][2])
    {
        if(e.usPlus)
        {
            theirUp = 4;
            //theirDown = 6;
            ourUp = 3;
            ourDown = 5;
            DEBUG(("P1"));
        }
        else
        {
            theirUp = 3;
            //theirDown = 5;
            ourUp = 4;
            ourDown = 6;
            DEBUG(("P2"));
        }
    }
    else
    {
        if(e.usPlus)
        {
            theirUp = 6;
            //theirDown = 4;
            ourUp = 5;
            ourDown = 3;
            DEBUG(("P3"));
        }
        else
        {
            theirUp = 5;
            //theirDown = 3;
            ourUp = 6;
            ourDown = 4;
            DEBUG(("P4"));
        }
    }
    if(fabs(spackPos[3][2] - spackPos[5][2]) > 0.3f)
    {
        if(game.hasItem(theirUp) == -1)
        {
            e.item = theirUp;
        }
    }
    else
    {
        if(game.hasItem(ourDown) == -1)
        {
            e.item = ourDown;
        }
    }
    if(game.hasItem(ourUp) == -1 && e.item == -1)
    {
        e.item = ourUp;
    }
    /*for(int i = 3; i < 7; i++)
    {
        int owner = game.hasItem(i);

        if(owner == -1)
        {
            game.getItemLoc(pos, i);

            d = distance(pos, e.myState) + pos[2]/3.0f*(e.spacks == 0);

            mathVecSubtract(e.vecBet, pos, e.otherState, 3);
            if(d < min &&
            (angle(e.otherState+3, e.vecBet) > 0.15f) &&
            pos[2] < 0.1f)
            {
                min = d;
                e.item = i;
            }
        }
        if(owner == 0)
        {
            e.spacks++;
        }
    }*/
}



//End page eGameFunctions
//Begin page main
void loop() {


    e.time++;
    //e.gotPicture = false;
    getStates();
     //e.preventRotation = false;
    //MOVEMENT
    //-------------------------------------------------------------------
    //float vecBet[3];
    //e.inGreyE = game.posInGrey(e.otherState);
    e.inLight = game.posInLight(e.myState);
    //getClosest();
    if(e.otherState[1] > -0.1f)
    {
        e.enableRotation = true;
    }
    //if(e.spacks > 1)
    //    e.item = -1;
    if(e.item != -1)
    {
        if(game.hasItem(e.item) != -1)// ||
        //distance(e.otherState, e.nextItem) < 0.1f) ||
        //(e.myState[2]-e.otherState[2] > 0.4f && !game.posInDark(e.myState)))
        {
            getClosest();
            positioningHelper();
        }
        game.getItemLoc(e.nextItem, e.item);
    }
    else
    {
        e.enableRotation = true;
        if(e.side[0] * e.otherState[0] > 0.0f &&
        fabs(e.otherState[0] - e.myState[0]) < 0.2f)
        {
            e.side[0] *= -1;
        }
        stayOnTop();
    }

    //go To Item

    //if(e.item != -1)
    float d;
    if((game.getMemoryFilled() > 0 && e.time > 160) || (e.myState[1] > 0.55f && e.driftingDark))
    {
        api.setVelocityTarget(e.zero);
    }
    else if((e.time > 135 && game.posInDark(e.myState)) || e.driftingDark)
    {
        e.driftingDark = true;
        DEBUG(("%f %f %f ", game.getLightInterfacePosition(), game.getDarkGreyBoundary(), game.getLightGreyBoundary()));
        api.setVelocityTarget(e.moveDark);
    }
    else
    {
        d = distance(e.myState, e.nextItem);
        float s = mathVecMagnitude(e.myState+3, 3);

        //mathVecSubtract(vecBet, e.nextItem, e.myState, 3);
        bool onTrack = angle(e.vecBet, e.myState+3) < 0.25f;
        if(game.getEnergy() < (1.0f))// || (game.getFuelRemaining() < 6.0f && !game.posInGrey(e.otherState)))
        {
            api.setVelocityTarget(e.zero);
        }
        else if(d > 0.2f && onTrack)
        {
            if(s < 0.035f)
            {
                DEBUG(("WAR NEVER CHANGES: %f", e.myState[3]));
                //DEBUG(("frick"));
                //e.preventRotation = true;
                api.setForces(e.vecBet);
            }
        }
        else
        {
            //DEBUG(("SET OUR TARGET"));
            //e.preventRotation = true;
            DEBUG(("WAR NEVER CHANGES: %f", e.myState[4]));
            api.setPositionTarget(e.nextItem); // it works like this apparently
        }
    }
    //-------------------------------------------------------------------
    //END MOVEMENT
    //not a rick roll https://www.youtube.com/watch?v=sJOD7Bsllso
    //PHOTO TAKING
    //-------------------------------------------------------------------
    //float points;

    bool still = mathVecMagnitude(e.myState+3, 3) < 0.008f;
    //bool otherStill = mathVecMagnitude(e.otherState+3, 3) < 0.008f;
    d = distance( e.myState, e.otherState);

    float ee = 2.1f - (e.inLight || still);
    DEBUG(("EE  %f", ee));
    bool inDarkE = game.posInDark(e.otherState);
    if(!inDarkE && d >= PHOTO_MIN_DISTANCE &&
    game.isFacingOther() && (game.getMemoryFilled() < (2 - (e.time < 40)) ) && game.isCameraOn() &&
    (game.getEnergy() > ee))
    {
        //points = game.getPicPoints();
        if(game.getPicPoints() > 0.0f)
        {
            game.takePic();
            DEBUG(("REAL"));
            //e.gotPicture = true;
            //e.scoreToUpload += points;
            //e.enemyHasMirror = false;
        }
        else
        {
            //e.enemyHasMirror = true;
        }
    }


    //-------------------------------------------------------------------
    //END PHOTOTAKING

    //ROTATE
    //-------------------------------------------------------------------
    float diff[3], usToThem[3], futurePos[3];
    mathVecSubtract(diff, e.otherState+3, e.myState+3, 3);
    mathVecScale(diff, diff, 3, false);
    mathVecSubtract(usToThem, e.otherState, e.myState, 3);
    mathVecAdd(futurePos, diff, usToThem, 3);
    float futureMagnitude = mathVecMagnitude(futurePos, 3);

    ee = game.getEnergy();
    if(e.uploading || (game.getMemoryFilled() == 2 && (ee > 1.0f || e.inLight)) ||
    (game.getMemoryFilled() == 1 && e.time > 100 &&
    (((e.enemyHasMirror || (game.posInDark(e.myState) && game.posInDark(e.otherState))) && ee > 2.0f) || (e.time > 164))))
    {
        e.uploading = true;
        api.setAttitudeTarget(e.uploadAtt);

    }
    else if(e.enableRotation)
    {
        api.setAttitudeTarget(e.vecBet);
    }


    float futureVec[3];
    memcpy(futureVec, e.otherState, 12);
    futureVec[1] -= 0.16f;


    if(((futureMagnitude < .5f && d < .5) && (e.inLight || (still && e.time > 90)) && !e.uploading) ||
    (ee < 1.0f && game.posInDark(futureVec) && inDarkE && game.getMemoryFilled() != 1) ||
    (inDarkE && e.time > 165 && !e.uploading))
    {
        DEBUG(("DUMB"));
        game.takePic();
    }
    //-------------------------------------------------------------------
    //END ROTATE


    //UPLOAD
    //-------------------------------------------------------------------
    if(canUpload() && (e.uploading) && ee > 1.0f)// || (game.getMemoryFilled() == 1 && ee > 2.5f && e.gotPicture)))
    {
        game.uploadPics();
        //e.scoreToUpload = 0;
        e.uploading = false;
        DEBUG(("YOU WIN: %d", e.time-e.upStart));
    }
    //-------------------------------------------------------------------
    //END UPLOAD

    DEBUG(("WAR NEVER CHANGES: %f, %f", e.myState[8]+1, mathVecMagnitude(&e.myState[9], 3)));
}
//End page main

};

ZRUser *zruser8 = new ZRUser1028;