/*
 *                This source code is part of
 *
 *                     E  R  K  A  L  E
 *                             -
 *                       DFT from Hel
 *
 * Written by Susi Lehtola, 2010-2011
 * Copyright (c) 2010-2011, Susi Lehtola
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 */

#include <armadillo>
#include <cfloat>
#include <cstdio>
#include <sstream>
#include <stdexcept>

#include "obara-saika.h"
#include "integrals.h"
#include "mathf.h"

// For comparison against Huzinaga integrals
//#define DEBUG

arma::mat overlap_int_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of overlap integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Returned matrix
  arma::mat S(carta.size(),cartb.size());
  S.zeros();

  // Get 1d overlaps
  arma::mat ox=overlap_ints_1d(xa,xb,zetaa,zetab,am_a,am_b);
  arma::mat oy=overlap_ints_1d(ya,yb,zetaa,zetab,am_a,am_b);
  arma::mat oz=overlap_ints_1d(za,zb,zetaa,zetab,am_a,am_b);

  int la, ma, na;
  int lb, mb, nb;
  double norma, normb;

  for(size_t i=0;i<carta.size();i++) {
      la=carta[i].l;
      ma=carta[i].m;
      na=carta[i].n;
      norma=carta[i].relnorm;

    for(size_t j=0;j<cartb.size();j++) {
      lb=cartb[j].l;
      mb=cartb[j].m;
      nb=cartb[j].n;
      normb=cartb[j].relnorm;

      S(i,j)=norma*normb*ox(la,lb)*oy(ma,mb)*oz(na,nb);

    }
  }

#ifdef DEBUG
  arma::mat huz(carta.size(),cartb.size());
  for(size_t i=0;i<carta.size();i++) {
    la=carta[i].l;
    ma=carta[i].m;
    na=carta[i].n;
    norma=carta[i].relnorm;

    for(size_t j=0;j<cartb.size();j++) {
      lb=cartb[j].l;
      mb=cartb[j].m;
      nb=cartb[j].n;
      normb=cartb[j].relnorm;

      huz(i,j)=norma*normb*overlap_int(xa,ya,za,zetaa,la,ma,na,xb,yb,zb,zetab,lb,mb,nb);
    }
  }

  int diff=0;
  for(size_t i=0;i<carta.size();i++)
    for(size_t j=0;j<cartb.size();j++)
      if(fabs(S(i,j)-huz(i,j))>10*DBL_EPSILON*fabs(huz(i,j)))
	diff++;

  if(diff==0)
    //    printf("Computed shell of overlaps (%e,%e,%e)-(%e,%e,%e) with zeta=(%e,%e) and am=(%i,%i), the results match.\n",xa,ya,za,xb,yb,zb,zetaa,zetab,am_a,am_b);
    ;
  else
      for(size_t i=0;i<carta.size();i++)
	for(size_t j=0;j<cartb.size();j++)
	  if(fabs(S(i,j)-huz(i,j))>10*DBL_EPSILON*fabs(huz(i,j))) {
	    printf("Computed overlap (%e,%e,%e)-(%e,%e,%e) with zeta=(%e,%e) and am=(%i,%i,%i)-(%i,%i,%i)\n",xa,ya,za,xb,yb,zb,zetaa,zetab,la,ma,na,lb,mb,nb);
	    printf("Huzinaga gives %e, OS gives %e.\n",huz(i,j),S(i,j));
	  }

#endif

  return S;
}

std::vector<arma::mat> overlap_int_pulay_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of overlap integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Returned matrix
  std::vector<arma::mat> S(6);
  for(int ic=0;ic<6;ic++)
    S[ic].zeros(carta.size(),cartb.size());

  // Get 1d overlaps
  arma::mat ox=overlap_ints_1d(xa,xb,zetaa,zetab,am_a+1,am_b+1);
  arma::mat oy=overlap_ints_1d(ya,yb,zetaa,zetab,am_a+1,am_b+1);
  arma::mat oz=overlap_ints_1d(za,zb,zetaa,zetab,am_a+1,am_b+1);

  int la, ma, na;
  int lb, mb, nb;
  double norma, normb;

  for(size_t i=0;i<carta.size();i++) {
      la=carta[i].l;
      ma=carta[i].m;
      na=carta[i].n;
      norma=carta[i].relnorm;

    for(size_t j=0;j<cartb.size();j++) {
      lb=cartb[j].l;
      mb=cartb[j].m;
      nb=cartb[j].n;
      normb=cartb[j].relnorm;

      // LHS derivatives
      S[0](i,j)=2*zetaa*ox(la+1,lb)*oy(ma,mb)*oz(na,nb);
      if(la>0)
	S[0](i,j)-=la*ox(la-1,lb)*oy(ma,mb)*oz(na,nb);
      S[0](i,j)*=-norma*normb;

      S[1](i,j)=2*zetaa*ox(la,lb)*oy(ma+1,mb)*oz(na,nb);
      if(ma>0)
	S[1](i,j)-=ma*ox(la,lb)*oy(ma-1,mb)*oz(na,nb);
      S[1](i,j)*=-norma*normb;

      S[2](i,j)=2*zetaa*ox(la,lb)*oy(ma,mb)*oz(na+1,nb);
      if(na>0)
	S[2](i,j)-=na*ox(la,lb)*oy(ma,mb)*oz(na-1,nb);
      S[2](i,j)*=-norma*normb;

      // RHS derivatives
      S[3](i,j)=2*zetab*ox(la,lb+1)*oy(ma,mb)*oz(na,nb);
      if(lb>0)
	S[3](i,j)-=lb*ox(la,lb-1)*oy(ma,mb)*oz(na,nb);
      S[3](i,j)*=-norma*normb;

      S[4](i,j)=2*zetab*ox(la,lb)*oy(ma,mb+1)*oz(na,nb);
      if(mb>0)
	S[4](i,j)-=mb*ox(la,lb)*oy(ma,mb-1)*oz(na,nb);
      S[4](i,j)*=-norma*normb;

      S[5](i,j)=2*zetab*ox(la,lb)*oy(ma,mb)*oz(na,nb+1);
      if(nb>0)
	S[5](i,j)-=nb*ox(la,lb)*oy(ma,mb)*oz(na,nb-1);
      S[5](i,j)*=-norma*normb;
  }
  }

  return S;
}

double overlap_int_os(double xa, double ya, double za, double zetaa, int la, int ma, int na, double xb, double yb, double zb, double zetab, int lb, int mb, int nb) {
  return overlap_int_1d(xa,xb,zetaa,zetab,la,lb)*overlap_int_1d(ya,yb,zetaa,zetab,ma,mb)*overlap_int_1d(za,zb,zetaa,zetab,na,nb);
}

double overlap_int_1d(double xa, double xb, double zetaa, double zetab, int la, int lb) {
  return overlap_ints_1d(xa,xb,zetaa,zetab,la,lb)(la,lb);
}

arma::mat overlap_ints_1d(double xa, double xb, double zetaa, double zetab, int la, int lb) {
  // In the following we assume la>lb.
  if(lb<la) // Switch arguments if necessary.
    return trans(overlap_ints_1d(xb,xa,zetab,zetaa,lb,la));

  // Compute exponents
  double p=zetaa+zetab;
  double mu=zetaa*zetab/(zetaa+zetab);

  // Compute center
  double px=(zetaa*xa+zetab*xb)/p;

  double xab=xa-xb;
  double xpb=px-xb;

  // We want to compute S_{la lb} with recurrence relations
  // S_{i+1,j} = X_{PA} S_{i,j} + 1/2p * (iS_{i-1,j}+jS_{i,j-1})
  // S_{i,j+1} = X_{PB} S_{i,j} + 1/2p * (iS_{i-1,j}+jS_{i,j-1})

  // We need some extra work space to use the recursion relations
  int lawrk=la+1;
  int lbwrk=lb+la+2;

  arma::mat S(lawrk,lbwrk);
  S.zeros();

  // Initialize S_{00}
  S(0,0)=sqrt(M_PI/p)*exp(-mu*xab*xab);

  if(la>0 || lb>0) {

    // Generate integrals S_{0,j}
    S(0,1)=xpb*S(0,0);
    for(int j=1;j<lbwrk-1;j++) {
      S(0,j+1)=xpb*S(0,j)+0.5/p*j*S(0,j-1);
    }

    // Use horizontal recurrence to generate S_{ij}
    // S_{i+1,j} = S_{i,j+1} - X_{AB} S_{ij}
    for(int i=0;i<la;i++) {
      for(int j=0;j<lb+la-i;j++) {
	S(i+1,j)=S(i,j+1)-xab*S(i,j);
      }
    }
  }

  // Return result, dropping the temporary
  return S.submat(0,0,la,lb);
}

arma::mat kinetic_int_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of kinetic energy integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Returned matrix
  arma::mat T(carta.size(),cartb.size());

  // Get 1d overlap integrals
  arma::mat ox_arr=overlap_ints_1d(xa,xb,zetaa,zetab,am_a,am_b);
  arma::mat oy_arr=overlap_ints_1d(ya,yb,zetaa,zetab,am_a,am_b);
  arma::mat oz_arr=overlap_ints_1d(za,zb,zetaa,zetab,am_a,am_b);

  // Get kinetic energy integrals
  arma::mat kx_arr=derivative_ints_1d(xa,xb,zetaa,zetab,am_a,am_b,2);
  arma::mat ky_arr=derivative_ints_1d(ya,yb,zetaa,zetab,am_a,am_b,2);
  arma::mat kz_arr=derivative_ints_1d(za,zb,zetaa,zetab,am_a,am_b,2);

  double ox, oy, oz;
  double kx, ky, kz;

  int la, ma, na;
  int lb, mb, nb;

  double anorm, bnorm;

  for(size_t i=0;i<carta.size();i++) {
    anorm=carta[i].relnorm;

    la=carta[i].l;
    ma=carta[i].m;
    na=carta[i].n;

    for(size_t j=0;j<cartb.size();j++) {
      lb=cartb[j].l;
      mb=cartb[j].m;
      nb=cartb[j].n;

      bnorm=cartb[j].relnorm;

      ox=ox_arr(la,lb);
      oy=oy_arr(ma,mb);
      oz=oz_arr(na,nb);

      kx=kx_arr(la,lb);
      ky=ky_arr(ma,mb);
      kz=kz_arr(na,nb);

      T(i,j)=-0.5*anorm*bnorm*(kx*oy*oz + ox*ky*oz + ox*oy*kz);
    }
  }

#ifdef DEBUG

  arma::mat huz(carta.size(),cartb.size());
  for(size_t i=0;i<carta.size();i++) {
    la=carta[i].l;
    ma=carta[i].m;
    na=carta[i].n;
    anorm=carta[i].relnorm;

    for(size_t j=0;j<cartb.size();j++) {
      lb=cartb[j].l;
      mb=cartb[j].m;
      nb=cartb[j].n;
      bnorm=cartb[j].relnorm;

      huz(i,j)=anorm*bnorm*kinetic_int(xa,ya,za,zetaa,la,ma,na,xb,yb,zb,zetab,lb,mb,nb);
    }
  }

  int diff=0;
  for(size_t i=0;i<carta.size();i++)
    for(size_t j=0;j<cartb.size();j++)
      if(fabs(T(i,j)-huz(i,j))>10*DBL_EPSILON*fabs(huz(i,j)))
	diff++;

  if(diff==0)
    //    printf("Computed shell of KE (%e,%e,%e)-(%e,%e,%e) with zeta=(%e,%e) and am=(%i,%i), the results match.\n",xa,ya,za,xb,yb,zb,zetaa,zetab,am_a,am_b);
    ;
  else
      for(size_t i=0;i<carta.size();i++)
	for(size_t j=0;j<cartb.size();j++)
	  if(fabs(T(i,j)-huz(i,j))>1000*DBL_EPSILON*fabs(huz(i,j))) {
	    printf("Computed KE (%e,%e,%e)-(%e,%e,%e) with zeta=(%e,%e) and am=(%i,%i,%i)-(%i,%i,%i)\n",xa,ya,za,xb,yb,zb,zetaa,zetab,la,ma,na,lb,mb,nb);
	    printf("Huzinaga gives %e, OS gives %e.\n",huz(i,j),T(i,j));
	  }

#endif

  return T;
}

