zzz

#define DEBUG_MODE 0
#define VAL_INF 1000.0f
#define NUM_DEBRIS_2D 11
#define NUM_LAYERS 3
#define NUM_WAYPOINTS 3 /// puncte road
#define SET_POSITION_TARGET_THRESHOLD 0.2f
#define SET_POSITION_TARGET_WAYPOINT 0.01f
#define SET_WAYPOINT_VELOCITY 0.02f
#define DELTA_RVOUS_Y 0.2f
#define RVOUS_VELOCITY 0.005f
#define RVOUS_ANGULAR 0.0175f

#define COMPARE_EPS 0.005
#define NUM_AFFECTED_INT_2D (NUM_DEBRIS_2D + 2) // trebuie modificat la 3D
#define INTERVAL_EPS (DIM_ROBOT / 2.0)

float MOVE_SPEED_COEFFICIENT;

#define DIM_ROBOT 0.25 /// 0.22
#define DIM_AREA_X 0.66 /// 0.64
float DIM_DEBRIS[NUM_LAYERS];
float PENALTY_DEBRIS[NUM_LAYERS];

struct Point{
    double x, y, z;
};
struct Debris{
    Point position;
    float radius;
};

struct Diapazon{
    int index;
    double left, right;
    Diapazon (int INDEX = 0, int LEFT = 0, int RIGHT = 0) : index(INDEX), left(LEFT), right(RIGHT) {}
};

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

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

inline our_abs(double x) { // there should be no my_abs()
    if (x > 0.0)
        return x;
    return -x;
}

inline bool is_equal(double A, double B) {
    return our_abs(A - B) < COMPARE_EPS;
}

inline bool greater(Diapazon A, Diapazon B){
    if (is_equal(A.left, B.left))
        return A.right > B.right;
    return A.left < B.left;
}

void sort(Diapazon v[], int n) {
    for(int i = 0; i < n; i++) {
        for(int j = i + 1; j < n; j++)
            if(!greater(v[i], v[j])) {
                Diapazon aux = v[i];
                v[i] = v[j];
                v[j] = aux;
            }
    }
}
inline int layer(int index){
    if(index <= 3) return 0;
    if(index <= 7) return 1;
    return 2;
}

void move_thruster_damage () {
    int health = game.getThrusterHealth ();
    if ( health <= 85 && myZRState[1] <= 0.6 ) /// imminent collision with a green asteroid penalty >= 10 && penalty < 25
        MOVE_SPEED_COEFFICIENT = 0.125f;

    if ( health <= 65 && myZRState[1] <= 0.4 ) /// imminent collision with a purple asteroid
        MOVE_SPEED_COEFFICIENT = 0.075f;
}

float debris_pos[NUM_DEBRIS_2D][4];
Debris debris[NUM_DEBRIS_2D];
Diapazon affected_interval[NUM_DEBRIS_2D];
Diapazon found_gap;
float expected_penalty;
int road_index;
int excluded_ast[NUM_DEBRIS_2D]; /// asteroizi exclusi
int time_stop; /// the robot should stop at least 3 seconds at a waypoint to reset momentum

float target[3]; /// target position
float myZRState[12]; /// current position
float otherZRState[12]; /// other position
Diapazon interv[NUM_AFFECTED_INT_2D]; /// bitmask
Point road[NUM_WAYPOINTS]; /// traseu

void move ( float positionToMoveTo[3] ) {
    move_thruster_damage ();
    float delta[3]; /// auxiliary vector for subtraction
    mathVecSubtract(delta, positionToMoveTo, myZRState, 3); // Set delta to be the displacement of positionToMove to from myZRState.
    float velocity[3] = { myZRState[3], myZRState[4], myZRState[5] };
    bool rvous_result = false;
    float smallarea_coef = 1.0f;

    if ( road_index > 0 )
        smallarea_coef = 3.0f;

    if ( mathVecMagnitude ( delta, 3 ) > SET_POSITION_TARGET_THRESHOLD ) {
        time_stop = api.getTime ();
        float between[3];
        for ( int index = 0; index < 3; index++ )
            between[index] = MOVE_SPEED_COEFFICIENT * ( positionToMoveTo[index] - myZRState[index] );

        api.setVelocityTarget (between);
    }
    else if ( mathVecMagnitude ( delta, 3 ) <= smallarea_coef * SET_POSITION_TARGET_WAYPOINT
    && ( mathVecMagnitude ( velocity, 3 ) <= SET_WAYPOINT_VELOCITY || road_index > 0 )
    && api.getTime() - time_stop >= 3 && road_index + 1 < NUM_WAYPOINTS ) {
        DEBUG (( "next road index %d Ox vrut %f Ox curent %f speed %f", road_index, road[road_index].x, myZRState[0], mathVecMagnitude ( velocity, 3 ) ));
        road_index++;
    }
    else { /// close enough, shift to other moving function
        api.setPositionTarget(positionToMoveTo);

        if ( road_index + 1 == NUM_WAYPOINTS )
        {
            /// doesn't point at target
            DEBUG (( "attempt rendezvous" ));
            float attitude[3] = { otherZRState[6], otherZRState[7], otherZRState[8] };
            api.setAttitudeTarget ( attitude );
            float angular[3] = { myZRState[9], myZRState[10], myZRState[11] };

            if ( mathVecMagnitude ( velocity, 3 ) < RVOUS_VELOCITY && mathVecMagnitude ( angular, 3 ) < RVOUS_ANGULAR )
            {
                DEBUG (( "try" ));
                rvous_result = game.completeRendezvous ();
            }

            if ( rvous_result == true )
                road_index++;
        }
        else
            DEBUG(("pos x %f curr %f v %f time diff %d", positionToMoveTo[0], myZRState[0], mathVecMagnitude ( velocity, 3 ), api.getTime() - time_stop ));
    }
}

