Zero Robotics High School Tournament 2014 (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 ZRUser07 : public ZRUser
{

//Begin page allDefines
#define ST_NEAR         99
#define ST_GOTO_POI     100
#define ST_GOTO_POIIN   110
#define ST_UPLOAD_PIC   3
#define ST_TURN         60
#define ST_HIDE         2

#define P               2.0f//1.0f//0.258f
#define I               2.0f//1.0f//0.0086f
#define D               11.0f//5.5f//1.935f

//0.258f,0.0086f,1.935f

//CUT v1
#define GAINSHI 1
#define GAINSLO 0
//end v1
//End page allDefines
//Begin page angle
float angle(float POI[3]){
    float disVec[3];
    mathVecSubtract(disVec, POI, myState, 3);
    float POIO[3];
    mathVecSubtract(POIO, origin, POI, 3);
    return acosf((mathVecInner(POIO, disVec, 3)) / (mathVecMagnitude(POIO, 3) *mathVecMagnitude(disVec, 3)));
}
//End page angle
//Begin page check
void check(int poiID){
    curPOI=poiID;
    //predict(23);
    posPicPOI(posPicP, 0.38f);
    if(turn(myState, disVecPic)==1 && memFilled==0){
        state=ST_GOTO_POIIN;
    } else {
        //memcpy(turnPointT, turnPoint, sizeof(float) * 3);
        state=ST_TURN;
    }
}
//End page check
//Begin page distance
float distance(float otherPos[3], float myPos[3]) {
    float disVec[3];
    mathVecSubtract(disVec, otherPos, myPos, 3);
    return mathVecMagnitude(disVec, 3);
}
//End page distance
//Begin page hideDis
//OPCODE WILL 2 -- KEEP!
/*
float hideDis(float mypos[3]){
    float disHideMGVec;
    if (turn(mypos,hide) == 1){
        disHideMGVec=distance(hide, mypos);
    } else {
        disHideMGVec=distance(mypos, turnPoint) + distance(turnPoint, hide);
    }
    return disHideMGVec;
}
*///end will 2
//End page hideDis
//Begin page main
float origin[3];
int state;
float myState[12];
int myGains;
int memFilled;
int fuelRemaining;
int flare;
int flareC;

float* myPosTarg;

float posPOI[3];
float posPOIP[3];
float posPicP[3];
float posPicPO[3];

float disVecPic[3];

float turnPoint[3];
float turnPointT[3];
float turnTurn[3];
int time;
int POII;

int curPOI;

float hide[3];

void init(){
    myGains=GAINSLO;
    memset(origin, 0, sizeof(float)*3);
    api.getMyZRState(myState);
    curPOI=(myState[1]<0);
    game.getPOILoc(posPOI, curPOI);
    time = 0;
    POII = 0;
    predict(21);
    state = ST_GOTO_POIIN;
    hide[0]=0.26f;
    turnTurn[0] = -0.268f;
    turnTurn[2] = 0.0f;
    turnPointT[0] = 0.0f;
    turnPointT[2] = 0.0f;
    flareC = 0;
    if (myState[1] > 0){
        curPOI = 0;
        hide[1] = 0.17f;
        turnTurn[1] = 0.268f;
        turnPointT[1] = 0.45f;
    } else {
        curPOI = 1;
        hide[1] = -0.17f;
        turnTurn[1] = -0.268f;
        turnPointT[1] = -0.45f;
    }
}

void loop(){
    api.getMyZRState(myState);
    flare = game.getNextFlare();
    memFilled=game.getMemoryFilled();
    fuelRemaining=game.getFuelRemaining();
    
    float earth[3];
    earth[0]=0.64f; earth[1]=earth[2]=0.0f;
    game.getPOILoc(posPOI, curPOI);
    
    //CUT 1
    hide[2]=(myState[2]>0.0f)?0.17f:-0.17f;
    /*
    if (myState[2] > 0){
        hide[2] = 0.17f;
    } else {
        hide[2] = -0.17f;
    }*///end cut 1
    if (time % 60 == 0 && time != 0){
        POII = 0;
    }
    
    if(fuelRemaining<1 && flareC < 2){
        game.takePic(0);
        if(flare<2 && flare>0){
            game.turnOff();
            game.turnOn();
        }
    }
    
    if((state!=ST_HIDE && flare != -1 && flare-(6.197f * logf((turn(myState, hide)==1) ? distance(hide, myState) : (distance(myState, turnPoint) + distance(turnPoint, hide))) + 27.641f) < 0.0f) || (fuelRemaining<15 && flareC < 2)){
        //CUT v1
        myGains=GAINSHI;
        //api.setPosGains(P,I,D);
        //end v1
        state = ST_HIDE;
    }
    
    if(flare == 0){
        flareC++;
    }
    
    DEBUG(("Next Flare: %d\n", flare));
    DEBUG(("State: %d\n", state));
    DEBUG(("Time: %d\n", time));
    //DEBUG(("FlareC: %d\n", flareC));
    /*DEBUG(("POII: %d\n", POII));
    DEBUG(("curPOI: %d\n", curPOI));
    DEBUG(("posPOI[1]: %f\n", posPOI[1]));*/
    /*DEBUG(("turnPoint[0]: %f\n", turnPoint[0]));
    DEBUG(("turnPoint[1]: %f\n", turnPoint[1]));
    DEBUG(("turnPoint[2]: %f\n", turnPoint[2]));*/
    /*DEBUG(("turnPointT[0]: %f\n", turnPointT[0]));
    DEBUG(("turnPointT[1]: %f\n", turnPointT[1]));
    DEBUG(("turnPointT[2]: %f\n", turnPointT[2]));*/
    
    switch(state){
        case ST_GOTO_POI:{
            //CUT v1
            myGains=GAINSHI;
            //api.setPosGains(P,I,D);
            //end v1
            posPicPOI(posPicPO, 0.51f);
            pointat(myState, posPOI);
            
            myPosTarg=disVecPic;
            if(game.alignLine(curPOI) && mathVecMagnitude(myState, 3)<0.53f && mathVecMagnitude(myState, 3)>0.42f && angle(posPOI)<0.39f){
                game.takePic(curPOI);
            }
            
            if(memFilled==2){
                //CUT v1
                myGains=GAINSLO;
                //api.setPosGains(0.258f,0.0086f,1.935f);
                //end v1
                state=ST_HIDE;
            }
            break;
        }
        
        case ST_GOTO_POIIN:{
            //CUT v1
            myGains=GAINSHI;
            //api.setPosGains(P,I,D);
            //end v1
            posPicPOI(posPicP, 0.38f);
            pointat(myState, posPOIP);
            myPosTarg=disVecPic;
            if(distance(myState, disVecPic) > 0.28f){
                game.takePic(curPOI);
            } else {
                //CUT v1
                myGains=GAINSLO;
                //api.setPosGains(0.258f,0.0086f,1.935f);
                //end v1
            }
            if(game.alignLine(curPOI) && mathVecMagnitude(myState, 3)<0.42f && angle(posPOI)<0.79f){
                game.takePic(curPOI);
            }
            if(memFilled==1 && game.getMemorySize() == 2){
                POII = 1;
                state=ST_GOTO_POI;
            }  else if (memFilled == 1){
                POII = 1;
                state = ST_HIDE;
            }
            break;
        }
        
        case ST_TURN:{
            //CUT v1
            myGains=GAINSLO;
            //api.setPosGains(0.258f, 0.0086f, 1.935f);
            //end v1
            //myPosTarg=turnPointT;
            game.takePic(2);
            //opCodev2
            
            if (flare != -1){
                state = ST_HIDE;
            } else if (myState[0] > 0.12){
                myPosTarg = turnPointT;
            } else if (distance(origin, myState) > 0.38f && distance(turnPointT,myState) < 0.2f && POII == 0 && (time % 60 < 40 || time > 120)){
                predict(17);
                state = ST_GOTO_POIIN;
            } else if (time % 60 > 40){
                myPosTarg = turnTurn;
            } else if(time % 60 == 0){
                predict(16);
                state = ST_GOTO_POIIN;
            }
            
            break;
        }
        
        case ST_HIDE:{
            game.takePic(curPOI);
            if(memFilled){
                pointat(myState,earth);
                game.uploadPic();
            }
            if (turn(myState,hide) == 1 || myState[0] > 0.1f){
                myPosTarg=hide;
            } else {
                turn(myState,hide);
                myPosTarg=turnPoint;
            }
            if(flare == -1 && memFilled==0 && (fuelRemaining >15 || flareC == 2) /*&& time % 60 < 30*/){
                /*if (time % 60 < 35 && POII == 1){
                    curPOI = 2;
                    //game.getPOILoc(posPOI,curPOI);
                    predict(10);
                    state = ST_GOTO_POIIN;
                } else */if (myState[1] > 0){
                    curPOI = 0;
                    hide[1] = 0.17f;
                    turnTurn[1] = 0.268f;
                    turnPointT[1] = 0.4f;
                    POII = 0;
                    state = ST_TURN;
                } else {
                    curPOI = 1;
                    hide[1] = -0.17f;
                    turnTurn[1] = -0.268;
                    turnPointT[1] = -0.4f;
                    POII = 0;
                    state = ST_TURN;
                }
            }
            if (distance(myState,hide) < 0.03f){
                myPosTarg=hide;
                //CUT v1
                myGains=GAINSLO;
                //api.setPosGains(0.258f,0.0086f,1.935f);
                //end v1
            }
            break;
        }
    }
    //CUT v1
    if(myGains==GAINSHI)
        api.setPosGains(P,I,D);
    else
        api.setPosGains(0.258f, 0.0086f, 1.935f);
    //end v1
    api.setPositionTarget(myPosTarg);
    time ++;
}
//End page main
//Begin page pointat
void pointat(float myPos[3], float otherPos[3]) {
    float displacementPOI[3];
    mathVecSubtract(displacementPOI, otherPos, myPos, 3);
    api.setAttitudeTarget(displacementPOI);
}
//End page pointat
//Begin page posPicPOI
void posPicPOI(float POI[3], float rate){
    mathVecSubtract(disVecPic, POI, origin, 3);
    mathVecNormalize(disVecPic,3);
    for (int x = 0; x < 3; x++){
        disVecPic[x] = disVecPic[x] * rate;
    }
}
//End page posPicPOI
//Begin page predict
void predict(int second){
    float POIVec[3];
    float originP[3] = {0.0f,posPOI[1],0.0f};
    float startVec[3] = {0.0f,posPOI[1],0.1f};
    float curAngle;
    float POIMGVec;
    float startMGVec;
        mathVecSubtract(POIVec,posPOI,originP,3);
        POIMGVec = mathVecMagnitude(POIVec,3);
        startMGVec = mathVecMagnitude(startVec,3);
        curAngle = (0.1f * second) + acosf(mathVecInner(POIVec,startVec,3) / (POIMGVec * startMGVec));
        if (curAngle > PI - 0.6 && curAngle < PI){
            curAngle = 0.0f;//curAngle - PI;
        } else if (curAngle > PI){
            curAngle = curAngle - PI;
        }
            posPOIP[0] = -sinf(curAngle) * POIMGVec;
            posPOIP[1] = posPOI[1];
            posPOIP[2] = cosf(curAngle) * POIMGVec;
        
        curAngle = curAngle + 0.1f * 6.0f;
        /*if (curAngle > PI){
            curAngle = curAngle - PI;
        }*/
            posPicP[0] = -sinf(curAngle) * POIMGVec;
            posPicP[1] = posPOI[1];
            posPicP[2] = cosf(curAngle) * POIMGVec;
        
        curAngle = curAngle + 0.1f * 5.0f;
        /*if (curAngle > PI){
            curAngle = curAngle - PI;
        }*/
            posPicPO[0] = -sinf(curAngle) * POIMGVec;
            posPicPO[1] = posPOI[1];
            posPicPO[2] = cosf(curAngle) * POIMGVec; 
    //}
    
    /*posPOIP[0][0] = x[0];
    posPOIP[0][1] = posPOI[0][1];
    posPOIP[0][2] = z[0];
    posPOIP[1][0] = x[1];
    posPOIP[1][1] = posPOI[1][1];
    posPOIP[1][2] = z[1];
    posPicP[0][0] = x[2];
    posPicP[0][1] = posPOI[0][1];
    posPicP[0][2] = z[2];
    posPicP[1][0] = x[3];
    posPicP[1][1] = posPOI[1][1];
    posPicP[1][2] = z[3];
    posPicPO[0][0] = x[4];
    posPicPO[0][1] = posPOI[0][1];
    posPicPO[0][2] = z[4];
    posPicPO[1][0] = x[5];
    posPicPO[1][1] = posPOI[1][1];
    posPicPO[1][2] = z[5];*/
    /*posPOIP[2][0] = x[2];
    posPOIP[2][1] = 0;
    posPOIP[2][2] = z[2];
    posPicP[2][0] = x[5];
    posPicP[2][1] = 0;
    posPicP[2][2] = z[5];
    posPicPO[2][0] = x[8];
    posPicPO[2][1] = 0;
    posPicPO[2][2] = z[8];*/
    /*for (int w = 0; w < 3; w++){
        posPOIP[w][0] = x[w];
        posPOIP[w][1] = posPOI[w][1];
        posPOIP[w][2] = z[w];
        posPicP[w][0] = x[w + 3];
        posPicP[w][1] = posPOI[w][1];
        posPicP[w][2] = z[w + 3];
        posPicPO[w][0] = x[w + 6];
        posPicPO[w][1] = posPOI[w][1];
        posPicPO[w][2] = x[w + 6];
    }*/
}


//End page predict
//Begin page turn
int turn(float myPos[3],float target[3]){
    //1:mypos, 2:target, 3:origin, 4:turnpoint
    float v12[3];
    float v13[3];
    float v34[3];
    mathVecSubtract(v12, target, myPos, 3);
    mathVecSubtract(v13, origin, myPos, 3);
    float MGv12 = mathVecMagnitude(v12,3);
    float dot = mathVecInner(v13,v12,3);
    float projection[3];
    for (int j = 0; j < 3; j++){
        projection[j] = v12[j] * (dot / (MGv12 * MGv12));
    }
    mathVecSubtract(v34,v13,projection,3);
    float MGprojection = mathVecMagnitude(projection,3);
    mathVecNormalize(v34,3);
    for(int x=0; x<3; x++){
        turnPoint[x] = -v34[x]*0.47f;
    }
    return MGprojection>=distance(myPos, target);
}
//End page turn


};

ZRUser *zruser792 = new ZRUser07;

#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 ZRUser07 : public ZRUser
{

//Begin page main
#define FUEL_LOW 0
#define HIDE_SHADOW 1
#define UPLOAD 2
#define TAKE_PICTURES 3
#define for3 for(int i=0;i<3;i++)
#define FAR_ORBIT 2
#define CLOSE_ORBIT 1

float myState[12];
float myPos[3];
float goodPOILoc[2][3];
float POILoc[3][3];
float POIPicPosition[3];
float dotProduct;
float earth[3];
float temp[3];

int whatToDoNext;
int POINumber[2];
int closePOI;
int farPOI;
int picturesTaken;
int picturesStart;
int picturesEnd;
int convenientPOI;
int side;
int convenientOrbit;

bool firstTime;
bool isBlue;
bool calculateDistances;
bool POIUndecided;
bool wellPositioned;
bool decidedSide;

void init(){
   whatToDoNext = TAKE_PICTURES;
   picturesTaken = 0;
   
   firstTime = true;
   POIUndecided = true;
   
   earth[0] = 0.64f;
   earth[1] = 0.0f;
   earth[2] = 0.0f;
   
   decidedSide = false;
}

void loop(){
    DEBUG(("\n Dot product is: %f.",dotProduct));
    DEBUG(("\n Pictures taken: %d.",picturesTaken));
    //Checks how many pictures we have at the start of a loop
    picturesStart = game.getMemoryFilled();
    //When the POI resets we once again check which POI to go to
    if(api.getTime() % 60 == 0){
        POIUndecided = true;
        calculateDistances = true;
        picturesTaken = 0;
    } 
    //We obtain all the relevant information
    obtainInformation();
    //We decide what to do
    decideWhatToDo();
    //We do whatever is apropiate
    switch(whatToDoNext){
        case FUEL_LOW :
        takePictures();
        goToShadow();
        break;
        
        case HIDE_SHADOW :
        decidedSide = false;
        goToShadow();
        break;
        
        case UPLOAD :
        upload();
        break;
        
        case TAKE_PICTURES :
        takePictures();
        break;
    }
    //If after executing the previous code we have more pictures than at the start
    //It means we have taken a picture so we increase picturesTaken and decide which
    //POI to go to
    //Also if we had 2 pictures at the beggining and 0 after the loop it means
    //we have uploaded so we calculate relative distances again
    if(game.getMemoryFilled() > picturesStart){
        picturesTaken++;
        POIUndecided = true;
    } else if(game.getMemoryFilled() == 0 && picturesStart == 2) calculateDistances = true;
}


void decideWhatToDo(){
    //Here are all the conditions that determine what the program is going to do
    //at any given moment
    if(game.getFuelRemaining() <= 5){
        whatToDoNext = FUEL_LOW;
    } else if(game.getNextFlare() <= 25 && game.getNextFlare() > 0){
        whatToDoNext = HIDE_SHADOW;
    } else if(game.getMemoryFilled() == game.getMemorySize()){
        whatToDoNext = UPLOAD;
    } else if(api.getTime() >= 165 && game.getMemoryFilled() > 0) {
        whatToDoNext = UPLOAD;
        } else whatToDoNext = TAKE_PICTURES;
}

//Function that takes care of taking pictures
void takePictures(){
    //First chooses which POI we might be interested to go to
    determinePOI();
    //Then if necessary checks which POI are closest
    if(calculateDistances){ 
        relativeDistances();
        calculateDistances = false;
    }
    //Then if undecided determines specifically which POI to go to
    if(POIUndecided){ 
        choosePOI();
        POIUndecided = false;
    }
    //Actually goes ahed to take pictures
    goTakePictures();
}

//Gets myState, myPos and determines if we are blue or red
void obtainInformation(){
    api.getMyZRState(myState);
    memcpy(myPos,myState,3*sizeof(float));
    if(firstTime) {
        if(myPos[1] > 0) isBlue = true;
        else isBlue = false;
        firstTime = false;
    }
    
}

//Depending on wether we are blue or red it gets rid of the furthestPOI from us and
//retains the closest and middle onw
void determinePOI(){
    int reverseInequality;
    for3 game.getPOILoc(POILoc[i],i);
    if(isBlue) reverseInequality = 1;
    else reverseInequality = -1;
    if(POILoc[0][1] * reverseInequality < 0) {
            memcpy(goodPOILoc,POILoc+3,3*sizeof(float));
            memcpy(goodPOILoc+3,POILoc+6,3*sizeof(float));
            POINumber[0] = 1;
            POINumber[1] = 2;
        } else if(POILoc[1][1] * reverseInequality < 0) {
            memcpy(goodPOILoc,POILoc,3*sizeof(float));
            memcpy(goodPOILoc+3,POILoc+6,3*sizeof(float));
            POINumber[0] = 0;
            POINumber[1] = 2;
        } else if(POILoc[2][1] * reverseInequality < 0) {
            memcpy(goodPOILoc,POILoc,3*sizeof(float));
            memcpy(goodPOILoc+3,POILoc,3*sizeof(float));
            POINumber[0] = 0;
            POINumber[1] = 1;
        }
}

//From the 2 POI we are interested in calculates the closest and furthest one
void relativeDistances(){
    DEBUG(("\n Calculating distances"));
    float distance1[3];
    float distance2[3];
    float POI1Loc[3];
    float POI2Loc[3];
    float dist1;
    float dist2;
    memcpy(POI1Loc,goodPOILoc,3*sizeof(float));
    memcpy(POI2Loc,goodPOILoc+3,3*sizeof(float));
    mathVecSubtract(distance1,POI1Loc,myPos,3);
    mathVecSubtract(distance2,POI2Loc,myPos,3);
    dist1 = mathVecMagnitude(distance1,3);
    dist2 = mathVecMagnitude(distance2,3);
    if(dist1 < dist2){
        closePOI = POINumber[0];
        farPOI = POINumber[1];
    } else {
        closePOI = POINumber[1];
        farPOI = POINumber[0];
    }
    
    DEBUG(("\n Close POI: %d.",closePOI));
    DEBUG(("\n Far POI: %d.",farPOI));
}

//Depending on the pictures we have taken so far determines which
//POI and in which orbit to go to
void choosePOI(){
    switch(picturesTaken){
        case 0:
        convenientPOI = closePOI;
        convenientOrbit = FAR_ORBIT;
        break;
        
        case 1:
        convenientPOI = farPOI;
        convenientOrbit = FAR_ORBIT;
        break;
        
        case 2:
        convenientPOI = closePOI;
        convenientOrbit = CLOSE_ORBIT;
        break;
        
        case 3:
        convenientPOI = farPOI;
        convenientOrbit = CLOSE_ORBIT;
        break;
    }
}

//Takes care of actually taking the pictures
void goTakePictures(){
    float vecBetween[3];
    game.getPOILoc(POIPicPosition,convenientPOI);
    mathVecSubtract(vecBetween,POIPicPosition,myPos,3);
    POIPicPosition[0] = 0.0;
    POIPicPosition[2] = /*side **/ sqrtf(0.04 -powf(POIPicPosition[1],2));
    face(POIPicPosition);
    dotProduct = mathVecInner(myPos,vecBetween,3);
    if(convenientOrbit == 2) {
    for3 POIPicPosition[i] *= 2.25;
    } else { 
        for3 POIPicPosition[i] *= 1.95;
    }
    arcMove(POIPicPosition);
    if(convenientOrbit == 2 ) {
        if(game.alignLine(convenientPOI) && dotProduct > -0.118){
        game.takePic(convenientPOI);
    }
    } else {
        if(game.alignLine(convenientPOI) && dotProduct > -0.079){
        game.takePic(convenientPOI);
    }
    }
}

//Faces the provided position
void face(float target[]){
    float tempTarget[3];
    float attitudeVector[3];
    memcpy(tempTarget,target,3*sizeof(float));
    mathVecSubtract(attitudeVector,tempTarget,myPos,3);
    api.setAttitudeTarget(attitudeVector);
}

void upload(){
    face(earth);
    float tempPos[3];
    memcpy(tempPos,myPos,3*sizeof(float));
    mathVecNormalize(tempPos,3);
    for3 tempPos[i] *= 0.6;
    arcMove(tempPos);
    game.uploadPic();
}

void goToShadow(){
    float szLU[3];
    float szLL[3];
    float szRU[3];
    float szRL[3];

    szLU[0] = 0.31f;
    szLL[0] = 0.31f;
    szRU[0] = 0.31f;
    szRL[0] = 0.31f;

    szLU[1] = -0.17f;
    szLL[1] = -0.17f;
    szRU[1] = 0.17f;
    szRL[1] = 0.17f;

    szLU[2] = -0.17f;
    szLL[2] = 0.17f;
    szRU[2] = -0.17f;
    szRL[2] = 0.17f;
    if(myPos[1] > 0.0) {
      if(myPos[2] > 0.0){
          arcMove(szRL);
      }
      else {
          arcMove(szRU);
      }
        } else if(myPos[2] > 0.0) {
          arcMove(szLL);

      } else {
          arcMove(szLU);
          }
    if(game.getMemoryFilled() > 0){
        face(earth);
        game.uploadPic();
    }
}

void arcMove(float posTarget2[3])
{
    float midpoint[3] = {(myPos[0]+posTarget2[0])/2, (myPos[1]+posTarget2[1])/2, (myPos[2]+posTarget2[2])/2};
    if (mathVecMagnitude(midpoint,3) < 0.35) {
        mathVecNormalize(midpoint,3);
      for (int i = 0; i<3; i++) {
        midpoint[i] *= 0.49;
      }
      setPositionTarget(midpoint,1.25);
      //DEBUG((" | Heading to waypoint | "));
    }
    else {
        setPositionTarget(posTarget2,1.25);
    }
}

void setPositionTarget(float target[], float mult) {
    float multTarget[3];
    //We are going to tell the satellite that our final destiny is further away than it really is so it goes faster
    for (int i = 0; i < 3; i++) multTarget[i] = myPos[i] + mult * (target[i] - myPos[i]);
    api.setPositionTarget(multTarget);
}



//End page main


};

ZRUser *zruser805 = new ZRUser07;

#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 ZRUser01 : public ZRUser
{
//Begin page main
//HAPPY BIRTHDAY PENNY!
float myPos[3];
float enemyPos[3];
short nextFlare;
short gameTime;
short chosenPOIID;
float zVec[3];
float lattitudeAngle;
float chosenPOILoc[3];
float height;
bool POIPhotoTaken[6];
bool bottom;
short flareCount;
float myHeight;
short timeToSwitch;
short timeToPoint;
bool running;
bool missed;

void init() {
    gameTime = -1;
    flareCount=zVec[0]=zVec[1]=0;
    zVec[2] = -1.0f;
    flushPOIPhotos(); 
    chosenPOIID = 2;
    missed=bottom=false;
}

void loop(){

    gameTime++;
    running = false;
    float zeroVec[3] = {0.0f, 0.0f, 0.0f};
    float darkSpot[3] = {0.39f, 0.0f, 0.0f};
    timeToSwitch = 60 - gameTime % 60;
    short upload = 0;
    float state[12];
    api.getOtherZRState(state);
    enemyPos[0] = state[0];
    enemyPos[1] = state[1];
  enemyPos[2] = state[2];
    api.getMyZRState(state);
    myPos[0] = state[0];
  myPos[1] = state[1];
  myPos[2] = state[2];
  
  myHeight = mathVecMagnitude(myPos, 3); 
  bool inShadowZone = myPos[0] > 0 && myPos[1]*myPos[1] + myPos[2]*myPos[2] < 0.04f; 
  
  if (gameTime == 3 && (enemyPos[0] < -0.012f || enemyPos[2] < -0.005f)) 
      bottom = true;

  if (inShadowZone) 
      bottom = (myPos[2] > enemyPos[2]) ? true:false;
      
  short memSize = game.getMemorySize();
  short preRoundPhotos = game.getMemoryFilled();
  nextFlare = game.getNextFlare();
  if (nextFlare < 0)
      nextFlare = 100;
      
  if (nextFlare == 1) {
      flareCount++;
      if (!inShadowZone) {
            game.turnOff();
            game.turnOn();
      }
    }
  
  if (timeToSwitch == 60) 
        flushPOIPhotos(); 

    getNewChosenPoint();
    
    game.getPOILoc(chosenPOILoc, chosenPOIID);
    lattitudeAngle = (bottom) ? 2.54f : 0.1f;
    if (memSize == 1 && bottom)
        lattitudeAngle += 0.3f;
    float rotAngle = getRotationAngle(chosenPOILoc);
    float futurePOISpot[3];
    float futurePhotoSpot[3];
    float vecToPOI[3];
    POIPredict(futurePOISpot, chosenPOILoc, rotAngle);
    vecManip(futurePhotoSpot, height*5, futurePOISpot);
       
  if (timeToLattitude(chosenPOILoc) >= 6 && memSize - preRoundPhotos == 2) {
        POIPredict(futurePOISpot, chosenPOILoc, rotAngle - 0.6f);
        POIPredict(futurePhotoSpot, chosenPOILoc, rotAngle - 0.3f);
        vecManip(futurePhotoSpot, 1.95f, futurePhotoSpot);
        mathVecSubtract(vecToPOI, futurePOISpot, futurePhotoSpot, 3);
    }
    else 
     mathVecSubtract(vecToPOI, chosenPOILoc, myPos, 3); 
  
  if (memSize == 1 || missed)
      mathVecSubtract(vecToPOI, futurePOISpot, futurePhotoSpot, 3);
  
      
    mathVecNormalize(vecToPOI, 3);

    if ((game.getMemoryFilled() > 0) 
        && (nextFlare < 8 || preRoundPhotos == memSize || gameTime >= 170
        || (timeToLattitude(chosenPOILoc) > 10 
        && distance(myPos, futurePhotoSpot) < 0.05f) || game.getFuelRemaining() <= 1)) {
        uploadPhotos();
        api.setVelocityTarget(zeroVec);
        upload = 1;
    }
    else if (game.alignLine(chosenPOIID)) {
            float vecPOItoSPHERE[3];
            mathVecSubtract(vecPOItoSPHERE, myPos, chosenPOILoc, 3);
            if ((angleCalc(vecPOItoSPHERE, chosenPOILoc) < ((myHeight > 0.42f) ? 0.45f : 0.9f))
                && !(myHeight < 0.42f && POIPhotoTaken[chosenPOIID * 2])) {
              game.takePic(chosenPOIID);
                }
            if (game.getMemoryFilled() > preRoundPhotos)  
                POIPhotoTaken[chosenPOIID * 2 + ((myHeight > 0.42f) ? 1:0)] = true;
    }
  if (running || (timeToLattitude(chosenPOILoc) > 14 && timeToSwitch > 3))
      game.takePic(0);
      
    if ((realTimeToLat(chosenPOILoc, chosenPOIID) + 20 > nextFlare) && flareCount == 0) {
        if (game.getMemoryFilled() > 0)
            uploadPhotos();
        running = true;
        moveToPoint(darkSpot);
    }    
    else if (upload == 0) {
        moveToPoint(futurePhotoSpot);
        api.setAttitudeTarget(vecToPOI);
    }
    
    if (gameTime < 3) 
        moveToPoint(zeroVec);
    
    if (game.getMemoryFilled() == 1 && timeToLattitude(chosenPOILoc) > 10)
        missed = true;
    else if (game.getMemoryFilled() != 1)
        missed = false;
    DEBUG(("timeToLattitude %i\n gameTime %i\n", timeToLattitude(chosenPOILoc), gameTime));
        
}

void uploadPhotos() {
    float vecToEarthPos[3] = {0.64f - myPos[0], -myPos[1], -myPos[2]};
    mathVecNormalize(vecToEarthPos, 3);
    api.setAttitudeTarget(vecToEarthPos);
    game.uploadPic();
}


short realTimeToLat(float givenPOILoc[3], short POIID) {
    float futurePOILoc[3];
    short timeToLat = timeToLattitude(givenPOILoc);
    POIPredict(futurePOILoc, givenPOILoc, (getRotationAngle(givenPOILoc) 
        - ((timeToLat > 6 && game.getMemorySize() != 1) ? 0.3f : 0.0f)));
    timeToPoint = angleCalc(futurePOILoc, myPos) * 12 + (fabs(myHeight - height)*40) + 2;
    if (POIID == chosenPOIID && timeToLat <= 8 && game.getMemoryFilled() != game.getMemorySize())
        timeToPoint -= 10;
    if (timeToPoint > timeToLat)
        timeToLat += 31;
    return timeToLat;
}

short timeToLattitude(float givenPOILoc[3]) {
    float vecCircleToPOILoc[3] = {givenPOILoc[0], 0.0f, givenPOILoc[2]};
    float timeToTop = angleCalc(vecCircleToPOILoc, zVec) * 10 - 1;
    return (bottom) ? ((timeToTop > 25 && timeToTop < 32) ? timeToTop - 25 : timeToTop + 8) : timeToTop;
}

float getRotationAngle(float POILoc[3]) {
    float vecCircToPOI[3] = {POILoc[0], 0.0f, POILoc[2]};
    return angleCalc(vecCircToPOI, zVec) - lattitudeAngle;  
}

void POIPredict(float futurePOILoc[3], float currentPOILoc[3], float theta) {
    float a1 = cosf(theta);
    float a2 = -sinf(theta);
    futurePOILoc[0] = currentPOILoc[0]*a1 + currentPOILoc[2]*a2;
    futurePOILoc[1] = currentPOILoc[1];
    futurePOILoc[2] = -currentPOILoc[0]*a2 + currentPOILoc[2]*a1;
}

void moveToPoint(float targetPos[3]) {
    float velocityTarget[3];
    if (angleCalc(targetPos, myPos) > 1.16f || myHeight < 0.35f) {
        mathVecAdd(targetPos, targetPos, myPos, 3);
        mathVecNormalize(targetPos, 3);
        vecManip(targetPos, (running) ? 0.38f : 0.42f, targetPos);
    }
    mathVecSubtract(velocityTarget, targetPos, myPos, 3);
    float dist = mathVecMagnitude(velocityTarget, 3);
    mathVecNormalize(velocityTarget, 3);
    vecManip(velocityTarget, (dist*dist < 0.02f) ? dist*dist : 0.02f, velocityTarget);
    if (realTimeToLat(chosenPOILoc, chosenPOIID) - timeToPoint < 10 || running) {
      api.setVelocityTarget(velocityTarget);
  }
    api.setPositionTarget(targetPos);
}




void getNewChosenPoint() {
    float POILoc[3];
    short interimPOIID = chosenPOIID;
    short minTimeToLat = 50;
    height = 0.5f;
    for (short POIID = 0; POIID < 3; POIID++) {
        game.getPOILoc(POILoc, POIID);
        short timeToLat = realTimeToLat(POILoc, POIID);
        short weight = (POIPhotoTaken[POIID * 2 + 1]) ? 8 : 0;
        if (POIID == (myPos[1] > 0) ? 1 : 0)
            weight += 3;
        if (enemyPos[1] * POILoc[1] > 0.0001f) {
            weight += 5;
            if (enemyPos[2] * myPos[2] > 0)
                weight += 10;
        }
        if (timeToLat < timeToSwitch
            && timeToLat + weight < minTimeToLat && !(POIPhotoTaken[POIID * 2] && POIPhotoTaken[POIID * 2 + 1])) { 
            interimPOIID = POIID;
            minTimeToLat = timeToLat + weight;
        }
    }
    chosenPOIID = interimPOIID;
    game.getPOILoc(chosenPOILoc, chosenPOIID);
    
    if (minTimeToLat > 49) 
        chosenPOIID = 2;
    
    if (POIPhotoTaken[chosenPOIID * 2 + 1] && timeToLattitude(chosenPOILoc) < timeToSwitch)
        height = 0.39f;
}

void flushPOIPhotos() { 
    POIPhotoTaken[0]=POIPhotoTaken[1]=POIPhotoTaken[2]=POIPhotoTaken[3]=POIPhotoTaken[4]=POIPhotoTaken[5]=false;
}

void vecManip(float outVec[3], float c, float inVec[3])
{   
  outVec[0] = c * inVec[0];
  outVec[1] = c * inVec[1];
  outVec[2] = c * inVec[2];
}

float angleCalc(float vec1[3], float vec2[3])
{
  return acosf(mathVecInner(vec1, vec2, 3)/(mathVecMagnitude(vec1,3) * mathVecMagnitude(vec2,3)));
}

float distance(float pos1[3], float pos2[3]) {
    float differenceVec[3];
    mathVecSubtract(differenceVec, pos1, pos2, 3);
    return mathVecMagnitude(differenceVec, 3);
}
//End page main
};

ZRUser *zruser787 = new ZRUser01;

#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 ZRUser01 : public ZRUser
{

//Begin page aInit
//Two POI changes :
//Line 8 use picsTaken[3] instead of picsTaken[2][3]
//setAttGains to (0.8, 0.0, 8.0)
struct
{
    float myState[12], otherState[12], POITarget[3], att[3], toEarth[3], zero[3], memPack[3], earth[3];
    int time, stageTime, flareTime, zone, flareCtr, numFlares, numPics, memSize, lastPics, targZone, targID, timeToPoi;
    int side;
    bool picsTaken[3], off, flareActive, shadowed, foundTarget;
    float radius;
} j;

void init()
{
    memset(&j, 0, sizeof(j));
    //j.memPack[0] = -0.5f;
    //j.maxAccel = .00375f;
    j.earth[0] = .64f;
    //j.other = -1;
    //j.pickedUpItem = true;
    api.setPosGains(0.25f, 0.0f, 2.5f);
    api.setAttGains(0.8f, 0.0f, 8.0f);
}
//End page aInit
//Begin page bUtilities
//Two POI Changes: none
void mathVecScale(float *save, float *v, float mag, bool norm)
{
    memcpy(save, v, 3*sizeof(float));
    if(norm) mathVecNormalize(save, 3);
    for(int i = 0; i < 3; ++i) save[i] *= mag;
}
bool notNan(float vec[3])
{
    for(int i = 0; i < 3; i++)
    {
        if(!(vec[i] < 1000))
        return false;
    }
    return true;
}
float mathVecDistance(float vec1[3], float vec2[3])
{
    float diffvec[3];
    mathVecSubtract(diffvec, vec1, vec2, 3);
    return mathVecMagnitude(diffvec, 3);
}
//End page bUtilities
//Begin page cMovement
//Two POI Changes: None
void getAround(float target[3], int recParam)
{
    
    //DEBUG(("\n Magnitude of Target = %f", mathVecMagnitude(target,3)));
    float centerVec[3];
    float radialVec[3];
    float targVec[3];
    float res[3];
    float scalar;
    mathVecSubtract(targVec, target, j.myState, 3);
    mathVecScale(centerVec, j.myState, -1.0f, false);
    mathVecNormalize(targVec,3);
    scalar = mathVecInner(targVec, centerVec, 3);
    mathVecScale(targVec, targVec, scalar, false);
    mathVecSubtract(radialVec, targVec, centerVec, 3);
    //DEBUG(("\n Magnitude of radialVec", mathVecMagnitude(radialVec,3)));
    if(scalar <= 0.0f || mathVecMagnitude(radialVec,3) > 0.33f)
    {
        memcpy(res,target,sizeof(float)*3);
    }
    else
    {
       mathVecScale(res, radialVec, 0.48f, true);
       //memcpy(res,radialVec,sizeof(float)*3); 
    }
    //DEBUG(("\n Magnitude of RES = %f", mathVecMagnitude(res,3)));
    if(!recParam && j.radius < 0.36f)
    {
        getAround(res, 1);
    }
    else
    {
        if(notNan(res))
        api.setPositionTarget(res);
    }
}


void goShadow()
{
    float shadowPoint[3] = {0.4f, 0.0f, 0.0f};
    game.takePic(j.targID);
    if(j.myState[0] > 0.05f) memcpy(j.att, j.toEarth, 3*sizeof(float));
    game.uploadPic();
    //memset(shadowPoint+1, 0, 2*sizeof(float));
    if(j.otherState[0]>0.3f && j.otherState[0]<0.64f)//if enemy is paralel with the shadow
    {

        if(isInShadow(j.otherState) && !j.shadowed)
        {
            shadowPoint[0] = 0.24f;
            for(int i = 1; i < 3; i++)
                shadowPoint[i] = 0.185f*(j.myState[i])/fabsf(j.myState[i]);
            if(j.otherState[0] > .45f)
                shadowPoint[0] = .33f;
            else
                shadowPoint[0] = j.otherState[0] + .15f;
        }
        else
        {   
            shadowPoint[0]=(j.otherState[0]+shadowPoint[0])/2.0f;
        }
    }
    getAround(shadowPoint, 0);
}
void goUpload()
{
    memcpy(j.att, j.toEarth, 3*sizeof(float));
    if(!seeEarth())
    {
        getAround(j.earth, 0);
    }
    else
    {
        api.setVelocityTarget(j.zero);
        if(faceEarth())
        {
            game.uploadPic();
        }
    }
}
//End page cMovement
//Begin page dRotation
//Two POI Changes: None
float angle(float v1[3], float v2[3])
{
    float angle = mathVecInner(v1, v2, 3)/(mathVecMagnitude(v1, 3) * mathVecMagnitude(v2, 3));
    angle = acosf(angle);
    if(!(angle < 1000))
        angle = 0.0f;
    return angle;
}

//End page dRotation
//Begin page eLogic
//Two POI Changes:
//Lines 24 through 33 are different
//Line 43 sets sizeof(bool)*3 rather than sizeof(bool)*6
void stateChecks()
{
    api.getMyZRState(j.myState);
    api.getOtherZRState(j.otherState);
    j.side = j.myState[1] < 0.0f;
    
    
    mathVecSubtract(j.toEarth, j.earth, j.myState, 3);
    j.shadowed = isInShadow(j.myState);
    
    j.numPics = game.getMemoryFilled();
    j.memSize = game.getMemorySize();
    if(j.numPics > j.lastPics)
    {
        j.picsTaken[j.targID] = true;
        if(j.zone == 0)
        {
            j.targZone = 1;
            mathVecScale(j.POITarget, j.POITarget, .52f, true);
        }
    }
    
    j.stageTime = j.time%60;
    j.flareTime = game.getNextFlare();
    
    if(j.stageTime < 2)
    {
        memset(j.picsTaken, 0, sizeof(bool) * 3);
        bestPOI(false);
    }
    
    
    if(j.flareTime < 0) j.flareTime = 31;
    if(j.flareTime == 1)
  {
      j.flareCtr = 4;
      j.numFlares++;
  }
  if(j.flareCtr > 0)
  {
      j.flareActive = true;
      j.flareCtr--;
  }
  else j.flareActive = false;
  if(j.flareActive) j.flareTime = -1;
  
  j.radius = mathVecMagnitude(j.myState, 3);
  j.zone = -1;
    float lim;
    for(lim = 0.31f; j.radius >= lim; lim += 0.11f)
        j.zone++;
    j.lastPics = j.numPics;
    DEBUG(("flares %d", j.numFlares));
}

bool isInShadow(float pos[3])
{
    return ((fabsf(pos[2]) < .2f) && (fabsf(pos[1]) < .2f) && ((pos[0] > 0.0f)));
}

bool seeEarth()
{
    float radius;
    if(j.myState[0] < 0.0f)
    {
        radius = -j.myState[0] * 0.33f + 0.22f;
        return j.myState[1]*j.myState[1] + j.myState[2]*j.myState[2] > radius*radius;
    }
    return true;
}
bool faceEarth()
{
    float tmp[3];
    mathVecSubtract(tmp, j.earth, j.myState, 3);
    return (angle(tmp, &j.myState[6]) <= .25);
}
//End page eLogic
//Begin page fPredictor
//Two POI Changes:
//Get rid of the targZone for loop in bestPOI()
//set targZone equal to zero, like in line 10, in bestPOI()
//on line 17 change timeToPoi to (timeToPoi - 5) in bestPOI()
void bestPOI(bool subOptimal)
{
    float fPoint[3];
    j.foundTarget = false;
    j.targZone = j.memSize < 2;
    
    for(j.timeToPoi=0; j.timeToPoi < (60 - j.stageTime); j.timeToPoi++)
    {
        for(j.targID = 0; j.targID < 2; ++j.targID)
        {
            
            DEBUG(("\npoi ID = %d\n", j.targID));
            
            futurePOIs(fPoint, j.targID, j.timeToPoi);
            mathVecScale(fPoint, fPoint, 0.41f + j.targZone*0.04f, true);
            DEBUG(("\ndistance to poi = %f\n", mathVecDistance(j.myState, fPoint)));
            if ((1066.66f*mathVecDistance(fPoint, j.myState)) < ((j.timeToPoi-3) * (j.timeToPoi-3)) 
               && (!j.picsTaken[j.targID]) 
               && (subOptimal || ((fPoint[2] < 0.33f) 
               && (fPoint[0] > -0.38f || j.numFlares > 0)))
               && (fPoint[0] < -0.1f) 
               && (j.time > 25 || j.targID == j.side)
               )
                {
                    memcpy(j.POITarget, fPoint, sizeof(float)*3);
                    j.foundTarget = true;
                    //j.other = -1;
                    return;
                }
        }
    }
    if(!subOptimal) bestPOI(true);
}

void futurePOIs(float res[3], int id, int steps)
{
    float POILoc[3];
    float angle;
    float radius;
    game.getPOILoc(POILoc, id);
    angle = atan2f(POILoc[2],POILoc[0]);
    radius = sqrtf(POILoc[2]*POILoc[2] + POILoc[0]*POILoc[0]);
    angle += 0.1f * steps;
    if((angle < 1.5708f) && (angle > -1.5708f))
    {
        angle += 3.1415f;
    }
    res[0] = radius*cosf(angle);
    res[1] = POILoc[1];
    res[2] = radius*sinf(angle);
}
//End page fPredictor
//Begin page main
void mainFunction()
{
    if(game.getFuelRemaining() == 0.0f || !j.foundTarget)
    {
        game.takePic(j.targID);
    }
    if(j.memSize == 0)
    {
        game.takePic(j.targID);
        getAround(j.otherState, 0);
    }
    float POI[3];
    float POILoc[3];
    float tmp[3];
    if((j.timeToPoi < -2) || !j.foundTarget)
    {
        bestPOI(false);
    }
    // futurePOIs(POI, j.targID, 1);
    game.getPOILoc(POILoc, j.targID);
    
    
    if(j.timeToPoi > 2)
    {
        futurePOIs(POI, j.targID, j.timeToPoi-4);
    }
    else
    {
        futurePOIs(POI, j.targID, 2);
    }
    
    mathVecSubtract(j.att, POI, j.myState, 3);
    // if(angle(j.myState, POI) > 1.6f && j.myState[2] > 0.0f)
    // {
    //     futurePOIs(POI, j.targID, (j.timeToPoi - 2));
    // }
    // mathVecSubtract(j.att, POI, j.myState , 3);
    mathVecSubtract(tmp, j.myState, POILoc, 3);
    
    if((j.time >= 165)  && (j.numPics > 0))
    {
        j.flareTime = 10;// we treat the end of the round as a solar flare
    }
    if(j.flareActive && !j.shadowed)
    {
        game.turnOff();
        j.off = true;
    }
    if(game.getFuelRemaining() < 15.0f && j.numFlares < 2)
    {
      j.flareTime = 10;
    }
    if(j.off && j.flareTime > 5 )
    {
        game.turnOn();
        bestPOI(false);
        j.off = false;
    }
    else
    {
        if(j.flareTime < 31 && j.numFlares<2)
        {
            goShadow();
        }
        else
        {
            if((j.numPics < j.memSize))
            {
                getAround(j.POITarget, 0);
                if(game.alignLine(j.targID) && j.zone == j.targZone)
                {
                    if(angle(tmp, POILoc) < (2-j.targZone)*.4f)
                    {
                        DEBUG(("\ntaking Picture\n"));
                        game.takePic(j.targID);
                    }
                }
            }
        }
    }
    if(j.numPics == j.memSize || (j.flareTime < 16 && j.numPics > 0))
    {
        goUpload();
    }
    

    
    if(notNan(j.att))
    api.setAttitudeTarget(j.att);
    j.timeToPoi--;
    //DEBUG(("\ntime To POI... might lie here... = %d\n", j.timeToPoi));
    
}
void loop()
{
    stateChecks();
    mainFunction();
    j.time++;
}
//End page main


};

ZRUser *zruser795 = new ZRUser01;

#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 ZRUser04 : public ZRUser
{

//Begin page main
//Begin page Facepos
//Begin page Setting
void Distanza(float state[12],float Item[3])
{
  float dif[3];
  mathVecSubtract(dif,state,Item,3);
  dist=mathVecMagnitude(dif,3); 
}
//End page Setting

//Begin page facePOS()
void Facepos(float topoint[3],float frompoint[3]){ 
 float attTarget[3];
 mathVecSubtract(attTarget,topoint,frompoint,3);
 mathVecNormalize(attTarget,3);
 api.setAttitudeTarget(attTarget);
 DEBUG(("\n attTargetX=%f  attTargetY=%f\n",attTarget[0],attTarget[1]));
}


//End page Facepos
//Begin page Localize
void localize()
{
  if(timer%60==0)
      {
       flagouter=true;
      }
   if(flagouter){
       M=2.2f;
   }
   else{M=2.f;}
   game.getPOILoc(Goal,IDPOI);
   Goal[0]=-M* sqrtf(0.04-Goal[1]*Goal[1]);
   Goal[1]*=M;
   Goal[2]=0; 
   timeflare=13;
   if((0.64-Goal[0])<2.7f*Goal[1]*lato)
    {
        timeflare=5;
    }
    else{
        if(Goal[0]>-1.5f*Goal[1]*lato) timeflare=9;
    }
   DEBUG(("tempo=%d  memoria=%d  valid=%d  flare=%d\n",timer,memory,validPicture,flare));
   DEBUG(("\n GOALX=%f  GOALY=%f\n",Goal[0],Goal[1]));
}

//End page Localize
//Begin page Stateloop
//Begin page LOOP();
void StateLoop()
{
  memory=game.getMemorySize();
  fuel=game.getFuelRemaining();
  timer=api.getTime();
  api.getMyZRState(mystate);
  api.getOtherZRState(youstate);
  validPicture=game.getMemoryFilled();
  LastScore=game.getScore();
  flare=game.getNextFlare();
  scarico[0]=-0.28f;
  scarico[2]=0.f;
  scarico[1]=0.34f;
  IDPOI=0; 
  lato=1;
  if(mystate[1]<0){  scarico[1]=-0.34f; IDPOI=1; lato=-1;}

}
//End page LOOP();
//End page Stateloop
//Begin page Take

void addMemory()
{ //float Spin[3]={0.766f,-0.643f,0.0f};//{0.84f,-0.55f,0.0f};
  float ferma[3]={0.0f,1.0f,0.0f};
  float Path[3]={-0.48f,0.58f,0.f}; 
  if(mystate[1]<0.f)
    {
      Path[1]=-0.58f;
      ferma[1]=-1.0f;
    }
  Distanza(mystate,Path);
  if(dist<0.02f && mathVecMagnitude(&mystate[3],3)<0.09f)
  {
      Facepos(Origin,Path);
      api.setPositionTarget(Path);   
  }
  else
  { 
    api.setAttitudeTarget(ferma);
    movetostop(Path,55.0f,0.04f);
  }
   game.takePic(IDPOI); 
}

//Begin page takePhoto()
void TakePhoto()
{
   if(game.alignLine(IDPOI))
     {
         game.takePic(IDPOI);
     }
   NowScore=game.getScore();
   if(LastScore-NowScore<(-0.10f))
   {flagouter=!flagouter;}
}
//End page takePhoto()
//End page Take
//Begin page help
bool help(){
    Distanza(mystate,Origin);
    if(dist<0.35)
    {
        for(int i=0;i<3;i++)
        {
            mystate[i]*=2.f;
        }
        api.setPositionTarget(mystate);
        Facepos(Origin,mystate);
       return true;
       DEBUG(("***************HELP \n"));
    }
    Distanza(mystate,youstate);
    if(dist<0.35f )
     {
    //scarico[1]=0.27f;     
    movetostop(Goal,55.0f,0.08f);
           DEBUG(("**************HELP 222222 \n"));
     return true;
     }   
    
    return false;
}
 /* 
*/
//End page help
//Begin page main
float mystate[12];
float attitude[3];
float force[3];
float dist;
float Goal[3];
float copyY;
float Earth[3];
float youstate[12];
float OppostMT[3];
float scarico[3];
float LastScore;
float NowScore;
int memory;
int fuel;
int lato;
int IDPOI;
int validPicture;
int LiveMemory;
int flare;
int timer;
int timeflare;
float Path[3];
bool canCalculateLocalize;
float raggio;
float CentralPoi[3];
float DistanceZeta[3];
float Gira[3];
float Mira[3];
int cases;
int flagStrategy;
int flagmemory;
float Origin[3];
int flagflare;
bool flagouter;
float M;

void init(){
Earth[2]=Earth[1]=Origin[0]=Origin[1]=Origin[2]=CentralPoi[2]=DistanceZeta[0]=cases=CentralPoi[1]=flagmemory=.0f;
Earth[0]=0.64f;
Gira[0]=0.f;
Gira[1]=0.f;
Gira[2]=0.5f;
CentralPoi[0]=-0.44f;
flagStrategy=0.f;
Mira[0]=-0.19f;
Mira[1]=0.f;
Mira[2]=0.056f;
M=2.5f;
flagouter=true;
flagflare=0;

}

void loop(){

    StateLoop();  
    localize();
    if(flare==1){ game.turnOff();}
    if(flare==0){ game.turnOn();}
    if(flare==0){ flagflare+=1;}
    
if(!help())   //!(  ( (timer>(180-timeflare))||(flare!=-1 && flare<=timeflare) ) && (validPicture!=0)  ) 

 if(
     (memory!=validPicture) && 
     !(  ( (flare!=-1 && flare<=timeflare) ) && (validPicture!=0)  ) 
    ) 
  {
      DEBUG(("1"));
    switch(cases)
      {
        case 0:   if(memory!=3)
                          {
                            addMemory();
                            break;
                          }
            
        case 1: cases=1;
                Facepos(Origin,Goal);
                TakePhoto();
                //api.setPositionTarget(CentralPoi);
                movetostop(Goal,60.0f,0.08f);  
             break;
      }

 }
 else
 {
     DEBUG(("2"));
   Upload();  
 }
}

//End page main





//End page main
//Begin page movetostop
//Begin page movetostop
void movetostop(float stop[3],float d, float v) {
float diff[3];
float vel;
 vel=mathVecMagnitude(&mystate[3],3);
 Distanza(mystate,stop); 
 if(dist>0.04f && dist>(vel*d+2)*vel) {
    mathVecSubtract(diff,stop,mystate,3);
    mathVecNormalize(diff,3);
    for(int i = 0; i < 3; i ++) {
  diff[i] *= v;
    }
    api.setVelocityTarget(diff);   
 }else
 {
    api.setPositionTarget(stop);
 }
}
//End page movetostop
//End page movetostop
//Begin page upload
//Begin page upload
void Upload()
{
    //if(mystate[0]<=2.7f*mystate[1])
    if((0.64-mystate[0])>=2.7f*mystate[1]*lato)
    {/* float S=2.75f / M;
      scarico[0]=S * Goal[0];
      scarico[2]=0.f;
      scarico[1]=S * Goal[1];  */
    movetostop(scarico,60.0f,0.08f);   
    }
    Facepos(Earth,mystate); 
    //api.setPositionTarget(scarico);  
 
    game.uploadPic(); 
    DEBUG(("\n scaricoX=%f  scaricoY=%f\n",scarico[0],scarico[1]));
}
//End page upload
//End page upload
//End page main


};

ZRUser *zruser803 = new ZRUser04;

#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 ZRUser02 : public ZRUser
{

//Begin page geometry
// ================================================================================================

float getAngleBetween(float pointA[3], float secondPointA[3], float pointB[3], float secondPointB[3])
{
    float vectorA[3];
    float vectorB[3];

    mathVecSubtract(vectorA,pointA,secondPointA,3);     // Vector from A --> secondA 
    mathVecSubtract(vectorB,pointB,secondPointB,3);     // Vector from B --> SecondB 

    return getAngleBetweenVector(vectorA, vectorB);
}

// ================================================================================================

// same thing but shorter -Christopher
float getAngleBetweenVector(float vectorA[3], float vectorB[3])
{
    return acosf(mathVecInner(vectorA, vectorB, 3) / (mathVecMagnitude(vectorA, 3) * mathVecMagnitude(vectorB, 3)));
}


// Note: this function breaks if you give it points within asteroid_radius of the origin
bool line_of_sight_between(float x[3], float y[3])
{
    float v[3];
    mathVecSubtract(v, y, x, 3);
    

    //float d = sqrtf(mathVecMagnitude(x,3)*mathVecMagnitude(x,3) - asteroid_radius*asteroid_radius);

    if (mathVecMagnitude(v, 3) < sqrtf(mathSquare(mathVecMagnitude(x, 3)) - mathSquare(0.33f)))
        return true;

    float cross[3];
    mathVecCross(cross, x, v);
    
    return ((mathVecMagnitude(cross, 3) / mathVecMagnitude(v, 3)) > 0.33f);
}

void calculate_intermediate_point(float ret[3], float a[3], float b[3])
{
    //float radius = 0.33f;
    float zero[3] = {0, 0, 0};
    // I'm defining a right-hand rule system.
    // i is the unit vector perpendicular to j and k given by the right-hand rule (j x k)
    // j is the unit vector in the direction from the SPHERE to the origin
    // k is the unit normal vector to the plane we're in
    float j[3];
    mathVecSubtract(j, zero, a, 3);
    mathVecNormalize(j, 3);
    
    float k[3];
    float ab[3];
    mathVecSubtract(ab, b, a, 3);
    mathVecCross(k, ab, j);
    mathVecNormalize(k, 3);
    
    float i[3];
    mathVecCross(i, j, k);
    
    // debug_vector("i", i);
    // debug_vector("j", j);
    // debug_vector("k", k);
    
    // So vectors defined in this system will have variable names starting with rh_
    
    // Let O be the origin (same in both systems)
    // P be sphere
    // float rh_P[3];
    // rh_P[0] = 0.0f;
    // rh_P[1] = -mathVecMagnitude(a, 3);
    // rh_P[2] = 0.0f;
    // I be point we're trying to find
    
    // theta is angle between OP and OI
    // Since PIO is a right angle, cos(theta) = |OI|/|OP|
    // |OI| is radius, and |OP| is just |P|, thus
    // float theta = acosf(radius / mathVecMagnitude(rh_P, 3));
    float theta = acosf(0.33f / mathVecMagnitude(a, 3));
    
    // // Use pythagorean thm to get magnitude of PI
    // // |OP|^2 = |PI|^2 + |OI|^2
    // // thus |PI| = sqrt(|OP|^2 - |OI|^2)
    // if (mathSquare(mathVecMagnitude(rh_P, 3)) - mathSquare(radius) < 0)
    //     DEBUG(("WWWWWWWWTTTTTTTTTTTTFFFFFFFFF???????????"));
    // float rh_PI_mag = sqrtf(mathSquare(mathVecMagnitude(rh_P, 3)) - mathSquare(radius));
    
    // Magnitude of OI is just the radius
    // Use magnitude and angle to find vector (we know it's in rh x-y plane)
    // Draw out the triangle if you have to
    float rh_OI[3];
    rh_OI[0] = 0.33f * sinf(theta);
    rh_OI[1] = 0.33f * -cosf(theta);
    rh_OI[2] = 0.0f;

    // I - O = OI
    // Since O is origin, I = OI

    // Convert back to original ZR system, and add a little buffer room.
    float buffer_scale = 1.1f;
    ret[0] = ((rh_OI[0] * i[0]) + (rh_OI[1] * j[0]) + (rh_OI[2] * k[0])) * buffer_scale;
    ret[1] = ((rh_OI[0] * i[1]) + (rh_OI[1] * j[1]) + (rh_OI[2] * k[1])) * buffer_scale;
    ret[2] = ((rh_OI[0] * i[2]) + (rh_OI[1] * j[2]) + (rh_OI[2] * k[2])) * buffer_scale;
    
    // If I screwed this up I'm gonna kill someone.
    // EDIT: Someone is officially going to die.
    // EDIT2: Fixed, people can live now.
    
}

void adjusted_target_pos(float ret[3])
{
    if (line_of_sight_between(position, target_position))
    {
        memcpy(ret, target_position, sizeof(float) * 3);
    }
    else
    {
        calculate_intermediate_point(ret, position, target_position);
    }
}

float adjusted_distance_to_point(float x[3])
{
    
    if (line_of_sight_between(position, x))
    {
        return vector_distance(position, x);
    }
    else
    {
        float real[3];
        calculate_intermediate_point(real, position, x);

        float fake[3];
        calculate_intermediate_point(fake, x, position);
        
        return ((getAngleBetweenVector(real, fake) * 0.33f) + 
                vector_distance(position, real) + vector_distance(x, fake));
    }
}
//End page geometry
//Begin page main
float target_position[3];
// target_attitude moved inside main

bool POI_status[3][2];

void init()
{
    //center_of_asteroid[0] = center_of_asteroid[1] = center_of_asteroid[2] =
    //shadow_zone_place[1] = shadow_zone_place[2] =
    //Earth_loc[1]= Earth_loc[2]=
    time_to_active_cam= 0;
    
    isOuter = true;
    
    //Earth_loc[0]=0.64f;
    
    // Distance to (0,0,0) must be > 0.33 for shadow_zone_place
    // shadow_zone_place[0] = 0.4f;
    
    timeZR = target_POI = target_time = -1;
    
}

void loop()
{
    float target_attitude[3];
    float zero[3] = {0, 0, 0};
    float shadow_zone_place[3] = {0.4f, 0, 0};
    float Earth_loc[3] = {0.64f, 0, 0};
    // Keep this at the beginning of the loop.
    timeZR++;

    float state[12];
    api.getMyZRState(state);
    position = state;
    velocity = state + 3;
    attitude = state + 6;
    time_to_active_cam--;
    
    int taken_pic = game.getMemoryFilled();
    
    float max_velocity = 0.035f;
    
    
    
    
    // I thought we were losing 1 second of movement per taken picture due the function to take the pictures is at the end of loop
    //      Now it's at the top !
    
    // PS We should try to make the choice of target_POI and target_time preferring the id of the POI
    //      in which we have to take a picture from the outer zone..
    //      something like subtract 5 seconds if that's true (and add it again, later);
    
    // PPS We should also avoid to go to the POI if that will respawn (each 60s)..
    //      so  we can stay put or move to a strategic position (to define)
    
    // PPPS Try to reduce code-size if u have time :)
    
    // PPPPS Don't kill me for so many posts xD
    
    //  - Marco
    
    // Marco we're obviously going to try to kill you over how many posts you make
    
    // PS yeah code size is a bit of a problem
    
    // PPS Don't kill me for not contributing at all to this project
    
    // - Sam
    
    
    
    
    for(int i=0; i<3; i++)
    {
        //if(game.alignLine(i)) 
        //{
            float POI[3];
            game.getPOILoc(POI, i);
            float fromPOI[3];
            mathVecSubtract(fromPOI, position, POI, 3);
            
            float cross[3];
            mathVecCross(cross, fromPOI, attitude);
            if      ((mathVecMagnitude(cross, 3) < 0.05f) && mathVecInner(fromPOI, attitude, 2) < 0)
            //if(getAngleBetweenVector(toPOI, attitude)<0.25f)    // The new tolerance..
            {
             
                float myRadius = mathVecMagnitude(position, 3);
                bool zone = false;  // false for outer, true for inner;
                float anglePosition = getAngleBetweenVector(fromPOI, POI);
                
                
                
                // PLEASE NOTE THAT I DON'T KNOW IF ALSO THESE TOLERANCES HAVE BEEN INCREASED OF 0.15%
                // DON'T WORRY I INCREASED THEM -CHRIS
                
                
                //  supposed to be only one = (not ==)
                //        v
                if(((myRadius<0.53f && !POI_status[i][0] && anglePosition < 0.46f) ||
                    (zone = (myRadius<0.42f && !POI_status[i][1] && anglePosition < 0.92f)))
                   && time_to_active_cam<1)
                {
                    game.takePic(i);
                    time_to_active_cam = 3;     //Changed after the bug deployment 28/12/2014 -Marco
                    //DEBUG(("\n \n POI ID: %d, radius: %f anglePos %f \n Magnitude_vec_cross: %f Vec_inner: %f\n \n", i, myRadius, anglePosition,mathVecMagnitude(cross, 3), mathVecInner(fromPOI, attitude, 2) ));    
                    
                    if(taken_pic != game.getMemoryFilled())
                    {
                        POI_status[i][zone] = true;
                    }
                    target_POI = -1; // reset POI and time
                }
            }
        //}
    }
    
    if((timeZR % 60) == 0)
    {
        // reset POI every 60 seconds
        target_POI = -1;
        for (int i = 0; i < 3; i++)
            memset(POI_status[i], false, 3 * sizeof(bool));
    }
    
    // start loop stuff
  
  // timeZR to next solar flare
  int nextF = game.getNextFlare();
  //DEBUG(("\nTime to Flare: %d", nextF));
  // a point closer to us should be selected to be more fancy
  // it'll give us more time to take pictures
  //DEBUG(("next %i time %f\n", nextF, time_to_point(shadow_zone_place)));
  
  // pick a POI and go to it
  
  //  TRYING TO MAKE A MORE ACCURATE ESTIMATE, SECOND BY SECOND CONTROL UNTIL WE ARE VERY NEAR TO THE POI (4s FOR NOW)
  //  -MARCO
  //  NOT WORKING FOR NOW (Caused by getPerfectTimeToPOI)
  //  UP: WORKING NOW -MARCO
  float target_time_less_Time = target_time - timeZR;
  if ((target_POI == -1) || (timeZR > target_time + 1) || target_time_less_Time > 5)              //  (count<4))
  {
      target_time = 10000;
      target_POI = 0;
      for (int id = 0; id < 3; id++)
      {
          float POI_pos[3];
          game.getPOILoc(POI_pos, id);
          int POI_time = ALTgetPerfectTimeToPOI(POI_pos, !POI_status[id][0]);
          
          if ((POI_time < target_time) && !(POI_status[id][0] && POI_status[id][1]))
          {
              target_time = POI_time;
              target_POI = id;
          }
      }
      target_time += timeZR;
      isOuter = !POI_status[target_POI][0];
      
      //DEBUG(("\n CALCULATING THE TIME: %d, Target:%d", timeZR+4, target_time));
  }
  target_time_less_Time = target_time - timeZR;
  
  DEBUG(("\nID: %d, target time: %i", target_POI, target_time));
  
  float init_location[3];
  game.getPOILoc(init_location, target_POI);
  float final_location[3];
  getPoiPosAfterTime(final_location, init_location, target_time_less_Time);
  
  //if( (target_time>60 && timeZR<60) || (target_time>120 && timeZR<120) )
  //    final_location[0]=10;
  
  //align_with_POI_loc(final_location, isOuter);
  // Attitude
  
    if(target_time_less_Time < 3)
  {
      float position_after_1[3];
      for(int i = 0; i<3; i++)
        {
            position_after_1[i] = position[i] + velocity[i];
        }
      mathVecSubtract(target_attitude, final_location, position_after_1, 3);
  }
  else
  {
        mathVecSubtract(target_attitude, zero, final_location, 3);
  }
    
    
    // Position
    memcpy(target_position, final_location, 3 * sizeof(float));
    scale_vector(target_position, isOuter ? 0.43f : 0.39f);       //0.51f : 0.38f)

  
  // ===================
  
  int time_to_shadow_less_nextF   = time_to_point(shadow_zone_place) - nextF;
  
  
  DEBUG(("\n timeZR to shadow zone: %f,\n timeZR to next flare: %d, \n timeZR to shadow less nextFlare: %d, \n ID: %d.",time_to_point(shadow_zone_place), nextF, time_to_shadow_less_nextF, target_POI ));
  // This must go after so that we know target_position
  if      (
                  (nextF != -1 && time_to_shadow_less_nextF > 0)
          ||      (taken_pic == game.getMemorySize())
          ||      ((target_time_less_Time)*2 > nextF && nextF!= -1)
          ||      (taken_pic && (( timeZR>165 && target_time_less_Time > 3) || (game.getFuelRemaining() < 5)))
          )
  // Moreover, we should stay in the shadow zone if we haven't time enough to reach the POI and then come back !
  // UPDATE: I added a prototype of this function
  {
        memcpy(target_position, shadow_zone_place, 3 * sizeof(float));
        
        if(time_to_shadow_less_nextF < -2)
        {
            DEBUG(("\n INCREASED VELOCITY"));
            max_velocity = 0.05f;
        }
        
        
        if(taken_pic) // taken_pic > 0
        {
            mathVecSubtract(target_attitude, Earth_loc, position, 3);
            game.uploadPic();
        }
        else
        {
            game.takePic(0);
            target_attitude[0]=10;      // Just an impossible number to be assigned in diffent way
        }
            
        if(((mathVecMagnitude(position+1, 2) < 0.18f) && (position[0] > 0)))
        // It's better 0.18 than 0.2 cause the other sphere could try to put our sphere out !
        
            target_position[0]=10;      // Just an impossible number to be assigned in diffent way
  }
  
//  DEBUG(("%i", min_time));
    
    // if ((target_time_less_Time) > 6)
    // {
    //     game.takePic(0); // worth .01 points each
    //     time_to_active_cam = 4;
    // }
    
    
  // Keep this at the end of the loop.
  // ============================= MOVEMENT ==========================================
  
  // =============== POSITION
    float adjusted_target_position[3];
    float radius = 0.33f;
    
    if(target_position[0]==10)
    {
        api.setVelocityTarget(zero);
    }
    else
    {
        adjusted_target_pos(adjusted_target_position);
        
        // DEBUG(("D: %f\n", (adjusted_distance_to_point(target_position) * 0.06f)));
        // DEBUG(("V: %f\n", mathVecMagnitude(velocity, 3)));
        // debug_vector("vel", velocity);
    if(mathVecMagnitude(position, 3) < radius)
    {
        
        DEBUG(("WARNING: INSIDE RADIUS\n"));
        scale_vector(position, 100);
        api.setVelocityTarget(position);
        
    } else if ((mathVecMagnitude(velocity, 3) > sqrtf(adjusted_distance_to_point(target_position) * 0.008f)) 
            || (adjusted_distance_to_point(target_position) < 0.015f)) 
        {
            
            //DEBUG(("position target\n"));
            api.setPositionTarget(adjusted_target_position);
            
        } else {
            
            //DEBUG(("velocity target\n"));
            
            float target_velocity[3];
            mathVecSubtract(target_velocity, adjusted_target_position, position, 3);
            mathVecNormalize(target_velocity, 3);
    
            scale_vector(target_velocity, max_velocity);
            
            api.setVelocityTarget(target_velocity);
            
        }
    }
   
    
    // =============== ATTITUDE
    if(target_attitude[0] != 10)
        api.setAttitudeTarget(target_attitude);
 
}
//End page main
//Begin page misc
// bool in_range(float x, float min, float max)
// {
//     return ((x <= max) && (x >= min));
// }
//End page misc
//Begin page movement


// POI inner outer
// whether we have taken the picture at inner or outer

/*
void align_with_POI_loc(float POI_location[3], bool outer)
{
    // Attitude
    mathVecSubtract(target_attitude, center_of_asteroid, POI_location, 3);
    
    // Position
    memcpy(target_position, POI_location, 3 * sizeof(float));
    // Make POI_location a unit vector so we can multiply and stuff.
    mathVecNormalize(target_position, 3);
    scale_vector(target_position, outer ? 0.43f : 0.39f);       //0.51f : 0.38f)
    
}
*/

// void reset_POI_status()
// {
//     for (int i = 0; i < 3; i++)
//         for (int j = 0; j < 2; j++)
//             POI_status[i][j] = false;
// }
//End page movement
//Begin page state
float *position, *velocity, *attitude;
bool isOuter;

int target_POI, target_time, time_to_active_cam, timeZR;     // Used to take picture and calculate the min position where we can reach the POI


// void update_state()
// {
//     float state[12];
    
//     timeZR++;

//     api.getMyZRState(state);
//     memcpy(position, state, 3 * sizeof(float));
//     memcpy(velocity, state + 3, 3 * sizeof(float));
//     memcpy(attitude, state + 6, 3 * sizeof(float));
//     time_to_active_cam--;
    
//     if((timeZR % 60) == 0)
//     {
//         // reset POI every 60 seconds
//         target_POI = -1;
//         reset_POI_status();
//     }
// }

// bool in_shadow_zone(float point[3])
// {
//     /*
//       Shadow zone dimensions:
//       0.0 x 0.64
//       -0.2 x 0.2
//       -0.2 x 0.2
//     */
//     return (in_range(point[0], 0.0f, 0.64f) && in_range(point[1], -0.2f, 0.2f) && in_range(point[2], -0.2f, 0.2f));
// }

// void sub_update_state(int i, float array[3], float state[12])
// {
//     for(int j = 0; j++; j < 3)
//         array[j] = state[i + j];
// }

// float distance_from_surface()
// {
//     // Asteroid radius is 0.2
//     return distance_from_center() - 0.2f;
// }

// float distance_to_target_position()
// {
//     return vector_distance(position, target_position);
// }
//End page state
//Begin page timing
// time_to_point updated by Christopher 2014-11-28
// basically it is the result of random guessing and tweaking the constants
float time_to_point(float x[3])
{
    float total = 0.0f;
    
    float target[3];
    adjusted_target_pos(target);
    float dir[3];
    mathVecSubtract(dir, target, position, 3);
    mathVecNormalize(dir, 3);
    float vel = mathVecInner(dir, velocity, 3);
    //DEBUG(("vel %f\n", vel));
    float dv = 0.034f - vel;
    
    if (vel < 0.034f)
    {
        total += dv * ((adjusted_distance_to_point(x) < 0.1f) ? -100 : 108);
    }
    
    return total + (adjusted_distance_to_point(x) / 0.0342f) + 3.5f;
}

// Changes by Marco 10/12..
// timeToArrive can't be lower than time_to_active_cam

// Changes by Christopher 29/11 14:50 UTC
// time_to_point was using initialPOI instead of posPOI
// adjusted depending on inner/outer zone

// ADDED BY MARCO 27/11 - 20.50 UTC
// START 

//  It returns the ideal time to reach the POI..
/*int getPerfectTimeToPOI(float initialPOI[3], bool outer)
{
    int timeToArrive = 0, timeRotPOI = 0;
    float posPOI[3];
    int i = 0;
    do
    {
        timeRotPOI = timeToArrive;
        getPoiPosAfterTime(posPOI, initialPOI, timeRotPOI);
        
        scale_vector(posPOI, outer ? 0.43f : 0.39f);        //0.51f(0.44) : 0.38f
        
        timeToArrive = (int)time_to_point(posPOI) + 1; //ceilf(time_to_point(posPOI))
        i++;
    }
    while (timeToArrive != timeRotPOI && (i < 1000));
    
    if(timeToArrive < time_to_active_cam)
        timeToArrive = time_to_active_cam;
    
    //DEBUG(("\nTIME: %d", timeToArrive));
    //DEBUG(("\n %i", i));
    
    return timeToArrive;
}*/

// Alternate version, hopefully fixed (No Chris, it doesn't work yet -Marco)
// It's working now. -Marco
int ALTgetPerfectTimeToPOI(float initialPOI[3], bool outer)
{   
    int timeToArrive = 1000;
    float posPOI[3];
    
    for(int i = 0; !(timeToArrive < i) || (i<100); i++)
    {
        getPoiPosAfterTime(posPOI, initialPOI, i);
        scale_vector(posPOI, outer ? 0.43f : 0.39f);        //0.51f(0.44) : 0.38f
        timeToArrive = (int)time_to_point(posPOI) + 1; //ceilf(time_to_point(posPOI))
        
    }
    
    if(timeToArrive < time_to_active_cam)
        timeToArrive = time_to_active_cam;
    
    return timeToArrive;
}



// It returns the position of the POI (finalPos) after "dT" seconds.
void getPoiPosAfterTime(float finalPos[3], float startPos[3], float dT )
{
    
    // getRadiusToY
    float Y[3] = {0, startPos[1], 0};
    float radius = vector_distance(startPos, Y);
    //end
    float v[3];
    
    memcpy(v, startPos, 3*sizeof(float));
    mathVecNormalize(v,3);
    
    finalPos[0]=radius*(v[0]*cosf(0.1f*dT)-v[2]*sinf(0.1f*dT));
    finalPos[1]=startPos[1];
    finalPos[2]=radius*(v[0]*sinf(0.1f*dT)+v[2]*cosf(0.1f*dT));
    
    if(finalPos[0]>0)
    {
        // getPoiPosAfterTime(finalPos, startPos, dT+30.4, false);
        finalPos[0] *= -1;
        finalPos[2] *= -1;
    }
    
}

// float getRadiusToY(float item[3])
// {
//     float Y[3] = {0, item[1], 0};
    
//     return vector_distance(item, Y);
// }



//  It returns after how many seconds the POI will be in the bottom side..

short int getTimeToSwap(float pos[3])
{
    short int i = 0;
    float future1[3], future2[3];
    
    do
    {
        i++;
        getPoiPosAfterTime(future1, pos, i);
        getPoiPosAfterTime(future2, pos, i+1);
    }
    while (future1[2] > future2[2]);
    
    return i;
}

//END
//End page timing
//Begin page vector
// void normalize_vector(float vec[3])
// {
//     float mag = sqrtf(mathSquare(vec[0]) + mathSquare(vec[1]) + mathSquare(vec[2]));
//     scale_vector(vec, (1 / mag));
//     // mathVecNormalize(vec, 3);
// }

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

// void debug_vector(char *name, float x[3])
// {
//     DEBUG(("%s: %f %f %f\n", name, x[0], x[1], x[2]));
// }

float vector_distance(float position[3], float other[3])
{
    float d[3];
    mathVecSubtract(d, position, other, 3);
    return mathVecMagnitude(d, 3);
}
//End page vector


};

ZRUser *zruser791 = new ZRUser02;

#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 ZRUser02 : public ZRUser
{

//Begin page POIAfterTime
void POIAfterTime(int timp, float POI[3], float poi[3])
// returns in 'poi' where the 'POI' will be after a number of seconds (or where it was, as it is used in the main program if you enter a negative number of seconds);
// timp = the number of seconds;
{
    float ui,poiProiectie,centru[3],unghi/*,raza*/;
    for (int i=0;i<3;i++){
        poi[i]=POI[i];
    }
    // poi[0]=POI[0];
    // poi[1]=POI[1];
    // poi[2]=POI[2];
    poiProiectie=sqrtf(POI[2]*POI[2]+POI[0]*POI[0]);
    ui=atan2f(POI[0],POI[2]);
    unghi=ui-0.1f*timp;
    centru[0]=centru[2]=0.0f;
    //centru[2]=0.0f;
    centru[1]=POI[1];
    if (unghi<-1*pi) unghi=unghi+pi;
    //raza=getLinearDistance(POI,centru);
    poi[0]=sinf(unghi)*poiProiectie;
    poi[2]=cosf(unghi)*poiProiectie;
}
//End page POIAfterTime
//Begin page POI_Align
void POI_Align(){ // direction of the camera, uses POIAfterTime 
                  // in order to keep the camera always on the POI, it will look 2 or 3 seconds ahead;
    float POIAfter[3];
    POIAfterTime(POILoc[1]==0.0f?3:2,POILoc,POIAfter);
    if(memCount==0 && myState[2]<0)
        POIAfter[2]=-1*fabsf(POIAfter[2]);   
    mathVecSubtract(att,POIAfter,myState,3);
    mathVecNormalize(att,3);
    for (int i=0;i<3;i++)
        aux[i]=tpos[i];
    mathVecNormalize(aux,3);
    mathVecSubtract(att,att,aux,3);
    mathVecNormalize(att,3);
    if(Time<180){
        api.setAttitudeTarget(att);
    }
    DEBUG(("SET ATTITUDE to %f, %f, %f",att[0],att[1],att[2]));
}
//End page POI_Align
//Begin page getAngle
float getAngle(float a[3],float b[3])
{
    return acosf(mathVecInner(a,b,3));// returns the angle between 2 vectors with the same origin;
                                      // they have to be normalized;
}

//End page getAngle
//Begin page getLinearDistance
// returns the distance between 2 points;
float getLinearDistance(float p1[],float p2[])
{
    float VectorBetween[3];
    mathVecSubtract(VectorBetween,p1,p2,3);
    return mathVecMagnitude(VectorBetween,3);
}
//End page getLinearDistance
//Begin page getPOI
void getPOI() // returns the most favorable POI;
{
// float POIqs[3],center[3]={0.0f,0.0f,0.0f},ratio=100.0f;
// for (int i=0;i<3;i++)
//     {
//     game.getPOILoc(POIqs,i);
//     POIAfterTime(myState[0]>0.0f?29:Time<10?20:14,POIqs,POIqs); // if you are in the shadow zone it will see where the POI will be after 29 seconds and check if you have the time to catch it
//     center[1]=POIqs[1];                              // in order not to wait 20+ seconds for the POIto reach the top of the asteroid;
//     if (POIStore[i*2+1]==0 && (POIqs[2]*0.2f/getLinearDistance(POIqs,center)0)))
//         {
//               POIid=i;
//               ratio=POIqs[2]*0.2f/getLinearDistance(POIqs,center); // ratio is used because the POIs move on circles with different radius;
//         }
//     }
if ((POIStore[POIid*2+(Time>60 || fabsf(POILoc[1])<.18f)]==1 && Time%60<40) || POIid==2) POIid=2;
}
//End page getPOI
//Begin page getValue
void getValue(float a[3], float b[3])
{
    for (int i=0;i<3;i++)
        a[i]=b[i];
}
//End page getValue
//Begin page goFast
void goFast(float targetPosition[3]) // movement function;
{
float vectorViteza[3],distance,viteza,vectorBetween[3];
for (int i=0;i<3;i++)
    {
    vectorViteza[i]=myState[i+3];
    }
viteza=mathVecMagnitude(vectorViteza,3);
mathVecSubtract(vectorBetween,targetPosition,myState,3);
distance=mathVecMagnitude(vectorBetween,3);
mathVecNormalize(vectorBetween,3);
for (int i=0;i<3;i++)
    vectorBetween[i]=vectorBetween[i]*100;
if (viteza*1000*viteza0.04f) //Originally was distance>.05f (Just changed back from .07)
api.setVelocityTarget(vectorBetween);
else api.setPositionTarget(targetPosition);
}
//End page goFast
//Begin page main
float POILoc[3],pi,myState[12],otherState[12],mySP,tpos[3],aux[3],rad,att[3],earth[3],RPos[3],PicPoint[3];
int POIid,flare,side,POIStore[6],memCount,memLim,Time,timp,updown,flarenum,fuel;

//mySP = my radius;
//att = camera direction;
//RPos = top position of the POI where x=0;
//side = tells the part from where we start (1 for blue sphere, -1 for red);
//earth = used as origin and the location of the earth for the upload;

void init(){
    for(int i=0;i<6;i++){
        POIStore[i]=0; // a float that helps us know how many pictures were taken from each POI;
                       // where i is from 0 to 2: i*2+1 for the outer pic and i*2+2 for the inner;
    }
    pi=3.141592f;
    flarenum=0;
    api.getMyZRState(myState); 
    side=myState[1]>0?1:-1;
    POIid=myState[1]>0?0:1;
    rad=.4f;
    updown=1;
    aux[0]=aux[1]=aux[2]=earth[0]=earth[1]=earth[2]=0.0f;
}
void loop(){
    api.getMyZRState(myState);
    api.getOtherZRState(otherState);
    fuel=game.getFuelRemaining();
    Time=api.getTime();
    if(Time%60==1){
        for(int i=0;i<6;i++) // done every 60 sec to reset the pic store;  
            POIStore[i]=0;
        if(getLinearDistance(POILoc,myState)>.3f || updown==-1){//just in case the new 2 is really close.
        POIid=side>0?0:1; //gets the ID of the best POI, called after 60 seconds, after going to shadow and after uploading;
        }
    }
    if (Time<=5)
        updown=otherState[2]<0.0f?-1:1;
        // if (otherState[2]<0.0f){ updown=-1;
        //     }else{ updown=1;}
    DEBUG (("\nUpdown: %d\n",updown));
    game.getPOILoc(POILoc,POIid);
    earth[0]=0.0f; 
    memCount=game.getMemoryFilled();
    memLim=game.getMemorySize();
    rad=memCount==memLim-2?.41f:.455f; // radius, it's 0.41 for the inner pic and 0.455 for the outer and upload;
    // if(Time<60 && fabsf(POILoc[1])>.18f)//For the first section go outer then to inner
    //     rad=memCount==0?.455f:.39f;
    flare=game.getNextFlare();
    if (flare==0) flarenum++;
    mySP=mathVecMagnitude(myState,3);
    RPos[0]=RPos[2]=0.0f;
    RPos[1]=POILoc[1];
    // RPos[2]=0.0f; 
    RPos[2]=-getLinearDistance(RPos,POILoc)*updown;
    if (memCount!=1)
        timp=updown>0?-3:3; // goes where the POI will be 3 seconds before reaching his highest possition in order to take the first pic;
        else timp=updown>0?-1:4; // for the outer zone pic;
    POIAfterTime(timp,RPos,RPos);
    for (int i=0;i<3 && Time<180;i++) 
       tpos[i]=RPos[i]*rad/getLinearDistance(RPos,earth);
 // gives you the possition from where you should take the picture, using the POI location on the asteroid;
   
    for (int i=0;i<3;i++)
            PicPoint[i]=tpos[i];
    POI_Align(); // used for pointing the camera at the POI;
    picCheckandTake(); // checks if you are able to take the photo and takes it;
   
    if(memCount==memLim || ((myState[0]>0.05f || Time>167 || fuel<=5) && memCount>=1)) // upload is you have 2 picture or the time is >217 or you are in the dark side of the asteroid;
       {
        rad=0.452f;
        //att[0]=myState[0];att[1]=myState[1];att[2]=myState[2]; //att used as an aux;
        //for (int i=0;i<3;i++)
        //    tpos[i]=att[i]*rad/getLinearDistance(att,earth);
        if(myState[0]<0){
        tpos[0]=0;
        }
        earth[0]=.64f; // earth becomes earth :)
        mathVecSubtract(att,earth,myState,3);
        //mathVecNormalize(att,3);
        api.setAttitudeTarget(att);
        getPOI();
        }
     game.uploadPic();
    
     if(((flare<22-(myState[0]*10) || myState[0]>0.1f || POIStore[POIid*2+1]>0) && flare>=0)||(flarenum<2 && fuel<7)) //going to the shadow;
     { 
    tpos[0]=.43f;
    tpos[1]=side*.1f;
    tpos[2]=-.1f*updown;
    float yz[]={myState[1],myState[2]};
    float yz1[]={otherState[1],otherState[2]};
    if((flare<7 && flare>=0 && mathVecMagnitude(yz,2)>.15f && mathVecMagnitude(yz1,2)<.15f && otherState[0]>0.31f)){
        tpos[0]=otherState[0];
        tpos[1]=otherState[1];
        tpos[2]=otherState[2];
    }
    else{
        earth[0]=0;
        api.setVelocityTarget(earth); //[0,0,0] not actually going to earth
    }
    }
        // in order to use only straight movement we need a intermediate point at 0.05 x to avoid collision;
        // it's placed above the top of the asteroid;
         if ((myState[0]<-.07f && tpos[0]>0.2f) || (tpos[0]<-.05f && myState[0]>0.2f)){// checks if you need that intermediate point;
        rad=0.44f;
//earth[0] was .05f
        //att[0]=earth[0]=myState[0]*.75f;att[1]=POILoc[1];att[2]=-.05f+-1*fabsf(myState[2]); // earth and att used as aux, because of the codesize;
        att[0]=earth[0]=0.05f;att[1]=myState[1];att[2]=-1*fabsf(myState[2]); // I've changed this line, it was messing with the sphere a little
        for (int i=0;i<3;i++)
            tpos[i]=att[i]*rad/getLinearDistance(att,earth); // same formula as before;
        tpos[0]=0.05f;
        
        if (getLinearDistance(tpos,aux)<0.1f) // checks that the waypoint remains the same; 
            for (int i=0;i<3;i++) tpos[i]=aux[i];
            
        for (int i=0;i<3;i++) aux[i]=tpos[i];  
        earth[0]=0.0f;
        //DEBUG(("PIVOT:%f",getLinearDistance(tpos,earth)));
    }
    // tpos[2]=tpos[2]*updown;
    //if ()
    //earth[0]=0.45f; //used for the DEBUG;
    if(getLinearDistance(earth,myState)<.35f){
        tpos[2]=updown*sqrtf(.1369f-myState[0]*myState[0]-myState[1]*myState[1]);
    }
    //DEBUG(("RADIUS:%f  AXIS Y:%f DIST:%f FLARE:%d",mySP,myState[1],getLinearDistance(myState,earth),flare));
    if(Time>6){
        tpos[2]=fabsf(tpos[2])*-1*updown;
    }
    // DEBUG(("\nTarget Position is %f, %f, %f\n",tpos[0],tpos[1],tpos[2]));
    // DEBUG(("POI ID is #%i and its position is %f, %f, %f\n",POIid,POILoc[0],POILoc[1],POILoc[2]));
     if(tpos[1]==otherState[1] && tpos[2]==otherState[2] && getLinearDistance(tpos,myState)<.3f){
        api.setPositionTarget(tpos);
        DEBUG(("Get closer"));
    }else{
        goFast(tpos); // movement function using setVelocityTarget;
    }
    // if (fuel==0 && flare==1) game.turnOff();
}
//End page main
//Begin page picCheckandTake
void picCheckandTake(){
    
    float vecPOIcpos[3],vecPOIOrigin[3],angle,limitAngle;
    
    mathVecSubtract(vecPOIcpos,myState,POILoc,3); 
    mathVecSubtract(vecPOIOrigin,POILoc,earth,3);
    mathVecNormalize(vecPOIcpos,3);
    mathVecNormalize(vecPOIOrigin,3);
    angle=getAngle(vecPOIcpos,vecPOIOrigin);
    if (getLinearDistance(myState,earth)>0.42f) limitAngle=0.383972f; // uses the limit angles provided by the game manual;
        else limitAngle=.785398f; 
    DEBUG(("%f",angle));
    // long freaking condition that checks if you are able to take a valid photo :)
    if (myState[0]>.15f || ((POILoc[2]<0 || POILoc[0]<-.01f) && game.alignLine(POIid)==true && angle0.42f && getLinearDistance(PicPoint,earth)>0.42f))))
          {  game.takePic(POIid);
             DEBUG(("TAKING A FREAKING PICTURE!!!"));
          }
            if(memCount!=game.getMemoryFilled()){
             if (getLinearDistance(myState,earth)>0.42f){
                POIStore[POIid*2+1]=1;  
             }
                else {
                    POIStore[POIid*2]=1;
                }
            }
    }

