#include <stdio.h>
#include <stdint.h>
#include <assert.h>
#include <strings.h>

/*
For mem = 65536 bits:
bit 0 <- 0
bit 1 <- 8192
bit 2 <- 16384
bit 3 <- 24576
bit 4 <- 32768
bit 5 <- 40960
bit 6 <- 49152
bit 7 <- 57344
bit 8 <- 8193
bit 9 <- 16385
bit 10 <- 24577
bit 11 <- 32769
bit 12 <- 40961
bit 13 <- 1281
bit 14 <- 49153
bit 15 <- 57345

For mem = 32768 bits:
bit 0 <- 0
bit 1 <- 8192
bit 2 <- 16384
bit 3 <- 24576
bit 4 <- 1
bit 5 <- 8193
bit 6 <- 16385
bit 7 <- 24577
bit 8 <- 2
bit 9 <- 8194
bit 10 <- 16386
bit 11 <- 24578
bit 12 <- 3
bit 13 <- 8195
bit 14 <- 16387
bit 15 <- 24579
*/

uint32_t xorshift32(uint32_t x)
{
	/* Algorithm "xor" from p. 4 of Marsaglia, "Xorshift RNGs" */
	x = x ^ (x << 13);
	x = x ^ (x >> 17);
	x = x ^ (x << 5);
	return x;
}

uint32_t get_rand(uint32_t x) {
    uint32_t out = 0;
    int i;
    for (i = 0; i < 32; i++) {
        x = xorshift32(x);
        if ((x & 1) == 1)
            out = out | (1<< i);
    }
    return out;
}

static uint8_t get_bit(uint32_t *field, int offset)
{
    // printf("offset&31: %d\n", offset & 31);
    // printf("offset/sizeof(*field): %d\n", offset >> 5);
    return !!(field[offset >> 5] & (1 << (offset & 31)));
}

static uint8_t set_bit(uint32_t *field, int offset)
{
    field[offset >> 5] |= (1 << (offset & 31));
}

static uint8_t clear_bit(uint32_t *field, int offset)
{
    field[offset >> 5] &= ~(1 << (offset & 31));
}

static uint8_t reverse_u8(uint8_t b)
{
    b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
    b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
    b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
    return b;
}

static uint16_t reverse_u16(uint16_t nonreversed)
{
    uint16_t reversed = 0;

    for (uint16_t i = 0; i < 16; i++)
    {
        reversed |= (nonreversed >> (16 - i - 1) & 1) << i;
    }

    return reversed;
}

static uint32_t get_bit_offset(int x, int total_bits) {
    // return (8192 * (x & 7)) + (x >> 3);
    int bitshift = ffs(total_bits)-1;
    return ((x * 8192) % total_bits) + ((x*8192) >> bitshift);
}

