// ----------------------------------------------------------------------------
//
// Copyright (C) Columbia University, 1994. All Rights Reserved.
// Sameer A. Nene, Shree K. Nayar, Hiroshi Murase
//
// See file LICENSE for details of software license agreement.
//
// ----------------------------------------------------------------------------
//
// bsplinevolume.C
//
// Author:               Sameer Nene
// Date:                 05/26/94
// Version:              1.0
// Modification History:
// Bugs:
//
// Classes:
//   BSplineVolume
//
// Notes:
//   This module contains implementation of classes declared in bsplinevolume.h
//
// ----------------------------------------------------------------------------

#include <stdio.h>
#include "errorscope.h"
#include "interpolation.h"
#include "persistent.h"
#include "registry.h"
#include "vector.h"
#include "bsplinevolume.h"

void* BSplineVolume::d_protocol = (void*)(&BSplineVolume::d_protocol);
void* BSplineVolume::d_ipProtocol = &BSplineVolume::d_ipProtocol;
char* BSplineVolume::d_name_p = "BSplineVolume";
Registrar BSplineVolume::d_registrar(BSplineVolume::d_name_p,
				     &(BSplineVolume::createFunc));

Persistent* BSplineVolume::createFunc()
{
  return new BSplineVolume;
}

int BSplineVolume::hasProtocol(void* protocol) const
{
  return d_protocol == protocol || BSpline::hasProtocol(protocol);
}

int BSplineVolume::hasInterpolationProtocol(void *protocol) const
{
  return d_ipProtocol == protocol || BSpline::hasInterpolationProtocol(protocol);
}

BSplineVolume::BSplineVolume() : d_sizei(0), d_sizej(0), d_sizek(0)
{
}

BSplineVolume::BSplineVolume(int i, int j, int k) : BSpline(i * j * k),
d_sizei(i), d_sizej(j), d_sizek(k), d_knoti(i + 3), d_knotj(j + 3),
d_knotk(k + 3)
{
  BSpline::formKnotVector(&d_knoti);
  BSpline::formKnotVector(&d_knotj);
  BSpline::formKnotVector(&d_knotk);
}

BSplineVolume::~BSplineVolume()
{
}

BSplineVolume* BSplineVolume::safeCast(Persistent *p)
{
  return p -> hasProtocol(d_protocol) ? (BSplineVolume*)p : 0;
}

BSplineVolume* BSplineVolume::safeCast(Interpolation *ip)
{
  return ip -> hasInterpolationProtocol(d_ipProtocol) ? (BSplineVolume*)ip : 0;
}

int BSplineVolume::getSizeI() const
{
  return d_sizei;
}

int BSplineVolume::getSizeJ() const
{
  return d_sizej;
}

int BSplineVolume::getSizeK() const
{
  return d_sizek;
}

Vector& BSplineVolume::getControl(int i, int j, int k) const
{
  return d_controlary[i * d_sizej * d_sizek + j * d_sizek + k];
}

Vector BSplineVolume::sample(const Vector &v) const
{
  Vector t;

  sample(v.element(0), v.element(1), v.element(2), &t);
  return t;
}

int BSplineVolume::dimension() const
{
  return 3;
}

int BSplineVolume::spaceDimension() const
{
  return d_controlary[0].length();
}

void BSplineVolume::sample(double u, double v, double x, Vector *c) const
{
  double t, t2[3], t3[3], su, sv, sx;
  int i = int(su = u * (d_sizei - 2)), j = int(sv = v * (d_sizej - 2)),
    k = int(sx = x * (d_sizek - 2)), l, m, n, o, p, q, r;
  
  
  t2[0] = basis(j, sv, d_knotj);
  if(j + 1 < d_sizej)
    t2[1] = basis(j + 1, sv, d_knotj);
  if(j + 2 < d_sizej)
    t2[2] = basis(j + 2, sv, d_knotj);

  t3[0] = basis(k, sx, d_knotk);
  if(k + 1 < d_sizek)
    t3[1] = basis(k + 1, sx, d_knotk);
  if(k + 2 < d_sizek)
    t3[2] = basis(k + 2, sx, d_knotk);

  for(l = i * d_sizej * d_sizek, m = 0; m < 3; ++i, l += d_sizej * d_sizek, ++m)
    if(i < d_sizei) {
      t = basis(i, su, d_knoti);
      for(n = 0, o = j * d_sizek, p = j; n < 3; ++n, o += d_sizek, ++p)
	if(p < d_sizej)
	  for(q = 0, r = k; q < 3; ++q, ++r)
	    if(r < d_sizek)
	      if(m == 0 && n == 0 && q == 0)
		*c = d_controlary[l + o + r] * t * t2[n] * t3[q];
	      else
		*c += d_controlary[l + o + r] * t * t2[n] * t3[q];
    }
}

