/* This file is (c) 2013 Richard Barrell <rchrd@brrll.co.uk>, see LICENSE.txt */
/* (it's the ISC license, which is 2-clause BSD with simplified wording) */

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>


typedef struct {
    int32_t x;
    int32_t y;
} pair;


/* This struct defines a circular queue. */
typedef struct {
    /* Pointer to the beginning of the block of memory containing all of the
     * elements in the queue. */
    pair *pairs;
    size_t size; // Number of elements in the queue
    size_t head; // Index of first element in the queue
    size_t tail; // Index *after* last element in the queue
} queue;


/* Start a new queue */
static int initQueue(queue *q)
{
    q->pairs = (pair*) malloc(sizeof(pair));
    if (q->pairs == NULL) {
        /* Memory allocation failed */
        return -1;
    }
    q->size = 1;
    q->head = 0;
    q->tail = 0;
    return 0;
}


/* Clean up memory used by the queue */
static void freeQueue(queue *q) {
    free(q->pairs);
}


/* Return the number of elements in the queue. */
static size_t getNumElements(queue *q) {
    size_t gap = q->tail - q->head;
    if (q->head > q->tail) {
        gap += q->size;
    }
    return gap;
}


/* Given a pair, insert it into the queue */
static int addToQueue(queue *q, pair p) {
    if ((getNumElements(q) + 1) == q->size) {
        /* Adding this pair would put us over the size limit, so we need
         * to make more room. Allocate double the old memory used. */
        size_t oldSize = q->size;
        q->size = q->size * 2;
        q->pairs = (pair*) realloc(q->pairs, q->size * sizeof(pair));
        if (q->pairs == NULL) {
            /* Memory allocation failed */
            return -1;
        }
        if (q->tail < q->head) {
            /* The active contents of the queue "wrapped around" the end of
             * the ring; shuffle things around so that the bits before
             * and after the wrap are now contiguous.
             * (BBB***AA) -> (***********AABBB)
             */
            memcpy(q->pairs + oldSize, q->pairs, q->tail * sizeof(pair));
            q->tail = oldSize + q->tail;
        }
    }
    q->tail++;
    if (q->tail == q->size) {
        /* Hit the end of the queue; start wrapping around */
        q->tail = 0;
    }
    q->pairs[q->tail] = p;
    return 0;
}


/* Remove the oldest element from the queue and return it */
static pair queuePopLeft(queue *q) {
    size_t oldHead = q->head;
    q->head = q->head + 1;
    if (q->head == q->size) {
        q->head = 0;
    }
    return q->pairs[oldHead];
}


static int getbit(uint8_t *u, int32_t x, int32_t y, int32_t xMax) {
    int32_t pos = y * xMax + x;
    return u[pos];
}


static void setbit(uint8_t *u, int32_t x, int32_t y, int32_t xMax) {
    int32_t pos = y * xMax + x;
    u[pos] = 1;
}


int burnHeatMap(int32_t xMax, int32_t yMax,
        int32_t *heatMap,
        size_t goals_length, int32_t *goals_xs, int32_t *goals_ys)
{

#define IX(x, y) heatMap[(y)*xMax + (x)]

    int failing = 0;

    size_t used_sz = xMax * yMax;
    uint8_t *used = (uint8_t*) calloc(used_sz, 1);

    queue cell_todo;
    if (initQueue(&cell_todo) != 0) {
        failing = __LINE__ || -1;
        goto cleanup_none;
    }

    if (used == NULL) {
        failing = __LINE__ || -1;
        goto cleanup_queue;
    }

#define GETUSED(x, y) getbit(used, (x), (y), xMax)
#define SETUSED(x, y) setbit(used, (x), (y), xMax)
#define MAX(a,b) ((a)>(b)?(a):(b))
#define MIN(a,b) ((a)<(b)?(a):(b))

    for (int32_t y = 0; y < yMax; y++) {
        for (int32_t x = 0; x < xMax; x++) {
            if (IX(x, y) != 0) {
                SETUSED(x, y);
            }
            IX(x, y) = -1;
        }
    }

    for (size_t i = 0; i < goals_length; i++) {
        int32_t x = goals_xs[i], y = goals_ys[i];
        IX(x, y) = 0;
        SETUSED(x, y);
        pair xy = {x, y};
        if (addToQueue(&cell_todo, xy) != 0) {
            failing = __LINE__ || -1;
            goto cleanup_getNumElements;
        }
    }

    int count = getNumElements(&cell_todo);
    while (getNumElements(&cell_todo) > 0) {
        pair xy = queuePopLeft(&cell_todo);
        int32_t x = xy.x, y = xy.y;
        count--;
        int32_t cost = IX(x, y);
        for (int32_t yi = MAX(0, y - 1); yi < MIN(yMax, y + 2); yi++) {
            for (int32_t xi = MAX(0, x - 1); xi < MIN(xMax, x + 2); xi++) {
                if ((x == xi && y == yi) || GETUSED(xi, yi)) {
                    continue;
                }
                SETUSED(xi, yi);
                IX(xi, yi) = cost + 1;
                pair xiyi = {xi, yi};
                if (addToQueue(&cell_todo, xiyi) != 0) {
                    failing = __LINE__ || -1;
                    goto cleanup_getNumElements;
                }
                count++;
            }
        }
    }

 cleanup_getNumElements:
    free(used);
 cleanup_queue:
    freeQueue(&cell_todo);
 cleanup_none:
    return failing;
}