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

void printIntInBinary(unsigned int n);
void intToBinary(unsigned int n, char* p);
void strrev(char* p, unsigned int len);
unsigned int mod32BitMersenne(unsigned int x, unsigned int m);

int main() {

    unsigned int q = 2;
    unsigned int x = pow(2, 2*q) - 1; // pow(2, 2*q) is the max value for x
    unsigned int p = pow(2, q) - 1;
    unsigned int y;

    y=(x&p)+(x>>q);
    if (y>=p) y-=p;

    printf("q = %d, x = %d, p = %d, y = %d\n", q, x, p, y);
    printf("x&p = %d, x>>q = %d\n", x&p, x>>q);

    printIntInBinary(x);
    printIntInBinary(p);
    printIntInBinary(x&p);
    printIntInBinary(x>>q);


    printf("\n");
    printIntInBinary( ((unsigned int)0b1111101111011101101<<26)>>26 );
    printIntInBinary( ((unsigned int)0b1111101111011101101<<27)>>27 );
    printIntInBinary( ((unsigned int)0B1111101111011101101<<28)>>28 );
    printIntInBinary( ((unsigned int)0B1111101111011101101<<29)>>29 );
    printIntInBinary( ((unsigned int)0B1111101111011101101<<30)>>30 );

    unsigned int a = 0b0110000000;
    unsigned int b = 0b1111111001;
    printf("\n");
    printIntInBinary(a);
    printIntInBinary(b);
    b = (b & ~0b110) | ((a & 0b110000000) >> 6);
    printIntInBinary(a);
    printIntInBinary(b);



    /* // Too general an approach */
    /* // TODO: Special case where ax+b < p < m */
    /* // ((ax+b) mod p) mod m, p=2^89-1, m=2^20 */
    /* size_t r = 89; // aka q */
    /* size_t bits = 32; */
    /* size_t remainingBits = r; */
    /* size_t lastIndex = 0; */
    /* size_t len = 4; */
    /* uint32_t data[len], tmp[len]; */

    /* // mod p, p = 2^89-1 */
    /* // x&p */
    /* for (size_t i = 0; (i+1)*bits < r; i++ ) { */
    /*     tmp[i] = data[i]&0b1111'1111'1111'1111'1111'1111'1111'1111; // 32 ones. Standard since C++14 */
    /*     remainingBits -= 32; */
    /*     lastIndex = i+1; */
    /* } */
    /* size_t shiftLen = bits-remainingBits; // TODO: Check for 1-off errors */
    /* tmp[lastIndex] = (data[lastIndex]<<shiftLen)>>shiftLen; // mod 2^remainingBits */
    /* // x>>q */
    /* /1* for (size_t i = 0; i < len; i++) { *1/ */
         
    /* /1* } *1/ */

    /* // mod m, m = 2^20 */
    /* unsigned int mMinusOne = 0b1111'1111'1111'1111'1111; // 20 ones. Standard since C++14 */
    /* data[len-1] = data[len-1]&mMinusOne; */



    /* // Can be optimized further */
    /* size_t len = 4; // input parameter */
    /* uint32_t data[len], tmp[len]; */

    /* //// mod p, p = 2^89-1 */
    /* // x&p */
    /* tmp[len] = data[len]&0b1111'1111'1111'1111'1111'1111'1111'1111; // 32 ones. */
    /* tmp[len] = data[len]&0b1111'1111'1111'1111'1111'1111'1111'1111; // 32 ones. */
    /* tmp[len] = data[len]&0b1'1111'1111'1111'1111'1111'1111; // remaining bits = 89-32*2 = 25 ones. */

    /* // x>>q */

    /* //// mod m, m = 2^20 */
    /* unsigned int mMinusOne = 0b1111'1111'1111'1111'1111; // 20 ones. */
    /* data[len-1] = data[len-1]&mMinusOne; */



    /* // Optimize more! */
    /* size_t len = 4; // input parameter */
    /* uint32_t data[len]; // input parameter */ 
    /* uint32_t tmp; */

    /* //// mod p, p = 2^89-1 */
    /* // x&p */
    /* tmp = data[len-1]&0b1111'1111'1111'1111'1111; // 20 ones */
    /* // x>>q */
    /* // 89 - 2*32 = 25 so */
    /* tmp = tmp + (data[len]>>5)&0b1111'1111'1111'1111'1111; // 20 ones */

    /* //// mod m, m = 2^20 */
    /* tmp = tmp&0b1111'1111'1111'1111'1111; // 20 ones = m - 1 */

    /* // clean up before output */
    /* for (size_t i = 0; i < len; i++) */
    /*     data[i] = 0; */
    /* // output */
    /* data[len-1] = tmp; */



    /* // More!!! */
    /* size_t len = 4; // input parameter */
    /* uint32_t data[len]; // input parameter */ 

    /* //// mod p, p = 2^89-1 */
    /* // x&p */
    /* data[len-1] = data[len-1]&0b1111'1111'1111'1111'1111; // 20 ones */
    /* // x>>q */
    /* // 89 - 2*32 = 25 so */
    /* data[len-1] = data[len-1] + ((data[len-3]>>5)&0b1111'1111'1111'1111'1111); // 20 ones */

    /* //// mod m, m = 2^20 */
    /* data[len-1] = data[len-1]&0b1111'1111'1111'1111'1111; // 20 ones = m - 1 */

    /* // data[len-1] is the result */



    size_t len = 4; // input parameter
    uint32_t data[len]; // input parameter 
    data[len-1] = (data[len-1] + (data[len-3]>>5))&0b1111'1111'1111'1111'1111; // 20 ones
    // data[len-1] is the result

    // Or as a function
    // void mod89mod20 (uint32_t * data, size_t len) {
    //     data[len-1] = (data[len-1] + (data[len-3]>>5))&0b1111'1111'1111'1111'1111; // 20 ones
    // }

    return EXIT_SUCCESS;
}

template <typename T>
T mod (T x, T p) {
    while (x >= p)
        x -= p;
    return x;

}

void printIntInBinary(unsigned int n) {
    char b[100];
    intToBinary(n, b);
    printf("%5u = %10s\n", n, b);
}

void intToBinary(unsigned int n, char* p) {
    if (NULL == p)
        return;

    char* q = p;

    int count = 0;
    do {
        if (n % 2 == 0)
            *p = '0';
        else
            *p = '1';

        p++;

        n = n>>1;
        count++;
    } while (n > 0);
    *p = '\0';
    strrev(q, count);

}

void strrev(char* p, unsigned int len) {
   char tmp;
   int i, j = 0;

   i = 0;
   j = len - 1;

   while (i < j) {
      tmp = p[i];
      p[i] = p[j];
      p[j] = tmp;
      i++;
      j--;
   }
}