// ----------------------------------------------------------------------------
//
// 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.
//
// ----------------------------------------------------------------------------
//
// bsplinesurface.C
//
// Author:               Sameer Nene
// Date:                 05/26/94
// Version:              1.0
// Modification History:
// Bugs:
//
// Classes:
//   BSplineSurface
//
// Notes:
//   This module contains implementation of classes declared in
//   bsplinesurface.h
//
// ----------------------------------------------------------------------------

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

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

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

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

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

BSplineSurface::BSplineSurface() : d_sizei(0), d_sizej(0)
{
}

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

BSplineSurface::~BSplineSurface()
{
}

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

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

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

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

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

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

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

int BSplineSurface::dimension() const
{
  return 2;
}

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

void BSplineSurface::sample(double u, double v, Vector *o) const
{
  double t, t2[3], su, sv;
  int i = int(su = u * (d_sizei - 2)), j = int(sv = v * (d_sizej - 2)), k, l, m, n;
  
  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);

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

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

  return s;
}

ErrorScope::Error BSplineSurface::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((error = d_controlary.get(file)) != OK)
    return error;

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

  return OK;
}

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

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

ErrorScope::Error BSplineSurface::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((error = d_controlary.put(file)) != OK)
    return error;
  
  return OK;
}

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

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

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

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

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

  for(i = 0; i < j; ++i)
    va[i] = va2[i];
  
  generateBand(&band[0], d_sizej + 1, 0);
  generateBand(&band[1], d_sizej, 0);
  generateBand(&band[2], d_sizej - 1, 0);
  generateBand(&band[3], 1, 0);
  generateBand(&band[4], 0, 0);
  generateBand(&band[5], 0, 1);
  generateBand(&band[6], 0, d_sizej - 1);
  generateBand(&band[7], 0, d_sizej);
  generateBand(&band[8], 0, d_sizej + 1);

  for(i = 0, j = m - d_sizej, k = d_sizej; i < j; ++i, ++k)
    if(band[3].element(i) != 0.) {
      r = band[0].element(k) / band[3].element(i);
      for(l = 0; l <= 5; ++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_sizej; i < j; ++i, ++k) {
    r = band[1].element(k) / band[4].element(i);
    band[1].element(k, band[1].element(k) - band[4].element(i) * r);
    band[4].element(k, band[4].element(k) - band[7].element(i) * r);
    va[k] -= va[i] * r;
    i += d_sizej - 1;
    k += d_sizej - 1;
    r = band[1].element(k) / band[4].element(i);
    band[1].element(k, band[1].element(k) - band[4].element(i) * r);
    band[4].element(k, band[4].element(k) - band[7].element(i) * r);
    va[k] -= va[i] * r;
  }

  for(i = m - 1, j = d_sizej, k = i - d_sizej; i >= j; --i, --k)
    if(band[5].element(i) != 0.) {
      r = band[8].element(k) / band[5].element(i);
      for(l = 8; l >= 6; --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 - d_sizej; i >= j; --i, --k) {
    r = band[7].element(k) / band[4].element(i);
    band[7].element(k, band[7].element(k) - band[4].element(i) * r);
    va[k] -= va[i] * r;
    i -= d_sizej - 1;
    k -= d_sizej - 1;
    r = band[7].element(k) / band[4].element(i);
    band[7].element(k, band[7].element(k) - band[4].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[5].element(i - 1) / r);
    r = band[4].element(i) - band[3].element(i) * gam.element(i);
    d_controlary[i] = (va[i] - band[3].element(i) * d_controlary[i - 1]) / r;
  }
  
  for(i -= 2; i >= 0; --i)
    d_controlary[i] -= gam.element(i + 1) * d_controlary[i + 1];
}
