#include <Rcpp.h>
using namespace Rcpp;

void rot(int n, int *x, int *y, int rx, int ry) {
  if (ry == 0) {
    if (rx == 1) {
      *x = n-1 - *x;
      *y = n-1 - *y;
    }

    //Swap x and y
    int t  = *x;
    *x = *y;
    *y = t;
  }
}

//' Hilbert d to x, y
//'
//' @param n index
//' @param d hilbert cell coordinate
//' @export
// [[Rcpp::export]]
IntegerVector d2xy(int n, int d) {
  int rx, ry, s, t=d;
  int x, y;
  x = y = 0;
  for (s=1; s<n; s*=2) {
    rx = 1 & (t/2);
    ry = 1 & (t ^ rx);
    rot(s, &x, &y, rx, ry);
    x += s * rx;
    y += s * ry;
    t /= 4;
  }
  return(IntegerVector::create(_["x"] = x, _["y"] = y ));
}

//' Hilbert x, y to d
//'
//' @param n index
//' @param x,y coords
//' @export
// [[Rcpp::export]]
int xy2d (int n, int x, int y) {
  int rx, ry, s, d=0;
  for (s=n/2; s>0; s/=2) {
    rx = (x & s) > 0;
    ry = (y & s) > 0;
    d += s * s * ((3 * rx) ^ ry);
    rot(s, &x, &y, rx, ry);
  }
  return(d);
}