#define IMG_PRECISION (1e-8)


typedef struct {
  int w, h;
  double *data;
} Img;


void setRgb(double *rgb, double r, double g, double b)
  { rgb[0] = r; rgb[1] = g; rgb[2] = b; }

int colorMin(const double *rgb)
  { return rgb[0] < rgb[1] ? (rgb[0] < rgb[2] ? 0 : 2) : (rgb[1] < rgb[2] ? 1 : 2); }
int colorMax(const double *rgb)
  { return rgb[0] < rgb[1] ? (rgb[1] < rgb[2] ? 2 : 1) : (rgb[0] < rgb[2] ? 2 : 0); }

void rgbToHsv(double *hsv, const double *rgb) {
  int cmin = colorMin(rgb);
  int cmax = colorMax(rgb);
  double d = rgb[cmax] - rgb[cmin];

  double h = 0;
  if (d > IMG_PRECISION) {
    double k = 1.0/d;
    switch(cmax){
    case 0: h = (rgb[1] - rgb[2])*k + 0; break;
    case 1: h = (rgb[2] - rgb[0])*k + 2; break;
    case 2: h = (rgb[0] - rgb[1])*k + 4; break;
    }
    h /= 6; h -= floor(h); h *= 360;
  }
  double s = rgb[cmax] > IMG_PRECISION ? d/rgb[cmax] : 0;
  double v = rgb[cmax];
  setRgb(hsv, h, s, v);
}


void hsvToRgb(double *rgb, const double *hsv) {
  double h = hsv[0], s = hsv[1], v = hsv[2];
  h -= floor(h/360)*360;
  h /= 60.0;
  int i = (int)h;
  double f = h - i;
  double p = v*(1 - s);
  double q = v*(1 - s*f);
  double t = v*(1 - s*(1 - f));
  switch(i) {
  case 0: setRgb(rgb, v, t, p); return;
  case 1: setRgb(rgb, q, v, p); return;
  case 2: setRgb(rgb, p, v, t); return;
  case 3: setRgb(rgb, p, q, v); return;
  case 4: setRgb(rgb, t, p, v); return;
  case 5: setRgb(rgb, v, p, q); return;
  }
  return setRgb(rgb, v, t, p);
}


double cubicInterpolation(double p0, double p1, double p2, double p3, double l) {
  double ll = l*l;
  double lll = ll*l;
  return 0.5*( p0*(  -lll + 2*ll - l)
             + p1*( 3*lll - 5*ll + 2)
             + p2*(-3*lll + 4*ll + l)
             + p3*(   lll -   ll    ) );
}

double cubicRow(double *row, int count, double x) {
  if (count < 0) return 0;
  if (!(x > 0)) x = 0;
  if (!(x < count)) x = count;
  double xi = floor(x);
  double l = x - xi;

  int i = (int)x - 1;
  double p0 = i < 0 ? row[0] : i >= count ? row[count-1] : row[i]; ++i;
  double p1 = i < 0 ? row[0] : i >= count ? row[count-1] : row[i]; ++i;
  double p2 = i < 0 ? row[0] : i >= count ? row[count-1] : row[i]; ++i;
  double p3 = i < 0 ? row[0] : i >= count ? row[count-1] : row[i]; ++i;

  return cubicInterpolation(p0, p1, p2, p3, l);
}



void imgDestroy(Img *img) {
  free(img->data);
  img->data = NULL;
  img->w = img->h = 0;
}


void imgSwap(Img *imgA, Img *imgB) {
  Img imgC;
  memcpy(&imgC, imgA, sizeof(imgC));
  memcpy(imgA, imgB, sizeof(imgC));
  memcpy(imgB, &imgC, sizeof(imgC));
}


int imgInit(Img *img, int w, int h) {
  imgDestroy(img);
  if (w < 0 || h < 0) return FALSE;
  img->data = calloc(1, sizeof(double)*4*w*h);
  if (!img->data) return FALSE;
  img->w = w;
  img->h = h;
  return TRUE;
}


double* imgPixel(const Img *img, int x, int y) {
  if (!img->data) return NULL;
  if (x >= img->w) x = img->w - 1;
  if (y >= img->h) y = img->h - 1;
  if (x < 0) x = 0;
  if (y < 0) y = 0;
  return img->data + (img->w*y + x)*4;
}


double* imgNearest(const Img *img, double x, double y)
  { return imgPixel(img, (int)round(x), (int)round(y)); }


double imgCubicHCh(const Img *img, int ch, double x, int y) {
  if (!img->data) return 0;
  if (!(x > 0)) x = 0;
  if (!(x < img->w)) x = img->w;
  double xi = floor(x);
  int i = (int)xi - 1;
  double p[] = {
    imgPixel(img, i+0, y)[ch],
    imgPixel(img, i+1, y)[ch],
    imgPixel(img, i+2, y)[ch],
    imgPixel(img, i+3, y)[ch] };
  return cubicInterpolation(p[0], p[1], p[2], p[3], x - xi);
}