//End page picCheckandTake


};

ZRUser *zruser790 = new ZRUser02;

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

#include 

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

//#define DIAGNOSTIC_MODE 1
#define TAKE_BAD_PICTURES 1
#define PERSISTENT_BEST_LINE 1

#define fuel_allocation 90.0f

#define horizon 10    // 30

// inner_zone has to be 0, outer_zone has to be 1 !

#define inner_zone 0
#define outer_zone 1
#define danger_zone 2
#define upload_zone 3
#define other_zone 4

#define mass 4.60f

#define picture_angle 0.968912f
#define inner_zone_angle 0.696707f
#define outer_zone_angle 0.921061f

#define cos01 0.99500417f
#define sin01 0.0998334f

#define iterations 4  // 16
#define levels 3    //4

#define adapt_initial_value 3
#define adapt_increment 2

#define n_moves 3
#define n_components 3

typedef struct {
  float l[horizon * n_components];

  //results of simulation
  float score;
  int fa_t;
  float att_target[3];
} line;

typedef struct
{
    float p[3]; // position
    float v[3]; // velocity
    float from_center;
    float angle;
    int zone;
    bool in_shadow;
    
} Position;

typedef struct {
    
  int weight[horizon * n_components][n_moves];

} distribution;

typedef struct
{
    line bl;
    
    int t;
    int flare_end;
    int memory_capacity;
    int memory_used;
    int memory_points;
    
    Position my, other;
    
    float poi[3][3];
    float poi_active[6];

} StateZap;

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