std::vector<arma::mat> kinetic_int_pulay_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of kinetic energy integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Returned matrix
  std::vector<arma::mat> T(6);
  for(int ic=0;ic<6;ic++)
    T[ic].zeros(carta.size(),cartb.size());

  // Get 1d overlap integrals
  arma::mat ox_arr=overlap_ints_1d(xa,xb,zetaa,zetab,am_a+1,am_b+1);
  arma::mat oy_arr=overlap_ints_1d(ya,yb,zetaa,zetab,am_a+1,am_b+1);
  arma::mat oz_arr=overlap_ints_1d(za,zb,zetaa,zetab,am_a+1,am_b+1);

  // Get kinetic energy integrals
  arma::mat kx_arr=derivative_ints_1d(xa,xb,zetaa,zetab,am_a+1,am_b+1,2);
  arma::mat ky_arr=derivative_ints_1d(ya,yb,zetaa,zetab,am_a+1,am_b+1,2);
  arma::mat kz_arr=derivative_ints_1d(za,zb,zetaa,zetab,am_a+1,am_b+1,2);

  int la, ma, na;
  int lb, mb, nb;

  double ox, oy, oz;
  double oxp, oyp, ozp;
  double oxm, oym, ozm;
  double kx, ky, kz;
  double kxp, kyp, kzp;
  double kxm, kym, kzm;

  double anorm, bnorm;

  for(size_t i=0;i<carta.size();i++) {
    anorm=carta[i].relnorm;

    la=carta[i].l;
    ma=carta[i].m;
    na=carta[i].n;

    for(size_t j=0;j<cartb.size();j++) {
      lb=cartb[j].l;
      mb=cartb[j].m;
      nb=cartb[j].n;

      bnorm=cartb[j].relnorm;

      // LHS, derivative acting on x component. The lhs function with
      // cartesian angular momentum (la,ma,na) becomes two functions:
      // sqrt[(2 la+1) zetaa] (la+1,ma,na) - 2 la sqrt[zetaa / (2 la -1)] (la-1,ma,na)
      // note that derivatives are with respect to normalized functions!

      {
	oxp=ox_arr(la+1,lb);
	oy=oy_arr(ma,mb);
	oz=oz_arr(na,nb);

	kxp=kx_arr(la+1,lb);
	ky=ky_arr(ma,mb);
	kz=kz_arr(na,nb);

	T[0](i,j)= 2*zetaa*(kxp*oy*oz + oxp*ky*oz + oxp*oy*kz);
	if(la>0) {
	  oxm=ox_arr(la-1,lb);
	  kxm=kx_arr(la-1,lb);
	  T[0](i,j)-=la*(kxm*oy*oz + oxm*ky*oz + oxm*oy*kz);
	}
	T[0](i,j)*=0.5*anorm*bnorm;
      }

      // LHS, derivative acting on y component
      {
	ox=ox_arr(la,lb);
	oyp=oy_arr(ma+1,mb);
	oz=oz_arr(na,nb);

	kx=kx_arr(la,lb);
	kyp=ky_arr(ma+1,mb);
	kz=kz_arr(na,nb);

	T[1](i,j)= 2*zetaa*(kx*oyp*oz + ox*kyp*oz + ox*oyp*kz);
	if(ma>0) {
	  oym=oy_arr(ma-1,mb);
	  kym=ky_arr(ma-1,mb);
	  T[1](i,j)-=ma*(kx*oym*oz + ox*kym*oz + ox*oym*kz);
	}
	T[1](i,j)*=0.5*anorm*bnorm;
      }

      // LHS, derivative acting on z component
      {
	ox=ox_arr(la,lb);
	oy=oy_arr(ma,mb);
	ozp=oz_arr(na+1,nb);

	kx=kx_arr(la,lb);
	ky=ky_arr(ma,mb);
	kzp=kz_arr(na+1,nb);

	T[2](i,j)= 2*zetaa*(kx*oy*ozp + ox*ky*ozp + ox*oy*kzp);
	if(na>0) {
	  ozm=oz_arr(na-1,nb);
	  kzm=kz_arr(na-1,nb);
	  T[2](i,j)-=na*(kx*oy*ozm + ox*ky*ozm + ox*oy*kzm);
	}
	T[2](i,j)*=0.5*anorm*bnorm;
      }

      // RHS, derivative acting on x component
      {
	oxp=ox_arr(la,lb+1);
	oy=oy_arr(ma,mb);
	oz=oz_arr(na,nb);

	kxp=kx_arr(la,lb+1);
	ky=ky_arr(ma,mb);
	kz=kz_arr(na,nb);

	T[3](i,j)=2*zetab*(kxp*oy*oz + oxp*ky*oz + oxp*oy*kz);
	if(lb>0) {
	  oxm=ox_arr(la,lb-1);
	  kxm=kx_arr(la,lb-1);
	  T[3](i,j)-=lb*(kxm*oy*oz + oxm*ky*oz + oxm*oy*kz);
	}
	T[3](i,j)*=0.5*anorm*bnorm;
      }

      // RHS, derivative acting on y component
      {
	ox=ox_arr(la,lb);
	oyp=oy_arr(ma,mb+1);
	oz=oz_arr(na,nb);

	kx=kx_arr(la,lb);
	kyp=ky_arr(ma,mb+1);
	kz=kz_arr(na,nb);

	T[4](i,j)= 2*zetab*(kx*oyp*oz + ox*kyp*oz + ox*oyp*kz);
	if(mb>0) {
	  oym=oy_arr(ma,mb-1);
	  kym=ky_arr(ma,mb-1);
	  T[4](i,j)-=mb*(kx*oym*oz + ox*kym*oz + ox*oym*kz);
	}
	T[4](i,j)*=0.5*anorm*bnorm;
      }

      // RHS, derivative acting on z component
      {
	ox=ox_arr(la,lb);
	oy=oy_arr(ma,mb);
	ozp=oz_arr(na,nb+1);

	kx=kx_arr(la,lb);
	ky=ky_arr(ma,mb);
	kzp=kz_arr(na,nb+1);

	T[5](i,j)= 2*zetab*(kx*oy*ozp + ox*ky*ozp + ox*oy*kzp);
	if(nb>0) {
	  ozm=oz_arr(na,nb-1);
	  kzm=kz_arr(na,nb-1);
	  T[5](i,j)-=nb*(kx*oy*ozm + ox*ky*ozm + ox*oy*kzm);
	}
	T[5](i,j)*=0.5*anorm*bnorm;
      }

    }
  }

  return T;
}

// Compute kinetic energy integral of unnormalized primitives at r_A and r_B
double kinetic_int_os(double xa, double ya, double za, double zetaa, int la, int ma, int na, double xb, double yb, double zb, double zetab, int lb, int mb, int nb) {

  // Kinetic energy contributions in x, y and z
  double kx, ky, kz;

  // Overlaps in x, y and z
  double ox, oy, oz;

  // Compute kinetic energy integrals
  //  kx=kinetic_int_1d(xa,xb,zetaa,zetab,la,lb);
  //  ky=kinetic_int_1d(ya,yb,zetaa,zetab,ma,mb);
  //  kz=kinetic_int_1d(za,zb,zetaa,zetab,na,nb);
  kx=derivative_int_1d(xa,xb,zetaa,zetab,la,lb,2);
  ky=derivative_int_1d(ya,yb,zetaa,zetab,ma,mb,2);
  kz=derivative_int_1d(za,zb,zetaa,zetab,na,nb,2);

  // Compute overlap integrals
  ox=overlap_int_1d(xa,xb,zetaa,zetab,la,lb);
  oy=overlap_int_1d(ya,yb,zetaa,zetab,ma,mb);
  oz=overlap_int_1d(za,zb,zetaa,zetab,na,nb);

#ifdef DEBUG
  // Check overlap
  double ov=ox*oy*oz;
  double hov=overlap_int(xa,ya,za,zetaa,la,ma,na,xb,yb,zb,zetab,lb,mb,nb);
  if(fabs(ov-hov)>10*DBL_EPSILON*fabs(hov)) {
    printf("Computed overlap integral (%e,%e,%e)-(%e,%e,%e) with zeta=(%e,%e) and am=(%i,%i,%i)-(%i,%i,%i)\n",xa,ya,za,xb,yb,zb,zetaa,zetab,la,ma,na,lb,mb,nb);
    printf("Huzinaga gives %e, OS gives %e.\n",hov,ov);
    printf("overlap\t%e\t%e\t%e\n\n",ox,oy,oz);
  }
#endif

  // The result is
  return -0.5*(kx*oy*oz + ox*ky*oz + ox*oy*kz);
}

// Worker function for kinetic energy
double kinetic_int_1d(double xa, double xb,double zetaa, double zetab, int la, int lb) {

  // In the following we assume la>lb.
  if(lb<la) // Switch arguments if necessary.
    return kinetic_int_1d(xb,xa,zetab,zetaa,lb,la);

  // Compute exponents
  double p=zetaa+zetab;

  // Compute center
  double px=(zetaa*xa+zetab*xb)/p;

  double xpa=px-xa;
  double xpb=px-xb;

  // Get overlap integrals
  arma::mat S=overlap_ints_1d(xa,xb,zetaa,zetab,la+1,lb);

  // Compute the kinetic energy integrals
  //      T_{ij} = - ½ <G_i | \partial_x^2 | G_j >
  // using recursion relations
  // T_{i+1,j} = X_{PA} T_{i,j} + 1/2p * ( i*T_{i-1,j} + j*T_{i,j-1} ) + zetab/p * (2 * zetaa * S_{i+1,j} - i*S_{i-1,j})
  // T_{i,j+1} = X_{PB} T_{i,j} + 1/2p * ( i*T_{i-1,j} + j*T_{i,j-1} ) + zetaa/p * (2 * zetab * S_{i,j+1} - j*S_{i,j-1})
  // and the initial result
  // T_{00} = zetaa * (1 - 2*zetaa*(X_{PA}^2 + 1/2p)) * S_{00}

  arma::mat T(la+2,lb+1);
  T.zeros();

  // Initialize array
  T(0,0)=zetaa*(1.0 - 2.0*zetaa*(xpa*xpa + 0.5/p))*S(0,0);

  // Generate integrals T_{i,0}
  T(1,0)=xpa*T(0,0) + zetab/p*2.0*zetaa*S(1,0);
  for(int i=1;i<=la;i++) {
    T(i+1,0)=xpa*T(i,0) + 0.5/p*i*T(i-1,0) + zetab/p*(2.0*zetaa*S(i+1,0)-i*S(i-1,0));
  }

  if(lb>0) {
    T(0,1)=xpb*T(0,0) + zetaa/p*2.0*zetab*S(0,1);
    for(int j=1;j<lb;j++)
      T(0,j+1)=xpb*T(0,j) + 0.5/p*j*T(0,j-1) + zetaa/p*(2.0*zetab*S(0,j+1)-j*S(0,j-1));

    // Form target integral T_{la,lb}
    for(int i=1;i<=la;i++)
      for(int j=1;j<lb;j++) {
	T(i,j+1)=xpb*T(i,j) + 0.5/p*(i*T(i-1,j) + j*T(i,j-1)) + zetaa/p*(2.0*zetab*S(i,j+1)-j*S(i,j-1));
      }
  }

  return T(la,lb);
}

// Compute matrix element of derivative
double derivative_int_1d(double xa, double xb, double zetaa, double zetab, int la, int lb, int eval) {

  return derivative_ints_1d(xa,xb,zetaa,zetab,la,lb,eval)(la,lb);

}

// Compute shell of matrix elements of derivative
arma::mat derivative_ints_1d(double xa, double xb, double zetaa, double zetab, int la, int lb, int eval) {

  // In the following we assume la>lb.
  if(lb<la) // Switch arguments if necessary.
    return trans(derivative_ints_1d(xb,xa,zetab,zetaa,lb,la,eval));

  // Work matrices
  std::vector<arma::mat> D;

  // We compute the wanted matrix with
  // D^{e+1}_{i,j} = 2*zetaa*D^e_{i+1,j} - iD^e_{i-1,j}

  // The lowest order matrix is simply the ovelap matrix,
  // which we need to get in a big enough form
  D.push_back(overlap_ints_1d(xa,xb,zetaa,zetab,la+eval,lb));

  // Now, perform the recursion.
  for(int e=1;e<=eval;e++) {
    // Number of rows in the matrix of the current iteration
    int laval=la+eval-e;

    // Create matrix
    D.push_back(arma::mat(laval+1,lb+1));
    D[e].zeros();

    // Do recursion
    for(int j=0;j<=lb;j++) // i=0
      D[e](0,j)=2*zetaa*D[e-1](1,j);
    for(int i=1;i<=laval;i++)
      for(int j=0;j<=lb;j++)
	D[e](i,j)=2*zetaa*D[e-1](i+1,j)-i*D[e-1](i-1,j);
  }

  // Return result
  return D[eval].submat(0,0,la,lb);
}

arma::mat nuclear_int_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of overlap integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Compute the matrix
  arma::mat V=nuclear_ints_os(xa,ya,za,zetaa,am_a,xnuc,ynuc,znuc,xb,yb,zb,zetab,am_b);

  // Plug in the relative normalization factors
  for(size_t i=0;i<carta.size();i++)
    for(size_t j=0;j<cartb.size();j++)
      V(i,j)*=carta[i].relnorm*cartb[j].relnorm;

  return V;
}

double nuclear_int_os(double xa, double ya, double za, double zetaa, int la, int ma, int na, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, int lb, int mb, int nb) {

  // Compute the shell
  int am_a=la+ma+na;
  int am_b=lb+mb+nb;

  arma::mat ints=nuclear_ints_os(xa,ya,za,zetaa,am_a,xnuc,ynuc,znuc,xb,yb,zb,zetab,am_b);

  // Find the integral in the table
  double os=ints(getind(la,ma,na),getind(lb,mb,nb));

  return os;
}

/*

  The Obara-Saika recursion routine for nuclear attraction integrals
  was heavily inspired by the routine in

  PSI3: An open-source ab initio electronic structure package version 3.1.0.

  by

  T. D. Crawford, C. D. Sherrill, E. F. Valeev, J. T. Fermann, R. A. King,
  M. L. Leininger, S. T. Brown, C. L. Janssen, E. T. Seidl, J. P. Kenny,
  and W. D. Allen [J. Comp. Chem. 28, 1610 (2007)].

  The original license was the GNU General Public License.
  This version is relicensed under GPLv2+.

*/