int main(int argc, char **argv)
{
    int i;
    // uint32_t test_1[] = {1};
    // uint32_t test_2[] = {0, 1};
    // uint32_t test_3[] = {2, 0};
    // uint32_t test_4[] = {1, 0x10, 0, 8};

    // for (i = 0; i < sizeof(test_1)*8; i++) {
    //     if (get_bit(test_1, i))
    //         printf("test_1: bit %d set\n", i);
    // }
    // for (i = 0; i < sizeof(test_2)*8; i++) {
    //     if (get_bit(test_2, i))
    //         printf("test_2: bit %d set\n", i);
    // }
    // for (i = 0; i < sizeof(test_3)*8; i++) {
    //     if (get_bit(test_3, i))
    //         printf("test_3: bit %d set\n", i);
    // }
    // for (i = 0; i < sizeof(test_4)*8; i++) {
    //     if (get_bit(test_4, i))
    //         printf("test_4: bit %d set\n", i);
    // }

    uint32_t input[2048] = {};
    uint32_t output[2048] = {};

    for (i = 0; i < 32; i++) {
        printf("bit %d: %d\n", i, get_bit_offset(i, sizeof(input)*8));
    }

    uint32_t init = 1;
    for (i = 0; i < sizeof(input) / 4; i++)
    {
        init = get_rand(init);
        input[i] = init;
        // input[i] = 0;
    }
    /*
    input[0] = 0xf0000000;
    input[256] = 0xf0000000;
    input[512] = 0xc0000000;
    input[768] = 0xc0000000;
    input[1024] = 0xc0000000;*/
    // print_hex(input, sizeof(input), 0);
    // return;
    // memset(input, 0xff, sizeof(input));

    for (i = 0; i < sizeof(input) * 8; i++)
    {
        int bit;
        assert(get_bit_offset(i, sizeof(output)*8) < sizeof(output) * 8);
        bit = get_bit(input, get_bit_offset(i, sizeof(input)*8));
        // bit = get_bit(input, i);
        // if (bit)
        //     printf("bit %d is set\n", i);
        // int xform = ((i * 2048) + ((i * 2048) >> 16)) & 0xffff;
        // printf("%d\n", xform);
        // assert(xform < sizeof(output) * 8);
        if (bit)
            set_bit(output, i);
            // set_bit(output, get_bit_offset(i));
//         int j;
//         for (j = 0; j < 16; j++)
//         {
//             // printf("o16[%d] |= (!!(i8[1792+%d] & (1 << %d)) << 0;\n", i, i, 2*(15-j)+1);
//             o16[i] |= (!!(i8[1792+i] & (1 << 2*(15-j)+1))) << 0;
//             o16[i] |= (!!(i8[1792+i] & (1 << 2*(15-j)+0))) << 1;
//             o16[i] |= (!!(i8[1536+i] & (1 << 2*(15-j)+1))) << 2;
//             o16[i] |= (!!(i8[1536+i] & (1 << 2*(15-j)+0))) << 3;
//             o16[i] |= (!!(i8[1280+i] & (1 << 2*(15-j)+1))) << 4;
//             o16[i] |= (!!(i8[1280+i] & (1 << 2*(15-j)+0))) << 5;
//             o16[i] |= (!!(i8[1024+i] & (1 << 2*(15-j)+1))) << 6;
//             o16[i] |= (!!(i8[1024+i] & (1 << 2*(15-j)+0))) << 7;
//             o16[i] |= (!!(i8[768+i] & (1 << 2*(15-j)+1))) << 8;
//             o16[i] |= (!!(i8[768+i] & (1 << 2*(15-j)+0))) << 9;
//             o16[i] |= (!!(i8[512+i] & (1 << 2*(15-j)+1))) << 10;
//             o16[i] |= (!!(i8[512+i] & (1 << 2*(15-j)+0))) << 11;
//             o16[i] |= (!!(i8[256+i] & (1 << 2*(15-j)+1))) << 12;
//             o16[i] |= (!!(i8[256+i] & (1 << 2*(15-j)+0))) << 13;
//             o16[i] |= (!!(i8[0+i] & (1 << 2*(15-j)+1))) << 14;
//             o16[i] |= (!!(i8[0+i] & (1 << 2*(15-j)+0))) << 15;
// //             o16[0] = i
// //             if (get_bit(input, i * 16 + j))
// //                 set_bit(output, j * 256 + i);
// //             else
// //                 clear_bit(output, j * 256 + i);
//         }
    }

    // uint8_t *o8 = (uint8_t *)output;
    // for (i = 0; i < sizeof(output)/4; i++)
    //     o8[i] = reverse_u8(o8[i]);

    // for (i = 0; i < sizeof(output)/4; i++)
    //     o16[i] = reverse_u16(o16[i]);

    // print_hex(output, sizeof(output), 0);
    FILE *infile = fopen("memtest/mem.init", "w");
    FILE *outfile = fopen("outfile.txt", "w");
    for (i = 0; i < sizeof(input)/4; i++) {
        fprintf(infile, "%08x\n", input[i]);
        fprintf(outfile, "%08x\n", output[i]);
    }

    return 0;
}