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

/*
#include "allvars.h"
#include "proto.h"
#include "domain.h"*/


/*! \file peano.c
 *  \brief constructs Peano-Hilbert ordering of particles
 */

typedef  long long int peanokey;
void peano_hilbert_key_(long long int *x, long long int *y, long long int *z, long long int *bits, peanokey *key);
void peano_hilbert_key_inverse_(long long int *key, long long int *bits, long long int *x, long long int *y, long long int *z);

static struct peano_hilbert_data
{
  peanokey key;
  int index;
}
 *mp;

static int *Id;


#define DOUBLE_to_PEANOGRID(y) ((int)(((*((long long *) &y)) & 0xFFFFFFFFFFFFF) >> (52 - PEANO_BITS_PER_DIMENSION)))





static char quadrants[24][2][2][2] = {
  /* rotx=0, roty=0-3 */
  {{{0, 7}, {1, 6}}, {{3, 4}, {2, 5}}},
  {{{7, 4}, {6, 5}}, {{0, 3}, {1, 2}}},
  {{{4, 3}, {5, 2}}, {{7, 0}, {6, 1}}},
  {{{3, 0}, {2, 1}}, {{4, 7}, {5, 6}}},
  /* rotx=1, roty=0-3 */
  {{{1, 0}, {6, 7}}, {{2, 3}, {5, 4}}},
  {{{0, 3}, {7, 4}}, {{1, 2}, {6, 5}}},
  {{{3, 2}, {4, 5}}, {{0, 1}, {7, 6}}},
  {{{2, 1}, {5, 6}}, {{3, 0}, {4, 7}}},
  /* rotx=2, roty=0-3 */
  {{{6, 1}, {7, 0}}, {{5, 2}, {4, 3}}},
  {{{1, 2}, {0, 3}}, {{6, 5}, {7, 4}}},
  {{{2, 5}, {3, 4}}, {{1, 6}, {0, 7}}},
  {{{5, 6}, {4, 7}}, {{2, 1}, {3, 0}}},
  /* rotx=3, roty=0-3 */
  {{{7, 6}, {0, 1}}, {{4, 5}, {3, 2}}},
  {{{6, 5}, {1, 2}}, {{7, 4}, {0, 3}}},
  {{{5, 4}, {2, 3}}, {{6, 7}, {1, 0}}},
  {{{4, 7}, {3, 0}}, {{5, 6}, {2, 1}}},
  /* rotx=4, roty=0-3 */
  {{{6, 7}, {5, 4}}, {{1, 0}, {2, 3}}},
  {{{7, 0}, {4, 3}}, {{6, 1}, {5, 2}}},
  {{{0, 1}, {3, 2}}, {{7, 6}, {4, 5}}},
  {{{1, 6}, {2, 5}}, {{0, 7}, {3, 4}}},
  /* rotx=5, roty=0-3 */
  {{{2, 3}, {1, 0}}, {{5, 4}, {6, 7}}},
  {{{3, 4}, {0, 7}}, {{2, 5}, {1, 6}}},
  {{{4, 5}, {7, 6}}, {{3, 2}, {0, 1}}},
  {{{5, 2}, {6, 1}}, {{4, 3}, {7, 0}}}
};


static char rotxmap_table[24] = { 4, 5, 6, 7, 8, 9, 10, 11,
  12, 13, 14, 15, 0, 1, 2, 3, 17, 18, 19, 16, 23, 20, 21, 22
};

static char rotymap_table[24] = { 1, 2, 3, 0, 16, 17, 18, 19,
  11, 8, 9, 10, 22, 23, 20, 21, 14, 15, 12, 13, 4, 5, 6, 7
};

static char rotx_table[8] = { 3, 0, 0, 2, 2, 0, 0, 1 };
static char roty_table[8] = { 0, 1, 1, 2, 2, 3, 3, 0 };

static char sense_table[8] = { -1, -1, -1, +1, +1, -1, -1, -1 };

static int flag_quadrants_inverse = 1;
static char quadrants_inverse_x[24][8];
static char quadrants_inverse_y[24][8];
static char quadrants_inverse_z[24][8];


void peano_hilbert_key_(long long int *x, long long int *y, long long int *z, long long int *bits, peanokey *keyr)
{
  long long int i, bitx, bity, bitz, mask, quad, rotation;
  char sense, rotx, roty;
  peanokey key;

  /*printf("*x %d *y %d *z %d *bits %d sizeof(int) %d%\n",*x,*y,*z,*bits,sizeof(int));*/
  mask = 1 << (*bits - 1);
  key = 0;
  rotation = 0;
  sense = 1;



  for(i = 0; i < *bits; i++, mask >>= 1)
    {
      bitx = (*x & mask) ? 1 : 0;
      bity = (*y & mask) ? 1 : 0;
      bitz = (*z & mask) ? 1 : 0;

      quad = quadrants[rotation][bitx][bity][bitz];

      key <<= 3;
      key += (sense == 1) ? (quad) : (7 - quad);

      rotx = rotx_table[quad];
      roty = roty_table[quad];
      sense *= sense_table[quad];

      while(rotx > 0)
	{
	  rotation = rotxmap_table[rotation];
	  rotx--;
	}

      while(roty > 0)
	{
	  rotation = rotymap_table[rotation];
	  roty--;
	}
    }

  *keyr = key;
   

  return;
}


void peano_hilbert_key_inverse_(long long int *key, long long int *bits, long long int *x, long long int *y, long long int *z)
{
  long long int i, keypart, bitx, bity, bitz, mask, quad,rotation;
  long long int shift;
  long long int seven;
  char sense, rotx, roty;

  if(flag_quadrants_inverse)
    {
      flag_quadrants_inverse = 0;
      for(rotation = 0; rotation < 24; rotation++)
	for(bitx = 0; bitx < 2; bitx++)
	  for(bity = 0; bity < 2; bity++)
	    for(bitz = 0; bitz < 2; bitz++)
	      {
		quad = quadrants[rotation][bitx][bity][bitz];
		quadrants_inverse_x[rotation][quad] = bitx;
		quadrants_inverse_y[rotation][quad] = bity;
		quadrants_inverse_z[rotation][quad] = bitz;
	      }
    }


  seven = 7;

  /*  printf("Here key = %lld *bits = %lld \n",*key,*bits);*/

  shift = 3 * (*bits - 1);

  /* printf("Shift = %lld\n",shift);*/

  mask = seven << shift;

  /*printf("Mask= %lld\n",mask);*/

  rotation = 0;
  sense = 1;

  *x = *y = *z = 0;

  for(i = 0; i < *bits; i++, mask >>= 3, shift -= 3)
    {
      /*printf("Index %lld  %lld\n",i,*bits);*/

      /*printf("Key %lld, mask %lld , shift %lld\n",*key,mask,shift);*/

      keypart = (*key & mask) >> shift;

      /*printf("keypart %lld\n",keypart);*/

      quad = (sense == 1) ? (keypart) : (7 - keypart);

      /*printf("rotation %lld  quad %lld",rotation,quad);*/

      *x = (*x << 1) + quadrants_inverse_x[rotation][quad];
      *y = (*y << 1) + quadrants_inverse_y[rotation][quad];
      *z = (*z << 1) + quadrants_inverse_z[rotation][quad];

      /*printf("Here 2\n");*/
      rotx = rotx_table[quad];
      roty = roty_table[quad];
      sense *= sense_table[quad];

      while(rotx > 0)
	{
	  rotation = rotxmap_table[rotation];
	  rotx--;
	}

      while(roty > 0)
	{
	  rotation = rotymap_table[rotation];
	  roty--;
	}
    }
}