wave.c

static inline void wv_2d_constrain(wave_2d_t* wave, state_2d_t* focus, bool* update_focus, state_2d_t* subject, bool* update_subject) {
    int32_t sx = (((int32_t)subject->x) - ((int32_t)focus->x)) % wave->width;
    if (sx >  (int32_t)wave->width / 2) sx -= wave->width;
    if (sx < -(int32_t)wave->width / 2) sx += wave->width;
    int32_t sy = (((int32_t)subject->y) - ((int32_t)focus->y)) % wave->height;
    if (sy >  (int32_t)wave->height / 2) sy -= wave->height;
    if (sy < -(int32_t)wave->height / 2) sy += wave->height;

    debug_assertf(abs(sx) < wave->source->n && abs(sy) < wave->source->n,
        "Delta-x/delta-y should have absolute values of less than n! dx: %i, dy: %i, n: %u.", sx, sy, wave->source->n);

    if (focus->observed && subject->observed) {
        // debug_print("Making assertion. Visualization, subject overlapping focus:");
        // src_2d_dbg_visualize_collision(wave->source,
        //                                wave->source->samples[focus->cfg.collapsed],
        //                                wave->source->samples[subject->cfg.collapsed],
        //                                sx, sy);
        // debug_print("And, focus overlapping subject:");
        // src_2d_dbg_visualize_collision(wave->source,
        //                                wave->source->samples[subject->cfg.collapsed],
        //                                wave->source->samples[focus->cfg.collapsed],
        //                                -sx, -sy);
        assert(src_2d_samples_coexist(wave->source->n,
                                      wave->source->samples[focus->cfg.collapsed],
                                      wave->source->samples[subject->cfg.collapsed],
                                      sx, sy));
    } else if (focus->observed && !subject->observed) {
        bsiter64_t subj_states = bs64_iter(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples));
        for (ptrdiff_t s_state = bs64_next(&subj_states); s_state != -1; s_state = bs64_next(&subj_states)) {
            if (!src_2d_samples_coexist(wave->source->n,
                                        wave->source->samples[focus->cfg.collapsed],
                                        wave->source->samples[s_state],
                                        sx, sy)) {
                bs_raw64_clear(subject->cfg.bits, s_state);
                *update_subject = true;
            }
        }
    } else if (!focus->observed && subject->observed) {
        bsiter64_t focus_states = bs64_iter(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples));
        for (ptrdiff_t f_state = bs64_next(&focus_states); f_state != -1; f_state = bs64_next(&focus_states)) {
            if (!src_2d_samples_coexist(wave->source->n,
                                        wave->source->samples[f_state],
                                        wave->source->samples[subject->cfg.collapsed],
                                        sx, sy)) {
                bs_raw64_clear(focus->cfg.bits, f_state);
                *update_focus = true;
            }
        }
    } else if (!focus->observed && !subject->observed) {
        bool done = false;
        size_t sanity = 0;
        while (!done) {
            done = true;

            uint64_t focus_coexist[bs_raw64_size(wave->source->n_distinct_samples)];
            uint64_t subj_coexist[bs_raw64_size(wave->source->n_distinct_samples)];

            for (size_t i = 0; i < bs_raw64_size(wave->source->n_distinct_samples); i++) {
                focus_coexist[i] = subj_coexist[i] = 0;
            }

            bsiter64_t focus_states = bs64_iter(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples));
            for (ptrdiff_t f_state = bs64_next(&focus_states); f_state != -1; f_state = bs64_next(&focus_states)) {
                bsiter64_t subj_states = bs64_iter(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples));
                for (ptrdiff_t s_state = bs64_next(&subj_states); s_state != -1; s_state = bs64_next(&subj_states)) {
                    if (src_2d_samples_coexist(wave->source->n,
                                               wave->source->samples[f_state],
                                               wave->source->samples[s_state],
                                               sx, sy)) {
                        bs_raw64_set(focus_coexist, f_state);
                        bs_raw64_set(subj_coexist, s_state);
                    }
                }
            }

            size_t focus_count = bs_raw64_count(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples));
            size_t subj_count = bs_raw64_count(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples));

            // debug_printf("Coexistable states: focus: %lu, subj: %lu.", focus_count, subj_count);
            // debug_printf("Coexisting states: focus: %lu, subj: %lu.",
            //     bs_raw64_count(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples)),
            //     bs_raw64_count(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples)));

            bs_raw64_intersect(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples), focus_coexist, bs_raw64_size(wave->source->n_distinct_samples));
            bs_raw64_intersect(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples), subj_coexist, bs_raw64_size(wave->source->n_distinct_samples));

            // debug_printf("After intersection: focus: %lu, subj: %lu.",
            //     bs_raw64_count(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples)),
            //     bs_raw64_count(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples)));

            if (focus_count > bs_raw64_count(focus->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples))) {
                done = false;
                *update_focus = true;
            }

            if (subj_count > bs_raw64_count(subject->cfg.bits, bs_raw64_size(wave->source->n_distinct_samples))) {
                done = false;
                *update_subject = true;
            }
        }
    }
}