Submission for 3D round of SpySPHERES 2015-16 ZeroRobotics competition

waterbears-3d-2015.cpp

/*
 *   _      _____ _____________  ___  _______   ___  ____
 *  | | /| / / _ |_  __/ __/ _ \/ _ )/ __/ _ | / _ \/ __/
 *  | |/ |/ / __ |/ / / _// . _/ _  / _// __ |/ . _/\ \
 *  |__/|__/_/ |_/_/ /___/_/|_/____/___/_/ |_/_/|_/___/
 *
 *          ZEROROBOTICS 2015-16 SPYSPHERES 3D
 */

/*
 * "Encrypt" our debug messages so that in public competition, we can still
 * debug data without revealing any internals of our strategy. The SPHERES
 * DSP compiler does not seem to support macros like this, so this code will
 * have to be removed when checking code size and presumably when it comes time
 * to submit to the ISS.
 *
 * e.g. Instead of DEBUG(("Hello, %s!", name)), use PRINT("Hello, %s!", name)
 */
/*
#define PRINT(...)                                      \
do {                                                    \
    const char *key = "waterbears";                     \
    const int keySize = 10;                             \
    unsigned int time = api.getTime();                  \
    char buf[512], hexBuf[512];                         \
    sprintf(buf, __VA_ARGS__);                          \
    memset(hexBuf, '\0', sizeof(hexBuf));               \
    for(int i = 0, len = strlen(buf); i < len; i++) {   \
        char encrypted = buf[i] ^ key[i % keySize] ^    \
            ((int)(time * 3.4) % 128);                  \
        sprintf(hexBuf, "%s%02x", hexBuf, encrypted);   \
    }                                                   \
    DEBUG(("!%02x%s?", time, hexBuf));                  \
} while(0)
*/

// Is the desparation apparent enough?
#define move(pos) { api.setPositionTarget(pos); }
#define stopRotating() { api.setAttRateTarget(zero); }
#define stopMoving() { api.setVelocityTarget(zero); }

#define sgn(n) ((n > 0.0f) ? 1.0f : -1.0f)

// Indicies for SPHERES states
#define X   0   // Position
#define Y   1
#define Z   2
#define VX  3   // Velocity
#define VY  4
#define VZ  5
#define NX  6   // Angular displacement (attitude)
#define NY  7
#define NZ  8
#define WX  9   // Angular velocity (pretend it's an ω)
#define WY  10
#define WZ  11

// Item IDs
#define ENERGY_PACK_1   0   // Blue
#define ENERGY_PACK_2   1   // Red
#define ENERGY_PACK_3   2   // Center
#define SCORE_PLUS_1    3   // Low Center
#define SCORE_PLUS_2    4   // Blue
#define SCORE_PLUS_3    5   // Red
#define SCORE_PLUS_4    6   // High Center
#define MIRROR_1        7   // Red
#define MIRROR_2        8   // Blue

// Return vaues of ZeroRoboticsGame::hasItem
#define ITEM_ON_FIELD   -1
#define ITEM_OURS       0
#define ITEM_THEIRS     1

float team;

unsigned int time;
float energy;

float state[12];
float enemyState[12];

int memoryFilled;

unsigned int lightSwitchTimes[4];

int enemyMirrorCount;
unsigned int enemyMirrorRemaining;

int mirrorTime;

float zero[3];

void init() {
    enemyMirrorCount = 0;
    enemyMirrorRemaining = 0;

    api.getMyZRState(state);
    team = sgn(state[X]);

    // Our strategy depends on the switching of light zones, might as well
    // precalculate them all
    unsigned int firstSwitch = api.getTime() + game.getLightSwitchTime();
    for(int i = 0; i < 4; i++)
        lightSwitchTimes[i] = firstSwitch + 60 * i;

    mirrorTime = 0;
    
    zero[0] = zero[1] = zero[2] = 0;
}