void init(){
    MOVE_SPEED_COEFFICIENT = 0.15f;
    road_index = 0;

    /// precalc
    DIM_DEBRIS[0] = 0.030;
    DIM_DEBRIS[1] = 0.045;
    DIM_DEBRIS[2] = 0.1; /// 0.090 -- reuseste sa se atinga ff usor de asteroizii mari desi nu trebuie
    PENALTY_DEBRIS[0] = 10.0;
    PENALTY_DEBRIS[1] = 25.0;
    PENALTY_DEBRIS[2] = 50.0;

    game.getDebris (debris_pos);

    for(int i = 0; i < NUM_DEBRIS_2D; i++){
        debris[i].position.x = debris_pos[i][0];
        debris[i].position.y = debris_pos[i][1];
        debris[i].position.z = debris_pos[i][2];
        debris[i].radius = debris_pos[i][3];

        affected_interval[i] = Diapazon(i, debris[i].position.x - debris[i].radius - INTERVAL_EPS, debris[i].position.x + debris[i].radius + INTERVAL_EPS);
    }

    affected_interval[NUM_AFFECTED_INT_2D - 2] = Diapazon(-1, -DIM_AREA_X * 2.0, -DIM_AREA_X + INTERVAL_EPS); // interval auxiliar in stanga
    affected_interval[NUM_AFFECTED_INT_2D - 1] = Diapazon(-1, DIM_AREA_X - INTERVAL_EPS, 2.0 * DIM_AREA_X); // interval auxiliar in dreapta

    sort(affected_interval, NUM_AFFECTED_INT_2D);

    for(int i = 0; i < NUM_DEBRIS_2D + 2; i++){
        DEBUG(("index initial: %d -> %f %f", affected_interval[i].index, affected_interval[i].left, affected_interval[i].right));
    }

    double min_penalty = 100.0, penalty;

    ///COD SCHIMBAT AICI

    int num_interv = 0;
    interv[num_interv++] = affected_interval[0];
    for (int i = 1; i < NUM_AFFECTED_INT_2D; ++i)
        if (interv[num_interv - 1].right < affected_interval[i].right)
            interv[num_interv++] = affected_interval[i];

    double left = 0.0, right = 0.0;
    for (int i = 0; i < (1 << num_interv); ++i) {
        penalty = 0.0;
        for (int j = 1; j < num_interv - 1; ++j)
            if (i & (1 << interv[j].index))
              penalty += PENALTY_DEBRIS[layer(interv[j].index)];
        if (penalty < min_penalty) {
            bool ok = 0;
            double maximum = interv[0].right;
            for (int j = 1; j < num_interv && ok == 0; ++j)
                if (interv[j].index == -1 || (i & (1 << interv[j].index)) == 0){
                    if (interv[j].left > maximum){
                        ok = 1;
                        left = maximum;
                        right = interv[j].left;
                        min_penalty = penalty;
                    }
                    maximum = max(maximum, interv[j].right);
                }
        }
    }

    found_gap.left = left;
    found_gap.right = right;
    expected_penalty = min_penalty;
    // DEBUG(("%f %f %f", left, right, min_penalty));

    /// CODMEISTER AICI

    DEBUG(("gap ox %f penalty %f", best_gap, expected_penalty));

    api.getMyZRState( myZRState );
    /// movement
    road[0].x = best_gap; road[0].y = myZRState[1]; road[0].z = myZRState[2];
    road[1].x = road[0].x; road[1].y = -0.3; road[1].z = road[0].z;
    /// time
    time_stop = api.getTime ();
}

void loop(){
    api.getMyZRState( myZRState );
    api.getOtherZRState( otherZRState );
    /// update road[2]
    road[2].x = otherZRState[0]; road[2].y = otherZRState[1] + DELTA_RVOUS_Y; road[2].z = otherZRState[2];

    target[0] = road[road_index].x;
    target[1] = road[road_index].y;
    target[2] = road[road_index].z;

    if ( road_index < NUM_WAYPOINTS )
        move( target );
}