BSpline* BSplineVolume::clone() const
{
  int i, j;
  
  BSplineVolume *s = new BSplineVolume(d_sizei, d_sizej, d_sizek);
  j = d_sizei * d_sizej * d_sizek;
  for(i = 0; i < j; ++i)
    s -> d_controlary[i] = d_controlary[i];

  return s;
}

ErrorScope::Error BSplineVolume::get(FILE *file)
{
  ErrorScope::Error error;

  if(fread((char *)&d_sizei, sizeof(int), 1, file) != 1)
    return FILE_READ_ERROR;
  d_knoti.length(d_sizei + 3);

  if(fread((char *)&d_sizej, sizeof(int), 1, file) != 1)
    return FILE_READ_ERROR;
  d_knotj.length(d_sizej + 3);

  if(fread((char *)&d_sizek, sizeof(int), 1, file) != 1)
    return FILE_READ_ERROR;
  d_knotk.length(d_sizek + 3);
  
  if((error = d_controlary.get(file)) != OK)
    return error;

  BSpline::formKnotVector(&d_knoti);
  BSpline::formKnotVector(&d_knotj);
  BSpline::formKnotVector(&d_knotk);

  return OK;
}

const char* BSplineVolume::name() const
{
  return d_name_p;
}

void BSplineVolume::setControl(int i, int j, int k, const Vector &v)
{
  d_controlary[i * d_sizej * d_sizek + j * d_sizek + k] = v;
}

ErrorScope::Error BSplineVolume::put(FILE *file) const
{
  ErrorScope::Error error;

  if(fwrite((char *)&d_sizei, sizeof(int), 1, file) != 1)
    return FILE_WRITE_ERROR;
  
  if(fwrite((char *)&d_sizej, sizeof(int), 1, file) != 1)
    return FILE_WRITE_ERROR;

  if(fwrite((char *)&d_sizek, sizeof(int), 1, file) != 1)
    return FILE_WRITE_ERROR;
  
  if((error = d_controlary.put(file)) != OK)
    return error;
  
  return OK;
}

void BSplineVolume::generateBand(Vector *v, int i, int j) const
{
  int k, l, m, n, o, p, q, r, jk = d_sizej * d_sizek;
  double u, w, x, incu = (double)(d_sizei - 2) / (d_sizei - 1),
    incw = (double)(d_sizej - 2) / (d_sizej - 1),
    incx = (double)(d_sizek - 2) / (d_sizek - 1);

  v -> length(d_sizei * jk);
  
  for(o = 0; o < i; ++o)
    v -> element(o, 0.);

  for(k = j / jk, l = (j % jk) / d_sizek, q = (j % jk) % d_sizek,
      u = (m = i / jk) * incu, w = (n = (i % jk) / d_sizek) * incw,
      x = (r = (i % jk) % d_sizek) * incx, p = d_sizei * jk - (j < i ? 0 : j);
      o < p; ++o) {
    v -> element(o, basis(k, u, d_knoti) * basis(l, w, d_knotj) *
		 basis(q, x, d_knotk));
    if(++q == d_sizek) {
      if(++l == d_sizej) {
	++k;
	l = 0;
      }
      q = 0;
    }
    if(++r == d_sizek) {
      if(++n == d_sizej) {
	++m;
	u += incu;
	n = 0;
	w = 0.;
      }
      else
	w += incw;
      r = 0;
      x = 0.;
    }
    else
      x += incx;
  }

  for(p = d_sizei * jk; o < p; ++o)
    v -> element(o, 0.);
}

