/**
* @file RANSACReg.cpp
* @brief Implementation of RANSAC (RANdom SAmple Consensus) for regression purposes
* @author Frank Prüfer
* @date 09/10/2013

*/  
#ifdef NICE_USELIB_OPENMP
#include <omp.h>
#endif

#include <iostream>
#include <ctime>

#include "vislearning/regression/linregression/LinRegression.h"
#include "vislearning/regression/linregression/RANSACReg.h"

using namespace OBJREC;

using namespace std;
using namespace NICE;

RANSACReg::RANSACReg ( const Config *_conf )
{
  if ( _conf->gB("RANSACReg","start_random_generator" ) )
    std::srand ( unsigned ( std::time(0) ) );
  threshold = _conf->gD("RANSACReg","threshold",0.5);
  iter = _conf->gI("RANSACReg","iterations",10);
}

RANSACReg::RANSACReg ( const RANSACReg & src ) : RegressionAlgorithm ( src )
{
  threshold = src.threshold;
  n = src.n;
  iter = src.iter;
  dataSet = src.dataSet;
  labelSet = src.labelSet;
  modelParams = src.modelParams;
}

RANSACReg::~RANSACReg()
{
}

RANSACReg* RANSACReg::clone ( void ) const
{
  return new RANSACReg(*this);
}

void RANSACReg::teach ( const NICE::VVector & dataSet, const NICE::Vector & labelSet )
{ 
  NICE::VVector best_CS(0,0);
  std::vector<double> best_labelCS;
  
  vector<int> indices;
  for ( uint i = 0; i < dataSet.size(); i++ )
    indices.push_back(i);
  
  n = dataSet[0].size()+1;

  for ( uint i = 0; i < iter; i++ ){
    random_shuffle( indices.begin(), indices.end() );
    NICE::VVector randDataSubset;
    std::vector<double> randLabelSubset;
    
    for ( uint j = 0; j < n; j++ ){	//choose random subset of n points
      randDataSubset.push_back( dataSet[indices[j]] );
      randLabelSubset.push_back( labelSet[indices[j]] );
    }
    
    LinRegression *linReg = new LinRegression ();
    linReg->teach ( randDataSubset, (NICE::Vector)randLabelSubset );	//do LinRegression on subset
    std::vector<double> tmp_modelParams = linReg->getModelParams();
    
    NICE::VVector current_CS;
    std::vector<double> current_labelCS;
    
#pragma omp parallel for    
    for ( uint j = n; j < indices.size(); j++ ){	//compute distance between each datapoint and current model
      double lengthNormalVector = 0; 
      double sum = 0;
      for ( uint k = 0; k < tmp_modelParams.size(); k++ ){
	sum += tmp_modelParams[k] * dataSet[indices[j]][k];
	lengthNormalVector += tmp_modelParams[k] * tmp_modelParams[k];
      }
      lengthNormalVector = sqrt(lengthNormalVector);
      
      double distance = ( sum - labelSet[indices[j]] ) / lengthNormalVector;

#pragma omp critical
      if ( abs(distance) < threshold ){	//if point is close to model, it belongs to consensus set
	current_CS.push_back ( dataSet[indices[j]] );
	current_labelCS.push_back ( labelSet[indices[j]] );
      }
    }
    
    if ( current_CS.size() > best_CS.size() ){	//if consensus set contains more points than any previous one, take this model as best_model
      best_CS = current_CS;
      best_labelCS = current_labelCS;
    }
  }
  
  LinRegression *best_linReg = new LinRegression ();	//compute best_model again with all points of best_consensusSet
  best_linReg->teach ( best_CS, (NICE::Vector)best_labelCS );
  modelParams = best_linReg->getModelParams();    
}
  
double RANSACReg::predict ( const NICE::Vector & x )
{
  NICE::Vector nModel(modelParams);
  NICE:: Vector xTmp(1,1.0);
  xTmp.append(x);
  double y = xTmp.scalarProduct(nModel);

  return y;
  
}