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

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

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

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

BSplineCurve::BSplineCurve()
{
}

BSplineCurve::BSplineCurve(int i) : BSpline(i), d_knot(i + 3)
{
  BSpline::formKnotVector(&d_knot);
}

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

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

BSplineCurve::~BSplineCurve()
{
}

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

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

int BSplineCurve::getSizeI() const
{
  return d_controlary.getSize();
}

const Vector& BSplineCurve::getControl(int i) const
{
  return d_controlary[i];
}

void BSplineCurve::sample(double t, Vector *v) const
{
  int i, j;
  double st;
  
  i = int(st = t * ((j = d_controlary.getSize()) - 2));
  if(i >= j)
    return;
  *v = d_controlary[i] * basis(i, st, d_knot);
  if(++i >= j)
    return;
  *v += d_controlary[i] * basis(i, st, d_knot);
  if(++i >= j)
    return;
  *v += d_controlary[i] * basis(i, st, d_knot);
}

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

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

int BSplineCurve::dimension() const
{
  return 1;
}

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

BSpline* BSplineCurve::clone() const
{
  int i, j;
  
  BSplineCurve *s = new BSplineCurve(j = d_controlary.getSize());

  for(i = 0; i < j; ++i)
    s -> d_controlary[i] = d_controlary[i];

  return s;
}

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

  if((error = d_controlary.get(file)) != OK)
    return error;

  d_knot.length(d_controlary.getSize() + 3);
  BSpline::formKnotVector(&d_knot);
  
  return OK;
}

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

void BSplineCurve::setControl(int i, const Vector &v)
{
  d_controlary[i] = v;
}

ErrorScope::Error BSplineCurve::put(FILE *file) const
{
  ErrorScope::Error error;
  
  if((error = d_controlary.put(file)) != OK)
    return error;
  
  return OK;
}

void BSplineCurve::fit(const VectAry &va)
{
  int i, k = va.getSize();
  double bet = 1., inc = (double)(k - 2) / (k - 1), t;
  Vector a(k), b(k), c(k), gam(k);

  b.element(0, 1.);
  c.element(0, 0.);
  for(i = 1, t = inc; i < k - 1; ++i, t += inc) {
    a.element(i, basis(i - 1, t, d_knot));
    b.element(i, basis(i, t, d_knot));
    c.element(i, basis(i + 1, t, d_knot));
  }
  a.element(i, 0.);
  b.element(i, 1.);

  d_controlary[0] = va[0];
  for(i = 1; i < k; ++i) {
    gam.element(i, c.element(i - 1) / bet);
    bet = b.element(i) - a.element(i) * gam.element(i);
    d_controlary[i] = (va[i] - a.element(i) * d_controlary[i - 1]) / bet;
  }
  
  for(i -= 2; i >= 0; --i)
    d_controlary[i] -= gam.element(i + 1) * d_controlary[i + 1];
}

void BSplineCurve::fit(const VectAry &va, const Vector &s)
{
  int i, k = va.getSize();
  double bet = 1., inc, t, f = s.element(0);
  Vector a(k), b(k), c(k), gam(k);

  inc = 1. / (s.element(s.length()) - f);
  b.element(0, 1.);
  c.element(0, 0.);
  for(i = 1; i < k - 1; ++i) {
    t = (s.element(i) - f) * inc;
    a.element(i, basis(i - 1, t, d_knot));
    b.element(i, basis(i, t, d_knot));
    c.element(i, basis(i + 1, t, d_knot));
  }
  a.element(i, 0.);
  b.element(i, 1.);

  d_controlary[0] = va[0];
  for(i = 1; i < k; ++i) {
    gam.element(i, c.element(i - 1) / bet);
    bet = b.element(i) - a.element(i) * gam.element(i);
    d_controlary[i] = (va[i] - a.element(i) * d_controlary[i - 1]) / bet;
  }
  
  for(i -= 2; i >= 0; --i)
    d_controlary[i] -= gam.element(i + 1) * d_controlary[i + 1];
}