void BSplineVolume::fit(const VectAry &va2)
{
  int i, j, k, l, m = d_sizei * d_sizej * d_sizek, o;
  double r;
  Vector gam(j = va2.getSize());
  VectAry band(27), va(j);

  for(i = 0; i < j; ++i)
    va[i] = va2[i];

  generateBand(&band[0], (d_sizej + 1) * d_sizek + 1, 0);
  generateBand(&band[1], (d_sizej + 1) * d_sizek, 0);
  generateBand(&band[2], (d_sizej + 1) * d_sizek - 1, 0);
  generateBand(&band[3], d_sizej * d_sizek + 1, 0);
  generateBand(&band[4], d_sizej * d_sizek, 0);
  generateBand(&band[5], d_sizej * d_sizek - 1, 0);
  generateBand(&band[6], (d_sizej - 1) * d_sizek + 1, 0);
  generateBand(&band[7], (d_sizej - 1) * d_sizek, 0);
  generateBand(&band[8], (d_sizej - 1) * d_sizek - 1, 0);
  
  generateBand(&band[9], d_sizek + 1, 0);
  generateBand(&band[10], d_sizek, 0);
  generateBand(&band[11], d_sizek - 1, 0);
  generateBand(&band[12], 1, 0);
  generateBand(&band[13], 0, 0);
  generateBand(&band[14], 0, 1);
  generateBand(&band[15], 0, d_sizek - 1);
  generateBand(&band[16], 0, d_sizek);
  generateBand(&band[17], 0, d_sizek + 1);
  
  generateBand(&band[18], 0, (d_sizej - 1) * d_sizek - 1);
  generateBand(&band[19], 0, (d_sizej - 1) * d_sizek);
  generateBand(&band[20], 0, (d_sizej - 1) * d_sizek + 1);
  generateBand(&band[21], 0, d_sizej * d_sizek - 1);
  generateBand(&band[22], 0, d_sizej * d_sizek);
  generateBand(&band[23], 0, d_sizej * d_sizek + 1);
  generateBand(&band[24], 0, (d_sizej + 1) * d_sizek - 1);
  generateBand(&band[25], 0, (d_sizej + 1) * d_sizek);
  generateBand(&band[26], 0, (d_sizej + 1) * d_sizek + 1);

  o = d_sizej * d_sizek + d_sizek;
  
  for(i = d_sizek, j = m - o, k = o; i < j; ++i, ++k)
    if(band[9].element(i) != 0.) {
      r = band[0].element(k) / band[9].element(i);
      for(l = 0; l < 18; ++l)
	band[l].element(k, band[l].element(k) - band[l + 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  
  for(i = d_sizek, k = o; i < j; ++i, ++k) {
    if(band[10].element(i) != 0.) {
      r = band[1].element(k) / band[10].element(i);
      for(l = 1; l < 18; ++l)
	band[l].element(k, band[l].element(k) - band[l + 9].element(i) * r);
      va[k] -= va[i] * r;
    }
    i += d_sizek - 1;
    k += d_sizek - 1;
    if(band[10].element(i) != 0.) {
      r = band[1].element(k) / band[10].element(i);
      for(l = 1; l < 18; ++l)
	band[l].element(k, band[l].element(k) - band[l + 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  }
  
  o -= d_sizek;

  for(i = 0, j = m - o, k = o; i < j; ++i, ++k)
    if(band[12].element(i) != 0.) {
      r = band[3].element(k) / band[12].element(i);
      for(l = 3; l < 18; ++l)
	band[l].element(k, band[l].element(k) - band[l + 9].element(i) * r);
      va[k] -= va[i] * r;
    }

  for(i = 0, k = o; i < j; ++i, ++k) {
    if(band[13].element(i) != 0.) {
      r = band[4].element(k) / band[13].element(i);
      for(l = 4; l < 18; ++l)
	band[l].element(k, band[l].element(k) - band[l + 9].element(i) * r);
      va[k] -= va[i] * r;
    }
    i += d_sizek - 1;
    k += d_sizek - 1;
    if(band[13].element(i) != 0.) {
      r = band[4].element(k) / band[13].element(i);
      for(l = 4; l < 18; ++l)
	band[l].element(k, band[l].element(k) - band[l + 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  }

  for(i = m - d_sizek - 1, j = d_sizej * d_sizek, k = i - j; i >= j; --i, --k)
    if(band[17].element(i) != 0.) {
      r = band[26].element(k) / band[17].element(i);
      for(l = 26; l >= 18; --l)
	band[l].element(k, band[l].element(k) - band[l - 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  
  for(i = m - d_sizek - 1, k = i - j; i >= j; --i, --k) {
    if(band[16].element(i) != 0) {
      r = band[25].element(k) / band[16].element(i);
      for(l = 25; l >= 18; --l)
	band[l].element(k, band[l].element(k) - band[l - 9].element(i) * r);
      va[k] -= va[i] * r;
    }
    i -= d_sizek - 1;
    k -= d_sizek - 1;
    if(band[16].element(i) != 0) {
      r = band[25].element(k) / band[16].element(i);
      for(l = 25; l >= 18; --l)
	band[l].element(k, band[l].element(k) - band[l - 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  }

  for(i = m - 1, j = d_sizej * d_sizek, k = i - j; i >= j; --i, --k)
    if(band[14].element(i) != 0.) {
      r = band[23].element(k) / band[14].element(i);
      for(l = 23; l >= 18; --l)
	band[l].element(k, band[l].element(k) - band[l - 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  
  for(i = m - 1, k = i - j; i >= j; --i, --k) {
    if(band[13].element(i) != 0) {
      r = band[22].element(k) / band[13].element(i);
      for(l = 22; l >= 18; --l)
	band[l].element(k, band[l].element(k) - band[l - 9].element(i) * r);
      va[k] -= va[i] * r;
    }
    i -= d_sizek - 1;
    k -= d_sizek - 1;
    if(band[13].element(i) != 0) {
      r = band[22].element(k) / band[13].element(i);
      for(l = 22; l >= 18; --l)
	band[l].element(k, band[l].element(k) - band[l - 9].element(i) * r);
      va[k] -= va[i] * r;
    }
  }

  for(i = 0, j = m - d_sizek, k = d_sizek; i < j; ++i, ++k)
    if(band[12].element(i) != 0.) {
      r = band[9].element(k) / band[12].element(i);
      for(l = 9; l < 15; ++l)
	band[l].element(k, band[l].element(k) - band[l + 3].element(i) * r);
      va[k] -= va[i] * r;
    }
  
  for(i = 0, k = d_sizek; i < j; ++i, ++k) {
    if(band[13].element(i) != 0.) {
      r = band[10].element(k) / band[13].element(i);
      for(l = 10; l < 15; ++l)
	band[l].element(k, band[l].element(k) - band[l + 3].element(i) * r);
      va[k] -= va[i] * r;
    }
    i += d_sizek - 1;
    k += d_sizek - 1;
    if(band[13].element(i) != 0.) {
      r = band[10].element(k) / band[13].element(i);
      for(l = 10; l < 15; ++l)
	band[l].element(k, band[l].element(k) - band[l + 3].element(i) * r);
      va[k] -= va[i] * r;
    }
  }
  
  for(i = m - 1, j = d_sizek, k = i - j; i >= j; --i, --k)
    if(band[14].element(i) != 0.) {
      r = band[17].element(k) / band[14].element(i);
      for(l = 17; l >= 15; --l)
	band[l].element(k, band[l].element(k) - band[l - 3].element(i) * r);
      va[k] -= va[i] * r;
    }
  
  for(i = m - 1, k = i - j; i >= j; --i, --k) {
    if(band[13].element(i) != 0) {
      r = band[16].element(k) / band[13].element(i);
      for(l = 16; l >= 15; --l)
	band[l].element(k, band[l].element(k) - band[l - 3].element(i) * r);
      va[k] -= va[i] * r;
    }
    i -= d_sizek - 1;
    k -= d_sizek - 1;
    if(band[13].element(i) != 0) {
      r = band[16].element(k) / band[13].element(i);
      for(l = 16; l >= 15; --l)
	band[l].element(k, band[l].element(k) - band[l - 3].element(i) * r);
      va[k] -= va[i] * r;
    }
  }
  
  r = 1.;
  d_controlary[0] = va[0];
  for(i = 1, k = m; i < k; ++i) {
    gam.element(i, band[14].element(i - 1) / r);
    r = band[13].element(i) - band[12].element(i) * gam.element(i);
    d_controlary[i] = (va[i] - band[12].element(i) * d_controlary[i - 1]) / r;
  }
  
  for(i -= 2; i >= 0; --i)
    d_controlary[i] -= gam.element(i + 1) * d_controlary[i + 1];
}