void PosPreCalculate(Position *pos)
{
    pos->from_center = mathVecMagnitude(pos->p, 3);
    
    if (pos->from_center <= 0.31f) {
        pos->zone = danger_zone;
    } else if (pos->from_center <= 0.42f) {
        pos->zone = inner_zone;
    } else if (pos->from_center <= 0.53f) {
        pos->zone = outer_zone;
    } else {
        pos->zone = upload_zone;   
    }
    
    pos->in_shadow = pos->p[0] >= 0.0f && pos->p[1] < 0.19f && pos->p[1] > -0.19f && pos->p[2] < 0.19f && pos->p[2] > -0.19f;
}

int StatePictureZone(StateZap *s)
{
    if (s->my.zone > 1) return -1;

    for (int i = 0; i < 3; i++) {
        
        float u[3];
        mathVecSubtract(u, s->my.p, s->poi[i], 3);

        if (mathVecInner(u, s->poi[i], 3) >= (s->my.zone ? outer_zone_angle : inner_zone_angle) * mathVecMagnitude(u, 3) * 0.20f)
        {
            return 2 * i + s->my.zone;
        }
    }
    
    return -1;
}

void StateScore(StateZap *s, line &l)
{
    float sc = 0.0f;
    l.fa_t = -1;
    
    // POI reset
    for (int tm = 0; tm < horizon && s->t + tm < 180; tm++)
    {
        // POI reset
        if ((tm > 0) && (((s->t + tm) % 60) == 0))
        {
            memset(s->poi_active, 0, 6*sizeof(float));
            
            if (s->flare_end < tm) sc -= s->my.p[0] * 0.01f;
        }
        
        // collison with SPHERE
        bool push_away = false;
        float other2my[3];
        mathVecSubtract(other2my, s->my.p, s->other.p, 3);
        float distance_to_other = mathVecMagnitude(other2my, 3);
        if (0 < tm && tm < 10 && distance_to_other <= 0.22f + 0.05f)
        {
            // change our acceleration vector to be in direction other than the other sphere
            // this may have weird interactions with NRPA!
            
            push_away = true;

            sc = sc - 0.05f; // penalty for fuel and control loss
        }
        
        // collision with asteroid
        if (s->my.zone == danger_zone)
        {
            sc -= 7.0f - s->my.from_center;
        }
        
        // penalty for being far from the shadow
        if (tm < s->flare_end && s->flare_end > 0 && s->my.p[0] < 0.0f)
        {
            sc += s->my.p[0] / float(s->flare_end - tm) * 10.0f;
        }
        if (s->my.p[0] < -0.35f)
        {
            sc += (0.35f + s->my.p[0]) * 2.0f;
        }
        
        // flare
        if (s->flare_end - 4 <= tm && tm < s->flare_end)
        {
            if (s->my.in_shadow)
            {
                if (tm == s->flare_end - 1)
                {
                    sc += - s->my.v[0] - s->my.p[0] * 0.01f;
                }
            }
            else
            {
                sc -= 7.0f - s->my.p[0];
                s->memory_points = 0;
                if (tm == s->flare_end - 3) s->memory_capacity--;
            }
        }
        
        float *record_fa;
        static const float earth[3] = {0.64f, 0.0f, 0.0f};

        // taking pics
        int pz = StatePictureZone(s);
        if (pz >= 0 && (s->my.zone == 0 || s->my.zone == 1)
            && s->poi_active[pz] && s->memory_used < s->memory_capacity)
        {
            sc += 0.11f;
            s->memory_points += 2 + (pz % 2);
            s->memory_used++;
            s->poi_active[pz] = 0;
            record_fa = s->poi[pz/2];
        }
        // uploading pics
        else if ((s->my.zone == upload_zone || s->my.in_shadow) && s->memory_used > 0)
        {
            sc += s->memory_points;
            s->memory_points = 0;
            s->memory_used = 0;
            
            if (tm > 235)
            {
                sc -= 0.4f * (s->t + tm - 235);
            }
            record_fa = (float *) earth;
        } else {
            record_fa = NULL;
        }

        // record first action
        if (record_fa && l.fa_t < 0)
        {
            l.fa_t = tm;
            
            mathVecSubtract(l.att_target, record_fa, s->my.p, 3);
            sc += 0.002f * (horizon - tm);
        }
        
        // too far from center
        if (s->my.from_center > 0.56f)
        {
            sc -= (s->my.from_center - 0.56f) * 10.0f;
        }
        
        // fuel
        if (l.l[3*tm] != 0.0f || l.l[3*tm+1] != 0.0f || l.l[3*tm+2] != 0.0f)
        {
            sc -= 0.0035f;
            mathVecNormalize(&l.l[tm*3], 3);
        }
        
        float thruster_limit;
        if (tm < s->flare_end && s->my.in_shadow) {
            thruster_limit = 0.005f; // no need to lose fuel in shadow
        } else if (push_away) {
            thruster_limit = 0.035f; // we are pushed away from opponent
        } else if (tm < s->flare_end) {
            thruster_limit = 0.03f; // we must go quickly to shadow
        } else {
            thruster_limit = 0.02f; // default thruster usage
        }
        
        for (int i = tm * 3; i < tm * 3 + 3; i++) {
            if (push_away && tm > 0 && distance_to_other > 0.01f) {
                l.l[i] = other2my[i-tm*3] / distance_to_other * thruster_limit;
            } else {
                l.l[i] = l.l[i] * thruster_limit;
            }
        }
        
        // calculating position for the next second
        for (int i = 0; i < 3; i++) {
            s->my.p[i] += s->my.v[i] + l.l[tm*3+i] * 0.5f / mass;
            s->my.v[i] += l.l[tm*3+i] / mass;
            if (!push_away) {
                s->other.p[i] += s->other.v[i];
            }
        }
        PosPreCalculate(&s->my);

        // POI rotation (don't read if you are not a mathematician)
        for (int j = 0; j < 3; j++)
        {
            float nx, nz;
            nx = cos01 * s->poi[j][0] - sin01 * s->poi[j][2];
            nz = sin01 * s->poi[j][0] + cos01 * s->poi[j][2];
            
            s->poi[j][0] = nx;

            if (nx > 0) s->poi[j][2] = -nz;
            else s->poi[j][2] = nz;
        }
    }

    l.score = sc;
}