arma::mat nuclear_ints_os(double xa, double ya, double za, double zetaa, int am_a, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, int am_b) {

  // Compute coordinates of center
  const double zeta=zetaa+zetab;
  const double o2g=1/(2.0*zeta);

  const double xp=(zetaa*xa+zetab*xb)/zeta;
  const double yp=(zetaa*ya+zetab*yb)/zeta;
  const double zp=(zetaa*za+zetab*zb)/zeta;

  const double PAx=xp-xa;
  const double PAy=yp-ya;
  const double PAz=zp-za;

  const double PBx=xp-xb;
  const double PBy=yp-yb;
  const double PBz=zp-zb;

  const double PCx=xp-xnuc;
  const double PCy=yp-ynuc;
  const double PCz=zp-znuc;

  const double ABsq=(xa-xb)*(xa-xb) + (ya-yb)*(ya-yb) + (za-zb)*(za-zb);

  // Sum of angular momenta
  const int mmax=am_a+am_b;

  const int size_a=(am_a+1)*(am_a+1)*am_a+1;
  const int size_b=(am_b+1)*(am_b+1)*am_b+1;

  // Work array for recursion formulas
  arma::cube ints(size_a,size_b,mmax+1);
  ints.zeros();

  // Helpers for computing indices on work array
  const int Nan = 1;
  const int Nam = am_a+1;
  const int Nal = Nam*Nam;

  const int Nbn = 1;
  const int Nbm = am_b+1;
  const int Nbl = Nbm*Nbm;

  // Argument of Boys' function
  const double boysarg=zeta*(PCx*PCx + PCy*PCy + PCz*PCz);
  // Evaluate Boys' function
  arma::vec bf;
  boysF_arr(mmax,boysarg,bf);

  // Constant prefactor for auxiliary integrals
  const double prefac=2.0*M_PI/zeta*exp(-zetaa*zetab*ABsq/zeta);

  // Initialize integral array (0_A | A(0) | 0_B)^(m)
  for(size_t m=0;m<bf.size();m++)
    ints(0,0,m)=prefac*bf[m];

  // Increase angular momentum on right hand side.

  // Loop over total angular momentum
  for(int lambdab=1;lambdab<=am_b;lambdab++)

    // Loop over angular momentum functions belonging to this shell
    for(int ii=0; ii<=lambdab; ii++) {
      int lb=lambdab - ii;
      for(int jj=0; jj<=ii; jj++) {
	int mb=ii - jj;
	int nb=jj;

	// Index in integrals table
	int bind = lb*Nbl+mb*Nbm+nb*Nbn;

	if (nb > 0) {
	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBz*ints(0,bind-Nbn,m)-PCz*ints(0,bind-Nbn,m+1);

	  if (nb > 1) {
	    for(int m=0;m<=mmax-lambdab;m++)
	      ints(0,bind,m) += o2g*(nb-1)*(ints(0,bind-2*Nbn,m)-ints(0,bind-2*Nbn,m+1));
	  }
	}

	else if (mb > 0) {

	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBy*ints(0,bind-Nbm,m)-PCy*ints(0,bind-Nbm,m+1);

	  if (mb > 1) {
	      for(int m=0;m<=mmax-lambdab;m++)
		ints(0,bind,m) += o2g*(mb-1)*(ints(0,bind-2*Nbm,m)-ints(0,bind-2*Nbm,m+1));
	  }
	}

	else if (lb > 0) {

	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBx*ints(0,bind-Nbl,m)-PCx*ints(0,bind-Nbl,m+1);

	  if (lb > 1) {
	    for(int m=0;m<=mmax-lambdab;m++)
	      ints(0,bind,m) += o2g*(lb-1)*(ints(0,bind-2*Nbl,m)-ints(0,bind-2*Nbl,m+1));
	  }

	}
	else {
	  ERROR_INFO();
	  throw std::runtime_error("Something went haywire in the Obara-Saika nuclear attraction integral algorithm.\n");
	}
      }
  }

  // Now, increase the angular momentum of the left-hand side, too.

  // Loop over total angular momentum of RHS
  for(int lambdab=0;lambdab<=am_b;lambdab++)

    // Loop over the functions belonging to the shell
    for(int ii=0; ii<=lambdab; ii++) {
      int lb=lambdab - ii;
      for(int jj=0; jj<=ii; jj++) {
	int mb=ii - jj;
	int nb=jj;

	// RHS index is
	int bind = lb*Nbl + mb*Nbm + nb*Nbn;

	// Loop over total angular momentum of LHS
	for(int lambdaa=1;lambdaa<=am_a;lambdaa++)

	  // Loop over angular momentum of second shell
	  for(int kk=0; kk<=lambdaa; kk++) {
	    int la=lambdaa-kk;

	    for(int ll=0; ll<=kk; ll++) {
	      int ma=kk-ll;
	      int na=ll;

	      // LHS index is
	      int aind = la*Nal + ma*Nam + na*Nan;

	      if (na > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  ints(aind,bind,m) = PAz*ints(aind-Nan,bind,m)-PCz*ints(aind-Nan,bind,m+1);
		}

		if (na > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(na-1)*(ints(aind-2*Nan,bind,m)-ints(aind-2*Nan,bind,m+1));
		}

		if (nb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*nb*(ints(aind-Nan,bind-Nbn,m)-ints(aind-Nan,bind-Nbn,m+1));
		}

	      } else if (ma > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  ints(aind,bind,m) = PAy*ints(aind-Nam,bind,m)-PCy*ints(aind-Nam,bind,m+1);
		}

		if (ma > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(ma-1)*(ints(aind-2*Nam,bind,m) - ints(aind-2*Nam,bind,m+1));
		}

		if (mb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*mb*(ints(aind-Nam,bind-Nbm,m) - ints(aind-Nam,bind-Nbm,m+1));
		}

	      }	else if (la > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  ints(aind,bind,m) = PAx*ints(aind-Nal,bind,m)-PCx*ints(aind-Nal,bind,m+1);
		}

		if (la > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(la-1)*(ints(aind-2*Nal,bind,m) - ints(aind-2*Nal,bind,m+1));
		}

		if (lb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*lb*(ints(aind-Nal,bind-Nbl,m) - ints(aind-Nal,bind-Nbl,m+1));
		}
	      }
	      else {
		ERROR_INFO();
		throw std::runtime_error("Something went haywire in the Obara-Saika nuclear attraction integral algorithm.\n");
	      }
	    }
	  }
    }
  }

  // Size of returned array
  int Na=(am_a+1)*(am_a+2)/2;
  int Nb=(am_b+1)*(am_b+2)/2;

  // Returned array
  arma::mat V(Na,Nb);
  V.zeros();

  // Fill in array
  int ia, ib;

  // Index of left basis function
  ia=0;

  // Loop over basis functions on this shell
  for(int ii=0; ii<=am_a; ii++) {
    int la=am_a - ii;
    for(int jj=0; jj<=ii; jj++) {
      int ma=ii - jj;
      int na=jj;

      // Index in worker array
      int aind=la*Nal+ma*Nam+na;

      // Index of right basis function
      ib=0;

      // Loop over angular momentum of second shell
      for(int kk=0; kk<=am_b; kk++) {
	int lb=am_b - kk;

	for(int ll=0; ll<=kk; ll++) {
	  int mb=kk-ll;
	  int nb=ll;

	  // Other index is
	  int bind=lb*Nbl+mb*Nbm+nb;

	  // Store result
	  V(ia,ib)=-ints(aind,bind,0);

	  // Increment index of basis function
	  ib++;
	}
      }

      // Increment index of basis function
      ia++;
    }
  }

#ifdef DEBUG
  // Compute Huzinaga integrals
  arma::mat huzint=V;
  huzint.zeros();

  int diff=0;

  ia=0;
  // Loop over basis functions on this shell
  for(int ii=0; ii<=am_a; ii++) {
    int ila=am_a - ii;
    for(int jj=0; jj<=ii; jj++) {
      int ima=ii - jj;
      int ina=jj;

      ib=0;
      // Loop over basis functions on this shell
      for(int kk=0; kk<=am_b; kk++) {
	int ilb=am_b - kk;
	for(int ll=0; ll<=kk; ll++) {
	  int imb=kk - ll;
	  int inb=ll;

	  huzint(ia,ib)=nuclear_int(xa,ya,za,zetaa,ila,ima,ina,xnuc,ynuc,znuc,xb,yb,zb,zetab,ilb,imb,inb);

	  if(fabs(huzint(ia,ib)-V(ia,ib))>100*DBL_EPSILON*fabs(huzint(ia,ib)))
	    diff++;

	  ib++;
	}
      }

      ia++;
    }
  }

  if(diff==0)
    //    printf("Computed NAI shell (%e,%e,%e) - (%e,%e,%e) with nucleus at (%e,%e,%e) and exponents %e and %e with am=(%i,%i), the results match.\n",xa,ya,za,xb,yb,zb,xnuc,ynuc,znuc,zetaa,zetab,am_a,am_b);
    ;
  else {
    ia=0;
    // Loop over basis functions on this shell
    for(int ii=0; ii<=am_a; ii++) {
      int ila=am_a - ii;
      for(int jj=0; jj<=ii; jj++) {
	int ima=ii - jj;
	int ina=jj;

	ib=0;
	// Loop over basis functions on this shell
	for(int kk=0; kk<=am_b; kk++) {
	  int ilb=am_b - kk;
	  for(int ll=0; ll<=kk; ll++) {
	    int imb=kk - ll;
	    int inb=ll;

	    if(fabs(huzint(ia,ib)-V(ia,ib))>100*DBL_EPSILON*fabs(huzint(ia,ib))) {
	      printf("Computed NAI shell (%e,%e,%e) - (%e,%e,%e) with nucleus at (%e,%e,%e) and exponents %e and %e.\n",xa,ya,za,xb,yb,zb,xnuc,ynuc,znuc,zetaa,zetab);
	      printf("The result for the integral (%i,%i,%i)-(%i,%i,%i) is %e with Huzinaga, whereas %e with Obara-Saika; difference is %e.\n\n",ila,ima,ina,ilb,imb,inb,huzint(ia,ib),V(ia,ib),huzint(ia,ib)-V(ia,ib));
	    }
	    ib++;
	  }
	}
	ia++;
      }
    }
  }

  /*    printf("Obara-Saika shell of integrals:\n");
	V.print();
	printf("Huzinaga shell of integrals:\n");
	huzint.print();
	printf("\n");*/
#endif

  return V;
}

std::vector<arma::mat> nuclear_int_pulay_os(double xa, double ya, double za, double zetaa, int amorig_a, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, int amorig_b) {

  // Compute coordinates of center
  const double zeta=zetaa+zetab;
  const double o2g=1/(2.0*zeta);

  const double xp=(zetaa*xa+zetab*xb)/zeta;
  const double yp=(zetaa*ya+zetab*yb)/zeta;
  const double zp=(zetaa*za+zetab*zb)/zeta;

  const double PAx=xp-xa;
  const double PAy=yp-ya;
  const double PAz=zp-za;

  const double PBx=xp-xb;
  const double PBy=yp-yb;
  const double PBz=zp-zb;

  const double PCx=xp-xnuc;
  const double PCy=yp-ynuc;
  const double PCz=zp-znuc;

  const double ABsq=(xa-xb)*(xa-xb) + (ya-yb)*(ya-yb) + (za-zb)*(za-zb);

  // Recursion formulas need am_a and am_b increased by one.
  int am_a=amorig_a+1;
  int am_b=amorig_b+1;

  // Sum of angular momenta
  const int mmax=am_a+am_b;

  const int size_a=(am_a+1)*(am_a+1)*am_a+1;
  const int size_b=(am_b+1)*(am_b+1)*am_b+1;

  // Work array for recursion formulas
  arma::cube ints(size_a,size_b,mmax+1);
  ints.zeros();

  // Helpers for computing indices on work array
  const int Nan = 1;
  const int Nam = am_a+1;
  const int Nal = Nam*Nam;

  const int Nbn = 1;
  const int Nbm = am_b+1;
  const int Nbl = Nbm*Nbm;

  // Argument of Boys' function
  const double boysarg=zeta*(PCx*PCx + PCy*PCy + PCz*PCz);
  // Evaluate Boys' function
  arma::vec bf;
  boysF_arr(mmax,boysarg,bf);

  // Constant prefactor for auxiliary integrals
  const double prefac=2.0*M_PI/zeta*exp(-zetaa*zetab*ABsq/zeta);

  // Initialize integral array (0_A | A(0) | 0_B)^(m)
  for(size_t m=0;m<bf.size();m++)
    ints(0,0,m)=prefac*bf[m];

  // Increase angular momentum on right hand side.

  // Loop over total angular momentum
  for(int lambdab=1;lambdab<=am_b;lambdab++)

    // Loop over angular momentum functions belonging to this shell
    for(int ii=0; ii<=lambdab; ii++) {
      int lb=lambdab - ii;
      for(int jj=0; jj<=ii; jj++) {
	int mb=ii - jj;
	int nb=jj;

	// Index in integrals table
	int bind = lb*Nbl+mb*Nbm+nb*Nbn;

	if (nb > 0) {
	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBz*ints(0,bind-Nbn,m)-PCz*ints(0,bind-Nbn,m+1);

	  if (nb > 1) {
	    for(int m=0;m<=mmax-lambdab;m++)
	      ints(0,bind,m) += o2g*(nb-1)*(ints(0,bind-2*Nbn,m)-ints(0,bind-2*Nbn,m+1));
	  }
	}

	else if (mb > 0) {

	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBy*ints(0,bind-Nbm,m)-PCy*ints(0,bind-Nbm,m+1);

	  if (mb > 1) {
	      for(int m=0;m<=mmax-lambdab;m++)
		ints(0,bind,m) += o2g*(mb-1)*(ints(0,bind-2*Nbm,m)-ints(0,bind-2*Nbm,m+1));
	  }
	}

	else if (lb > 0) {

	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBx*ints(0,bind-Nbl,m)-PCx*ints(0,bind-Nbl,m+1);

	  if (lb > 1) {
	    for(int m=0;m<=mmax-lambdab;m++)
	      ints(0,bind,m) += o2g*(lb-1)*(ints(0,bind-2*Nbl,m)-ints(0,bind-2*Nbl,m+1));
	  }

	}
	else {
	  ERROR_INFO();
	  throw std::runtime_error("Something went haywire in the Obara-Saika nuclear attraction integral algorithm.\n");
	}
      }
  }

  // Now, increase the angular momentum of the left-hand side, too.

  // Loop over total angular momentum of RHS
  for(int lambdab=0;lambdab<=am_b;lambdab++)

    // Loop over the functions belonging to the shell
    for(int ii=0; ii<=lambdab; ii++) {
      int lb=lambdab - ii;
      for(int jj=0; jj<=ii; jj++) {
	int mb=ii - jj;
	int nb=jj;

	// RHS index is
	int bind = lb*Nbl + mb*Nbm + nb*Nbn;

	// Loop over total angular momentum of LHS
	for(int lambdaa=1;lambdaa<=am_a;lambdaa++)

	  // Loop over angular momentum of second shell
	  for(int kk=0; kk<=lambdaa; kk++) {
	    int la=lambdaa-kk;

	    for(int ll=0; ll<=kk; ll++) {
	      int ma=kk-ll;
	      int na=ll;

	      // LHS index is
	      int aind = la*Nal + ma*Nam + na*Nan;

	      if (na > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  ints(aind,bind,m) = PAz*ints(aind-Nan,bind,m)-PCz*ints(aind-Nan,bind,m+1);
		}

		if (na > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(na-1)*(ints(aind-2*Nan,bind,m)-ints(aind-2*Nan,bind,m+1));
		}

		if (nb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*nb*(ints(aind-Nan,bind-Nbn,m)-ints(aind-Nan,bind-Nbn,m+1));
		}

	      } else if (ma > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  ints(aind,bind,m) = PAy*ints(aind-Nam,bind,m)-PCy*ints(aind-Nam,bind,m+1);
		}

		if (ma > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(ma-1)*(ints(aind-2*Nam,bind,m) - ints(aind-2*Nam,bind,m+1));
		}

		if (mb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*mb*(ints(aind-Nam,bind-Nbm,m) - ints(aind-Nam,bind-Nbm,m+1));
		}

	      }	else if (la > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  ints(aind,bind,m) = PAx*ints(aind-Nal,bind,m)-PCx*ints(aind-Nal,bind,m+1);
		}

		if (la > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(la-1)*(ints(aind-2*Nal,bind,m) - ints(aind-2*Nal,bind,m+1));
		}

		if (lb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*lb*(ints(aind-Nal,bind-Nbl,m) - ints(aind-Nal,bind-Nbl,m+1));
		}
	      }
	      else {
		ERROR_INFO();
		throw std::runtime_error("Something went haywire in the Obara-Saika nuclear attraction integral algorithm.\n");
	      }
	    }
	  }
    }
  }

  // Size of returned array
  int Na=(amorig_a+1)*(amorig_a+2)/2;
  int Nb=(amorig_b+1)*(amorig_b+2)/2;

  // Returned array
  std::vector<arma::mat> V(6);
  for(int ic=0;ic<6;ic++)
    V[ic].zeros(Na,Nb);

  // Fill in array
  int ia, ib;

  // Index of left basis function
  ia=0;

  // Loop over basis functions on this shell
  for(int ii=0; ii<=amorig_a; ii++) {
    int la=amorig_a - ii;
    for(int jj=0; jj<=ii; jj++) {
      int ma=ii - jj;
      int na=jj;

      // Index of right basis function
      ib=0;

      // Loop over angular momentum of second shell
      for(int kk=0; kk<=amorig_b; kk++) {
	int lb=amorig_b - kk;

	for(int ll=0; ll<=kk; ll++) {
	  int mb=kk-ll;
	  int nb=ll;

	  int aind=la*Nal+ma*Nam+na;
	  // x
	  int alpind=(la+1)*Nal+ma*Nam+na;
	  int almind=(la-1)*Nal+ma*Nam+na;
	  // y
	  int ampind=la*Nal+(ma+1)*Nam+na;
	  int ammind=la*Nal+(ma-1)*Nam+na;
	  // z
	  int anpind=la*Nal+ma*Nam+na+1;
	  int anmind=la*Nal+ma*Nam+na-1;

	  int bind=lb*Nbl+mb*Nbm+nb;
	  // x
	  int blpind=(lb+1)*Nbl+mb*Nbm+nb;
	  int blmind=(lb-1)*Nbl+mb*Nbm+nb;
	  // y
	  int bmpind=lb*Nbl+(mb+1)*Nbm+nb;
	  int bmmind=lb*Nbl+(mb-1)*Nbm+nb;
	  // z
	  int bnpind=lb*Nbl+mb*Nbm+nb+1;
	  int bnmind=lb*Nbl+mb*Nbm+nb-1;

	  // LHS, x
	  V[0](ia,ib)=2*zetaa*ints(alpind,bind,0);
	  if(la>0)
	    V[0](ia,ib)-=la*ints(almind,bind,0);
	  // LHS, y
	  V[1](ia,ib)=2*zetaa*ints(ampind,bind,0);
	  if(ma>0)
	    V[1](ia,ib)-=ma*ints(ammind,bind,0);
	  // LHS, z
	  V[2](ia,ib)=2*zetaa*ints(anpind,bind,0);
	  if(na>0)
	    V[2](ia,ib)-=na*ints(anmind,bind,0);

	  // RHS, x
	  V[3](ia,ib)=2*zetab*ints(aind,blpind,0);
	  if(lb>0)
	    V[3](ia,ib)-=lb*ints(aind,blmind,0);
	  // RHS, y
	  V[4](ia,ib)=2*zetab*ints(aind,bmpind,0);
	  if(mb>0)
	    V[4](ia,ib)-=mb*ints(aind,bmmind,0);
	  // RHS, z
	  V[5](ia,ib)=2*zetab*ints(aind,bnpind,0);
	  if(nb>0)
	    V[5](ia,ib)-=nb*ints(aind,bnmind,0);

	  // Increment index of basis function
	  ib++;
	}
      }

      // Increment index of basis function
      ia++;
    }
  }

  return V;
}