/*
 * This is a recreation BB&N Knights' strategy as of 10-23. Judging from their
 * debug messages, the state machine is divided into three states, correlating
 * to the number of zone switches so far. In this first stage, we move to the
 * arbitrary position (+-0.4, -0.5, -0.2), just above and forward of where we
 * spawn. There, we try to take pictures and once we either have 2 or the zones
 * are about to change, we face Earth and upload ASAP. The first light switch
 * begins stage two, in which we first wait to regain 4.5 energy and then move
 * to pick up our mirror. We then move pick up our Score+, if available. If the
 * enemy is not at the top middle Score+, go there as well. As soon as the zone
 * changes again, we enter stage three. At this point, BB&N prints out "Rise
 * and Stay," uses their mirror, and, nominally, rises to z=-0.425. At this
 * point, things get a little less clear, but typically they either "stay" and
 * take/upload pictures, or, if the enemy has not taken their side's Score+ and
 * they don't appear to anytime soon, BB&N rushes to take it.
 */
void loop() {
    time = api.getTime();
    energy = game.getEnergy();

    api.getMyZRState(state);
    api.getOtherZRState(enemyState);

    memoryFilled = game.getMemoryFilled();
    
    if(enemyMirrorRemaining > 0)
        enemyMirrorRemaining--;

    int currentCount = 0;
    for(int i = MIRROR_1; i <= MIRROR_2; i++)
        if(game.hasItem(i) == ITEM_THEIRS)
            currentCount++;
    
    if(enemyMirrorCount > currentCount)
    {
        enemyMirrorRemaining = ITEM_MIRROR_DURATION;
    }

    enemyMirrorCount = currentCount;

    /*
     *  Hibernation (Dark)
     */
    if(time < lightSwitchTimes[0]) {
        // Move to our waiting position and stay put
        float waitingPos[] = {0.4f * sgn(state[X]), -0.5f, -0.4f};
        move(waitingPos);
    }
    /*
     *  Preparation (Light->Dark)
     */
    else if(time < lightSwitchTimes[1]) {
        // If we took two pics in hibernation, upload them
        static bool hiberPicsUploaded = (memoryFilled < 2);
        if(!hiberPicsUploaded) {
            stopMoving();
            faceEarth();
            uploadPics();

            hiberPicsUploaded = (memoryFilled < 2);
            if(!hiberPicsUploaded) return;
        }

        // If we used a lot of energy in hibernation, try to regain some
        static bool sufficientEnergy = (energy >= 4.5f);
        if(!sufficientEnergy) {
            stopMoving();
            stopRotating();
            sufficientEnergy = (energy >= 4.5f);
            if(!sufficientEnergy) return;
        }

        // Pick up the mirror, then pickup either of the Score+
        static bool mirrorPickedUp = (game.getNumMirrorsHeld() == 1);
        if(!mirrorPickedUp) {
            float mirrorPos[] = {0.4f * sgn(state[X]), 0.15f, -0.4f};
            move(mirrorPos);

            if(energy >= 3.0f) {
                static bool picsUploaded = false;
                if(!picsUploaded && memoryFilled < 2) {
                    faceEnemy();
                    takePic();
                } else if(!picsUploaded) {
                    faceEarth();
                    uploadPics();
                    picsUploaded = (memoryFilled == 0);
                }
            } else {
                stopRotating();
            }

            mirrorPickedUp = (game.getNumMirrorsHeld() == 1);
        } else {
            static int selectedPOI = getBestPOI();

            if(selectedPOI != 0 && game.hasItem(selectedPOI) == ITEM_THEIRS) {
                selectedPOI = getBestPOI();
            } else if(selectedPOI != 0 &&
                    game.hasItem(selectedPOI) == ITEM_OURS) {
                stopMoving();
                selectedPOI = 0;
            }

            if(selectedPOI > 0) {
                float scorePos[3];
                game.getItemLoc(scorePos, getBestPOI());
                move(scorePos);
            } else {
                stopMoving();
            }

            faceEnemy();
        }
        
        return;
    }
    /*
     *  Annihilation (Light)
     */
    else if(time < lightSwitchTimes[2]) {
        static int bestPOI = getBestPOI();
        if(bestPOI != 0 && bestPOI != SCORE_PLUS_1) {
            if(lightSwitchTimes[2] - time > 30) {
                bestPOI = getBestPOI();
            }

            if(game.hasItem(bestPOI) == ITEM_ON_FIELD) {
                float pos[3];
                game.getItemLoc(pos, bestPOI);
                move(pos);
            } else {
                float risePos[] = {state[X], state[Y], -0.4f};
                move(risePos);
            }
        }

        //TODO optimize the time we choose to deploy the mirror at
        static bool mirrorUsed = false;
        if(!mirrorUsed) {
            if(!game.posInDark(enemyState) && memoryFilled < 2) {
                faceEnemy();
                takePic();
            } else {
                game.useMirror();
                mirrorTime = time;
            }

            mirrorUsed = (game.getNumMirrorsHeld() == 0);
            if(!mirrorUsed) return;
        }
    }
    /*
     *  End game-ation (Dark)
     */
    else if(time < lightSwitchTimes[3]) {
        static bool goToPOI = !canTakePic();
        if(goToPOI) {
            int POI = getBestPOI();
            if(POI != 0) {
                float pos[3];
                game.getItemLoc(pos, POI);
                move(pos);
                return;
            }

            goToPOI = !canTakePic();
        }

        stopMoving();
    }
    
    
    if(memoryFilled == 2 || (memoryFilled == 1 &&
                    ((energy <= 1.5f && time > lightSwitchTimes[0]) || time > 170))) {
        faceEarth();
        uploadPics();
    } else {
        faceEnemy();
        takePic();
    }
}