void nrpa(line &best_line, const int level)
{
    StateZap sim;

    distribution d;
  memset(d.weight, 0, sizeof(d.weight));

    for (int i = 0; i < iterations; i++) {
        line l;
    
        if (level <= 1) {

            //simulate
            for(int i=0;i best_line.score) {
            best_line = l; 
        }
        
        for(int i=0;i=0){
        s.flare_end = next_flare + 3;
    }else{
        s.flare_end--;
    }

    // read POI locations
    for(int i=0;i<3;i++) game.getPOILoc(s.poi[i],i);

    // setting POIs active
    if((s.t%60)==0){
        memset(s.poi_active, 1, sizeof(s.poi_active));
    }
    
#if DIAGNOSTIC_MODE
    DEBUG(("FB %d FE %d dist %f shadow %d\n", s.flare_end - 3, s.flare_end, s.my.from_center, s.my.in_shadow));
#endif

#if PERSISTENT_BEST_LINE
    for(int i=0;i<(horizon-1)*n_components;i++) {
        s.bl.l[i]=s.bl.l[i+n_components];
    }
//    for(int i=0;i<(horizon-1)*n_components;i+=n_components){
//    normalize(s.bl.l+i);
//    }
    s.bl.l[horizon*n_components-3]= 0.0f;
    s.bl.l[horizon*n_components-2]= 0.0f;
    s.bl.l[horizon*n_components-1]= 0.0f;

    StateZap sim = s;
    StateScore(&sim, s.bl);
    #if DIAGNOSTIC_MODE
    DEBUG(("Previous best line score %f\n", s.bl.score));
    #endif
#else
    s.bl.score = -1000.0f;
#endif
    
    //finding the best line
    nrpa(s.bl, levels);

    //setting attitude
    if (mathVecNormalize(s.bl.att_target,3) > 0.0f) {
        api.setAttitudeTarget(s.bl.att_target);
    }

    //setting forces
#if DIAGNOSTIC_MODE
    DEBUG(("Applying forces %f %f %f, thruster %f\n",
    s.bl.l[0],s.bl.l[1],s.bl.l[2], mathVecMagnitude(s.bl.l,3)));
#endif
    api.setForces(s.bl.l);

    //taking pictures
    int pz = StatePictureZone(&s);
    if(pz >= 0){
        #if DIAGNOSTIC_MODE
        DEBUG(("In range of POI %d, alignLine %d\n", pz, game.alignLine(pz/2)));
        #endif
        if(game.alignLine(pz/2)){
            game.takePic(pz/2);
            if(game.getMemoryFilled() > s.memory_used){
                s.poi_active[pz] = 0;
                s.memory_points = 2 + s.my.zone;
            }
        }
    }

    //uploading pictures
    //if((s.my.zone = upload_zone || s.my.in_shadow) && s.memory_used > 0){

    // VERIFY: unsuccesfull upload does not block camera
    game.uploadPic();
    
    //    #if DIAGNOSTIC_MODE
    //    DEBUG(("Trying to upload picture.\n"));
    //    #endif
    //}

    //taking bad pictures
#if TAKE_BAD_PICTURES
    if(s.bl.fa_t > 4 || s.bl.fa_t ==-1) game.takePic(0);
#endif

    s.t++;
}

//End page main


};

ZRUser *zruser811 = new ZRUser05;

#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 ZRUser05 : public ZRUser
{

//Begin page libs
//Begin page libs
//**********************************************
void mVecMultAndNorm(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;
}

//***************************************************
// a routine to move quite fast towards a target
// if k < 0.9 a constant speed uqual to k is assumed
//***************************************************
float setPosAndGo(float k){
  float v[3],r;
  mathVecSubtract(v,Target,myState,3);
  r=mathVecMagnitude(v,3);
    if(k>0.9f || k<0.001f){
      mVecMultAndNorm (v,v,k,false);
      mathVecAdd (v,v,myState,3);
      api.setPositionTarget(v);
      return r;
    }
    if(r>0.10f){//0.05
        mVecMultAndNorm (v,v,k,true);
        api.setVelocityTarget(v);
    }else api.setPositionTarget(Target);   
    return r;
}

//**********************************************
// Future position of the closest POI is predicted
//**********************************************
void locPOIs(){
    float v[3],v1[3];
    float AngleT, tfin;//,FutPOIloc[3];
    v[0]=0.f;v[1]=0.f;v[2]=1.f;
    mVecMultAndNorm(v1,vposPOI,1.0f,false);v1[1]=0.f;
    mathVecNormalize(v1,3);
    AngleT=acosf(mathVecInner(v,v1,3));
    tfin=2.976f-7.08f*fabsf(vposPOI[1]); 
//DEBUG(("AngleT %f; tfin %f\n",AngleT, tfin));
    if((ShadowZ&&AngleT<=1.0f)||(!ShadowZ&&myState[2]<-0.18f))tfin=3.f-AngleT;
    else if((ShadowZ&&AngleT>1.0f&&AngleT<1.3f)||(!ShadowZ&&myState[2]>0.18f))tfin=3.2f-AngleT;
    else if(ShadowZ&&AngleT>=1.3f&&vposPOI[1]<0.18f){idPOI=2;game.getPOILoc(vposPOI,idPOI);}
    if(idPOI<2){
        v1[1]=vposPOI[1];
        v1[0]=vposPOI[0]*cosf(tfin)-vposPOI[2]*sinf(tfin);
        v1[2]=vposPOI[0]*sinf(tfin)+vposPOI[2]*cosf(tfin);
        if(AngleT+tfin>3.1415f){
            v1[2]=-v1[2];v1[0]=-v1[0]; 
        }
    }    
    else{
        v1[0]=-0.01743f;v1[1]=0.0f;v1[2]=0.19696f;//0.f,0.f,0.2f
        if(myState[2]<0.f)v1[2]=-0.19696f;
    }
        mVecMultAndNorm(versPOI,v1,1.f,true);
        mVecMultAndNorm(vAttPOI,versPOI,-1.0f,false);
}
// *********************************************
//      check: is flare arriving?
// *********************************************
void checkFlares(){
    int rit, prevFt;
    prevFt=safe;
if(game.getNextFlare()==-1)safe=70;
else safe=game.getNextFlare();

rit=20-34*myState[0];if(myState[0]>0.05f||ShadowZ||distFromO<0.4f||(nFlares==0&&idPOI<2))rit=31;

if(safe0;
}

if( (nPicts==game.getMemorySize())
    ||((game.getFuelRemaining()<4.0f||Time>173) && game.getMemoryFilled()>0))upLoad=true;//170,5

if(safe==1){
    nFlares++;exitShadow();
    if(!ShadowZ&&!isOff){
        game.turnOff();isOff=true;
    }   
}
if((prevFt==0&&safe==70)&&(game.getFuelRemaining()>10.0f||nFlares>1))exShadow=true;
if(isOff&&safe>=29){game.turnOn();isOff=false;}
//DEBUG(("Flare Time %i; Flare: %i\n",safe, nFlares));
}

//**********************************************
// move to target
// dfz: distance from zero of next Target
//**********************************************
void moveToTarget(float dfz, float kV) {
    float v[3], versBis[3];
    float Angle;
//    
    if(!ShadowZ&&!toShadow&&(distFromO<0.3f||game.getFuelRemaining()<0.5f))kV=0.0f;//0.3
    mVecMultAndNorm(v,myState,1.0f,true);
    mathVecAdd(versBis,v,versPOI,3);
    mathVecNormalize(versBis,3);
    Angle=mathVecInner(v,versPOI,3);
    mVecMultAndNorm(Target,versBis,0.45f,false);//0.45 
    if(Angle>0.2f)mVecMultAndNorm(Target,versPOI,dfz,false);
//
    if(ShadowZ&&!exShadow)kV=0.0f;
//
    setPosAndGo(kV);
    api.setAttitudeTarget(vAttPOI);
}

//**********************************************
// Trying to take a picture
//**********************************************
void takePict() {
    float v[3],v1[3];
    float AngleErr;
//
    mathVecSubtract(v,myState,vposPOI,3);
    mathVecNormalize(v,3);mVecMultAndNorm(v1,vposPOI,1.0f,true);
    AngleErr=mathVecInner(v,v1,3);
    if(vposPOI[2]<0.10&&vposPOI[2]>-0.10f&&(0.979f-AngleErr)>0.2f)game.takePic(-1);
//DEBUG(("aligned100 %i; AngleErr %f\n",game.alignLine(idPOI),AngleErr));
    if(game.alignLine(idPOI)&&AngleErr>=0.979f){
        game.takePic(idPOI);
        if(game.getMemoryFilled()>nPicts) {
            nPicts++;
            if(idPOI<2){
                if(nPicts<2&&TimeForNextChange>10)idPOI=2;
            } else if(idPOI==2){
                if(nPicts<2&&TimeForNextChange>10){
                    idPOI=(1-iside)/2;otherPOI=1-idPOI;}
            }
            upLoad=(nPicts==game.getMemorySize());
            if(upLoad&&idPOI==2)idPOI=otherPOI;
            game.getPOILoc(vposPOI,idPOI);locPOIs();
        }
    }
}

//**********************************************
// Trying to upload pictures to the Earth
//**********************************************
void goAndUpload(){
        vAttPOI[0]=0.64-myState[0];vAttPOI[1]=-myState[1];vAttPOI[2]=-myState[2];
        mathVecNormalize(vAttPOI,3);
//        vel=0.04f;if(safe==70)vel=0.03f;
        moveToTarget(0.42f,0.035f);//0.40;0.04
        if(game.getMemoryFilled()>0)game.uploadPic();
        upLoad=(game.getMemoryFilled()>0);
        if(!upLoad)locPOIs();
}

//**********************************************
// Going to the shadow zone to avoid flares
//**********************************************
void goToShadow(){
    toShadow=true;exShadow=false;
    versPOI[0]=1.0f;versPOI[1]=0.0f;versPOI[2]=0.0f;
}

//**********************************************
// Exit the shadow zone to take other pics
//**********************************************
void exitShadow(){
        idPOI=(1-iside)/2;otherPOI=1-idPOI;
        game.getPOILoc(vposPOI,idPOI);locPOIs();
}

//**********************************************
// Avoid asteroid and wall collisions
//**********************************************
void alertState(){
    mVecMultAndNorm(Target,myState,2.0f,true);
    if(myDist>0.6f)mVecMultAndNorm(Target,Target,-0.001f,true);
    game.takePic(-1);setPosAndGo(1.7f);
}
//End page CSlibs

//End page libs
//Begin page main
//Begin page main
// CORONA Sphere ZR 2014 - ISS Final Phase
// Alliance teams: Herder Berlin - Proxima Centauri - The Quark Charm
// Changes with respect to previous version:
// code memory size: 100%
// date: 05/01/2015

// Definition of main variables:
//
// ShadowZ: true - sphere is in shadow Zone
// toShadow: true - directing to shadow zone
// nPicts: number of taken pictures
// nFlares: number of flares
// safe: time to next flare (30 -->70)
// myDist: my distance from asteroid centre;
// distFromO: my distance from opp sphere
// Target: where sphere must go
// vposPOI: coordinates of closest POI
// iside = +1 (blue sphere) - iside =  -1 (red sphere)
//
bool ShadowZ, update, toShadow, exShadow, isOff, upLoad; 
int Time, iside, idPOI, otherPOI, nPicts, nFlares, safe;
float myDist, distFromO, TimeForNextChange;
float Target[3], versPOI[3], vposPOI[3], vAttPOI[3]; 
state_vector myState, otherState;

//Main variables are initialised
void init(){
    Target[0]=0.f;Target[1]=0.f;Target[2]=0.5f;
    versPOI[0]=1.0f;versPOI[1]=0.0f;versPOI[2]=0.0f;
  Time=0;nFlares=0;safe=70;idPOI=0;otherPOI=1;
    ShadowZ=false;toShadow=false;update=false;exShadow=false;isOff=false;upLoad=false;
}

void loop(){
  float vet1[3];// float kVel;

// evaluate myDist and otherDist from asteroid centre and dist from opponent
 api.getMyZRState(myState);
 api.getOtherZRState(otherState);
 myDist=mathVecMagnitude(myState,3);
 mathVecSubtract(vet1,myState,otherState,3);distFromO=mathVecMagnitude(vet1,3);

 iside=1;
 if(myState[1]<=0.0f)iside=-1;

// locate and predict closest POI when their position change, if sphere is not going to shadow
 if(Time%60==0)update=true;
 if(update&&!toShadow&&idPOI<2){
    idPOI=(1-iside)/2;otherPOI=1-idPOI;
    game.getPOILoc(vposPOI,idPOI);
    locPOIs();
    update=false;
  DEBUG(("TEST: P. Centauri(I)-The Quark Charm(USA)-Herder B.(D)\n"));
 }

// get the closest POI location
 game.getPOILoc(vposPOI,idPOI);
//DEBUG(("Time %i; myDist %f\n",Time, myDist));
//DEBUG(("nphoto %i; nslot %i\n",game.getMemoryFilled(),game.getMemorySize()));

TimeForNextChange=60-Time%60;

// is our and opp spheres in the shadow zone?
 ShadowZ=(myState[0]>0.f&&fabs(myState[1])<0.2f&&fabs(myState[2])<0.2f);
 if(toShadow&&ShadowZ)toShadow=false;
 if(exShadow&&myState[0]<0.05f&&!isOff)exShadow=false;
// check if a flare is arriving
 checkFlares();

 nPicts=game.getMemoryFilled();

//DEBUG(("exShadow: %i; toShadow: %i; ShadowZ: %i\n", exShadow, toShadow, ShadowZ));
//DEBUG(("idPOI: %i; upLoad: %i\n",idPOI, upLoad));
//DEBUG(("vPOI %f; vry %f; vrz %f\n", vposPOI[0],vposPOI[1],vposPOI[2]));
//DEBUG(("Tx %f; Ty %f; Tz %f\n", Target[0],Target[1],Target[2]));

// try to take a pic
 if(!upLoad&&!toShadow){
//     kVel=0.04f;if(!exShadow)kVel=1.7f;
//    kVel=1.5f;
    if(myState[0]>0.03||myDist>0.51f||game.getFuelRemaining()<0.01f)game.takePic(-1);//0.53
    moveToTarget(0.46f,1.55f);//0.980 ;0.435
    if((myDist>0.42f&&myDist<0.53f))takePict();
 }

// upload captured pics and go to the shadow zone. When flare is over, exit to take other pics
 if(upLoad||toShadow){
        game.takePic(-1);
        goAndUpload();
 }

 if(myDist>0.6f||myDist<0.33f) alertState();

 Time++;
}

//End page main

//End page main


};

ZRUser *zruser794 = new ZRUser05;

#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 ZRUser03 : public ZRUser
{

//Begin page gameLib
void selectIndexPOI(){
     float tm;
     indexPOI=mySide;
     if(myState[1]<0)indexPOI=otherSide;
     tm=31.4f-detectInitialTheta(indexPOI)/0.1f;
     if(tm>15.0f||tm<4.0f)return;
     indexPOI=2;
}

void selectPOI(){
    float ps1=0.0f,rz;
    bool sclt[2];
    game.getPOILoc(vPOILocation,indexPOI);
    int dt=18;
      if(indexPOI==2)dt=20;
      else {
         if(fabsf(vPOILocation[1])<0.1f)dt=16; 
      }
    do{
         ps1=predictPOIPosition(dt);
         sclt[0]=ps1>PI/6.0f;sclt[1]=ps1<(5.0f)*PI/6.0f;
         dt++;
      }while(sclt[indexUpDown]);
    rz=sqrtf(0.2f*0.2f-vPOILocation[1]*vPOILocation[1]);
    vPOILocation[0]=-rz*sinf(ps1);vPOILocation[2]=rz*cosf(ps1);
}

float detectInitialTheta(int idPOI){
  float v[3],w[3],az[3]={0.0f,0.0f,1.0f};
  game.getPOILoc(v,idPOI);
  mathVecMult(w,v,1.0f,true);w[1]=0.0f;
  return evaluateAngle(w,az,true);
}

float predictPOIPosition(int dt){
  float ps1; 
  ps1=detectInitialTheta(indexPOI);
  if(updatingUpDown){
      float tm=31.4f-detectInitialTheta(indexPOI)/0.1f;
      updatingUpDown=false;
      indexUpDown=0;
      if(tm>24.0f || tm<=7.24f)indexUpDown=1;//6.24???????
  }
  ps1=ps1+0.1f*dt;
  while(ps1>PI)ps1=ps1-PI;
  return ps1;
}


void evaluateTarget(){
    float ps1;//,dr[2]={0.39f,0.47f};
    float w[3],v[3],z[3];
    mathVecMult(w,vPOILocation,1.0f,true);
     if(isGoingInShadow){
          w[0]=1.0f;w[1]=w[2]=0.0f;
    }
    ps1=evaluateAngle(w,myState,false);
    if(ps1>=60.0f){
       mathVecMult(z,myState,1.0f,true);
       mathVecAdd(v,z,w,3);mathVecMult(w,v,1.0f,true);
     }
     mathVecMult(vTarget,w,0.47f,false);
    
        
}

int getZone(){
    if(myState[9]>0.31f && myState[9]<0.42f)return 0;
    if(myState[9]>0.425f && myState[9]<0.53f)return 1;
    return -1;
}

void takeImage(){
    int iZone=getZone();
    int nfoto=game.getMemoryFilled();
    float z[3],thetaMax[2]={0.43f,0.215f};
    float theta;
    mathVecSubtract(z,myState,vPOILocation,3);
    theta=evaluateAngle(vPOILocation,z,true);
    if(iZone>=0 && game.alignLine(indexPOI) && theta0){
         game.uploadPic();flagUpLoadOut=true;
     }
     if(game.getMemoryFilled()==0&&flagUpLoadOut){
         flagUpLoadOut=false;
         if(isTackedPOI[mySide]&&isTackedPOI[2])indexPOI=otherSide; 
           else{
               if(indexPOI==2){
                   indexPOI=mySide;if(isTackedPOI[mySide])indexPOI=otherSide;
               }else{
                      if(indexPOI==mySide || indexPOI==otherSide)indexPOI=2;
                }
         
          }
          selectPOI();return;
       }
     
 }
 
 
float evaluateAngle(float *v1,float *v2,bool rad){
    float ps,v[3],w[3]; 
    mathVecMult(v,v1,1.0f,true);
    mathVecMult(w,v2,1.0f,true); 
    ps=acosf(mathVecInner(w,v,3));
    if(rad)return ps;
    ps=ps*180.0f/PI;
    return ps;
}
//End page gameLib
//Begin page main
//CORONA_ISS_FINAL
//03-01-2015 H:22.00
//CANDIDATE TO FINAL
bool isConnected,isGoingInShadow,isInShadow;
bool selectIndexPOIM2;
bool alertOppCollision;
bool execUpload,firstFotoTaked,flagUpLoadOut;
bool isTackedPOI[3];
bool updatingUpDown;
//int indexShadow;
int timeForNextFlare;
int timeForNextConfig;
int indexPOI;
int mySide,otherSide;
int time;
int nFlare,durataFlare;
int indexUpDown;//0-> Down, 1->Up
//myDistFromAst->myState[9],myDistFromOpp->myState[10],kSpeedTransf->myState[11];
float vTarget[3],vPOILocation[3],vAtt[3],vActAtt[3];
state_vector myState,oppState;

void init(){
 isGoingInShadow=false;isConnected=false;updatingUpDown=false;flagUpLoadOut=false;    
 time=0;mySide=0;otherSide=1;indexUpDown=1;nFlare=0;durataFlare=-1;
 
}

void loop(){
 float v[3],dt,k=0.045f;
 api.getMyZRState(myState);api.getOtherZRState(oppState);
 mathVecMult(vActAtt,&myState[6],1.0f,false);
 myState[9]=mathVecMagnitude(myState,3);
 mathVecSubtract(v,myState,oppState,3);myState[10]=mathVecMagnitude(v,3);
 alertOppCollision=myState[10]<0.40f;
 timeForNextFlare=game.getNextFlare();
 timeForNextConfig=60-time%60;
 if(game.getMemoryFilled()==0)firstFotoTaked=false;
 if(time==0){
     if(myState[1]<0){mySide=1;otherSide=0;}
     updatingUpDown=true;
     indexPOI=mySide;
    // selectIndexPOI();
     selectPOI();
 }
 
 if(timeForNextFlare==-1)timeForNextFlare=31;
 isInShadow=myState[0]>0.0f && fabsf(myState[1])<0.2f && fabsf(myState[2])<0.2f;
 if(timeForNextConfig==60){
     for(int i=0;i<3;i++)isTackedPOI[i]=false;
     selectPOI();
 }

if(timeForNextFlare==0)durataFlare=0; 
if(timeForNextFlare>4 && !isConnected){//Attivazione dopo Flare
          game.turnOn();isConnected=true;
     }
dt=28;if(alertOppCollision)dt=30;
selectIndexPOIM2=timeForNextFlare<=dt&&(nFlare==0||(nFlare==1&&game.getMemorySize()==1));


if(selectIndexPOIM2){
         indexPOI=-2;isGoingInShadow=true;
    }
 
if(timeForNextFlare<6 && !isInShadow){//Gestione Flare
     moveToFaster(myState,0.0f);game.takePic(4);
     if(timeForNextFlare==1){
         game.turnOff();
         isConnected=false;
     }
  } 
    myState[11]=0.049f;//0.45
    if(indexPOI>=0){
            if(game.getMemoryFilled()>0&&nFlare==0){
               myState[11]=0.05f;
               if(!firstFotoTaked){
                   if(indexPOI!=2)indexPOI=2;
                   else {
                       indexPOI=mySide;
                       if(isTackedPOI[mySide])indexPOI=otherSide;
                   }
                    selectPOI();firstFotoTaked=true;
               }
            }
           mathVecMult(vAtt,vPOILocation,-1.0f,true);
           api.setAttitudeTarget(vAtt);
           evaluateTarget();
           if(alertOppCollision)myState[11]=0.03f;
           moveToFaster(vTarget,myState[11]);  
           takeImage();
        }
        
execUpload=game.getMemoryFilled()==game.getMemorySize();
execUpload=execUpload||(game.getMemoryFilled()>0 && (time>174||game.getFuelRemaining()<2.0f||nFlare>0));
if(execUpload)executeUpLoad();
if(durataFlare>=0)durataFlare++;
if(isInShadow && timeForNextFlare>29&&durataFlare>=3){//&&timeForNextConfig>9
            isGoingInShadow=false;durataFlare=-1;
    }else if(isInShadow){k=0.0f;mathVecMult(vTarget,myState,1.0f,false);}
 if(indexPOI==-2){
     nFlare++;
    evaluateTarget();
    moveToFaster(vTarget,k);
    executeUpLoad();
  }
 
 if(myState[9]>0.53f||game.getFuelRemaining()==0.0f)game.takePic(4);
 emergencyReactions();
 time++;
}

//End page main
//Begin page moveLib
void moveToFaster(float *target,float k){
    float v[3],r;
    mathVecSubtract(v,target,myState,3);
    r=mathVecMagnitude(v,3);
    mathVecMult(v,v,k,true);
    if(r<0.12f)api.setPositionTarget(target);
      else api.setVelocityTarget(v);
}

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;
}

void emergencyReactions() {
    float kk=0.0,vv[3];
    if(myState[9]>0.619f)kk=-2.0f;
    if(myState[9]<0.32f)kk=2.0f;
    if(kk!=0.0f){
      mathVecMult(vv,myState,kk,false);    
      moveToFaster(vv,0.02f);  
    }
    
}
//End page moveLib


};

