#include "ncnf.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#define CHECK(X, Y) if (X == NULL) { \
    printf(Y);                       \
    return NULL;                     \
}


ncnf* nreadDIMACS(char* path) {
    ncnf* c = malloc(sizeof(ncnf));
    CHECK(c, "Failed to alloc CNF struct\n")

    FILE* f = fopen(path, "r");
    CHECK(f, "Failed to open file\n")

    u32 bufsize = 1000;
    char* buf = malloc(sizeof(char) * bufsize);
    CHECK(buf, "Failed to alloc read buffer\n")

    CHECK(fgets(buf, sizeof(char) * bufsize, f), "Failed to read file\n")

    while (buf[0] == 'c') {
        CHECK(fgets(buf, sizeof(char) * bufsize, f), "Failed to read file\n");
    }

    char* temp = buf;
    while (*temp == ' ' || *temp == 'p' || *temp == 'c' || *temp == 'n' || *temp == 'f') temp++;

    c->varcnt = 0;
    while (((u8) (*temp - '0')) < 10) {
        c->varcnt *= 10;
        c->varcnt += (*temp - '0');
        temp++;
    }

    // Skip any trailing whitespace
    while (*temp == ' ') temp++;

    // Read in clausecnt
    c->clausecnt = 0;
    while (((u8) (*temp - '0')) < 10) {
        c->clausecnt *= 10;
        c->clausecnt += (*temp - '0');
        temp++;
    }

    u32 nsize = sizeof(char) * (c->varcnt * 12LU) + 10;
    char* nbuf = realloc(buf, nsize);
    CHECK(nbuf, "Failed to realloc read buffer\n");

    buf = nbuf;

    u32 cap = 32;
    c->totcnt = 0;

    c->cptrs = malloc(sizeof(u32) * c->clausecnt);
    CHECK(c->cptrs, "Failed to allocate clause offsets\n")
    c->clens = malloc(sizeof(u32) * c->clausecnt);
    CHECK(c->clens, "Failed to allocate clause lens\n")
    c->lvars = malloc(sizeof(u32) * c->clausecnt);
    CHECK(c->lvars, "Failed to alloc clause lvars\n")

    c->varbak = calloc(cap, sizeof(u32));
    CHECK(c->varbak, "Failed to allocate variable backing array\n")
    c->parbak = malloc(sizeof(u8) * cap);
    CHECK(c->parbak, "Failed to allocate parity backing array\n");

    for (u32 i = 0; i < c->clausecnt; ++i) {
        CHECK(fgets(buf, nsize, f), "Failed to read clause\n")
        c->clens[i] = 0;
        c->cptrs[i] = c->totcnt;

        temp = buf;
        while (*temp == ' ') temp++;

        bool empty = true;
        bool pos = true;
        while (*temp != '\n' && *temp != '\0') {
            if (c->totcnt == cap) {
                u32 ncap = cap << 1U;
                c->varbak = realloc(c->varbak, sizeof(u32) * ncap);
                CHECK(c->varbak, "Failed to realloc variable backing array\n")
                memset(c->varbak + cap, 0, sizeof(u32) * cap);
                c->parbak = realloc(c->parbak, sizeof(u8) * ncap);
                CHECK(c->parbak, "Failed to realloc parity backing array\n")
                cap = ncap;
            }

            if (*temp == '-') {
                pos = false;
            } else if (((u8) (*temp - '0')) < 10) {
                if (*temp == '0' && c->clens[i] == 0 && c->varbak[c->totcnt] == 0) {
                    break;
                }
                c->varbak[c->totcnt] *= 10;
                c->varbak[c->totcnt] += (*temp - '0');

            } else {
                while (temp[1] == ' ') temp++;
                c->parbak[c->totcnt] = pos;
                c->clens[i]++;
                c->varbak[c->totcnt] -= 1;
                if (c->lvars[i] < c->varbak[c->totcnt]) c->lvars[i] = c->varbak[c->totcnt];
                empty = false;
                pos = true;
                (c->totcnt)++;
                if (temp[1] == '0') {
                    break;
                }
            }
            temp++;
        }

        if (empty) {
            printf("UNSAT: Empty clause %u\n", i);
            return NULL;
        }
    }

    for (u32 i = 0; i < c->clausecnt; ++i) {
        c->lvars[i] += 1;
        c->lvars[i] = c->varcnt - c->lvars[i];
    }

    c->varbak = realloc(c->varbak, sizeof(u32) * c->totcnt);
    CHECK(c->varbak, "Failed to do final resize on variable backing array\n")
    c->parbak = realloc(c->parbak, sizeof(u8) * c->totcnt);
    CHECK(c->parbak, "Failed to do final resize on parity backing array\n")
    free(buf);
    if (fclose(f)) {
        printf("Failed to close file\n");
        return NULL;
    }
    return c;
}

