/**
* @file CRSplineReg.cpp
* @brief Implementation of Catmull-Rom-Splines for regression purposes
* @author Frank Prüfer
* @date 09/03/2013

*/  

#include <iostream>

#include "vislearning/regression/splineregression/CRSplineReg.h"

#include "vislearning/math/mathbase/FullVector.h"

using namespace OBJREC;

using namespace std;
using namespace NICE;

CRSplineReg::CRSplineReg ( )
{
  tau = 0.5;
}

CRSplineReg::CRSplineReg ( const CRSplineReg & src ) : RegressionAlgorithm ( src )
{
  tau = src.tau;
  dataSet = src.dataSet;
  labelSet = src.labelSet;
}

CRSplineReg::~CRSplineReg()
{
}

void CRSplineReg::teach ( const NICE::VVector & _dataSet, const NICE::Vector & _labelSet)
{
    fprintf (stderr, "teach using all !\n");
    //NOTE this is crucial if we clear _teachSet afterwards!
    //therefore, take care NOT to call _techSet.clear() somewhere out of this method
    this->dataSet = _dataSet;
    this->labelSet = _labelSet.std_vector();
    
    std::cerr << "number of known training samples: " << this->dataSet.size() << std::endl;   
    
}

void CRSplineReg::teach ( const NICE::Vector & x, const double & y )
{
    std::cerr << "CRSplineReg::teach one new example" << std::endl;
    
    for ( size_t i = 0 ; i < x.size() ; i++ )
      if ( isnan(x[i]) ) 
      {
          fprintf (stderr, "There is a NAN value in within this vector: x[%d] = %f\n", (int)i, x[i]);
          cerr << x << endl;
          exit(-1);
      }

    dataSet.push_back ( x );
    
    labelSet.push_back ( y );
    
    std::cerr << "number of known training samples: " << dataSet.size()<< std::endl;
}

double CRSplineReg::predict ( const NICE::Vector & x )
{
  
  if ( dataSet.size() <= 0 ) {
    fprintf (stderr, "CRSplineReg: please use the train method first\n");
    exit(-1);
  }
  
  if ( dataSet[0].size() == 1 ){	//one-dimensional case
    FullVector data ( dataSet.size()+1 );
    for ( uint i = 0; i < dataSet.size(); i++ ){
      data[i] = dataSet[i][0];
    }
    cerr<<"data x: "<<x[0]<<endl;
    data[dataSet.size()] = x[0];
    
    std::vector<int> ind;
    data.getSortedIndices(ind);
    
    int index;
    for ( uint i = 0; i < ind.size(); i++ ){
      if ( ind[i] == dataSet.size() ){
	index = i;
	break;
      }
    }
    
    
    NICE::Matrix points (4,2,0.0);
    if ( index >= 2 && index < (ind.size() - 2) ){	//everything is okay
      points(0,0) = data[ind[index-2]];
      points(0,1) = labelSet[ind[index-2]];
      points(1,0) = data[ind[index-1]];
      points(1,1) = labelSet[ind[index-1]];
      points(2,0) = data[ind[index+1]];
      points(2,1) = labelSet[ind[index+1]];
      points(3,0) = data[ind[index+2]];
      points(3,1) = labelSet[ind[index+2]];      
    }
    else if ( index == 1 ){	//just one point left from x
      points(0,0) = data[ind[index-1]];
      points(0,1) = labelSet[ind[index-1]];
      points(1,0) = data[ind[index-1]];
      points(1,1) = labelSet[ind[index-1]];
      points(2,0) = data[ind[index+1]];
      points(2,1) = labelSet[ind[index+1]];
      points(3,0) = data[ind[index+2]];
      points(3,1) = labelSet[ind[index+2]];       
    }
    else if ( index == 0 ){	//x is the farthest left point
      //do linear approximation
    }
    else if ( index == (ind.size() - 2) ){	//just one point right from x
      points(0,0) = data[ind[index-2]];
      points(0,1) = labelSet[ind[index-2]];
      points(1,0) = data[ind[index-1]];
      points(1,1) = labelSet[ind[index-1]];
      points(2,0) = data[ind[index+1]];
      points(2,1) = labelSet[ind[index+1]];
      points(3,0) = data[ind[index+1]];
      points(3,1) = labelSet[ind[index+1]];  
    }
    else if ( index == (ind.size() - 1) ){	//x is the farthest right point
      //do linear approximation
    }
    
    double t = (x[0] - points(1,0)) / (points(2,0) - points(1,0));
    cerr<<"t: "<<t<<endl;
    
//     NICE::Vector vecT(4,1.0);
//     
//     vecT[1] = t;
//     vecT[2] = t*t;
//     vecT[3] = t*t*t;
//     
//     Matrix coeffMatrix (4,4,0.0);	// M = (0 2 0 0
//     coeffMatrix(0,1) = 2.0;		//	-1 0 1 0
//     coeffMatrix(1,0) = -1.0;		//	2 -5 4 -1
//     coeffMatrix(1,2) = 1.0;		//	-1 3 -3 1)
//     coeffMatrix(2,0) = 2.0;
//     coeffMatrix(2,1) = -5.0;
//     coeffMatrix(2,2) = 4.0;
//     coeffMatrix(2,3) = -1.0;
//     coeffMatrix(3,0) = -1.0;
//     coeffMatrix(3,1) = 3.0;
//     coeffMatrix(3,2) = -3.0;
//     coeffMatrix(3,3) = 1.0;
//     
//     // P(t) = tau * vecT * coeffMatrix * points;
//     NICE::Vector P;
//     NICE::Matrix tmp;
//     tmp.multiply(coeffMatrix,points);
//     P.multiply(vecT,tmp);
//     P *= tau;
    
    //P(t) = b0*P0 + b1*P1 + b2*P2 + b3*P3
    NICE::Vector P(2);
    double b0,b1,b2,b3;
    b0 = tau * (-(t*t*t) + 2*t*t - t);
    b1 = tau * (3*t*t*t - 5*t*t + 2);
    b2 = tau * (-3*t*t*t + 4*t*t + t);
    b3 = tau * (t*t*t - t*t);
    
    P[0] = b0*points(0,0) + b1*points(1,0) + b2*points(2,0) + b3*points(3,0);
    P[1] = b0*points(0,1) + b1*points(1,1) + b2*points(2,1) + b3*points(3,1);
    
    cerr<<"Response x: "<<P[0]<<endl;
    cerr<<"Response y: "<<P[1]<<endl;
    
    return P[1];  
  }
  
}