ZRUser *zruser797 = new ZRUser03;

#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 ZRUser03 : public ZRUser
{

//Begin page main
//Begin page main
//Begin page 0_Vars
float myPos[3];
int timeToFlare;
float earth[3];
float zero[3];
int activePOI; // select a POI based on the sign of the y coord of the POI
float initY;
float currTarget[3];
float targ[3][3];
float poi[3][3];
float pic_rad;
float side_rad;
int timeElapsed;
int currSign;
bool on;
int numPics;
int tasd;
bool numFlares;

//////// MODIFICATIONS
// t1: time from start to outer POI
// t2: time from shadow to outer POI
// t3: time from shadow to central POI
// all scaled by .1 to account for .1rad/sec
/////////

//End page 0_Vars
//Begin page 1_Standard_Activity

//End page 1_Standard_Activity
//Begin page 2_Movement

// wrapper for movement
void moveToMdpt(float t[3], bool uploadFlag) {
    float Vtmp[3];
    float myD = dist(myPos, t);
    float dan_rad = 0.40f; // dist(myPos, zero) < dan_rad
    if(myD >= 0.10f) { // get a mdpt in path myD >= 0.10f
        float R = dist(t, zero); // radius of target
        if(R < dan_rad) R = dan_rad;
        float mdpt[3];
//        float normMyPos[3];
        for(int i=0; i<3; i++) {
            mdpt[i]=t[i];
            Vtmp[i]=myPos[i];
        }
        mathVecNormalize(Vtmp,3);
        mathVecNormalize(mdpt,3);
        float x = 2; // originally 2
        for(int i=0; i<3; i++) mdpt[i] = (mdpt[i]+Vtmp[i])/x;
        mathVecNormalize(mdpt,3);
        for(int i=0; i<3; i++) mdpt[i]*=R;
        
        moveToNaren(mdpt);
    }
    else {
        moveToNaren(t);
    }
    // attitude
    float ti[3];
    if(uploadFlag) {
        for(int i = 0; i < 3; i++ )
            ti[i] = earth[i];
        // mathVecSubtract(Vtmp, earth, myPos, 3);
    } else {
        ti[0] = ti[1] = ti[2] = 0.0f;
        // mathVecSubtract(Vtmp, zero, myPos, 3); // currPOI
    }
    mathVecSubtract(Vtmp, ti, myPos, 3);
    mathVecNormalize(Vtmp, 3);
    api.setAttitudeTarget(Vtmp);
    if(uploadFlag) {
        game.uploadPic();
    }
}

void move2(float t[3], bool uploadFlag) {
    moveToNaren(t);
    float Vtmp[3];
    if(uploadFlag) {
        mathVecSubtract(Vtmp, earth, myPos, 3);
    } else {
        mathVecSubtract(Vtmp, zero, myPos, 3); // currPOI
    }
    mathVecNormalize(Vtmp, 3);
    api.setAttitudeTarget(Vtmp);
    if(uploadFlag) {
        game.uploadPic();
    }
}

void moveToNaren(float t[3]) {
float Vtmp[3];
    float factor = 0.127f;
    if(dist(myPos,t) > 0.55f) {
        game.takePic(0);
        factor = 0.180f;
    }
    else if(dist(myPos,t) > 0.25f) {
        game.takePic(0);
        factor = 0.173f;
    }
    else if(dist(myPos,t) > 0.04f) {
        factor = 0.171f; // originally 0.150f then 0.165f then 0.170f
    }
    // else {
    //     factor = 0.060;
    // }
    mathVecSubtract(Vtmp, t, myPos, 3);
    for(int i = 0; i < 3; i++ ) {
        Vtmp[i]*=factor;
    }
    api.setVelocityTarget(Vtmp);
}
//End page 2_Movement
//Begin page 3_Funcs
float dist(float a[3], float b[3]) {
    return sqrtf((a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1])+(a[2]-b[2])*(a[2]-b[2]));
}