std::vector<arma::mat> nuclear_int_pulay_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of overlap integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Compute the matrix
  std::vector<arma::mat> V=nuclear_int_pulay_os(xa,ya,za,zetaa,am_a,xnuc,ynuc,znuc,xb,yb,zb,zetab,am_b);

  // Plug in the relative normalization factors
  for(size_t i=0;i<carta.size();i++)
    for(size_t j=0;j<cartb.size();j++)
      for(size_t ic=0;ic<V.size();ic++)
	V[ic](i,j)*=carta[i].relnorm*cartb[j].relnorm;

  return V;
}

std::vector<arma::mat> nuclear_int_ders_os(double xa, double ya, double za, double zetaa, int am_a, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, int am_b) {

  // Compute coordinates of center
  const double zeta=zetaa+zetab;
  const double o2g=1/(2.0*zeta);

  const double xp=(zetaa*xa+zetab*xb)/zeta;
  const double yp=(zetaa*ya+zetab*yb)/zeta;
  const double zp=(zetaa*za+zetab*zb)/zeta;

  const double PAx=xp-xa;
  const double PAy=yp-ya;
  const double PAz=zp-za;

  const double PBx=xp-xb;
  const double PBy=yp-yb;
  const double PBz=zp-zb;

  const double PCx=xp-xnuc;
  const double PCy=yp-ynuc;
  const double PCz=zp-znuc;

  const double ABsq=(xa-xb)*(xa-xb) + (ya-yb)*(ya-yb) + (za-zb)*(za-zb);

  // Sum of angular momenta
  const int mmax=am_a+am_b;

  const int size_a=(am_a+1)*(am_a+1)*am_a+1;
  const int size_b=(am_b+1)*(am_b+1)*am_b+1;

  // Work array for recursion formulas
  arma::cube ints(size_a,size_b,mmax+1);
  ints.zeros();

  arma::cube xint(size_a,size_b,mmax+1);
  xint.zeros();
  arma::cube yint(size_a,size_b,mmax+1);
  yint.zeros();
  arma::cube zint(size_a,size_b,mmax+1);
  zint.zeros();

  // Helpers for computing indices on work array
  const int Nan = 1;
  const int Nam = am_a+1;
  const int Nal = Nam*Nam;

  const int Nbn = 1;
  const int Nbm = am_b+1;
  const int Nbl = Nbm*Nbm;

  // Argument of Boys' function
  const double boysarg=zeta*(PCx*PCx + PCy*PCy + PCz*PCz);
  // Evaluate Boys' function
  arma::vec bf;
  boysF_arr(mmax+1,boysarg,bf);

  // Constant prefactor for auxiliary integrals
  const double prefac=2.0*M_PI/zeta*exp(-zetaa*zetab*ABsq/zeta);

  // Initialize integral array (0_A | A(0) | 0_B)^(m)
  for(int m=0;m<=mmax;m++)
    ints(0,0,m)=prefac*bf[m];
  // and (0_A | A(1) | 0_B)^(m)
  for(int m=0;m<=mmax;m++) {
    xint(0,0,m)=2.0*zeta*PCx*prefac*bf[m+1];
    yint(0,0,m)=2.0*zeta*PCy*prefac*bf[m+1];
    zint(0,0,m)=2.0*zeta*PCz*prefac*bf[m+1];
  }

  // Increase angular momentum on right hand side.

  // Loop over total angular momentum
  for(int lambdab=1;lambdab<=am_b;lambdab++)

    // Loop over angular momentum functions belonging to this shell
    for(int ii=0; ii<=lambdab; ii++) {
      int lb=lambdab - ii;
      for(int jj=0; jj<=ii; jj++) {
	int mb=ii - jj;
	int nb=jj;

	// Index in integrals table
	int bind = lb*Nbl+mb*Nbm+nb*Nbn;

	if (nb > 0) {
	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBz*ints(0,bind-Nbn,m)-PCz*ints(0,bind-Nbn,m+1);
	  for(int m=0;m<=mmax-lambdab;m++) {
	    xint(0,bind,m) = PBz*xint(0,bind-Nbn,m)-PCz*xint(0,bind-Nbn,m+1);
	    yint(0,bind,m) = PBz*yint(0,bind-Nbn,m)-PCz*yint(0,bind-Nbn,m+1);
	    zint(0,bind,m) = PBz*zint(0,bind-Nbn,m)-PCz*zint(0,bind-Nbn,m+1) + ints(0,bind-Nbn,m+1);
	  }

	  if (nb > 1) {
	    for(int m=0;m<=mmax-lambdab;m++)
	      ints(0,bind,m) += o2g*(nb-1)*(ints(0,bind-2*Nbn,m)-ints(0,bind-2*Nbn,m+1));
	    for(int m=0;m<=mmax-lambdab;m++) {
	      xint(0,bind,m) += o2g*(nb-1)*(xint(0,bind-2*Nbn,m)-xint(0,bind-2*Nbn,m+1));
	      yint(0,bind,m) += o2g*(nb-1)*(yint(0,bind-2*Nbn,m)-yint(0,bind-2*Nbn,m+1));
	      zint(0,bind,m) += o2g*(nb-1)*(zint(0,bind-2*Nbn,m)-zint(0,bind-2*Nbn,m+1));
	    }
	  }
	}

	else if (mb > 0) {

	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBy*ints(0,bind-Nbm,m)-PCy*ints(0,bind-Nbm,m+1);
	  for(int m=0;m<=mmax-lambdab;m++) {
	    xint(0,bind,m) = PBy*xint(0,bind-Nbm,m)-PCy*xint(0,bind-Nbm,m+1);
	    yint(0,bind,m) = PBy*yint(0,bind-Nbm,m)-PCy*yint(0,bind-Nbm,m+1) + ints(0,bind-Nbm,m+1);
	    zint(0,bind,m) = PBy*zint(0,bind-Nbm,m)-PCy*zint(0,bind-Nbm,m+1);
	  }

	  if (mb > 1) {
	      for(int m=0;m<=mmax-lambdab;m++)
		ints(0,bind,m) += o2g*(mb-1)*(ints(0,bind-2*Nbm,m)-ints(0,bind-2*Nbm,m+1));
	      for(int m=0;m<=mmax-lambdab;m++) {
		xint(0,bind,m) += o2g*(mb-1)*(xint(0,bind-2*Nbm,m)-xint(0,bind-2*Nbm,m+1));
		yint(0,bind,m) += o2g*(mb-1)*(yint(0,bind-2*Nbm,m)-yint(0,bind-2*Nbm,m+1));
		zint(0,bind,m) += o2g*(mb-1)*(zint(0,bind-2*Nbm,m)-zint(0,bind-2*Nbm,m+1));
	      }
	  }
	}

	else if (lb > 0) {

	  for(int m=0;m<=mmax-lambdab;m++)
	    ints(0,bind,m) = PBx*ints(0,bind-Nbl,m)-PCx*ints(0,bind-Nbl,m+1);
	  for(int m=0;m<=mmax-lambdab;m++) {
	    xint(0,bind,m) = PBx*xint(0,bind-Nbl,m)-PCx*xint(0,bind-Nbl,m+1) + ints(0,bind-Nbl,m+1);
	    yint(0,bind,m) = PBx*yint(0,bind-Nbl,m)-PCx*yint(0,bind-Nbl,m+1);
	    zint(0,bind,m) = PBx*zint(0,bind-Nbl,m)-PCx*zint(0,bind-Nbl,m+1);
	  }

	  if (lb > 1) {
	    for(int m=0;m<=mmax-lambdab;m++)
	      ints(0,bind,m) += o2g*(lb-1)*(ints(0,bind-2*Nbl,m)-ints(0,bind-2*Nbl,m+1));
	    for(int m=0;m<=mmax-lambdab;m++) {
	      xint(0,bind,m) += o2g*(lb-1)*(xint(0,bind-2*Nbl,m)-xint(0,bind-2*Nbl,m+1));
	      yint(0,bind,m) += o2g*(lb-1)*(yint(0,bind-2*Nbl,m)-yint(0,bind-2*Nbl,m+1));
	      zint(0,bind,m) += o2g*(lb-1)*(zint(0,bind-2*Nbl,m)-zint(0,bind-2*Nbl,m+1));
	    }
	  }

	}
	else {
	  ERROR_INFO();
	  throw std::runtime_error("Something went haywire in the Obara-Saika nuclear attraction integral derivative algorithm.\n");
	}
      }
  }

  // Now, increase the angular momentum of the left-hand side, too.

  // Loop over total angular momentum of RHS
  for(int lambdab=0;lambdab<=am_b;lambdab++)

    // Loop over the functions belonging to the shell
    for(int ii=0; ii<=lambdab; ii++) {
      int lb=lambdab - ii;
      for(int jj=0; jj<=ii; jj++) {
	int mb=ii - jj;
	int nb=jj;

	// RHS index is
	int bind = lb*Nbl + mb*Nbm + nb*Nbn;

	// Loop over total angular momentum of LHS
	for(int lambdaa=1;lambdaa<=am_a;lambdaa++)

	  // Loop over angular momentum of second shell
	  for(int kk=0; kk<=lambdaa; kk++) {
	    int la=lambdaa-kk;

	    for(int ll=0; ll<=kk; ll++) {
	      int ma=kk-ll;
	      int na=ll;

	      // LHS index is
	      int aind = la*Nal + ma*Nam + na*Nan;

	      if (na > 0) {
		for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		  ints(aind,bind,m) = PAz*ints(aind-Nan,bind,m)-PCz*ints(aind-Nan,bind,m+1);
		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  xint(aind,bind,m) = PAz*xint(aind-Nan,bind,m)-PCz*xint(aind-Nan,bind,m+1);
		  yint(aind,bind,m) = PAz*yint(aind-Nan,bind,m)-PCz*yint(aind-Nan,bind,m+1);
		  zint(aind,bind,m) = PAz*zint(aind-Nan,bind,m)-PCz*zint(aind-Nan,bind,m+1) + ints(aind-Nan,bind,m+1);
		}

		if (na > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(na-1)*(ints(aind-2*Nan,bind,m)-ints(aind-2*Nan,bind,m+1));
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		    xint(aind,bind,m) += o2g*(na-1)*(xint(aind-2*Nan,bind,m)-xint(aind-2*Nan,bind,m+1));
		    yint(aind,bind,m) += o2g*(na-1)*(yint(aind-2*Nan,bind,m)-yint(aind-2*Nan,bind,m+1));
		    zint(aind,bind,m) += o2g*(na-1)*(zint(aind-2*Nan,bind,m)-zint(aind-2*Nan,bind,m+1));
		  }
		}

		if (nb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*nb*(ints(aind-Nan,bind-Nbn,m)-ints(aind-Nan,bind-Nbn,m+1));
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		    xint(aind,bind,m) += o2g*nb*(xint(aind-Nan,bind-Nbn,m)-xint(aind-Nan,bind-Nbn,m+1));
		    yint(aind,bind,m) += o2g*nb*(yint(aind-Nan,bind-Nbn,m)-yint(aind-Nan,bind-Nbn,m+1));
		    zint(aind,bind,m) += o2g*nb*(zint(aind-Nan,bind-Nbn,m)-zint(aind-Nan,bind-Nbn,m+1));
		  }
		}

	      } else if (ma > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		  ints(aind,bind,m) = PAy*ints(aind-Nam,bind,m)-PCy*ints(aind-Nam,bind,m+1);
		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  xint(aind,bind,m) = PAy*xint(aind-Nam,bind,m)-PCy*xint(aind-Nam,bind,m+1);
		  yint(aind,bind,m) = PAy*yint(aind-Nam,bind,m)-PCy*yint(aind-Nam,bind,m+1) + ints(aind-Nam,bind,m+1);
		  zint(aind,bind,m) = PAy*zint(aind-Nam,bind,m)-PCy*zint(aind-Nam,bind,m+1);
		}

		if (ma > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(ma-1)*(ints(aind-2*Nam,bind,m) - ints(aind-2*Nam,bind,m+1));
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		    xint(aind,bind,m) += o2g*(ma-1)*(xint(aind-2*Nam,bind,m) - xint(aind-2*Nam,bind,m+1));
		    yint(aind,bind,m) += o2g*(ma-1)*(yint(aind-2*Nam,bind,m) - yint(aind-2*Nam,bind,m+1));
		    zint(aind,bind,m) += o2g*(ma-1)*(zint(aind-2*Nam,bind,m) - zint(aind-2*Nam,bind,m+1));
		  }
		}

		if (mb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*mb*(ints(aind-Nam,bind-Nbm,m) - ints(aind-Nam,bind-Nbm,m+1));
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		    xint(aind,bind,m) += o2g*mb*(xint(aind-Nam,bind-Nbm,m) - xint(aind-Nam,bind-Nbm,m+1));
		    yint(aind,bind,m) += o2g*mb*(yint(aind-Nam,bind-Nbm,m) - yint(aind-Nam,bind-Nbm,m+1));
		    zint(aind,bind,m) += o2g*mb*(zint(aind-Nam,bind-Nbm,m) - zint(aind-Nam,bind-Nbm,m+1));
		  }
		}

	      }	else if (la > 0) {

		for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		  ints(aind,bind,m) = PAx*ints(aind-Nal,bind,m)-PCx*ints(aind-Nal,bind,m+1);
		for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		  xint(aind,bind,m) = PAx*xint(aind-Nal,bind,m)-PCx*xint(aind-Nal,bind,m+1) + ints(aind-Nal,bind,m+1);
		  yint(aind,bind,m) = PAx*yint(aind-Nal,bind,m)-PCx*yint(aind-Nal,bind,m+1);
		  zint(aind,bind,m) = PAx*zint(aind-Nal,bind,m)-PCx*zint(aind-Nal,bind,m+1);
		}

		if (la > 1) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*(la-1)*(ints(aind-2*Nal,bind,m) - ints(aind-2*Nal,bind,m+1));
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		    xint(aind,bind,m) += o2g*(la-1)*(xint(aind-2*Nal,bind,m) - xint(aind-2*Nal,bind,m+1));
		    yint(aind,bind,m) += o2g*(la-1)*(yint(aind-2*Nal,bind,m) - yint(aind-2*Nal,bind,m+1));
		    zint(aind,bind,m) += o2g*(la-1)*(zint(aind-2*Nal,bind,m) - zint(aind-2*Nal,bind,m+1));
		  }
		}

		if (lb > 0) {
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++)
		    ints(aind,bind,m) += o2g*lb*(ints(aind-Nal,bind-Nbl,m) - ints(aind-Nal,bind-Nbl,m+1));
		  for(int m=0;m<=mmax-lambdaa-lambdab;m++) {
		    xint(aind,bind,m) += o2g*lb*(xint(aind-Nal,bind-Nbl,m) - xint(aind-Nal,bind-Nbl,m+1));
		    yint(aind,bind,m) += o2g*lb*(yint(aind-Nal,bind-Nbl,m) - yint(aind-Nal,bind-Nbl,m+1));
		    zint(aind,bind,m) += o2g*lb*(zint(aind-Nal,bind-Nbl,m) - zint(aind-Nal,bind-Nbl,m+1));
		  }
		}
	      }
	      else {
		ERROR_INFO();
		throw std::runtime_error("Something went haywire in the Obara-Saika nuclear attraction integral derivative algorithm.\n");
	      }
	    }
	  }
      }
    }

  // Size of returned array
  int Na=(am_a+1)*(am_a+2)/2;
  int Nb=(am_b+1)*(am_b+2)/2;

  // Returned array
  std::vector<arma::mat> V(3);
  for(int ic=0;ic<3;ic++)
    V[ic].zeros(Na,Nb);

  // Fill in array
  int ia, ib;

  // Index of left basis function
  ia=0;

  // Loop over basis functions on this shell
  for(int ii=0; ii<=am_a; ii++) {
    int la=am_a - ii;
    for(int jj=0; jj<=ii; jj++) {
      int ma=ii - jj;
      int na=jj;

      // Index in worker array
      int aind=la*Nal+ma*Nam+na;

      // Index of right basis function
      ib=0;

      // Loop over angular momentum of second shell
      for(int kk=0; kk<=am_b; kk++) {
	int lb=am_b - kk;

	for(int ll=0; ll<=kk; ll++) {
	  int mb=kk-ll;
	  int nb=ll;

	  // Other index is
	  int bind=lb*Nbl+mb*Nbm+nb;

	  // Store result
	  V[0](ia,ib)=xint(aind,bind,0);
	  V[1](ia,ib)=yint(aind,bind,0);
	  V[2](ia,ib)=zint(aind,bind,0);

	  // Increment index of basis function
	  ib++;
	}
      }

      // Increment index of basis function
      ia++;
    }
  }