void nprintcnf(ncnf* c) {
    printf("p cnf %u %u\n", c->varcnt, c->clausecnt);
    for (u32 i = 0; i < c->clausecnt; ++i) {
        for (u32 j = 0; j < c->clens[i]; ++j) {
            printf("%s%u ", (c->parbak[c->cptrs[i] + j]) ? ("") : ("-"), c->varbak[c->cptrs[i] + j] + 1);
        }
        printf("0\n");
    }
    printf("\n");
}

void nprintcnf2(ncnf* c) {
    printf("p cnf %u %u\n", c->varcnt, c->clausecnt);
    for (u32 i = 0; i < c->clausecnt; ++i) {
        printf("lvr: %u - ", c->lvars[i]);
        for (u32 j = 0; j < c->clens[i]; ++j) {
            printf("%s%u ", (c->parbak[c->cptrs[i] + j]) ? ("") : ("-"), c->varbak[c->cptrs[i] + j] + 1);
        }
        printf("0\n");
    }
    printf("\n");
}

void nfreecnf(ncnf* c) {
    free(c->varbak);
    free(c->parbak);
    free(c->cptrs);
    free(c->clens);
    free(c->lvars);
    free(c);
}

const u32 RBITS2 = 8;
const u32 RSIZE2 = 1U << RBITS2;
const u32 RLVLS2 = (31U / RBITS2) + 1;
const u32 RMASK2 = RSIZE2 - 1;

