#include "spline.hpp"

void
Spline::setPoints(Point* _points,size_t _n){
  P=_points;
  n=_n;
  q=new Poly3[n-1];
}

void Spline::compute(){
  double* Asub=new double[n-1];
  double* Adiag=new double[n];
  double* Asup=new double[n-1];
  double* B=new double[n];
  double* m=new double[n];
  double* h=new double[n-1];

  //Compute h
  for(size_t i=0;i<n-1;++i){
    h[i]=P[i+1].x-P[i].x;
  }

  //Compute B
  B[0]=0;
  for(size_t i=1;i<n-1;++i){
    B[i]=6*((P[i+1].y-P[i].y)/h[i]-(P[i].y-P[i-1].y)/h[i-1]);
  }
  B[n-1]=0;

  //Compute A
  Adiag[0]=1;
  Asup[0]=0;
  for(size_t i=1;i<n-1;++i){
    Asub[i-1]=h[i-1];
    Adiag[i]=2*(h[i-1]+h[i]);
    Asup[i]=h[i];
  }
  Asub[n-2]=0;
  Adiag[n-1]=1;

  //Compute m
  Thomas(n,Asub,Adiag,Asup,B,m);

  //Compute ploynimials q
  for(size_t i=0;i<n-1;++i){
    q[i].d=P[i].y;
    q[i].b=m[i]/2;
    q[i].a=(m[i+1]-m[i])/(6*h[i]);
    q[i].c=(P[i+1].y-P[i].y)/h[i]-h[i]*(m[i+1]-m[i])/6-(h[i]*m[i])/2;

  }

  delete[] Asub;
  delete[] Adiag;
  delete[] Asup;
  delete[] B;
  delete[] m;
  delete[] h;
}

size_t
Spline::findIndex(double x) const{
//Find minimal i s.t. P[i].x>x
  size_t i=0;
  while(i<n-1 and P[i].x<=x){
    ++i;
  }
  // Here i=n-1 or P[i].x>x
  return i-1;
}

double
Spline::operator()(double x) const{
  size_t i=findIndex(x);
  return q[i](x-P[i].x);
}

double
Spline::derivate(double x) const{
  size_t i=findIndex(x);
  return q[i].derivate(x-P[i].x);
}