#ifdef DEBUG
  // Compute Huzinaga integrals
  std::vector<arma::mat> huzint(V);
  for(size_t ic=0;ic<V.size();ic++)
    huzint[ic].zeros();

  int diff=0;

  ia=0;
  // Loop over basis functions on this shell
  for(int ii=0; ii<=am_a; ii++) {
    int ila=am_a - ii;
    for(int jj=0; jj<=ii; jj++) {
      int ima=ii - jj;
      int ina=jj;

      ib=0;
      // Loop over basis functions on this shell
      for(int kk=0; kk<=am_b; kk++) {
	int ilb=am_b - kk;
	for(int ll=0; ll<=kk; ll++) {
	  int imb=kk - ll;
	  int inb=ll;

	  nuclear_int_der(xa,ya,za,zetaa,ila,ima,ina,xnuc,ynuc,znuc,xb,yb,zb,zetab,ilb,imb,inb,huzint[0](ia,ib),huzint[1](ia,ib),huzint[2](ia,ib));

	  for(int ic=0;ic<3;ic++)
	    // Differring sign convention - we actually do the force, not the derivative
	    if(fabs(huzint[ic](ia,ib)+V[ic](ia,ib))>100*DBL_EPSILON*std::max(fabs(huzint[ic](ia,ib)),fabs(V[ic](ia,ib))))
	      diff++;

	  ib++;
	}
      }

      ia++;
    }
  }

  if(diff==0)
    //    printf("Computed NAI shell (%e,%e,%e) - (%e,%e,%e) with nucleus at (%e,%e,%e) and exponents %e and %e with am=(%i,%i), the results match.\n",xa,ya,za,xb,yb,zb,xnuc,ynuc,znuc,zetaa,zetab,am_a,am_b);
    ;
  else {
    ia=0;
    // Loop over basis functions on this shell
    for(int ii=0; ii<=am_a; ii++) {
      int ila=am_a - ii;
      for(int jj=0; jj<=ii; jj++) {
	int ima=ii - jj;
	int ina=jj;

	ib=0;
	// Loop over basis functions on this shell
	for(int kk=0; kk<=am_b; kk++) {
	  int ilb=am_b - kk;
	  for(int ll=0; ll<=kk; ll++) {
	    int imb=kk - ll;
	    int inb=ll;

	    for(int ic=0;ic<3;ic++)
	      if(fabs(huzint[ic](ia,ib)-V[ic](ia,ib))>100*DBL_EPSILON*std::max(fabs(huzint[ic](ia,ib)),fabs(V[ic](ia,ib)))) {
		printf("Computed NAI shell (%e,%e,%e) - (%e,%e,%e) with nucleus at (%e,%e,%e) and exponents %e and %e.\n",xa,ya,za,xb,yb,zb,xnuc,ynuc,znuc,zetaa,zetab);
		printf("The result for the %i derivative (%i,%i,%i)-(%i,%i,%i) is %e with Huzinaga, whereas %e with Obara-Saika; difference is %e.\n\n",(int) ic, ila,ima,ina,ilb,imb,inb,huzint[ic](ia,ib),V[ic](ia,ib),huzint[ic](ia,ib)-V[ic](ia,ib));
	      }
	    ib++;
	  }
	}
	ia++;
      }
    }
  }
#endif

  return V;
}

std::vector<arma::mat> nuclear_int_ders_os(double xa, double ya, double za, double zetaa, const std::vector<shellf_t> & carta, double xnuc, double ynuc, double znuc, double xb, double yb, double zb, double zetab, const std::vector<shellf_t> & cartb) {
  // Compute shell of overlap integrals

  // Angular momenta of shells
  int am_a=carta[0].l+carta[0].m+carta[0].n;
  int am_b=cartb[0].l+cartb[0].m+cartb[0].n;

  // Compute the matrices
  std::vector<arma::mat> V=nuclear_int_ders_os(xa,ya,za,zetaa,am_a,xnuc,ynuc,znuc,xb,yb,zb,zetab,am_b);

  // Plug in the relative normalization factors
  for(size_t i=0;i<carta.size();i++)
    for(size_t j=0;j<cartb.size();j++)
      for(size_t ic=0;ic<V.size();ic++)
	V[ic](i,j)*=carta[i].relnorm*cartb[j].relnorm;

  return V;
}