float ang(float vec1[3], float vec2[3]) {
    float dot = 0;
    for(int i = 0; i < 3; i++ ) {
        dot += vec1[i]*vec2[i];
    }
    dot/=mathVecMagnitude(vec1, 3);
    dot/=mathVecMagnitude(vec2, 3);
    return acosf(dot);
}

//End page 3_Funcs
//Begin page main

void init(){
    
    tasd = 0;
    numFlares=false;
    currSign = 1;
    on = true;
    pic_rad = 0.45f; // originally 0.45f
    
    for(int i  = 0; i < 3; i++ ) {
        zero[i] = 0.0f;
        myPos[i] = 0.0f;
    }
    earth[0] = 0.64f;
    earth[1] = earth[2] = 0.0f;
    timeToFlare = -1;
    activePOI = 2;
    
    numPics = 0;
    //////////////INTELLIGENCE +0.75//////////////
    // t1 = 2.025f;
    // t2 = 3.395f;
    // t3 = 3.365f;
    // t1 = 1.975f;
    // t2 = 3.2345f;
    // t3 = 3.215f;
    //////////////////////////////////////////////
}

void loop(){
    float score = game.getScore();
    // obtaining data
    DEBUG(("[%d]: numPics %d; activePOI: %d; (%f, %f, %f)", timeElapsed, numPics, activePOI, poi[activePOI][0], poi[activePOI][1], poi[activePOI][2]));
    float myState[12];
    timeToFlare = game.getNextFlare();
    if(timeToFlare == -1) timeToFlare = 35;
    float Vtmp[3];
    api.getMyZRState(myState);
    
    for(int i = 0; i < 3; i++ ) {
        myPos[i] = myState[i];
    }
    
    for(int i = 0; i < 3; i++) {
        game.getPOILoc(targ[i], i);
        game.getPOILoc(poi[i], i);
    }
    timeElapsed = api.getTime();
    // end getting data
    if(!timeElapsed) { // initial setting of POI; this is our first target
        initY = myPos[1];
        activePOI = -1;
        do {
            activePOI++;
        } while(poi[activePOI][1]/myPos[1] <= 0.0f);
        // currSign
        // if(((poi[activePOI][2]*(-0.393f)+poi[activePOI][0]*0.919f) < 0.0f) || 
        //     ((poi[activePOI][0]*(-0.393f)-poi[activePOI][2]*0.919f)>0)) currSign = -1;
        // else currSign = 1;
        if(poi[activePOI][2]>0.0f) currSign = 1;
        else currSign = -1;
    } // end initial POI setting

    for(int i = 0; i < 3; i++ ) { // begin setting targ
        if(targ[i][1] != 0) { // one of the POIS on either end
            side_rad = sqrtf(0.04f - targ[i][1]*targ[i][1]);
            Vtmp[0] = 0.0f;
            Vtmp[1] = targ[i][1];
            Vtmp[2] = side_rad*currSign;
            mathVecNormalize(Vtmp, 3);
            for(int j = 0; j < 3; j++ ) {
                targ[i][j] = pic_rad * Vtmp[j];
            }
        } 
        else
        {
            targ[i][0] = targ[i][1] = 0.0f;
            targ[i][2] = pic_rad*currSign;
        }
    } // end setting targ
    if(!on) { // begin startup
        game.turnOn();
        on = true;
    } // end startup
    if(timeElapsed > 180) { // begin endgame
        game.takePic(activePOI);
        move2(zero, true);
    } // end endgame
    ///////////////////// WHAT IS THIS, I THINK IT'S UNNECESSARY+HARMFUL - RAHUL /////
    // if(poi[activePOI][2] > 0 && asd > 0) { // add one time condition
    //     currSign = -1;
    //     tasd++;
    // } else if(poi[activePOI][2] < 0 && asd > 0) {
    //     currSign = 1;
    //     tasd++;
    // }
    //////////////////////////////////////////////////////////////////////////////////
    if(/*asd>0 || */tasd>0) tasd++;
    if(tasd==5) { // begin setting active POI and currSign
        activePOI=-1;
        do {
            activePOI++;
        } while(targ[activePOI][1]!=0);
        if(poi[activePOI][1]==0) { // t3
            // if((((poi[activePOI][2]*(-0.997f)+poi[activePOI][0]*(-0.073f)) < 0.0f) || 
            // ((poi[activePOI][0]*(-0.997f)-poi[activePOI][2]*(-0.073f))>0))) currSign=-1;
            // else currSign=1;
            if(poi[activePOI][2] > 0.0f) currSign = 1;
            else currSign = -1;
        }
        // else { // t2
        //     if((((poi[activePOI][2]*cosf(t2)+poi[activePOI][0]*sinf(t2)) < 0.0f) || 
        //     ((poi[activePOI][0]*cosf(t2)-poi[activePOI][2]*sinf(t2))>0))) currSign=-1;
        //     else currSign=1;
        // }
        tasd=0;
        DEBUG(("[%d]*****TASD SELECTION******", timeElapsed));
    } // end setting activePOI and currSign
    if(game.getFuelRemaining() == 0) {
        game.takePic(activePOI);
        if(timeToFlare == 1) {
            game.turnOff();
        }
    }
    bool insideShadow = ((myPos[0] < 0.62f) && (myPos[0] > 0.0f) && 
                         (myPos[1] < 0.18f) && (myPos[1] > -0.18f) &&
                         (myPos[2] < 0.18f) && (myPos[2] > -0.18f));

    if(insideShadow) game.takePic(activePOI);
    if(timeToFlare < 23 && (((score>19 || (score>16 && game.getMemoryFilled()>0)) && numFlares)||!numFlares)) { // begin flare response
        // traveling to shadow zone
        Vtmp[0] = 0.33f;
        Vtmp[1] = sqrtf(0.02f);
        if(myPos[1] < 0) Vtmp[1]*=-1;
        Vtmp[2] = sqrtf(0.02f)*currSign;
        game.takePic(0); // ffpicture
        if(myPos[0] < 0) {
            moveToMdpt(Vtmp, true);
        }
        else {
            move2(Vtmp, true);
        }
        
        // moveToMdpt(s, UL);
        if(timeToFlare < 2 && !insideShadow) {
            game.turnOff();
            on = false;
        }
        if(timeToFlare == 1) {
            tasd++;
            numFlares=true;
        }
        return;
    } // end flare response
    // if(timeToFlare < 2 && !insideShadow) {
    //         game.turnOff();
    //         on = false;
    // }
    // if(asd > 0) {
    //     asd = 0;
    // }
    if(myPos[0] < 0) { // begin movement commands
        move2(targ[activePOI], false);
    } else {
        moveToMdpt(targ[activePOI], false);
    } // end movement commands
    bool pic = false;
//    float v[3];
    mathVecSubtract(Vtmp, myPos, poi[activePOI], 3);
    //DEBUG(("\n***%03d: current angle: %f   myPos: %f %f %f POI %d: %f %f %f v: %f %f %f\n", timeElapsed, ang(poi[activePOI], Vtmp),
  //  myPos[0], myPos[1], myPos[2],
  //  activePOI, poi[activePOI][0], poi[activePOI][1], poi[activePOI][2],
  //  Vtmp[0], Vtmp[1], Vtmp[2]
  //  ));
    if(ang(poi[activePOI], Vtmp) < 0.40f && game.alignLine(activePOI)) { // begin picture taking
        if(currSign==-1) DEBUG(("||TAKING PIC||"));
        int mi = game.getMemoryFilled();
        game.takePic(activePOI);
        int mf = game.getMemoryFilled();
        pic = (mf > mi);
    } // end picture taking
    if(timeElapsed%60==0) { // begin reset numPics
        numPics=0;
    } // end reset numPics

    if(pic) { // begin setting POI after picture taken
        numPics++;
        
        DEBUG(("\n**;*%03d: PIC TAKEN...\n", timeElapsed));        
        
        
        if(poi[activePOI][1] == 0.0f || (numPics==2 && timeElapsed>60)) {
            activePOI = -1;
            do {
                activePOI++;
            } while(poi[activePOI][1]/initY<=0.0f);
        } else {
            activePOI = -1;
            do {
                activePOI++;
            } while(poi[activePOI][1]!=0.0f);
        }
        if(timeElapsed>120) {
            if(numPics==2) {
                activePOI=-1;
                DEBUG(("***4***"));
                do {
                    activePOI++;
                } while(poi[activePOI][1]/initY>= 0.0f);
            }
        }
    } // end setting POI after picture taken

    if((game.getMemoryFilled() == game.getMemorySize()) ||
    (game.getMemoryFilled() > 0 && game.getFuelRemaining() < 20) ||
    (game.getMemoryFilled() > 0 && timeToFlare < 10)) { // begin upload condition
        for(int i = 0; i < 3; i++ ) {
            Vtmp[i] = myPos[i];
        } mathVecNormalize(Vtmp, 3);
        for(int i = 0; i < 3; i++ ) {
            Vtmp[i]*=0.53f;
        }
        moveToMdpt(Vtmp, true);
        if(game.getMemoryFilled() == 0) {
            moveToMdpt(targ[activePOI], false);
            // numPics=0;
        }
    } // end upload condition
  DEBUG(("next flare: %d", timeToFlare));
}