void nsortlastnum(ncnf* c) {
    // Radix sort on the clauses based on their lvars
    bool v = false;
    u32* d = malloc(sizeof(u32) * c->clausecnt);
    u32* d2 = malloc(sizeof(u32) * c->clausecnt);
    u32* d22 = malloc(sizeof(u32) * c->clausecnt);



    u32 qb[RLVLS2 * RSIZE2];
    u32 qe[RLVLS2 * RSIZE2];
    memset(qb, 0, sizeof(u32) * RLVLS2 * RSIZE2);
    memset(qe, 0, sizeof(u32) * RLVLS2 * RSIZE2);

    for (u32 pass = 0; pass < RLVLS2; ++pass) {
        u32 shift = pass * RBITS2;

        for (u32 i = 0; i < c->clausecnt; ++i) {
            u32 val = UINT32_MAX - c->lvars[i];
            u32 ind = (val >> shift) & RMASK2;
            qe[pass * RSIZE2 + ind]++;
        }

        u32 uc = 0;
        for (u32 i = 0; i < RSIZE2; ++i) uc += (qe[pass * RSIZE2 + i] != 0);
        if (uc == 1) continue;

        qb[pass * RSIZE2] = 0;
        for (u32 i = 1; i < RSIZE2; ++i) {
            qb[pass * RSIZE2 + i] = qb[pass * RSIZE2 + i - 1] + qe[pass * RSIZE2 + i - 1];
        }

        for (u32 i = 0; i < c->clausecnt; ++i) {
            u32 val = UINT32_MAX - c->lvars[i];
            u32 ind = (val >> shift) & RMASK2;
            d[qb[pass * RSIZE2 + ind]] = c->cptrs[i];
            d2[qb[pass * RSIZE2 + ind]] = c->clens[i];
            d22[qb[pass * RSIZE2 + ind]] = c->lvars[i];
            qb[pass * RSIZE2 + ind]++;
            __builtin_prefetch(d2 + qb[pass * RSIZE2 + ind] + 1);
            __builtin_prefetch(d + qb[pass * RSIZE2 + ind] + 1);
        }

        u32* tptr = c->cptrs;
        c->cptrs = d;
        d = tptr;
        tptr = c->clens;
        c->clens = d2;
        d2 = tptr;
        tptr = c->lvars;
        c->lvars = d22;
        d22 = tptr;
        v = !v;
    }

    if (v) {
        u32* tptr = c->cptrs;
        c->cptrs = d;
        d = tptr;
        tptr = c->clens;
        c->clens = d2;
        d2 = tptr;
        tptr = c->lvars;
        c->lvars = d22;
        d22 = tptr;
        v = !v;
        memcpy(c->cptrs, d, c->clausecnt * sizeof(u32));
        memcpy(c->clens, d2, c->clausecnt * sizeof(u32));
        memcpy(c->lvars, d22, c->clausecnt * sizeof(u32));
    }
    free(d2);
    free(d22);
    
    u8* d3 = malloc(sizeof(u8) * c->clausecnt);
    // Radix sort the literals of each clause (for reasons, TM)
    u32 dlen = c->clausecnt;

    for (u32 k = 0; k < c->clausecnt; ++k) {
        if (c->clens[k] > dlen) {
            d = realloc(d, sizeof(u32) * c->clens[k]);
            d3 = realloc(d3, sizeof(u8) * c->clens[k]);
            if (d == NULL || d3 == NULL) {
                printf("I'm giving up\n");
            }
            dlen = c->clens[k];
        }

        memset(qb, 0, sizeof(u32) * RLVLS2 * RSIZE2);
        memset(qe, 0, sizeof(u32) * RLVLS2 * RSIZE2);

        for (u32 pass = 0; pass < RLVLS2; ++pass) {
            u32 shift = pass * RBITS2;

            for (u32 i = 0; i < c->clens[k]; ++i) {
                u32 val = c->varbak[c->cptrs[k] + i];
                u32 ind = (val >> shift) & RMASK2;
                qe[pass * RSIZE2 + ind]++;
            }

            u32 uc = 0;
            for (u32 i = 0; i < RSIZE2; ++i) uc += (qe[pass * RSIZE2 + i] != 0);
            if (uc == 1) continue;

            qb[pass * RSIZE2] = 0;
            for (u32 i = 1; i < RSIZE2; ++i) {
                qb[pass * RSIZE2 + i] = qb[pass * RSIZE2 + i - 1] + qe[pass * RSIZE2 + i - 1];
            }

            for (u32 i = 0; i < c->clens[k]; ++i) {
                u32 val = c->varbak[c->cptrs[k] + i];
                u32 ind = (val >> shift) & RMASK2;
                d[qb[pass * RSIZE2 + ind]] = c->varbak[c->cptrs[k] + i];
                d3[qb[pass * RSIZE2 + ind]] = c->parbak[c->cptrs[k] + i];
                qb[pass * RSIZE2 + ind]++;
                __builtin_prefetch(d + qb[pass * RSIZE2 + ind] + 1);
                __builtin_prefetch(d3 + qb[pass * RSIZE2 + ind] + 1);
            }

            memcpy(c->varbak + c->cptrs[k], d, sizeof(u32) * c->clens[k]);
            memcpy(c->parbak + c->cptrs[k], d3, sizeof(u8) * c->clens[k]);
        }
    }

    free(d);
    free(d3);


}