arma::cube three_overlap_int_os(double xa, double ya, double za, double xc, double yc, double zc, double xb, double yb, double zb, double zetaa, double zetac, double zetab, const std::vector<shellf_t> & carta, const std::vector<shellf_t> & cartc, const std::vector<shellf_t> & cartb) {

  // Angular momenta of shells
  const int am_a=carta[0].l+carta[0].m+carta[0].n;
  const int am_b=cartb[0].l+cartb[0].m+cartb[0].n;
  const int am_c=cartc[0].l+cartc[0].m+cartc[0].n;

  //  printf("am_a = %i, am_b = %i, am_c = %i.\n",am_a,am_b,am_c);

  // Necessary size for work array
  const int size_a=(am_a+1)*(am_a+1)*am_a+1;
  const int size_c=(am_c+1)*(am_c+1)*am_c+1;
  const int size_b=(am_b+1)*(am_b+1)*am_b+1;

  // Work array for recursion formulas
  arma::cube ints(size_a,size_c,size_b);
  ints.zeros();

  // Helpers for computing indices on work array
  const int Nan = 1;
  const int Nam = am_a+1;
  const int Nal = Nam*Nam;

  const int Nbn = 1;
  const int Nbm = am_b+1;
  const int Nbl = Nbm*Nbm;

  const int Ncn = 1;
  const int Ncm = am_c+1;
  const int Ncl = Ncm*Ncm;

  // Reduced exponents
  double xi=zetaa*zetab/(zetaa+zetab);
  double zeta=zetaa+zetab;

  // r_ab
  double rabsq=(xa-xb)*(xa-xb)+(ya-yb)*(ya-yb)+(za-zb)*(za-zb);

  // P
  double px=(zetaa*xa+zetab*xb)/zeta;
  double py=(zetaa*ya+zetab*yb)/zeta;
  double pz=(zetaa*za+zetab*zb)/zeta;

  // r_pc
  double rpcsq=(px-xc)*(px-xc)+(py-yc)*(py-yc)+(pz-zc)*(pz-zc);

  // G
  double gx=(zeta*px+zetac*xc)/(zeta+zetac);
  double gy=(zeta*py+zetac*yc)/(zeta+zetac);
  double gz=(zeta*pz+zetac*zc)/(zeta+zetac);

  // GA
  double gax=gx-xa;
  double gay=gy-ya;
  double gaz=gz-za;
  // GB
  double gbx=gx-xb;
  double gby=gy-yb;
  double gbz=gz-zb;
  // GC
  double gcx=gx-xc;
  double gcy=gy-yc;
  double gcz=gz-zc;

  // Compute initial data.
  ints(0,0,0)=(M_PI/(zeta+zetac))*sqrt(M_PI/(zeta+zetac))*exp(-xi*rabsq - zeta*zetac/(zeta+zetac)*rpcsq);

  // Now am=(0,0,0). Increase LHS angular momentum.
  // Loop over total angular momentum
  for(int lambdaa=1;lambdaa<=am_a;lambdaa++)
    // Loop over angular momentum functions belonging to this shell
    for(int ii=0; ii<=lambdaa; ii++) {
      int la=lambdaa - ii;
      for(int jj=0; jj<=ii; jj++) {
	int ma=ii - jj;
	int na=jj;

	// Index in integrals table
	int aind = la*Nal+ma*Nam+na*Nan;

	// Compute integral
	if(la>0) {

	  ints(aind,0,0)+=gax*ints(aind-Nal,0,0);
	  if(la>1)
	    ints(aind,0,0)+=0.5/(zeta+zetac)*(la-1)*ints(aind-2*Nal,0,0);

	} else if(ma>0) {

	  ints(aind,0,0)+=gay*ints(aind-Nam,0,0);
	  if(ma>1)
	    ints(aind,0,0)+=0.5/(zeta+zetac)*(ma-1)*ints(aind-2*Nam,0,0);

	} else if(na>0) {

	  ints(aind,0,0)+=gaz*ints(aind-Nan,0,0);
	  if(na>1)
	    ints(aind,0,0)+=0.5/(zeta+zetac)*(na-1)*ints(aind-2*Nan,0,0);

	}
      }
    }

  // Increase total angular momentum on right-hand side
  // Loop over total angular momentum of LHS
  for(int lambdaa=0;lambdaa<=am_a;lambdaa++)

    // Loop over angular momentum of second shell
    for(int kk=0; kk<=lambdaa; kk++) {
      int la=lambdaa-kk;

      for(int ll=0; ll<=kk; ll++) {
	int ma=kk-ll;
	int na=ll;

	// LHS index is
	int aind = la*Nal + ma*Nam + na*Nan;

	// Loop over total angular momentum of RHS
	for(int lambdab=1;lambdab<=am_b;lambdab++)

	  // Loop over the functions belonging to the shell
	  for(int ii=0; ii<=lambdab; ii++) {
	    int lb=lambdab - ii;
	    for(int jj=0; jj<=ii; jj++) {
	      int mb=ii - jj;
	      int nb=jj;

	      // RHS index is
	      int bind = lb*Nbl + mb*Nbm + nb*Nbn;

	      // Compute integral
	      if(lb>0) {

		ints(aind,0,bind)+=gbx*ints(aind,0,bind-Nbl);
		if(lb>1)
		  ints(aind,0,bind)+=0.5/(zeta+zetac)*(lb-1)*ints(aind,0,bind-2*Nbl);
		if(la>0)
		  ints(aind,0,bind)+=0.5/(zeta+zetac)*la*ints(aind-Nal,0,bind-Nbl);

	      } else if(mb>0) {

		ints(aind,0,bind)+=gby*ints(aind,0,bind-Nbm);
		if(mb>1)
		  ints(aind,0,bind)+=0.5/(zeta+zetac)*(mb-1)*ints(aind,0,bind-2*Nbm);
		if(ma>0)
		  ints(aind,0,bind)+=0.5/(zeta+zetac)*ma*ints(aind-Nam,0,bind-Nbm);

	      } else if(nb>0) {

		ints(aind,0,bind)+=gbz*ints(aind,0,bind-Nbn);
		if(nb>1)
		  ints(aind,0,bind)+=0.5/(zeta+zetac)*(nb-1)*ints(aind,0,bind-2*Nbn);
		if(na>0)
		  ints(aind,0,bind)+=0.5/(zeta+zetac)*na*ints(aind-Nan,0,bind-Nbn);

	      } else {
		ERROR_INFO();
		throw std::runtime_error("Something went haywire in the Obara-Saika three-center overlap algorithm.\n");
	      }
	    }
	  }
      }
    }

  // Finally, increase angular momentum in the middle
  // Loop over total angular momentum of LHS
  for(int lambdaa=0;lambdaa<=am_a;lambdaa++)

    // Loop over angular momentum of second shell
    for(int kk=0; kk<=lambdaa; kk++) {
      int la=lambdaa-kk;

      for(int ll=0; ll<=kk; ll++) {
	int ma=kk-ll;
	int na=ll;

	// LHS index is
	int aind = la*Nal + ma*Nam + na*Nan;

	// Loop over total angular momentum of RHS
	for(int lambdab=0;lambdab<=am_b;lambdab++)

	  // Loop over the functions belonging to the shell
	  for(int ii=0; ii<=lambdab; ii++) {
	    int lb=lambdab - ii;
	    for(int jj=0; jj<=ii; jj++) {
	      int mb=ii - jj;
	      int nb=jj;

	      // RHS index is
	      int bind = lb*Nbl + mb*Nbm + nb*Nbn;

	      // Loop over total angular momentum of middle
	      for(int lambdac=1;lambdac<=am_c;lambdac++)
		// Loop over the functions belonging to the shell
		for(int mm=0; mm<=lambdac; mm++) {
		  int lc=lambdac - mm;
		  for(int nn=0; nn<=mm; nn++) {
		    int mc=mm - nn;
		    int nc=nn;

		    // RHS index is
		    int cind = lc*Ncl + mc*Ncm + nc*Ncn;

		    // Calculate integral
		    if(lc>0) {

		      ints(aind,cind,bind)+=gcx*ints(aind,cind-Ncl,bind);
		      if(lc>1)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*(lc-1)*ints(aind,cind-2*Ncl,bind);
		      if(la>0)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*la*ints(aind-Nal,cind-Ncl,bind);
		      if(lb>0)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*lb*ints(aind,cind-Ncl,bind-Nbl);

		    } else if(mc>0) {

		      ints(aind,cind,bind)+=gcy*ints(aind,cind-Ncm,bind);
		      if(mc>1)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*(mc-1)*ints(aind,cind-2*Ncm,bind);
		      if(ma>0)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*ma*ints(aind-Nam,cind-Ncm,bind);
		      if(mb>0)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*mb*ints(aind,cind-Ncm,bind-Nbm);

		    } else if(nc>0) {

		      ints(aind,cind,bind)+=gcz*ints(aind,cind-Ncn,bind);
		      if(nc>1)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*(nc-1)*ints(aind,cind-2*Ncn,bind);
		      if(na>0)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*na*ints(aind-Nan,cind-Ncn,bind);
		      if(nb>0)
			ints(aind,cind,bind)+=0.5/(zeta+zetac)*nb*ints(aind,cind-Ncn,bind-Nbn);

		    } else {
		      ERROR_INFO();
		      throw std::runtime_error("Something went haywire in the Obara-Saika three-center overlap algorithm.\n");
		    }
		  }
		}
	    }
	  }
      }
    }

  // Size of returned array
  const int Na=carta.size();
  const int Nc=cartc.size();
  const int Nb=cartb.size();

  // Fill in returned array
  arma::cube S(Na,Nc,Nb);
  S.zeros();

  for(size_t i=0;i<carta.size();i++) {
    int la=carta[i].l;
    int ma=carta[i].m;
    int na=carta[i].n;

    double ca=carta[i].relnorm;

    // LHS index in worker array
    int aind=la*Nal+ma*Nam+na;

    for(size_t j=0;j<cartb.size();j++) {
      int lb=cartb[j].l;
      int mb=cartb[j].m;
      int nb=cartb[j].n;

      double cb=cartb[j].relnorm;

      // RHS index in worker array
      int bind=lb*Nbl+mb*Nbm+nb;

      for(size_t k=0;k<cartc.size();k++) {
	int lc=cartc[k].l;
	int mc=cartc[k].m;
	int nc=cartc[k].n;

	double cc=cartc[k].relnorm;

	// Middle index in worker array
	int cind = lc*Ncl + mc*Ncm + nc*Ncn;

	/*
	printf("(%i,%i,%i) (%i,%i,%i) (%i,%i,%i)\n",la,ma,na,lb,mb,nb,lc,mc,nc);
	printf("S is (%i,%i,%i), accessing element (%i,%i,%i).\n",Na,Nc,Nb,i,k,j);
	printf("ints is (%i,%i,%i), accessing element (%i,%i,%i).\n",size_a,size_c,size_b,aind,cind,bind);
	fflush(stdout);
	*/

	S(i,k,j)=ca*cb*cc*ints(aind,cind,bind);
      }
    }
  }

  return S;
}


#include "RoutingCost.h"
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/LaneletOrArea.h>
#include <lanelet2_core/utility/Units.h>
#include <boost/geometry/algorithms/perimeter.hpp>

namespace lanelet {
namespace routing {
namespace {
Velocity speedLimit(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& la) {
  auto limit = la.applyVisitor([&](auto& la) { return trafficRules.speedLimit(la); });
  if (limit.speedLimit.value() == 0.) {
    limit.speedLimit = std::numeric_limits<double>::max() * Velocity();
  }
  if (std::isinf(limit.speedLimit.value())) {
    throw lanelet::InvalidInputError("Infinite speed limit returned by trafficRules object");
  }
  return limit.speedLimit;
}
}  // namespace

double RoutingCostTravelTime::travelTime(const traffic_rules::TrafficRules& trafficRules, const ConstLanelet& ll) {
  auto limit = speedLimit(trafficRules, ll);
  return units::SecondQuantity(geometry::approximatedLength2d(ll) * units::Meter() / limit).value();
}

double RoutingCostTravelTime::travelTime(const traffic_rules::TrafficRules& trafficRules, const ConstArea& ar) {
  auto limit = speedLimit(trafficRules, ar);
  auto diameter = boost::geometry::perimeter(utils::to2D(ar.outerBoundPolygon()));
  return units::SecondQuantity(diameter * units::Meter() / limit).value();
}

double RoutingCostDistance::length(const ConstLanelet& ll) noexcept { return geometry::approximatedLength2d(ll); }

double RoutingCostDistance::length(const ConstArea& ar) noexcept {
  return double(boost::geometry::perimeter(utils::to2D(ar.outerBoundPolygon())));
}
}  // namespace routing
}  // namespace lanelet


#include <iostream>
#include <vector>
#include <zmqpp/zmqpp.hpp>
#include <string>
#include <sstream>
#include <stdlib.h>     /* for realloc() and free() */
#include <string.h>     /* for memset() */
#include <errno.h>      /* for errno */
#include "nlohmann/json.hpp"
#include <armadillo>
#include <typeinfo>
#include "Threadpool.hh"

using namespace zmqpp;
using namespace arma;
using json = nlohmann::json;

class EWorker{
    private:
        mat r_ic;
        std::vector<mat> cov;
        mat regcov;
        mat mu;
        vec pivector;
        mat data;
    public:
        EWorker(std::vector<std::vector<double>> muvec,
                std::vector<double> piv,
                std::vector<std::vector<double>> regcovvec,
                std::vector<std::vector<std::vector<double>>> covvec,
                std::vector<std::vector<double>> xvec

                ){
            // int n_of_sources = data.size();
            // int n_of_data = data.at(0).size();
            int counter = 0;
            for (std::vector<double> cmu: muvec){
                mu.insert_cols(counter, vec(cmu));
                counter++;
            }
            pivector = vec(piv);
            counter = 0;
            for (std::vector<double> cregcov: regcovvec){
                regcov.insert_cols(counter, vec(cregcov));
                counter++;
            }
            counter = 0;
            int counter2 = 0;
            for (std::vector<std::vector<double>> covmatrix : covvec){
                mat temp;
                for(std::vector<double>covmatrixrow: covmatrix){
                    temp.insert_cols(counter,vec(covmatrixrow));
                    counter++;
                }
                counter = 0;
                cov.insert(cov.begin() + counter2, temp);
                counter2++;
            }

            // cout<<"MU:\n-------------------\n"<<mu<<"\n-------------------\n";
            // cout<<"PI:\n-------------------\n"<<pivector<<"\n-------------------\n";
            // // cout<<"COV:\n-------------------\n";

            // for (mat covtest : cov)
            //     cout<<covtest<<endl;
            // cout<<"\n-------------------\n";
            for(int i=0;i<xvec.at(0).size();i++){
                std::vector<double>temp(xvec.size());
                counter = 0;
                for(std::vector<double> datarow : xvec){
                    temp.at(counter)= datarow.at(i);
                    counter++;
                }
                data.insert_cols(i, vec(temp));
            }
            // cout<<"\n-------------------\n";

            // cout<<"DATA: "<<endl<<data;
            // cout<<"\n-------------------\n";

            // r_ic = new mat(n_of_data, n_of_sources);
            // cov = new mat(n_of_data, n_of_data);
            // std::cout<<mu<<endl;
        }

        mat& getRIC(){
            // r_ic.zeros();
            mat datatranspose = data.t();
            for(int i = 0; i < mu.n_cols; i++){
                {
                    ThreadPool pool(4);
                    pool.enqueue([i, this, datatranspose]{
                    mat co = cov.at(i);
                    co += regcov;
                    std::vector<double> r(data.n_rows);
                    double detco = det(co);
                    double pic = pivector.at(i);
                    double fraction = 1/sqrt(pow((2*datum::pi), mu.n_rows)* det(co));
                    vec m(mu.colptr(i),mu.n_rows);

                        for(int j = 0; j < datatranspose.n_cols; j++){

                            vec datasubset(datatranspose.colptr(j), datatranspose.n_rows);
                            mat delta = datasubset - m;
                            mat deltatranspose = delta.t();
                            double exparg = ((-0.5)*deltatranspose*inv(co)*delta).eval()(0,0);
                            double eulerexp = exp(exparg);
                            // cout<<"First: "<<(-0.5)*deltatranspose;
                            // cout<<"Second:\n "<<inv(co)*delta<<endl;

                            // cout<<"Fraction: "<<fraction<<"\tExparg: "<<exparg<<"\tEulerexp:"<<eulerexp<<endl;
                            double pdf = eulerexp * fraction;
                            pdf *= pic;
                            r.at(j)=pdf;

                        }
                    r_ic.insert_cols(i,vec(r));
                });
                }

            }

            r_ic = normalise(r_ic,1,1);
            return r_ic;
        }
};

// Lo que debe hacer el worker es:
// 1. Recibir un arreglo de tuplas
// 2. Iterar sobre calcular el arreglo ric para estos datos
// 3. Enviar Ric al sink

// class EWorker {
//   public:
//     EWorker(std::vector <std::vector <float>> data) {

//     };

//     std::vector <std::vector <float>> obtenerVectorRIC() {
    // """E Step"""
    // r_ic = np.zeros((len(self.X),len(self.cov)))
    // for m,co,p,r in zip(self.mu,self.cov,self.pi,range(len(r_ic[0]))):
    //     co+=self.reg_cov
    //     mn = multivariate_normal(mean=m,cov=co)
    //     r_ic[:,r] = p*mn.pdf(self.X)/np.sum([pi_c*multivariate_normal(mean=mu_c,cov=cov_c).pdf(X) for pi_c,mu_c,cov_c in zip(self.pi,self.mu,self.cov+self.reg_cov)],axis=0)

//     }
// };

// std::vector <std::vector<float>> convertToVector(std::string npArray) {
//     std::vector <float> vec;
//     vec.push_back(1);
//     std::vector <std::vector <float>> vecs;
//     vecs.push_back(vec);
//     return vecs;