int getBestPOI() {
    //TODO account for enemy's movement towards POI
    int ourSide = api.getTime() > 0 ? SCORE_PLUS_2 : SCORE_PLUS_3;
    int theirSide = api.getTime() > 0 ? SCORE_PLUS_3 : SCORE_PLUS_2;

    return
        (game.hasItem(ourSide) == ITEM_ON_FIELD) ? ourSide :
        ((game.hasItem(SCORE_PLUS_4) == ITEM_ON_FIELD) ? SCORE_PLUS_4 :
        ((game.hasItem(theirSide) == ITEM_ON_FIELD) ? theirSide :
        ((game.hasItem(SCORE_PLUS_1) == ITEM_ON_FIELD) ? SCORE_PLUS_1 : 0)));
}

void faceEnemy() {
    float towardsEnemy[3];
    mathVecSubtract(towardsEnemy, enemyState, state, 3);
    mathVecNormalize(towardsEnemy, 3);
    api.setAttitudeTarget(towardsEnemy);
}

bool isFacingEarth() {
    float earth[3];
    memcpy(earth, EARTH, 3 * sizeof(float));
    return mathVecInner(&state[NX], earth, 3) > MAX_FACING_ANGLE;
}

void faceEarth() {
    // Stop rotating once we are facing the Earth to avoid the equilibrium
    // effect against random noise
    if(isFacingEarth()) {
        stopRotating();
        return;
    }

    // Earth is actually at (0, 0, 1), however by setting the point farther
    // away, the resulting attitude vector will be greater
    float earth[] = {0.0f, 0.0f, 5.0f};
    float towardsEarth[3];
    mathVecSubtract(towardsEarth, earth, state, 3);
    mathVecNormalize(towardsEarth, 3);
    api.setAttitudeTarget(towardsEarth);
}

bool canTakePic() {
    return !game.posInDark(enemyState) &&
        enemyMirrorRemaining == 0 &&
        energy >= 1.0f &&
        game.isCameraOn() &&
        memoryFilled < game.getMemorySize() &&
        game.getMirrorTimeRemaining() == 0;
}

void takePic() {
    // We're too cheap to call getPicPoints
    if(canTakePic()) {
        game.takePic();
    }
}

void uploadPics() {
    if(memoryFilled > 0 &&
            energy >= 1.0f &&
            mathVecMagnitude(&state[WX], 3) <= 0.05f &&
            time - mirrorTime > 24 &&
            isFacingEarth()) {
        game.uploadPics();
    }
}

float mathVecDistance(float a[3], float b[3]) {
    float diff[3];
    mathVecSubtract(diff, a, b, 3);
    return mathVecMagnitude(diff, 3);
}