//End page main

//End page main
//End page main


};

ZRUser *zruser786 = new ZRUser03;

#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 ZRUser06 : public ZRUser
{

//Begin page Functions
bool isClose (float point[3], float dist) {
    //COPYRIGHT NARDAVIN 2013-2015
    api.getMyZRState(myState);
    float actDist[3];
    
    for (int i=0; i<3; i++) {
        actDist[i] = (point[i] - myState[i]) * (point[i] - myState[i]);
    }
    
  if (actDist[0] + actDist[1] + actDist[2] < dist*dist){
    
    return true;
  }
  else{
      
    return false;
  }
}

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

void att(float point[3]){
    float attLine[3];
    mathVecSubtract(attLine,point,myPos,3);
    mathVecNormalize(attLine,3);
    api.setAttitudeTarget(attLine);
}

void setLoc(int ID){
    game.getPOILoc(poiLoc, ID);
  //DEBUG(("%f %f %f", poiLoc[0], poiLoc[1], poiLoc[2]));
  mathVecScale(takePic, poiLoc, picRadius, true);
  
  takePic[0] = -.1;
  takePic[2] = -sqrtf((picRadius*picRadius)-(takePic[0] * takePic[0])-(takePic[1]*takePic[1]));
  
  //DEBUG(("%f %f %f", takePic[0], takePic[1], takePic[2]));
  mathVecScale(sendPic, takePic, sendRadius, true);
  api.setPositionTarget(takePic);
}

//End page Functions
//Begin page main
ZRState myState;
float myPos[3];
float takePic[3];
float sendPic[3];
float center[3];
float poiLoc[3];
float earth[3];
float shadow[3];
int state;
int sideID;
int midID;
float picRadius;
float sendRadius;
int timer;
bool midHigher;
void init(){
    api.getMyZRState(myState);
    if(myState[1]>0){
        sideID = 0;
    }
     else{
        sideID = 1;
    }
    
    midID = 2;
    
    
    picRadius = .5;
    sendRadius = .6;
    
  //memcpy(center, (float[3]){0,0,0}, 3*sizeof(float));
  center[0] = 0;
  center[1] = 0;
  center[2] = 0;
  //memcpy(earth, (float[3]){.64,0,0}, 3*sizeof(float));
  earth[0] = .64;
  earth[1] = 0;
  earth[2] = 0;
  //memcpy(shadow, (float[3]){.34,0,0}, 3*sizeof(float));
  shadow[0] = .34;
  shadow[1] = 0;
  shadow[2] = 0;
  
  state = 1;
  

}

void loop(){
    
    
  api.getMyZRState(myState);
  for(int i = 0; i < 3; i++){
      myPos[i] = myState[i];
  }

  switch(state){
      case 1:
          setLoc(sideID);
          att(center);
          if(isClose(center, .53) && game.alignLine(sideID)){
              game.takePic(sideID);
          }
          
          if(game.getMemoryFilled() > 0){
              state = 2;
          }
          
          break;
          
        case 2:
          setLoc(midID);
          att(center);
          if(isClose(center, .53) && game.alignLine(midID)){
              game.takePic(midID);
          }
          
          if(game.getMemoryFilled() > 1){
              state = 3;
          }
          if(game.getMemoryFilled() == 0){
              state = 1;
          }
          
          break;
          
      case 3:
          api.setPositionTarget(sendPic);
          att(earth);
          game.takePic(midID);
      
          if(!isClose(center, .54)){
              game.uploadPic();
          }
          
          if(game.getMemoryFilled() == 0){
              state = 1;
          }
          break;
  }
  
// Added by alex "pulling a quicky"

    if(api.getTime() > 159 && game.getMemoryFilled() == 1){
        att(earth);
        api.setPositionTarget(sendPic);
        game.uploadPic();
    }

  if((game.getNextFlare() < 20 && game.getNextFlare() != -1) || (api.getTime() > 225 && game.getMemoryFilled() > 0)){
      api.setPositionTarget(earth);
      att(earth);
      game.takePic(midID);
        game.uploadPic();
  }
  
  if(game.getNextFlare() < 18 && game.getNextFlare() != -1){
      DEBUG(("[Avoiding Flare] "));
  }
  else if(state == 1){
      DEBUG(("[Taking Picture] "));
  }
  else{
      DEBUG(("[Sending Picture] "));
  }
  DEBUG(("[Time until flare: %d] ", game.getNextFlare()));
    

}

//End page main


};