// }
int main() {
    context ctx;
    zmqpp::message message;
    socket work(ctx, socket_type::pull);
    socket sink(ctx, socket_type::push);
    std::cout<<"connecting"<<std::endl;
    work.connect("tcp://localhost:5557");
    sink.connect("tcp://localhost:5558");
    std::cout<<"Receiving"<<std::endl;

    while(true){
    work.receive(message);
    json info = json::parse(message.get(0));

    std::cout<<"using json"<<std::endl;
    // std::cout << info["mu"] << std::endl;
    // std::vector<double> armadillotest{10,20,30};
    // mat hi(armadillotest);
    std::vector<std::vector<double>> mu = info["mu"].get <std::vector<std::vector<double>>> ();
    std::vector<double> pi = info["pi"].get <std::vector <double>>();
    std::vector<std::vector<double>> regcov = info["regcov"].get <std::vector<std::vector<double>>> ();
    std::vector<std::vector<std::vector<double>>> cov = info["cov"]
    .get <std::vector<std::vector<std::vector<double>>>> ();
    std::vector<std::vector<double>> x = info["x"].get <std::vector<std::vector<double>>> ();
    // std::cout<<"mu: "<<info["mu"]<<std::endl;

    EWorker worker(mu,pi,regcov, cov, x);
    mat  r_ic = worker.getRIC();
    std::vector<std::vector <double>> vectosend(x.size());
    for (int i=0; i<r_ic.n_rows;i++){
        vectosend[i]=arma::conv_to<std::vector<double>>::from(r_ic.row(i));
    }
    cout<<"R_IC:\n-------------------\n";

    cout<<r_ic<<endl;

    json tosend;
    tosend["ric"] = vectosend;
    tosend["X"] = x;
    auto t = tosend.dump();
    sink.send(t);
    }
    // std::vector <std::vector <float>> dataAsVector = convertToVector(message.get(0));
    // EWorker worker = EWorker(dataAsVector);
    // std::vector <std::vector <float>> ric = worker.obtenerVectorRIC();
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// Generalized Pose-and-Scale Estimation using 4-Point Congruence Constraints
//
// Victor Fragoso and Sudipta Sinha.
// In Proc. of the IEEE International Conf. on 3D Vision (3DV) 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <gp4pc/gp4pc.h>
#include <gtest/gtest.h>
#include <math/utils.h>
#include <random>

namespace msft {
namespace {

// Random seed.
const int kSeed = 67;

using Eigen::AngleAxisd;
using Eigen::Quaterniond;
using Eigen::Vector3d;

// Useful aliases.
using Input = Gp4pc::Input;
using Solution = Gp4pc::Solution;

struct TestParams {
  double max_reprojection_error;
  double max_rotation_difference;
  double max_translation_difference;
  double max_scale_difference;
  double projection_noise_std_dev;
  double max_depth_sq_error;
  double max_alignment_sq_error;
  Gp4pc::Params solver_params;
};

struct ExpectedSolution {
  Quaterniond rotation;
  Vector3d translation;
  double scale;
};

struct TestInput {
  std::vector<Vector3d> camera_centers;
  std::vector<Vector3d> world_points;
};

// Fixture to test the solver.
class Gp4pcTest : public ::testing::Test {
 public:
  static void SetUpTestCase() {
    // Create random number generator.
    rng = new std::mt19937(kSeed);
  }

  static void TearDownTestCase() {
    delete rng;
  }

  static Input GenerateTestingData(const ExpectedSolution& expected_solution,
                                   const TestParams& test_params,
                                   const TestInput& test_input,
                                   Eigen::Vector4d* depths);

  static void TestWithNoise(const ExpectedSolution& expected_solution,
                            const TestParams& test_params,
                            const TestInput& test_input,
                            const bool use_planar_solver);

  static void TestWithNoise(const ExpectedSolution& expected_solution,
                            const TestParams& test_params,
                            const TestInput& test_input);

  static void AddNoiseToRay(const double nose_std_dev, Eigen::Vector3d* ray);

  static std::mt19937* rng;
};

std::mt19937* Gp4pcTest::rng = nullptr;

inline bool CheckDepthErrors(const Eigen::Vector4d& estimated_depths,
                             const Eigen::Vector4d& gt_depths,
                             const double max_depth_sq_error) {
  const double sq_error = (estimated_depths - gt_depths).squaredNorm();
  return sq_error < max_depth_sq_error;
}

bool CheckReprojectionErrors(const Input& input,
                             const Eigen::Quaterniond& soln_rotation,
                             const Eigen::Vector3d& soln_translation,
                             const double soln_scale,
                             const double max_reprojection_error) {
  const int num_points = input.world_points.size();
  const std::vector<Vector3d>& camera_rays = input.ray_directions;
  const std::vector<Vector3d>& ray_origins = input.ray_origins;
  const std::vector<Vector3d>& world_points = input.world_points;
  double good_reprojection_errors = true;
  for (int i = 0; i < num_points; ++i) {
    const Quaterniond unrot =
        Quaterniond::FromTwoVectors(camera_rays[i], Vector3d(0, 0, 1));
    const Vector3d reprojected_point =
        soln_scale * (soln_rotation * world_points[i]) + soln_translation -
        ray_origins[i];

    const Vector3d unrot_cam_ray = unrot * camera_rays[i];
    const Vector3d unrot_reproj_pt = unrot * reprojected_point;

    const double reprojection_error =
        (unrot_cam_ray.hnormalized() - unrot_reproj_pt.hnormalized()).norm();
    good_reprojection_errors = (good_reprojection_errors &&
                                (reprojection_error < max_reprojection_error));
  }
  return good_reprojection_errors;
}

void Gp4pcTest::AddNoiseToRay(const double noise_std_dev,
                              Eigen::Vector3d* ray) {
  static std::normal_distribution noise_dist(0.0, noise_std_dev);
  const double scale = CHECK_NOTNULL(ray)->norm();
  const double noise_x = noise_dist(*rng);
  const double noise_y = noise_dist(*rng);

  Eigen::Quaterniond rot =
      Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), *ray);
  Eigen::Vector3d noisy_point(noise_x, noise_y, 1);
  noisy_point *= scale;

  *ray = (rot * noisy_point).normalized();
}

Input Gp4pcTest::GenerateTestingData(const ExpectedSolution& expected_solution,
                                     const TestParams& test_params,
                                     const TestInput& test_input,
                                     Eigen::Vector4d* depths) {
  Input input;
  const int num_points = test_input.world_points.size();
  CHECK_EQ(num_points, 4) << "Num. of world points must be 4.";
  const int num_cameras = test_input.camera_centers.size();
  std::vector<Vector3d>& camera_rays = input.ray_directions;
  std::vector<Vector3d>& ray_origins = input.ray_origins;
  camera_rays.reserve(num_points);
  ray_origins.reserve(num_points);
  input.world_points = test_input.world_points;
  const std::vector<Vector3d>& world_points = test_input.world_points;
  const std::vector<Vector3d>& camera_centers = test_input.camera_centers;
  const Quaterniond expected_rotation = expected_solution.rotation;
  const Vector3d expected_translation = expected_solution.translation;
  const double expected_scale = expected_solution.scale;
  Eigen::Vector3d ray;
  for (int i = 0; i < num_points; ++i) {
    // Setting camera positions or ray origins.
    ray_origins.emplace_back(
        expected_scale * (expected_rotation * camera_centers[i % num_cameras]) +
        expected_translation);

    // Reproject 3D points into camera frame.
    ray = (expected_scale * (expected_rotation * world_points[i]) +
           expected_translation -
           ray_origins[i]);
    (*depths)(i) = ray.norm();
    camera_rays.emplace_back(ray.normalized());
  }

  // Add noise to the camera rays.
  if (test_params.projection_noise_std_dev > 0.0) {
    // Adds noise to both of the rays.
    for (int i = 0; i < num_points; ++i) {
      AddNoiseToRay(test_params.projection_noise_std_dev, &camera_rays[i]);
    }
  }

  return input;
}

void Gp4pcTest::TestWithNoise(const ExpectedSolution& expected_solution,
                              const TestParams& test_params,
                              const TestInput& test_input,
                              const bool use_planar_solver) {
  static const double kDefaultCoplanarThresh = 1e-3;
  static const double kDefaultColinearThresh = 1e-2;
  // Generate testing data.
  Eigen::Vector4d depths;
  const Input input =
      GenerateTestingData(expected_solution, test_params, test_input, &depths);

  // Estimate.
  Solution solution;
  Gp4pc solver(test_params.solver_params);
  EXPECT_TRUE(solver.EstimateSimilarityTransformation(input, &solution));

  // Check the solutions here.
  bool matched_transform = false;
  EXPECT_GT(solution.rotations.size(), 0);
  for (int i = 0; i < solution.rotations.size(); ++i) {
    const bool good_reprojection_errors =
        CheckReprojectionErrors(input,
                                solution.rotations[i],
                                solution.translations[i],
                                solution.scales[i],
                                test_params.max_reprojection_error);
    const bool good_depth_estimate =
        CheckDepthErrors(solution.depths[i], depths,
                         test_params.max_depth_sq_error);
    const double rotation_difference =
        expected_solution.rotation.angularDistance(solution.rotations[i]);
    const bool matched_rotation =
        rotation_difference < test_params.max_rotation_difference;
    const double translation_difference =
        (expected_solution.translation -
         solution.translations[i]).squaredNorm();
    const bool matched_translation =
        translation_difference < test_params.max_translation_difference;
    const double scale_difference =
        fabs(expected_solution.scale - solution.scales[i]);
    const bool matched_scale =
        scale_difference < test_params.max_scale_difference;
    VLOG(3) << "Matched rotation: " << matched_rotation
            << " rotation error [deg]=" << RadToDeg(rotation_difference);
    VLOG(3) << "Matched translation: " << matched_translation
            << " translation error=" << translation_difference;
    VLOG(3) << "Matched scale: " << matched_scale
            << " scale difference=" << scale_difference;
    VLOG(3) << "Good reprojection errors: " << good_reprojection_errors;
    VLOG(3) << "Estimated depths: " << solution.depths[i].transpose();
    VLOG(3) << "GT depths: " << depths.transpose();
    VLOG(3) << "Good depth estimate: " << good_depth_estimate;
    if (matched_rotation && matched_translation && matched_scale &&
        good_reprojection_errors && good_depth_estimate) {
      matched_transform = true;
    }
  }

  EXPECT_TRUE(matched_transform);
}

inline void Gp4pcTest::TestWithNoise(const ExpectedSolution& expected_solution,
                                     const TestParams& test_params,
                                     const TestInput& test_input) {
  TestWithNoise(expected_solution, test_params, test_input, false);
}

TEST_F(Gp4pcTest, BasicRigidTransformationTest) {
  TestInput test_input;
  // World points:
  test_input.world_points = {
    Eigen::Vector3d(-1.0, 3.0, 3.0),
    Eigen::Vector3d(1.0, -1.0, 2.0),
    Eigen::Vector3d(-1.0, 1.0, 2.0),
    Eigen::Vector3d(2.0, 1.0, 3.0)
  };

  // Camera centers.
  test_input.camera_centers = {
    Eigen::Vector3d(-1.0, 0.0, 0.0),
    Eigen::Vector3d(0.0, 0.0, 0.0),
    Eigen::Vector3d(2.0, 0.0, 0.0),
    Eigen::Vector3d(3.0, 0.0, 0.0)
  };

  // Test parameters.
  TestParams test_params;
  test_params.projection_noise_std_dev = 0.0;
  test_params.max_reprojection_error =  1.0 / 512;
  test_params.max_rotation_difference = DegToRad(1.0);
  test_params.max_translation_difference = 1e-3;
  test_params.max_scale_difference = 1e-3;
  test_params.max_depth_sq_error = 1e-3;

  // Expected solution.
  ExpectedSolution solution;
  solution.rotation = Quaterniond(AngleAxisd(
      DegToRad(21.0), Vector3d(0.5, 0.0, 1.0).normalized()));
  solution.translation = Vector3d(1.0, 1.0, 1.0);
  solution.scale = 1.0;

  TestWithNoise(solution, test_params, test_input);
}

TEST_F(Gp4pcTest, RigidTransformationTestWithNoise) {
  TestInput test_input;
  // World points:
  test_input.world_points = {
    Eigen::Vector3d(-1.0, 3.0, 3.0),
    Eigen::Vector3d(1.0, -1.0, 2.0),
    Eigen::Vector3d(-1.0, 1.0, 2.0),
    Eigen::Vector3d(2.0, 1.0, 3.0)
  };

  // Camera centers.
  test_input.camera_centers = {
    Eigen::Vector3d(-1.0, 0.0, 0.0),
    Eigen::Vector3d(0.0, 0.0, 0.0),
    Eigen::Vector3d(2.0, 0.0, 0.0),
    Eigen::Vector3d(3.0, 0.0, 0.0)
  };

  // Test parameters.
  TestParams test_params;
  test_params.projection_noise_std_dev = 1e-3;
  test_params.max_reprojection_error = 5.0 / 512;
  test_params.max_rotation_difference = DegToRad(1.0);
  test_params.max_translation_difference = 1e-2;
  test_params.max_scale_difference = 1e-2;
  test_params.max_depth_sq_error = 1e-2;

  // Expected solution.
  ExpectedSolution solution;
  solution.rotation = Quaterniond(AngleAxisd(
      DegToRad(21.0), Vector3d(0.5, 0.0, 1.0).normalized()));
  solution.translation = Vector3d(1.0, 1.0, 1.0);
  solution.scale = 1.0;

  TestWithNoise(solution, test_params, test_input);
}

TEST_F(Gp4pcTest, BasicSimilarityTransformationTest) {
  TestInput test_input;
  // World points:
  test_input.world_points = {
    Eigen::Vector3d(-1.0, 3.0, 3.0),
    Eigen::Vector3d(1.0, -1.0, 2.0),
    Eigen::Vector3d(-1.0, 1.0, 2.0),
    Eigen::Vector3d(2.0, 1.0, 3.0)
  };

  // Camera centers.
  test_input.camera_centers = {
    Eigen::Vector3d(-1.0, 0.0, 0.0),
    Eigen::Vector3d(0.0, 0.0, 0.0),
    Eigen::Vector3d(2.0, 0.0, 0.0),
    Eigen::Vector3d(3.0, 0.0, 0.0)
  };

  // Test parameters.
  TestParams test_params;
  test_params.projection_noise_std_dev = 0.0;
  test_params.max_reprojection_error =  1.0 / 512;
  test_params.max_rotation_difference = DegToRad(1.0);
  test_params.max_translation_difference = 1e-3;
  test_params.max_scale_difference = 1e-3;
  test_params.max_depth_sq_error = 1e-3;

  // Expected solution.
  ExpectedSolution solution;
  solution.rotation = Quaterniond(AngleAxisd(
      DegToRad(21.0), Vector3d(0.5, 0.0, 1.0).normalized()));
  solution.translation = Vector3d(1.0, 1.0, 1.0);
  solution.scale = 2.0;

  TestWithNoise(solution, test_params, test_input);
}

TEST_F(Gp4pcTest, SimilarityTransformationTestWithNoise) {
  TestInput test_input;
  // World points:
  test_input.world_points = {
    Eigen::Vector3d(-1.0, 3.0, 3.0),
    Eigen::Vector3d(1.0, -1.0, 2.0),
    Eigen::Vector3d(-1.0, 1.0, 2.0),
    Eigen::Vector3d(2.0, 1.0, 3.0)
  };

  // Camera centers.
  test_input.camera_centers = {
    Eigen::Vector3d(-1.0, 0.0, 0.0),
    Eigen::Vector3d(0.0, 0.0, 0.0),
    Eigen::Vector3d(2.0, 0.0, 0.0),
    Eigen::Vector3d(3.0, 0.0, 0.0)
  };

  // Test parameters.
  TestParams test_params;
  test_params.projection_noise_std_dev = 1e-3;
  test_params.max_reprojection_error =  5.0 / 512;
  test_params.max_rotation_difference = DegToRad(1.0);
  test_params.max_translation_difference = 1e-2;
  test_params.max_scale_difference = 1e-2;
  test_params.max_depth_sq_error = 1e-2;

  // Expected solution.
  ExpectedSolution solution;
  solution.rotation = Quaterniond(AngleAxisd(
      DegToRad(21.0), Vector3d(0.5, 0.0, 1.0).normalized()));
  solution.translation = Vector3d(1.0, 1.0, 1.0);
  solution.scale = 2.0;

  TestWithNoise(solution, test_params, test_input);
}

TEST_F(Gp4pcTest, BailOutOnSingleCameraCenter) {
  Input input;
  input.world_points = {
    Eigen::Vector3d(-5.2868, 13.6551, 37.7451),
    Eigen::Vector3d(0.907803, 9.2807, 42.1868),
    Eigen::Vector3d(3.80463, 14.9699, 42.3962),
    Eigen::Vector3d(0.0431288, 10.2583, 42.5212)
  };

  input.ray_directions = {
    Eigen::Vector3d(-0.334288, 00.265068, 00.904428),
    Eigen::Vector3d(0-0.37081, -0.479971, 00.795065),
    Eigen::Vector3d(-0.14836, 0.236824, 0.960158),
    Eigen::Vector3d(-0.26705, 0.138519, 0.953675)
  };

  input.ray_origins = {
    Eigen::Vector3d(5.49048, -2.04053, -2.34362),
    Eigen::Vector3d(5.49048, -2.04053, -2.34362),
    Eigen::Vector3d(5.49048, -2.04053, -2.34362),
    Eigen::Vector3d(5.49048, -2.04053, -2.34362)
  };

  Eigen::Matrix3d gt_rotation;
  gt_rotation <<
      000.986208, 000.163803, 00.0237108,
      00-0.15569, 000.966737, 0-0.202928,
      -0.0561624, 000.196438, 000.978906;
  const Eigen::Vector3d translation(-1.3, 2.1, 00.5);
  const double scale = 2.5;

  // Estimate.
  Solution solution;
  TestParams test_params;
  Gp4pc solver(test_params.solver_params);
  EXPECT_FALSE(solver.EstimateSimilarityTransformation(input, &solution));
  EXPECT_TRUE(solution.rotations.empty());
  EXPECT_TRUE(solution.translations.empty());
  EXPECT_TRUE(solution.scales.empty());
}

TEST_F(Gp4pcTest, BasicPlanarCase) {
  TestInput test_input;
  // World points:
  test_input.world_points = {
    Eigen::Vector3d(-1.0, 3.0, 2.0),
    Eigen::Vector3d(1.0, -1.0, 2.0),
    Eigen::Vector3d(-1.0, 1.0, 2.0),
    Eigen::Vector3d(2.0, 1.0, 2.0)
  };

  // Camera centers.
  test_input.camera_centers = {
    Eigen::Vector3d(-1.0, 0.0, 0.0),
    Eigen::Vector3d(0.0, 0.0, 0.0),
    Eigen::Vector3d(2.0, 0.0, 0.0),
    Eigen::Vector3d(3.0, 0.0, 0.0)
  };

  // Test parameters.
  TestParams test_params;
  test_params.projection_noise_std_dev = 0.0;
  test_params.max_reprojection_error =  1.0 / 512;
  test_params.max_rotation_difference = DegToRad(1.0);
  test_params.max_translation_difference = 1e-3;
  test_params.max_scale_difference = 1e-3;
  test_params.max_depth_sq_error = 1e-3;
  // Set false to use_general_solver to allow the planar solver to be used.
  test_params.solver_params.use_general_solver = false;

  // Expected solution.
  ExpectedSolution solution;
  solution.rotation = Quaterniond(AngleAxisd(
      DegToRad(21.0), Vector3d(0.5, 0.0, 1.0).normalized()));
  solution.translation = Vector3d(1.0, 1.0, 1.0);
  solution.scale = 2.0;

  TestWithNoise(solution, test_params, test_input, true);
}

}  // namespace
}  // namespace msft


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Daniela Cabiddu                                                           *
*     http://www.imati.cnr.it/index.php/people/8-curricula/119-daniela-cabiddu  *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/polygon_maximum_inscribed_circle.h>
#include <cinolib/geometry/segment.h>
#include <cinolib/min_max_inf.h>