void nsort(ncnf* c) {
    bool v = false;
    u32 dlen = c->clens[0];
    u32* d = malloc(sizeof(u32) * dlen);
    u8* dpar = malloc(sizeof(u8) * dlen);

    u32 qb[RLVLS2 * RSIZE2];
    u32 qe[RLVLS2 * RSIZE2];

    for (u32 k = 0; k < c->clausecnt; ++k) {
        if (c->clens[k] > dlen) {
            d = realloc(d, sizeof(u32) * c->clens[k]);
            dpar = realloc(dpar, sizeof(u8) * c->clens[k]);
            if (d == NULL || dpar == NULL) {
                printf("I'm giving up\n");
            }
            dlen = c->clens[k];
        }

        memset(qb, 0, sizeof(u32) * RLVLS2 * RSIZE2);
        memset(qe, 0, sizeof(u32) * RLVLS2 * RSIZE2);

        for (u32 pass = 0; pass < RLVLS2; ++pass) {
            u32 shift = pass * RBITS2;

            for (u32 i = 0; i < c->clens[k]; ++i) {
                u32 val = c->varbak[c->cptrs[k] + i];
                u32 ind = (val >> shift) & RMASK2;
                qe[pass * RSIZE2 + ind]++;
            }

            u32 uc = 0;
            for (u32 i = 0; i < RSIZE2; ++i) uc += (qe[pass * RSIZE2 + i] != 0);
            if (uc == 1) continue;

            qb[pass * RSIZE2] = 0;
            for (u32 i = 1; i < RSIZE2; ++i) {
                qb[pass * RSIZE2 + i] = qb[pass * RSIZE2 + i - 1] + qe[pass * RSIZE2 + i - 1];
            }

            for (u32 i = 0; i < c->clens[k]; ++i) {
                u32 val = c->varbak[c->cptrs[k] + i];
                u32 ind = (val >> shift) & RMASK2;
                d[qb[pass * RSIZE2 + ind]] = c->varbak[c->cptrs[k] + i];
                dpar[qb[pass * RSIZE2 + ind]] = c->parbak[c->cptrs[k] + i];
                qb[pass * RSIZE2 + ind]++;
                __builtin_prefetch(d + qb[pass * RSIZE2 + ind] + 1);
                __builtin_prefetch(dpar + qb[pass * RSIZE2 + ind] + 1);
            }

            memcpy(c->varbak + c->cptrs[k], d, sizeof(u32) * c->clens[k]);
            memcpy(c->parbak + c->cptrs[k], dpar, sizeof(u8) * c->clens[k]);
        }
    }

    free(dpar);
    /*
    if (c->clausecnt > dlen) {
        d = realloc(d, sizeof(u32) * c->clausecnt);
        if (d == NULL) {
            printf("I'm giving up x2\n");
        }
        dlen = c->clausecnt;
    }
    u32* d2 = malloc(sizeof(u32) * c->clausecnt);
    u32* d3 = malloc(sizeof(u32) * c->clausecnt);

    memset(qb, 0, sizeof(u32) * RLVLS2 * RSIZE2);
    memset(qe, 0, sizeof(u32) * RLVLS2 * RSIZE2);

    for (u32 pass = 0; pass < RLVLS2; ++pass) {
        u32 shift = pass * RBITS2;

        for (u32 i = 0; i < c->clausecnt; ++i) {
            u32 val = c->varbak[c->cptrs[i]];
            u32 ind = (val >> shift) & RMASK2;
            qe[pass * RSIZE2 + ind]++;
        }

        u32 uc = 0;
        for (u32 i = 0; i < RSIZE2; ++i) uc += (qe[pass * RSIZE2 + i] != 0);
        if (uc == 1) continue;

        qb[pass * RSIZE2] = 0;
        for (u32 i = 1; i < RSIZE2; ++i) {
            qb[pass * RSIZE2 + i] = qb[pass * RSIZE2 + i - 1] + qe[pass * RSIZE2 + i - 1];
        }

        for (u32 i = 0; i < c->clausecnt; ++i) {
            u32 val =  c->varbak[c->cptrs[i]];
            u32 ind = (val >> shift) & RMASK2;
            d[qb[pass * RSIZE2 + ind]] = c->cptrs[i];
            d2[qb[pass * RSIZE2 + ind]] = c->clens[i];
            d3[qb[pass * RSIZE2 + ind]] = c->lvars[i];
            qb[pass * RSIZE2 + ind]++;
            __builtin_prefetch(d2 + qb[pass * RSIZE2 + ind] + 1);
            __builtin_prefetch(d + qb[pass * RSIZE2 + ind] + 1);
        }

        u32* tptr = c->cptrs;
        c->cptrs = d;
        d = tptr;
        tptr = c->clens;
        c->clens = d2;
        d2 = tptr;
        tptr = c->lvars;
        c->lvars = d3;
        d3 = tptr;
        v = !v;
    }

    if (v) {
        u32* tptr = c->cptrs;
        c->cptrs = d;
        d = tptr;
        tptr = c->clens;
        c->clens = d2;
        d2 = tptr;
        tptr = c->lvars;
        c->lvars = d3;
        d3 = tptr;
        v = !v;
        memcpy(c->cptrs, d, c->clausecnt * sizeof(u32));
        memcpy(c->clens, d2, c->clausecnt * sizeof(u32));
        memcpy(c->lvars, d3, c->clausecnt * sizeof(u32));
    }
     */
    free(d);
    //free(d2);
    // free(d3);
}