double imgCubicVCh(const Img *img, int ch, int x, double y) {
  if (!img->data) return 0;
  if (!(y > 0)) x = 0;
  if (!(y < img->h)) y = img->h;
  double yi = floor(y);
  int i = (int)yi - 1;
  double p[] = {
    imgPixel(img, i+0, y)[ch],
    imgPixel(img, i+1, y)[ch],
    imgPixel(img, i+2, y)[ch],
    imgPixel(img, i+3, y)[ch] };
  return cubicInterpolation(p[0], p[1], p[2], p[3], y - yi);
}


void imgCubicH(const Img *img, double x, int y, double *pix) {
  if (!img->data) { pix[0] = pix[1] = pix[2] = pix[3] = 0; return; }
  if (!(x > 0)) x = 0;
  if (!(x < img->w)) x = img->w;
  double xi = floor(x);
  double l = x - xi;

  int i = (int)xi - 1;
  double *p[] = {
    imgPixel(img, i+0, y),
    imgPixel(img, i+1, y),
    imgPixel(img, i+2, y),
    imgPixel(img, i+3, y) };

  pix[0] = cubicInterpolation(p[0][0], p[1][0], p[2][0], p[3][0], l);
  pix[1] = cubicInterpolation(p[0][1], p[1][1], p[2][1], p[3][1], l);
  pix[2] = cubicInterpolation(p[0][2], p[1][2], p[2][2], p[3][2], l);
  pix[3] = cubicInterpolation(p[0][3], p[1][3], p[2][3], p[3][3], l);
}


void imgCubicV(const Img *img, int x, double y, double *pix) {
  if (!img->data) { pix[0] = pix[1] = pix[2] = pix[3] = 0; return; }
  if (!(y > 0)) y = 0;
  if (!(y < img->h)) y = img->h;
  double yi = floor(y);
  double l = y - yi;

  int i = (int)yi - 1;
  double *p[] = {
    imgPixel(img, x, i+0),
    imgPixel(img, x, i+1),
    imgPixel(img, x, i+2),
    imgPixel(img, x, i+3) };

  pix[0] = cubicInterpolation(p[0][0], p[1][0], p[2][0], p[3][0], l);
  pix[1] = cubicInterpolation(p[0][1], p[1][1], p[2][1], p[3][1], l);
  pix[2] = cubicInterpolation(p[0][2], p[1][2], p[2][2], p[3][2], l);
  pix[3] = cubicInterpolation(p[0][3], p[1][3], p[2][3], p[3][3], l);
}


double imgCubicCh(const Img *img, int ch, double x, double y) {
  if (!img->data) return 0;
  if (!(y > 0)) y = 0;
  if (!(y < img->h)) y = img->h;
  double yi = floor(y);
  double l = y - yi;

  int i = (int)yi - 1;
  double p[] = {
    imgCubicHCh(img, ch, x, i+0),
    imgCubicHCh(img, ch, x, i+1),
    imgCubicHCh(img, ch, x, i+2),
    imgCubicHCh(img, ch, x, i+3) };

  return cubicInterpolation(p[0], p[1], p[2], p[3], l);
}


void imgCubic(const Img *img, double x, double y, double *pix) {
  if (!img->data) { pix[0] = pix[1] = pix[2] = pix[3] = 0; return; }
  if (!(y > 0)) y = 0;
  if (!(y < img->h)) y = img->h;
  double yi = floor(y);
  double l = y - yi;

  int i = (int)yi - 1;
  double p[4][4];
  imgCubicH(img, x, i+0, p[0]);
  imgCubicH(img, x, i+1, p[1]);
  imgCubicH(img, x, i+2, p[2]);
  imgCubicH(img, x, i+3, p[3]);

  pix[0] = cubicInterpolation(p[0][0], p[1][0], p[2][0], p[3][0], l);
  pix[1] = cubicInterpolation(p[0][1], p[1][1], p[2][1], p[3][1], l);
  pix[2] = cubicInterpolation(p[0][2], p[1][2], p[2][2], p[3][2], l);
  pix[3] = cubicInterpolation(p[0][3], p[1][3], p[2][3], p[3][3], l);
}


void imgFromInt(Img *img, int w, int h, const unsigned char *data) {
  imgInit(img, w, h);
  if (!img->data || !data) return;
  for(double *p = img->data, *e = p + 4*img->w*img->h; p < e; ++p)
    *p = (*data++)/255.0;
}


unsigned char* imgToInt(Img *img) {
  if (!img->data) return NULL;
  unsigned char *data = (unsigned char*)malloc(sizeof(unsigned char)*4*img->w*img->h), *dp = data;
  for(double *p = img->data, *e = p + 4*img->w*img->h; p < e; ++p)
    *dp++ = *p > 0 ? (*p < 1 ? (unsigned char)floor(*p*255.9999999) : 255) : 0;
  return data;
}