// Most of this is coming from here:
// http://www.boost.org/doc/libs/1_65_1/libs/polygon/doc/voronoi_diagram.htm
// http://www.boost.org/doc/libs/1_65_0/libs/polygon/example/voronoi_basic_tutorial.cpp
//
#ifdef CINOLIB_USES_BOOST
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry.hpp>
#include <boost/polygon/voronoi.hpp>
//
using boost::polygon::voronoi_builder;
using boost::polygon::voronoi_diagram;
using boost::polygon::voronoi_edge;
using boost::polygon::x;
using boost::polygon::y;
using boost::polygon::low;
using boost::polygon::high;
//
struct polygon_point
{
    int x;
    int y;
    polygon_point(int x, int y) : x(x), y(y) {}
};
//
struct polygon_segment
{
    polygon_point p0;
    polygon_point p1;
    polygon_segment(int x1, int y1, int x2, int y2) : p0(x1, y1), p1(x2, y2) {}
};
//
template<>
struct boost::polygon::geometry_concept<polygon_point>
{
    typedef boost::polygon::point_concept type;
};
//
template<>
struct boost::polygon::point_traits<polygon_point>
{
    typedef int coordinate_type;
    static inline coordinate_type get(const polygon_point & point, orientation_2d orient)
    {
        return (orient == HORIZONTAL) ? point.x : point.y;
    }
};
//
template<>
struct boost::polygon::geometry_concept<polygon_segment>
{
    typedef boost::polygon::segment_concept type;
};
//
template<>
struct boost::polygon::segment_traits<polygon_segment>
{
    typedef int coordinate_type;
    typedef polygon_point point_type;

    static inline point_type get(const polygon_segment& segment, direction_1d dir)
    {
        return dir.to_int() ? segment.p1 : segment.p0;
    }
};
//
typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint>  BoostPolygon;

#endif // CINOLIB_USES_BOOST

namespace cinolib
{

#ifdef CINOLIB_USES_BOOST

BoostPolygon make_boost_poly(const std::vector<vec2d> & poly)
{
    BoostPolygon boost_poly;
    for(vec2d p : poly) boost::geometry::append(boost_poly, BoostPoint(p.x(), p.y()));
    boost::geometry::correct(boost_poly);
    return boost_poly;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void polygon_maximum_inscribed_circle(const std::vector<vec2d> & poly,
                                            vec2d              & center,
                                            double             & radius)
{
    radius = 0.0;
    center = vec2d(0,0);

    vec2d min( inf_double,  inf_double);
    vec2d max(-inf_double, -inf_double);
    for(auto p : poly)
    {
        min = min.min(p);
        max = max.max(p);
    }

    double scale_factor;
    double diag = min.dist(max);

    if(diag >= 1.0) scale_factor = std::numeric_limits<int>::max() / diag;
    else            scale_factor = std::numeric_limits<int>::max() * diag;

    scale_factor *= 0.5; // dumb attempt to stay on the safe side and avoid overflows...

    // Boost implementation of Generazlied Voronoi uses integer coordinates.
    // In order to achieve maximum precision I am scaling the polygon as much
    // as I can in order to minimize loss of precision during integer roundoff...

    std::vector<polygon_segment> segments;
    for(uint i=0; i<poly.size(); ++i)
    {
        vec2d v0 = scale_factor * poly.at(i);
        vec2d v1 = scale_factor * poly.at((i+1)%poly.size());
        segments.push_back(polygon_segment(v0.x(), v0.y(), v1.x(), v1.y()));
    }

    voronoi_diagram<double> vd;
    construct_voronoi(segments.begin(), segments.end(), &vd);

    BoostPolygon boost_poly = make_boost_poly(poly);
    for(auto it=vd.vertices().begin(); it!=vd.vertices().end(); ++it)
    {
        const voronoi_diagram<double>::vertex_type &v = *it;
        const voronoi_diagram<double>::edge_type   *e = v.incident_edge();
        const voronoi_diagram<double>::cell_type   *c = e->cell();
        const polygon_segment                        &s = segments.at(c->source_index());

        // do not consider Voronoi vertices outside the polygon
        if (boost::geometry::within(BoostPoint(v.x()/scale_factor, v.y()/scale_factor), boost_poly))
        {
            // annoying wrap to vec3d (TODO: template cinolib::Segment to make it work in 2D too)
            vec3d beg(s.p0.x, s.p0.y, 0);
            vec3d end(s.p1.x, s.p1.y, 0);
            vec3d c3d(v.x(),  v.y(),  0);
            cinolib::Segment tmp(beg,end);
            double d = tmp.dist_to_point(c3d);

            if (d > radius)
            {
                radius = d;
                center = vec2d(c3d); // will automatically drop z
            }
        }
    }

    radius /= scale_factor;
    center /= scale_factor;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void polygon_maximum_inscribed_circle(const std::vector<vec3d> & poly,   // will drop z component
                                            vec3d              & center, // will have z=0
                                            double             & radius)
{
    std::vector<vec2d> poly_2d;
    for(auto p : poly) poly_2d.push_back(vec2d(p.x(), p.y()));

    vec2d center_2d;
    polygon_maximum_inscribed_circle(poly_2d, center_2d, radius);
    center = vec3d(center_2d.x(), center_2d.y(), 0);
}

#endif // CINOLIB_USES_BOOST

}


#include <frovedis.hpp>
#include <frovedis/ml/clustering/kmeans.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

void do_kmeans(const string& input, bool dense,
               const string& output, int k,
               int n_init, int num_iteration,
               double eps, long seed,
               bool binary, bool shrink) {
  if(binary) {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_loadbinary<double>(input);
      t.show("load matrix: ");
      auto r = kmeans(mat, k, num_iteration, eps, seed, n_init);
      t.show("kmeans time: ");
      r.transpose().savebinary(output);
      t.show("save centroid time: ");
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_loadbinary<double>(input);
      t.show("load matrix: ");
      rowmajor_matrix_local<double> r;
      if (shrink)
        r = frovedis::shrink::kmeans(mat, k, num_iteration,
                                     eps, seed, n_init);
      else
        r = kmeans(mat, k, num_iteration, eps, seed, n_init);
      t.show("kmeans time: ");
      r.transpose().savebinary(output);
      t.show("save centroid time: ");
    }
  } else {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_load<double>(input);
      t.show("load matrix: ");
      auto r = kmeans(mat, k, num_iteration, eps, seed, n_init);
      t.show("kmeans time: ");
      r.transpose().save(output);
      t.show("save model time: ");
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_load<double>(input);
      t.show("load matrix: ");
      rowmajor_matrix_local<double> r;
      if (shrink)
        r = frovedis::shrink::kmeans(mat, k, num_iteration,
                                     eps, seed, n_init);
      else
        r = kmeans(mat, k, num_iteration, eps, seed, n_init);
      t.show("kmeans time: ");
      r.transpose().save(output);
      t.show("save model time: ");
    }
  }
}

void do_assign(const string& input, bool dense,
               const string& input_centroid,
               const string& output, bool binary) {
  float score = 0.0f;
  if(binary) {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_local_loadbinary<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_loadbinary<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct, score);
      t.show("assign time: ");
      make_dvector_scatter(r).savebinary(output);
      t.show("prediction save time: ");
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_local_loadbinary<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_loadbinary<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct, score);
      t.show("assign time: ");
      make_dvector_scatter(r).savebinary(output);
      t.show("prediction save time: ");
    }
  } else {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_local_load<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_load<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct, score);
      t.show("assign time: ");
      make_dvector_scatter(r).saveline(output);
      t.show("prediction save time: ");
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_local_load<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_load<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct, score);
      t.show("assign time: ");
      make_dvector_scatter(r).saveline(output);
      t.show("prediction save time: ");
    }
  }
  std::cout << "prediction score: " << -1.0f * score << std::endl;
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("assign,a", "assign data to cluster mode")
    ("input,i", value<string>(), "input matrix")
    ("centroid,c", value<string>(), "input centroid for assignment")
    ("output,o", value<string>(), "output centroids or cluster")
    ("k,k", value<int>(), "number of clusters")
    ("num-init,t", value<int>(), "number of time for running with different centroid seeds (default: 1)")
    ("num-iteration,n", value<int>(), "maximum number of iteration (default: 300)")
    ("eps,e", value<double>(), "epsilon to stop the iteration (default: 0.0001)")
    ("seed,r", value<long>(), "seed for init randomizer (default: 0)")
    ("sparse,s", "use sparse matrix (default)")
    ("dense,d", "use dense matrix")
    ("use-shrink", "whether to use shrinking for sparse data (default: false)")
    ("verbose", "set loglevel to DEBUG")
    ("verbose2", "set loglevel to TRACE")
    ("binary,b", "use binary input/output");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  string input, output, input_centroid;
  int k = 0;
  int num_iteration = 300, n_init = 1;
  double eps = 0.0001;
  long seed = 0;
  bool shrink = false;
  bool assign = false;
  bool dense = false;
  bool binary = false;

  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<string>();
  } else {
    cerr << "input is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("output")){
    output = argmap["output"].as<string>();
  } else {
    cerr << "output is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("assign")){
    assign = true;
  }

  if(argmap.count("centroid")){
    input_centroid = argmap["centroid"].as<string>();
  } else {
    if(assign == true) {
      cerr << "output is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("k")){
    k = argmap["k"].as<int>();
  } else {
    if(assign == false) {
      cerr << "number of cluster is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("num-iteration")){
    num_iteration = argmap["num-iteration"].as<int>();
  }

  if(argmap.count("num-init")){
    n_init = argmap["num-init"].as<int>();
  }

  if(argmap.count("epsilon")){
    eps = argmap["epsilon"].as<double>();
  }

  if(argmap.count("seed")){
    seed = argmap["seed"].as<long>();
  }

  if(argmap.count("use-shrink")){
    shrink = true;
  }

  if(argmap.count("sparse")){
    dense = false;
  }

  if(argmap.count("dense")){
    dense = true;
  }

  if(argmap.count("binary")){
    binary = true;
  }

  if(argmap.count("verbose")){
    set_loglevel(DEBUG);
  }

  if(argmap.count("verbose2")){
    set_loglevel(TRACE);
  }

  if(assign) do_assign(input, dense, input_centroid, output, binary);
  else do_kmeans(input, dense, output, k, n_init, num_iteration, eps,
                 seed, binary, shrink);
}