//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#include "ImplicitDeformation.hpp"

// #ifdef THRUST_BACKEND_CUDA
// #include <DivisionAnalysisCudaExport.hpp>
// #endif

#include <GraphAlgorithms.hpp>

namespace mgx 
{


void ImplicitDeformation::initialize(std::vector<Point3d>& data, std::vector<Point3d>& values){

  this->data.clear();// = data;
  this->values.clear();// = values;

  //std::cout << "read data/values: " << std::endl;
  for(uint i = 0; i < data.size(); i++){
    //std::cout << data[i] << "/" << values[i] << std::endl;
    Point3d zeroP(0,0,0);
    if(norm(data[i]-zeroP)<0.001 or norm(values[i]-zeroP)<0.001){
      std::cout << "ignored zeroP (data/value): " << data[i] << "/" << values[i] << std::endl;
      continue;
    } 
    if(values.size() < i) continue;
    this->data.push_back(data[i]);
    this->values.push_back(values[i]);
  }

  initCoeffs();

  //Create system of equations from initialized deformation
  CreateMatrix();
  CreateBVector();
  CreatePVector(data.size());

}

void ImplicitDeformation::clear()
{
  data.clear();
  values.clear();
  rb_coeffs.clear();
  poly_coeffs.clear();

}

void ImplicitDeformation::createCoeffs(){

  //Copy solution back into deformation
  for(uint i=0;i<xvec.size();i++){
    if(i<3*data.size())
      rb_coeffs[i%3][i/3] = xvec[i];
    else {
      //xvec[i] = 0;
      if(i/3 == data.size())
        poly_coeffs[i%3][0] = xvec[i];
      if(i/3 == data.size()+1)
        poly_coeffs[i%3][1] = xvec[i];
      if(i/3 == data.size()+2)
        poly_coeffs[i%3][2] = xvec[i];
      if(i/3 == data.size()+3)
        poly_coeffs[i%3][3] = xvec[i];
    }
  }
}


void ImplicitDeformation::solve(int cgStep){

  //Solve for coefficients
  for(int i=0;i<cgStep;i++)
    xvec = CGsolve(bvec,Mat,xvec);

  createCoeffs();

}

void ImplicitDeformation::solveInv(){
  std::vector<std::vector<double> > MatInv;

  std::cout << "Mat " << Mat.size() << "/" << Mat[0].size() << std::endl;

  MatInv.resize(Mat.size());
  for(uint i=0;i<Mat.size();i++){
    MatInv[i].resize(Mat.size());
  }

  //#ifdef THRUST_BACKEND_CUDA 
  //  matrixInverseGPU(Mat,MatInv);
  //#else
//    matrixInverseGSL(Mat,MatInv);
  //#endif
    systemSolveGSL(Mat,bvec);


  //std::vector<double> r = BinOp(b,Ax,-1);
  std::vector<double> res = bvec;//MatVecMult(MatInv,bvec);
  xvec = res;

  // for(uint i = 0; i < bvec.size(); i++){
  // 	std::cout << "bbb " << bvec[i] << "/" << xvec[i] << std::endl;
  // }

  createCoeffs();
}


}