ZRUser *zruser812 = new ZRUser06;

#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 ZRUser06 : public ZRUser
{

//Begin page getDistance
float getDistance(float Loc1[3], float Loc2[3])
{
    float relVec[3];
    mathVecSubtract(relVec,Loc2,Loc1,3);
    return mathVecMagnitude(relVec,3);
}
//End page getDistance
//Begin page main
ZRState myState, otherState;
float origine[3], POI[3], POInow[3], vaiQui[3], myPos[3], earth[3], dir[3], shadow[3], takingPic[3][8];//IDPOI XYZout bontàXYZpoi
int nextFlare, POIid, Time, t;
bool outer;//AGGIUNTO ORA
bool flagTime, sign, scelto;
void init()
{
    memset(origine, 0, 3*sizeof(float));
    flagTime=true;
    scelto = false;
    Time=0;
    outer=false;//AGGIUNTO ORA
    t=0;
    earth[0]=0.64;
    earth[1]=0.0;
    earth[2]=0.0;
}
void loop()
{
    if (Time == 60)
    {
        Time = 0;
        outer = false;//AGGIUNTO ORA
    }
    nextFlare=game.getNextFlare();
    api.getMyZRState(myState);
    api.getOtherZRState(otherState);
    stayInBounds();
    if (flagTime)
    {
        if(myState[1]>0)
        {
            sign=true; //BLU
            shadow[0]= 0.43;
            shadow[1]=0.11;
            shadow[2]=0.11;
        }
        else
        {
            sign=false; //ROSSO
            shadow[0]= 0.43;
            shadow[1]=-0.11;
            shadow[2]=0.11;
        }
        flagTime=false;
    }
    Time = Time+1;//AGGIUNTO ORA
    for(int i(0); i < 3; i++)
        predictPOI(i,outer);//AGGIUNTO ORA
    if(scelto == false)
    {
        if(sign == true)
        {
            if(takingPic[0][4]((takingPic[POIid][7]*2)/33*4) && takingPic[POIid][4]<((takingPic[POIid][7]*2)/33*31))
    {
        mathVecSubtract(dir, POInow, myState,3);
        mathVecNormalize(dir,3);
        api.setAttitudeTarget(dir);
        game.takePic(POIid);
    }
    if(nextFlare<18 && nextFlare>-1)
    {
        moveToPrecise(shadow,0.8f,myState);
        toTheEarth();
    }
    else if (api.getTime()>165 && game.getMemoryFilled()!=0 || game.getMemoryFilled()==2)
        toTheEarth();
    else
    {
        moveToPrecise(vaiQui,0.8f,myState);
        mathVecSubtract(dir, POI, myState,3);
        mathVecNormalize(dir,3);
        api.setAttitudeTarget(dir);
        if(game.alignLine(POIid) && POInow[2]>0)
        {
            game.takePic(POIid);
            if(game.getMemoryFilled()!=0)
                scelto = false;
        }
        if(game.getMemorySize()== 1)
            if(game.getMemoryFilled()==1)
                toTheEarth();
        else if(game.getMemorySize()==2)
            if(game.getMemoryFilled()==2)
            {
                toTheEarth();
                outer=true;
            }
        else
            moveToPrecise(shadow,0.8f,myState);
    }
    if(api.getTime()>179 && game.getScore()>25.0f && game.getOtherScore()<25.0f)
        DEBUG(("\nPizza e mandolino. Prendi, incarta e porta a casa.\n"));
}
//End page main
//Begin page moveToPrecise
void moveToPrecise(float loc[3],float tarSpeed,ZRState myZRState)
{
    float stopping_distance=((tarSpeed*tarSpeed)/(2.0f*0.008f));
    if(stopping_distance<0.02f)stopping_distance=0.02f;
        if(nearLocation(loc, myZRState, stopping_distance))
            api.setPositionTarget(loc);
    else
        moveTowardsAtSpeed(loc, tarSpeed, myZRState); 
}
//End page moveToPrecise
//Begin page moveTowardsAtSpeed
void moveTowardsAtSpeed(float loc[3],float tarSpeed,ZRState myZRState)
{
    float velTar[3];float myLoc[3];
    for(int i=0;i < 3;i++)
        myLoc[i]=myZRState[i];
    mathVecSubtract(velTar,loc,myLoc,3);
    float ratio=tarSpeed/mathVecMagnitude(velTar,3);
    velTar[0]*=ratio;velTar[1]*=ratio;velTar[2]*=ratio;
    api.setVelocityTarget(velTar);
}
//End page moveTowardsAtSpeed
//Begin page nearLocation
bool nearLocation(float loc[3],ZRState state,float tolerance)
{
    float relVec[3];
    mathVecSubtract(relVec,loc,&state[0],3);
    return mathVecMagnitude(relVec,3)=0.62 || myState[0] <=-0.62) 
        force[0] = -myState[0];
    if (myState[1] >=0.76 || myState[1] <=-0.76) 
        force[1] = -myState[1];
    if (myState[2] >=0.62 || myState[2] <=-0.62) 
        force[2] = -myState[2];
    api.setForces(force);
}

//End page stayInBounds
//Begin page toTheEarth
void toTheEarth() //function that rotate the SPh
{
    if(game.getMemoryFilled() != 0)
    {
        mathVecSubtract(dir, earth, myState, 3);
        api.setAttitudeTarget(dir);
        game.uploadPic();
    }
    else
        game.takePic(POIid);
}
//End page toTheEarth


};

ZRUser *zruser796 = new ZRUser06;