/*
 * fft.cpp
 *
 *  Created on: 13.12.2012
 *      Author: stephaniebayer
 */

#include "fft.h"

#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "functions.h"
#include "multi_expo.h"

#include <NTL/ZZ.h>
#include <NTL/mat_ZZ.h>
NTL_CLIENT

fft::fft() {
	// TODO Auto-generated constructor stub

}

fft::~fft() {
	// TODO Auto-generated destructor stub
}

ZZ fft::r_o_u(ZZ gen, long m, ZZ ord){
//	ZZ ord = H.get_ord();
	ZZ rou;
	ZZ pow;
	ZZ temp;
	long t = 2*m;
	if ((ord-1) % t  == 0)
	{
		temp = (ord-1)/t;
		PowerMod(rou,gen,temp , ord);

			if(GCD(rou,ord)==to_ZZ(1))
			{

				if (t&1)
				{
					PowerMod(pow,rou,t, ord);
					if(pow==1){
						//cout<<"rou: "<<ord<<" "<<rou;
						return rou;
					}
				}
				else
				{
					PowerMod(pow,rou,t/2,ord);
					if(pow == (ord-1))
					{
						//cout<<"rou "<<ord<<" "<<rou;
						return rou;
					}
				}
			}
	}
	else
	{
		cout << "There is no" << 2*m <<"-th root of unity"<< endl;
		return to_ZZ(1);
	}
	return to_ZZ(1);
}

long fft::to_long(vector<int>* bit_r){

	long  t, length;
	double two,i;
	two = 2;

	length =bit_r->size();
	t=0;
	for(i = 0; i<length; i++ ){
		t =t +bit_r->at(i)*pow(two,i);
	}
	return t;
}

void fft::bitreverse(long& z, long x, long d){
	long i;
	vector<int>* temp=0;
	temp = new vector<int>(d);
	for(i = 0; i<d; i++){
		if(bit(x,i)==1){
			temp->at(d-i-1)=1;
		}
		else{
			temp->at(d-i-1)=0;
		}
	}
	z = to_long(temp);
	delete temp;
}

void  fft::brevorder(vector<ZZ>* ret, vector<ZZ>* v){

	long i,k,d,l, lv;
	ZZ temp;

	lv = v->size();
	l=ret->size();
	d = NumBits(l)-1;
	for(i = 0; i<lv; i++){
		ret->at(i)=v->at(i);
	}
	for(i = 0; i<l; i++){
		bitreverse(k,i,d);
		if(i<k){
			temp = ret->at(i);
			ret->at(i) = ret->at(k);
			ret->at(k)= temp;
		}
	}
}

void fft::FFT(vector<ZZ>* fft, vector<ZZ>* v , long N, ZZ rootofunity, ZZ ord){

	int n, m, m2, i, i0, i1, k, l;
	ZZ rho, rr, z;
	double two = 2;
	l= v->size();

	//cout<<fft->size()<< "  "<<N;
	brevorder(fft,v);

    n = NumBits(N)-1;	//(* N = 2**n *)
    for(k= 0; k<n; k++){
    	//cout<<k<<" ";
   		m =  pow(two,k);
    	m2 = 2*m;
    	rr = 1;
    	PowerMod(rho,rootofunity,pow(two, n-k-1),ord);
    //	cout<<m2<<" rho "<<rho<<" "<<m<<" ";
        for (i = 0; i<m; i++){
        	i0=i;
			//cout<<" rr "<<rr;
				while( i0 < N){
					i1=i0+m; //cout<<" : "<<i0<<" "<<i1<<" ";
					MulMod(z, fft->at(i1), rr, ord);        //(* rr = rho**i *)
					//cout<<" z "<<z<<" ";
					SubMod(fft->at(i1),fft->at(i0), z, ord);
					//cout<<fft->at(i0)<<" "<<fft->at(i1)<<" ";
					AddMod(fft->at(i0), fft->at(i0), z, ord);
					//cout<<fft->at(i0)<<" "<<fft->at(i1)<<" ";
					i0 = i0 + m2;
				}
				MulMod(rr ,rr,rho, ord) ;

        }
      //  cout<<endl;
    }

}

void fft::FFTinv(vector<ZZ>* ret, vector<ZZ>* points, long N, ZZ rootofunity, ZZ ord){
long  i;
ZZ s, omega;
	InvMod(omega, rootofunity, ord);
    InvMod(s,to_ZZ(N),ord);
    FFT(ret, points,N,omega,ord);
    for (i = 0; i< N; i++){
        MulMod(ret->at(i), s , ret->at(i), ord);
    }
}

void fft::fft_in(vector<ZZ>* ret, vector<ZZ>* v, ZZ rootofunity, ZZ ord, ZZ mod){
	int nb,t,t2,i,i0,i1,k,l;
	ZZ rho, rr, z;
	double two = 2;
	l = v->size();

	brevorder(ret, v);
	nb = NumBits(l)-1;

	for(k = 0 ;  k<nb; k++){
		t =  pow(two,k);
		t2 = 2*t;
		rr = 1;
		PowerMod(rho,rootofunity,pow(two, nb-k-1),ord);
		for(i = 0; i<t; i ++){
			i0 = i;
			if(rr != 1){
				while (i0 < l){
					i1=i0+t;
					PowerMod(z,ret->at(i1), rr, mod);
					MulMod(ret->at(i1),ret->at(i0), InvMod(z,mod),mod);
					MulMod(ret->at(i0),ret->at(i0), z, mod);
					i0 = i0 + t2;

				}
			}
			else{
				while (i0 < l){
					i1=i0+t;
					z = ret->at(i1);
					MulMod(ret->at(i1),ret->at(i0), InvMod(z,mod),mod);
					MulMod(ret->at(i0),ret->at(i0), z, mod);
					i0 = i0 + t2;

				}
			}
			 MulMod(rr, rr,rho,ord);
		}
	}
}

void fft::fft_sum_in(vector<ZZ>* ret, vector<ZZ>* v, ZZ rootofunity, ZZ ord){
	int nb,t,t2,i,i0,i1,k,l;
	ZZ rho, rr, z;
	double two = 2;
	l = v->size();

	brevorder(ret, v);
	nb = NumBits(l)-1;
	for(k = 0 ;  k<nb; k++){
		t =  pow(two,k);
		t2 = 2*t;
		rr = 1;
		PowerMod(rho,rootofunity,pow(two, nb-k-1),ord);
		for(i = 0; i<t; i ++){
			i0 = i;
			while (i0 < l){
				i1=i0+t;
				MulMod(z,ret->at(i1), rr, ord);
				SubMod(ret->at(i1),ret->at(i0), z, ord);
				AddMod(ret->at(i0),ret->at(i0), z, ord);
				i0 = i0 + t2;

			}
			 MulMod(rr,rr,rho,ord);
		}
	}
}

void fft::fft_mult_cipher(vector<vector<vector<ZZ>* >*>* ret, vector<vector<Cipher_elg>* >* v, ZZ rootofunity, ZZ ord, ZZ mod){

	vector<vector<ZZ>* >* fft = 0;
	vector<ZZ>* temp_u = 0;
	vector<ZZ>* temp_v = 0;
	long i,j, m, n,l;

	n= v->at(0)->size();
	m = v->size();
	l=2*m;

	for(i = 0; i<n; i++){
		fft =new vector<vector<ZZ>* >(2);
		temp_u = new vector<ZZ>(l);
		temp_v = new vector<ZZ>(l);
		temp_u ->at(0) = to_ZZ(1);
		temp_v ->at(0) = to_ZZ(1);
		for(j = 1; j <=m; j++){
			temp_u->at(j)=v->at(j-1)->at(i).get_u();
			temp_v->at(j)=v->at(j-1)->at(i).get_v();
		}
		for(j = m+1; j<l; j++){
			temp_u->at(j) = to_ZZ(1);
			temp_v->at(j) = to_ZZ(1);
		}
		fft->at(0)=new vector<ZZ>(l);
		fft->at(1)=new vector<ZZ>(l);

		fft_in(fft->at(0), temp_u, rootofunity, ord, mod);
		fft_in(fft->at(1), temp_v,rootofunity, ord, mod);
		ret->at(i)=fft;
		delete temp_u;
		delete temp_v;
	}
}

void fft::fft_matrix(vector<vector<ZZ>* >*  ret, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord){

	vector<ZZ>*  fft = 0;
	vector<ZZ>* temp = 0;
	long i,j, m, n,l;
	double two = 2;
	int e;
//	cout<<"in sumT "<< T->size();
	n= T->at(0)->size();
	m = T->size();
	//In the case of the Ciphertexts m is a power of 2, in the normal setting m is odd in our case
	if(m%2==0){
		e = NumBits(2*m+1)-	1;
	}
	else{
		e = NumBits(2*m+1);
	}
	l = pow(two, e);
	for(i = 0; i<n; i++){
		temp = new vector<ZZ>(m);
		for(j = 0; j <m; j++){
			temp->at(j)=T->at(j)->at(i);
		}
		fft=new vector<ZZ>(l);
		FFT(fft, temp, l, rootofunity, ord );
		//fft_sum_in(fft, temp, rootofunity, ord);
		ret->at(i)=fft;
		delete temp;
	}
//	cout<<endl;
}

void fft::fft_matrix_inv(vector<vector<ZZ>* >*  ret, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord){

	vector<ZZ>*  fft = 0;
	vector<ZZ>* temp = 0;
	long i,j, m, n,l;
	double two = 2;
	int e;
	//cout<<"in sumT "<< ret->size();
	//In the case of the Ciphertexts m is a power of 2, in the normal setting m is odd in our case
	n= T->at(0)->size();
	m = T->size();
	if(m%2==0){
		e = NumBits(2*m+1)-	1;
	}
	else{
		e = NumBits(2*m+1);
	}
	l = pow(two, e);
//	cout<<" l ist "<<l<<endl;
	for(i = 0; i<n; i++){
		temp = new vector<ZZ>(m);
		for(j = 0; j <m; j++){
			temp->at(j)=T->at(m-j-1)->at(i);
		}
		fft=new vector<ZZ>(l);
		FFT(fft, temp, l, rootofunity, ord );
		//fft_sum_in(fft, temp, rootofunity, ord);
		ret->at(i)=fft;
		delete temp;
	}
	//cout<<endl;
}

void fft::sum_t(vector<vector<ZZ>*>* ret, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord){
	vector<ZZ>* tem =  0;
	ZZ temp, t;
	long m,n,k,i,j,te,l;
	m = T->size();
	n = T->at(0)->size();
	l=2*m;
	for(k= 0; k<l; k++){
		tem = new vector<ZZ>(n);
		for(j = 0; j<n;j++){
			temp = 0;
			for (i = m-1; i>= 0; i--){
				PowerMod(t,rootofunity, i*(k+1),ord);
				te =fabs(i-m+1);
				MulMod(t,t,T->at(te)->at(j),ord);
				AddMod(temp, temp, t,ord);
			}
			tem->at(j) = temp;
		}
		ret->at(k) = tem;

	}
}

 void fft::calc_Pk(vector<vector<ZZ>*>* ret, vector<vector<Cipher_elg>* >* v, vector<vector<ZZ>* >* T, ZZ rootofunity, ZZ ord, ZZ mod, int omega_sw){
	long j,m,n,l;
	vector<vector<vector<ZZ>* >*>* fft_t=0;
	vector<vector<ZZ>* >* sumT=0;
	ZZ temp_u, te_u, temp_v, te_v,temp ;
	m = T->size();
	n= T->at(0)->size();
	vector<ZZ>* ret_u = new vector<ZZ>(2*m);
	vector<ZZ>* ret_v = new vector<ZZ>(2*m);

	fft_t = new vector<vector<vector<ZZ>* >*>(n);
	fft::fft_mult_cipher(fft_t, v,rootofunity, ord, mod);

	sumT = new vector<vector<ZZ>* >(n);
	fft::fft_matrix_inv(sumT, T, rootofunity, ord);

	l=2*m-1;
	for (j = 0; j<l; j++){

		multi_expo::multi_expo_LL(temp_u,fft_t, sumT, omega_sw ,j+1,0);
		multi_expo::multi_expo_LL(temp_v,fft_t, sumT, omega_sw ,j+1,1);
		ret_u->at(j) = temp_u;
		ret_v->at(j) = temp_v;
	}

	multi_expo::multi_expo_LL(temp_u ,fft_t, sumT, omega_sw ,0,0);
	multi_expo::multi_expo_LL(temp_v ,fft_t, sumT, omega_sw ,0,1);
	ret_u->at(l) = temp_u;
	ret_v->at(l) = temp_v;

	ret->at(0) = ret_u;
	ret->at(1) = ret_v;

	Functions::delete_vector(fft_t);
	Functions::delete_vector(sumT);

}

void fft::calc_m( vector<vector<ZZ>*>* M , long m, ZZ rootofunity, ZZ ord){
	vector<ZZ>* Rootofunity;
	vector<ZZ>* div;
	ZZ prod,temp,temp_1;
	long i,j,l;
	l=2*m;
	vector<ZZ>* Rootofun = new vector<ZZ>(l);
	div =new vector<ZZ>(l);

	Rootofun->at(0) = rootofunity;
	for(i = 1; i<l; i++){
		MulMod(Rootofun->at(i), Rootofun->at(i-1), rootofunity,ord);
	}
	for(i = 0 ; i<l; i++){
		prod = 1;
		for(j = 0; j<l; j++){
			if (i!=j){
				MulMod(prod,prod, SubMod(Rootofun->at(i),Rootofun->at(j),ord),ord);
			}
		}
		div->at(i)= prod;
	}
	for(i = 0; i<l; i++){
		Rootofunity = new vector<ZZ>(l);
		Rootofunity->at(2*m-1) = to_ZZ(1);
		PowerMod(temp,rootofunity, i+1,ord);

		for(j = l-2; j>=0; j--){
			 MulMod(Rootofunity->at(j),Rootofunity->at(j+1),temp,ord);
		}
		for(j = l-1; j>=0; j--){
			 MulMod(Rootofunity->at(j),Rootofunity->at(j), InvMod(div->at(i),ord),ord);
		}
		M->at(i) = Rootofunity;
	}
	delete Rootofun;
	delete div;
}


// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

#ifdef G2O_USE_VENDORED_CERES
#include "g2o/EXTERNAL/ceres/autodiff.h"
#else
#include <ceres/internal/autodiff.h>
#endif

#include "g2o/core/auto_differentiation.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/solver.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/stuff/command_args.h"

#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#else
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#endif

using namespace std;

namespace g2o {
namespace bal {
using Vector9 = VectorN<9>;
}
}  // namespace g2o

/**
 * \brief camera vertex which stores the parameters for a pinhole camera
 *
 * The parameters of the camera are
 * - rx,ry,rz representing the rotation axis, whereas the angle is given by
 * ||(rx,ry,rz)||
 * - tx,ty,tz the translation of the camera
 * - f the focal length of the camera
 * - k1, k2 two radial distortion parameters
 */
class VertexCameraBAL : public g2o::BaseVertex<9, g2o::bal::Vector9> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexCameraBAL() {}

  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual void setToOriginImpl() {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
  }

  virtual void oplusImpl(const double* update) {
    g2o::bal::Vector9::ConstMapType v(update, VertexCameraBAL::Dimension);
    _estimate += v;
  }
};

/**
 * \brief 3D world feature
 *
 * A 3D point feature in the world
 */
class VertexPointBAL : public g2o::BaseVertex<3, g2o::Vector3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexPointBAL() {}

  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  virtual void setToOriginImpl() {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
  }

  virtual void oplusImpl(const double* update) {
    g2o::Vector3::ConstMapType v(update);
    _estimate += v;
  }
};

/**
 * \brief edge representing the observation of a world feature by a camera
 *
 * see: http://grail.cs.washington.edu/projects/bal/
 * We use a pinhole camera model; the parameters we estimate for each camera
 * area rotation R, a translation t, a focal length f and two radial distortion
 * parameters k1 and k2. The formula for projecting a 3D point X into a camera
 * R,t,f,k1,k2 is:
 * P  =  R * X + t     (conversion from world to camera coordinates)
 * p  = -P / P.z       (perspective division)
 * p' =  f * r(p) * p  (conversion to pixel coordinates) where P.z is the third
 * (z) coordinate of P.
 *
 * In the last equation, r(p) is a function that computes a scaling factor to
 * undo the radial distortion: r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.
 *
 * This gives a projection in pixels, where the origin of the image is the
 * center of the image, the positive x-axis points right, and the positive
 * y-axis points up (in addition, in the camera coordinate system, the positive
 * z-axis points backwards, so the camera is looking down the negative z-axis,
 * as in OpenGL).
 */
class EdgeObservationBAL
    : public g2o::BaseBinaryEdge<2, g2o::Vector2, VertexCameraBAL,
                                 VertexPointBAL> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  EdgeObservationBAL() {}
  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }
  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  /**
   * templatized function to compute the error as described in the comment above
   */
  template <typename T>
  bool operator()(const T* p_camera, const T* p_point, T* p_error) const {
    typename g2o::VectorN<9, T>::ConstMapType camera(p_camera);
    typename g2o::VectorN<3, T>::ConstMapType point(p_point);

    typename g2o::VectorN<3, T> p;

    // Rodrigues' formula for the rotation
    T theta = camera.template head<3>().norm();
    if (theta > T(0)) {
      g2o::VectorN<3, T> v = camera.template head<3>() / theta;
      T cth = cos(theta);
      T sth = sin(theta);

      g2o::VectorN<3, T> vXp = v.cross(point);
      T vDotp = v.dot(point);
      T oneMinusCth = T(1) - cth;

      p = point * cth + vXp * sth + v * vDotp * oneMinusCth;
    } else {
      // taylor expansion for theta close to zero
      p = point + camera.template head<3>().cross(point);
    }

    // translation of the camera
    p += camera.template segment<3>(3);

    // perspective division
    g2o::VectorN<2, T> projectedPoint = -p.template head<2>() / p(2);

    // conversion to pixel coordinates
    T radiusSqr = projectedPoint.squaredNorm();
    const T& f = camera(6);
    const T& k1 = camera(7);
    const T& k2 = camera(8);
    T r_p = T(1) + k1 * radiusSqr + k2 * radiusSqr * radiusSqr;
    g2o::VectorN<2, T> prediction = f * r_p * projectedPoint;

    // compute the error
    typename g2o::VectorN<2, T>::MapType error(p_error);
    error = prediction - measurement().cast<T>();
    (void)error;
    return true;
  }

  G2O_MAKE_AUTO_AD_FUNCTIONS
};

int main(int argc, char** argv) {
  int maxIterations;
  bool verbose;
  bool usePCG;
  string outputFilename;
  string inputFilename;
  string statsFilename;
  g2o::CommandArgs arg;
  arg.param("i", maxIterations, 5, "perform n iterations");
  arg.param("o", outputFilename, "", "write points into a vrml file");
  arg.param("pcg", usePCG, false, "use PCG instead of the Cholesky");
  arg.param("v", verbose, false, "verbose output of the optimization process");
  arg.param("stats", statsFilename, "", "specify a file for the statistics");
  arg.paramLeftOver("graph-input", inputFilename, "",
                    "file which will be processed");

  arg.parseArgs(argc, argv);

  typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BalBlockSolver;
#ifdef G2O_HAVE_CHOLMOD
  string choleskySolverName = "CHOLMOD";
  typedef g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>
      BalLinearSolver;
#else
  string choleskySolverName = "Eigen";
  typedef g2o::LinearSolverEigen<BalBlockSolver::PoseMatrixType>
      BalLinearSolver;
#endif
  typedef g2o::LinearSolverPCG<BalBlockSolver::PoseMatrixType>
      BalLinearSolverPCG;

  g2o::SparseOptimizer optimizer;
  std::unique_ptr<g2o::LinearSolver<BalBlockSolver::PoseMatrixType>>
      linearSolver;
  if (usePCG) {
    cout << "Using PCG" << endl;
    linearSolver = g2o::make_unique<BalLinearSolverPCG>();
  } else {
    cout << "Using Cholesky: " << choleskySolverName << endl;
    auto cholesky = g2o::make_unique<BalLinearSolver>();
    cholesky->setBlockOrdering(true);
    linearSolver = std::move(cholesky);
  }
  g2o::OptimizationAlgorithmLevenberg* solver =
      new g2o::OptimizationAlgorithmLevenberg(
          g2o::make_unique<BalBlockSolver>(std::move(linearSolver)));

  // solver->setUserLambdaInit(1);
  optimizer.setAlgorithm(solver);
  if (statsFilename.size() > 0) {
    optimizer.setComputeBatchStatistics(true);
  }

  vector<VertexPointBAL*> points;
  vector<VertexCameraBAL*> cameras;

  // parse BAL dataset
  cout << "Loading BAL dataset " << inputFilename << endl;
  {
    ifstream ifs(inputFilename.c_str());
    int numCameras, numPoints, numObservations;
    ifs >> numCameras >> numPoints >> numObservations;

    cerr << PVAR(numCameras) << " " << PVAR(numPoints) << " "
         << PVAR(numObservations) << endl;

    int id = 0;
    cameras.reserve(numCameras);
    for (int i = 0; i < numCameras; ++i, ++id) {
      VertexCameraBAL* cam = new VertexCameraBAL;
      cam->setId(id);
      optimizer.addVertex(cam);
      cameras.push_back(cam);
    }

    points.reserve(numPoints);
    for (int i = 0; i < numPoints; ++i, ++id) {
      VertexPointBAL* p = new VertexPointBAL;
      p->setId(id);
      p->setMarginalized(true);
      bool addedVertex = optimizer.addVertex(p);
      if (!addedVertex) {
        cerr << "failing adding vertex" << endl;
      }
      points.push_back(p);
    }

    // read in the observation
    for (int i = 0; i < numObservations; ++i) {
      int camIndex, pointIndex;
      double obsX, obsY;
      ifs >> camIndex >> pointIndex >> obsX >> obsY;

      assert(camIndex >= 0 && (size_t)camIndex < cameras.size() &&
             "Index out of bounds");
      VertexCameraBAL* cam = cameras[camIndex];
      assert(pointIndex >= 0 && (size_t)pointIndex < points.size() &&
             "Index out of bounds");
      VertexPointBAL* point = points[pointIndex];

      EdgeObservationBAL* e = new EdgeObservationBAL;
      e->setVertex(0, cam);
      e->setVertex(1, point);
      e->setInformation(g2o::Matrix2::Identity());
      e->setMeasurement(g2o::Vector2(obsX, obsY));
      bool addedEdge = optimizer.addEdge(e);
      if (!addedEdge) {
        cerr << "error adding edge" << endl;
      }
    }

    // read in the camera params
    for (int i = 0; i < numCameras; ++i) {
      g2o::bal::Vector9 cameraParameter;
      for (int j = 0; j < 9; ++j) ifs >> cameraParameter(j);
      VertexCameraBAL* cam = cameras[i];
      cam->setEstimate(cameraParameter);
    }

    // read in the points
    for (int i = 0; i < numPoints; ++i) {
      g2o::Vector3 p;
      ifs >> p(0) >> p(1) >> p(2);
      VertexPointBAL* point = points[i];
      point->setEstimate(p);
    }
  }
  cout << "done." << endl;

  cout << "Initializing ... " << flush;
  optimizer.initializeOptimization();
  cout << "done." << endl;
  optimizer.setVerbose(verbose);
  cout << "Start to optimize" << endl;
  optimizer.optimize(maxIterations);

  if (statsFilename != "") {
    cerr << "writing stats to file \"" << statsFilename << "\" ... ";
    ofstream fout(statsFilename.c_str());
    const g2o::BatchStatisticsContainer& bsc = optimizer.batchStatistics();
    for (size_t i = 0; i < bsc.size(); i++) fout << bsc[i] << endl;
    cerr << "done." << endl;
  }

  // dump the points
  if (outputFilename.size() > 0) {
    ofstream fout(outputFilename.c_str());  // loadable with meshlab
    fout << "#VRML V2.0 utf8\n"
         << "Shape {\n"
         << "  appearance Appearance {\n"
         << "    material Material {\n"
         << "      diffuseColor " << 1 << " " << 0 << " " << 0 << "\n"
         << "      ambientIntensity 0.2\n"
         << "      emissiveColor 0.0 0.0 0.0\n"
         << "      specularColor 0.0 0.0 0.0\n"
         << "      shininess 0.2\n"
         << "      transparency 0.0\n"
         << "    }\n"
         << "  }\n"
         << "  geometry PointSet {\n"
         << "    coord Coordinate {\n"
         << "      point [\n";
    for (vector<VertexPointBAL*>::const_iterator it = points.begin();
         it != points.end(); ++it) {
      fout << (*it)->estimate().transpose() << endl;
    }
    fout << "    ]\n"
         << "  }\n"
         << "}\n"
         << "  }\n";
  }

  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// 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 NONINFRINGEMENT. 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.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_RANDOM_ALGEBRAIC_RANDOM_DEVICE_HPP
#define CRYPTO3_RANDOM_ALGEBRAIC_RANDOM_DEVICE_HPP

#include <type_traits>

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

namespace nil {
    namespace crypto3 {
        namespace random {
            /*!
             * @brief
             * @tparam AlgebraicType denote an some algebraic type (field, curve group types).
             *
             * algebraic_random_device is adapter wrapping boost::random_device producing random values of algebraic
             * type.
             *
             * The class template algebraic_random_device models a \UniformRandomBitGenerator.
             * https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator
             */
            template<typename AlgebraicType, typename = void>
            struct algebraic_random_device;

            template<typename AlgebraicType>
            struct algebraic_random_device<
                AlgebraicType, typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                                       !algebra::is_extended_field<AlgebraicType>::value>::type> {
            protected:
                typedef AlgebraicType field_type;
                typedef typename field_type::value_type field_value_type;
                typedef typename field_type::integral_type integral_type;

                typedef boost::random_device internal_generator_type;
                typedef boost::random::uniform_int_distribution<integral_type> internal_distribution_type;

                constexpr static integral_type _min = 0;
                constexpr static integral_type _max = field_type::modulus - 1;

            public:
                typedef field_value_type result_type;

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    return dist(gen);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type min() {
                    constexpr result_type min_value(_min);
                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type max() {
                    constexpr result_type max_value(_max);
                    return max_value;
                }

            protected:
                internal_generator_type gen;
                internal_distribution_type dist = internal_distribution_type(_min, _max);
            };

            template<typename AlgebraicType>
            struct algebraic_random_device<
                AlgebraicType, typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                                       algebra::is_extended_field<AlgebraicType>::value>::type> {
            protected:
                typedef AlgebraicType extended_field_type;
                typedef typename extended_field_type::value_type extended_field_value_type;
                typedef typename extended_field_type::underlying_field_type underlying_field_type;

                typedef algebraic_random_device<underlying_field_type> internal_generator_type;

            public:
                typedef extended_field_value_type result_type;

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    result_type result;
                    for (auto &coord : result.data) {
                        coord = gen();
                    }

                    return result;
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate min_value at compile-time
                constexpr static inline result_type min() {
                    result_type min_value;
                    for (auto &coord : min_value.data) {
                        coord = internal_generator_type::min();
                    }

                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate max_value at compile-time
                constexpr static inline result_type max() {
                    result_type max_value;
                    for (auto &coord : max_value.data) {
                        coord = internal_generator_type::max();
                    }

                    return max_value;
                }

            protected:
                internal_generator_type gen;
            };

            template<typename AlgebraicType>
            struct algebraic_random_device<
                AlgebraicType, typename std::enable_if<algebra::is_curve_group<AlgebraicType>::value>::type> {
            protected:
                typedef AlgebraicType group_type;
                typedef typename group_type::value_type group_value_type;
                typedef typename group_type::curve_type::scalar_field_type scalar_field_type;

                typedef algebraic_random_device<scalar_field_type> internal_generator_type;

            public:
                typedef group_value_type result_type;

                /**
                 * Returns a random value in the range [min, max]. Elements of group are ordered in exponent growing
                 * order with respect to group base element.
                 */
                // TODO: check correctness of the generation method
                result_type operator()() {
                    return result_type::one() * gen();
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate returned value at compile-time
                constexpr static inline result_type min() {
                    return result_type::zero();
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate returned value at compile-time
                constexpr static inline result_type max() {
                    return result_type::one() * (scalar_field_type::modulus - 1);
                }

            protected:
                internal_generator_type gen;
            };
        }    // namespace random
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_RANDOM_ALGEBRAIC_RANDOM_DEVICE_HPP


/*
 * GridMapMath.hpp
 *
 *  Created on: Dec 2, 2013
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include <Eigen/Core>
#include <vector>
#include <map>

namespace grid_map_lib {

/*!
 * Gets the position of a cell specified by its index in the map frame.
 * @param[out] position the position of the center of the cell in the map frame.
 * @param[in] index of the cell.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the size of the buffer (optional).
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if index not within range of buffer.
 */
bool getPositionFromIndex(Eigen::Vector2d& position,
                          const Eigen::Array2i& index,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Gets the index of the cell which contains a position in the map frame.
 * @param[out] index of the cell.
 * @param[in] position the position in the map frame.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the size of the buffer (optional).
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if position outside of map.
 */
bool getIndexFromPosition(Eigen::Array2i& index,
                          const Eigen::Vector2d& position,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Checks if position is within the map boundaries.
 * @param[in] position the position which is to be checked.
 * @param[in] mapLength the length of the map.
 * @param[in] mapPosition the position of the map.
 * @return true if position is within map, false otherwise.
 */
bool checkIfPositionWithinMap(const Eigen::Vector2d& position,
                              const Eigen::Array2d& mapLength,
                              const Eigen::Vector2d& mapPosition);

/*!
 * Gets the position of the data structure origin.
 * @param[in] position the position of the map.
 * @param[in] mapLength the map length.
 * @param[out] positionOfOrigin the position of the data structure origin.
 */
void getPositionOfDataStructureOrigin(const Eigen::Vector2d& position,
                                      const Eigen::Array2d& mapLength,
                                      Eigen::Vector2d& positionOfOrigin);

/*!
 * Computes how many cells/indeces the map is moved based on a position shift in
 * the grid map frame. Use this function if you are moving the grid map
 * and want to ensure that the cells match before and after.
 * @param[out] indexShift the corresponding shift of the indices.
 * @param[in] positionShift the desired position shift.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
bool getIndexShiftFromPositionShift(Eigen::Array2i& indexShift,
                                    const Eigen::Vector2d& positionShift,
                                    const double& resolution);

/*!
 * Computes the corresponding position shift from a index shift. Use this function
 * if you are moving the grid map and want to ensure that the cells match
 * before and after.
 * @param[out] positionShift the corresponding shift in position in the grid map frame.
 * @param[in] indexShift the desired shift of the indeces.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
bool getPositionShiftFromIndexShift(Eigen::Vector2d& positionShift,
                                    const Eigen::Array2i& indexShift,
                                    const double& resolution);

/*!
 * Checks if index is within range of the buffer.
 * @param[in] index to check.
 * @param[in] bufferSize the size of the buffer.
 * @return true if index is within, and false if index is outside of the buffer.
 */
bool checkIfIndexWithinRange(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize);

/*!
 * Maps an index that runs out of the range of the circular buffer back into allowed the region.
 * This is the 2d version of mapIndexWithinRange(int&, const int&).
 * @param[in/out] index the indeces that will be mapped into the valid region of the buffer.
 * @param[in] bufferSize the size of the buffer.
 */
void mapIndexWithinRange(Eigen::Array2i& index,
                         const Eigen::Array2i& bufferSize);

/*!
 * Maps an index that runs out of the range of the circular buffer back into allowed the region.
 * @param[in/out] index the index that will be mapped into the valid region of the buffer.
 * @param[in] bufferSize the size of the buffer.
 */
void mapIndexWithinRange(int& index, const int& bufferSize);

/*!
 * Limits (cuts off) the position to lie inside the map.
 * @param[in/out] position the position to be limited.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 */
void limitPositionToRange(Eigen::Vector2d& position,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition);

/*!
 * Provides the alignment transformation from the buffer order (outer/inner storage)
 * and the map frame (x/y-coordinate).
 * @return the alignment transformation.
 */
const Eigen::Matrix2i getBufferOrderToMapFrameAlignment();

/*!
 * Given a map and a desired submap (defined by position and size), this function computes
 * various information about the submap. The returned submap might be smaller than the requested
 * size as it respects the boundaries of the map.
 * @param[out] submapTopLeftIndex the top left index of the returned submap.
 * @param[out] submapBufferSize the buffer size of the returned submap.
 * @param[out] submapPosition the position of the submap (center) in the map frame.
 * @param[out] submapLength the length of the submap.
 * @param[out] requestedIndexInSubmap the index in the submap that corresponds to the requested
 *             position of the submap.
 * @param[in] requestedSubmapPosition the requested submap position (center) in the map frame.
 * @param[in] requestedSubmapLength the requested submap length.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the buffer size of the map.
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful.
 */
bool getSubmapInformation(Eigen::Array2i& submapTopLeftIndex,
                          Eigen::Array2i& submapBufferSize,
                          Eigen::Vector2d& submapPosition,
                          Eigen::Array2d& submapLength,
                          Eigen::Array2i& requestedIndexInSubmap,
                          const Eigen::Vector2d& requestedSubmapPosition,
                          const Eigen::Vector2d& requestedSubmapLength,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Computes the regions in the circular buffer that make up the data for
 * a requested submap.
 * @param[out] submapIndeces the list of indeces (top-left) for the buffer regions.
 * @param[out] submapSizes the sizes of the buffer regions.
 * @param[in] submapIndex the index (top-left) for the requested submap.
 * @param[in] submapBufferSize the size of the requested submap.
 * @param[in] bufferSize the buffer size of the map.
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if requested submap is not fully contained in the map.
 */
bool getBufferRegionsForSubmap(std::vector<Eigen::Array2i>& submapIndeces,
                               std::vector<Eigen::Array2i>& submapSizes,
                               const Eigen::Array2i& submapIndex,
                               const Eigen::Array2i& submapBufferSize,
                               const Eigen::Array2i& bufferSize,
                               const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Increases the index by one to iterate through the map.
 * Increments either to the neighboring index to the right or to
 * the start of the lower row. Returns false if end of iterations are reached.
 * @param[in/out] index the index in the map that is incremented (corrected for the circular buffer).
 * @param[in] bufferSize the map buffer size.
 * @param[in] bufferStartIndex the map buffer start index.
 * @return true if successfully incremented indeces, false if end of iteration limits are reached.
 */
bool incrementIndex(Eigen::Array2i& index, const Eigen::Array2i& bufferSize,
                    const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Increases the index by one to iterate through the cells of a submap.
 * Increments either to the neighboring index to the right or to
 * the start of the lower row. Returns false if end of iterations are reached.
 *
 * Note: This function does not check if submap actually fits to the map. This needs
 * to be checked before separately.
 *
 * @param[in/out] submapIndex the index in the submap that is incremented.
 * @param[out] index the index in the map that is incremented (corrected for the circular buffer).
 * @param[in] submapTopLefIndex the top left index of the submap.
 * @param[in] submapBufferSize the submap buffer size.
 * @param[in] bufferSize the map buffer size.
 * @param[in] bufferStartIndex the map buffer start index.
 * @return true if successfully incremented indeces, false if end of iteration limits are reached.
 */
bool incrementIndexForSubmap(Eigen::Array2i& submapIndex, Eigen::Array2i& index, const Eigen::Array2i& submapTopLeftIndex,
                             const Eigen::Array2i& submapBufferSize, const Eigen::Array2i& bufferSize,
                             const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Returns the 1d index corresponding to the 2d index for either row- or column-major format.
 * Note: Eigen is defaulting to column-major format.
 * @param[in] index the 2d index.
 * @param[in] bufferSize the map buffer size.
 * @param[in] (optional) rowMajor if the 1d index is generated for row-major format.
 * @return the 1d index.
 */
unsigned int get1dIndexFrom2dIndex(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize, const bool rowMajor);

/*!
 * The definition of the buffer regions.
 */
enum class BufferRegion
{
  TopLeft,
  TopRight,
  BottomLeft,
  BottomRight
};

/*!
 * The definition of the position in the list for the buffer regions.
 */
static std::map<BufferRegion, int> bufferRegionIndeces =
{
{ BufferRegion::TopLeft, 0 },
{ BufferRegion::TopRight, 1 },
{ BufferRegion::BottomLeft, 2 },
{ BufferRegion::BottomRight, 3 } };

} // namespace


/*
Authors: Deevashwer Rathee
Copyright:
Copyright (c) 2021 Microsoft Research
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 NONINFRINGEMENT. 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.
*/

#include "LinearOT/linear-ot.h"
#include <cmath>

#define MAX_NUM_OT (1 << 20)

#define USE_EIGEN
#ifdef USE_EIGEN
#include <Eigen/Dense>
#endif

using namespace std;
using namespace sci;

void matrix_transpose(uint64_t *A, int32_t m, int32_t n, int d = 1) {
  uint64_t *tmpA = new uint64_t[m * n * d];
  memcpy(tmpA, A, m * n * d * sizeof(uint64_t));
  for (int i = 0; i < m; i++) {
    for (int j = 0; j < n; j++) {
      if (d == 1) {
        A[j * m + i] = tmpA[i * n + j];
      } else {
        memcpy(A + (j * m + i) * d, tmpA + (i * n + j) * d,
               d * sizeof(uint64_t));
      }
    }
  }
  delete[] tmpA;
}

LinearOT::LinearOT(int party, NetIO *io, OTPack<NetIO> *otpack) {
  this->party = party;
  this->io = io;
  this->otpack = otpack;
  this->aux = new AuxProtocols(party, io, otpack);
  this->trunc = new Truncation(party, io, otpack, aux);
  this->xt = new XTProtocol(party, io, otpack, aux);
}

LinearOT::~LinearOT() {
  delete aux;
  delete trunc;
}

void LinearOT::hadamard_cleartext(int dim, uint64_t *inA, uint64_t *inB,
                                  uint64_t *outC) {
  matmul_cleartext(1, dim, 1, inA, inB, outC, false);
}

#ifdef USE_EIGEN
void matmul_cleartext_eigen(int dim1, int dim2, int dim3, uint64_t *inA,
                            uint64_t *inB, uint64_t *outC) {
  Eigen::Matrix<uint64_t, Eigen::Dynamic, Eigen::Dynamic> eigen_A(dim1, dim2);
  Eigen::Matrix<uint64_t, Eigen::Dynamic, Eigen::Dynamic> eigen_B(dim2, dim3);
  Eigen::Matrix<uint64_t, Eigen::Dynamic, Eigen::Dynamic> eigen_C(dim1, dim3);

  for (int i = 0; i < dim1; i++) {
    for (int j = 0; j < dim2; j++) {
      eigen_A(i, j) = Arr2DIdxRowM(inA, dim1, dim2, i, j);
    }
  }
  for (int i = 0; i < dim2; i++) {
    for (int j = 0; j < dim3; j++) {
      eigen_B(i, j) = Arr2DIdxRowM(inB, dim2, dim3, i, j);
    }
  }
  eigen_C = eigen_A * eigen_B;
  for (int i = 0; i < dim1; i++) {
    for (int j = 0; j < dim3; j++) {
      Arr2DIdxRowM(outC, dim1, dim3, i, j) = eigen_C(i, j);
    }
  }
}
#endif

void LinearOT::matmul_cleartext(int dim1, int dim2, int dim3, uint64_t *inA,
                                uint64_t *inB, uint64_t *outC,
                                bool accumulate) {
  if (!accumulate) {
    for (int i = 0; i < dim1; i++) {
      for (int j = 0; j < dim3; j++) {
        for (int k = 0; k < dim2; k++) {
          outC[dim2 * dim3 * i + dim2 * j + k] =
              inA[dim2 * i + k] * inB[dim3 * k + j];
        }
      }
    }
    return;
  }
#ifndef USE_EIGEN
  for (int i = 0; i < dim1; i++) {
    for (int j = 0; j < dim3; j++) {
      outC[i * dim3 + j] = 0;
      for (int k = 0; k < dim2; k++) {
        outC[i * dim3 + j] += (inA[dim2 * i + k] * inB[dim3 * k + j]);
      }
    }
  }
#else
  assert(accumulate == true && "Eigen not configured for accumulate = false");
  matmul_cleartext_eigen(dim1, dim2, dim3, inA, inB, outC);
#endif
}

void LinearOT::hadamard_cross_terms(int32_t dim, uint64_t *inA, uint64_t *inB,
                                    uint64_t *outC, int32_t bwA, int32_t bwB,
                                    int32_t bwC, MultMode mode) {
  matmul_cross_terms(1, dim, 1, inA, inB, outC, bwA, bwB, bwC, false, mode);
}

void LinearOT::hadamard_product(int32_t dim, uint64_t *inA, uint64_t *inB,
                                uint64_t *outC, int32_t bwA, int32_t bwB,
                                int32_t bwC, bool signed_arithmetic,
                                bool signed_B, MultMode mode, uint8_t *msbA,
                                uint8_t *msbB) {
  matrix_multiplication(1, dim, 1, inA, inB, outC, bwA, bwB, bwC,
                        signed_arithmetic, signed_B, false, mode, msbA, msbB);
}

void LinearOT::matmul_cross_terms(int32_t dim1, int32_t dim2, int32_t dim3,
                                  uint64_t *inA, uint64_t *inB, uint64_t *outC,
                                  int32_t bwA, int32_t bwB, int32_t bwC,
                                  bool accumulate, MultMode mode) {
  bool use_straight_ot = false, use_reversed_ot = false;
  uint64_t maskC = (bwC == 64 ? -1 : ((1ULL << bwC) - 1));
  uint64_t *inS, *inR;
  int32_t bwR, dimS1, dimS2, dimR1, dimR2;
  // A whole row of values is multiplied to a element using only bwR OTs
  bool row_batching;
  if (mode == MultMode::Alice_has_A) {
    use_straight_ot = true;
    row_batching = false;
  } else if (mode == MultMode::Bob_has_A) {
    use_straight_ot = true;
    row_batching = true;
  } else if (mode == MultMode::Alice_has_B) {
    use_reversed_ot = true;
    row_batching = false;
  } else if (mode == MultMode::Bob_has_B) {
    use_reversed_ot = true;
    row_batching = true;
  } else {
    use_straight_ot = true;
    use_reversed_ot = true;
    // if (bwA*dim1*dim2 > bwB*dim2*dim3) {
    if (bwA > bwB) {
      row_batching = false;
    } else {
      row_batching = true;
    }
  }
  if (row_batching) {
    inS = inB;
    inR = inA;
    bwR = bwA;
    dimS1 = dim2;
    dimS2 = dim3;
    dimR1 = dim1;
    dimR2 = dim2;
  } else {
    // inS = inA;
    inS = new uint64_t[dim1 * dim2];
    memcpy(inS, inA, dim1 * dim2 * sizeof(uint64_t));
    inR = inB;
    bwR = bwB;
    dimS1 = dim1;
    dimS2 = dim2;
    dimR1 = dim2;
    dimR2 = dim3;
  }

  int32_t num_ot = dimR1 * dimR2 * bwR;
  int32_t msgs_per_ot = (row_batching ? dimS2 : dimS1);
  int32_t dim = dimR1 * dimR2;
  // max_num_ot is multiple of dim
  int32_t max_num_ot = ceil(float(MAX_NUM_OT) / (dim * msgs_per_ot)) * dim;
  int32_t batch_size;
  if (num_ot < max_num_ot)
    batch_size = num_ot;
  else
    batch_size = max_num_ot;

  uint64_t *corr = new uint64_t[batch_size * msgs_per_ot];
  uint64_t *ABs = new uint64_t[batch_size * msgs_per_ot];
  uint64_t *ABr = new uint64_t[batch_size * msgs_per_ot];
  bool *choice = new bool[batch_size];
  uint64_t *tmpR = new uint64_t[dim];
  memcpy(tmpR, inR, dim * sizeof(uint64_t));

  memset(ABs, 0, batch_size * msgs_per_ot * sizeof(uint64_t));
  memset(ABr, 0, batch_size * msgs_per_ot * sizeof(uint64_t));
  if (accumulate) {
    memset(outC, 0, dim1 * dim3 * sizeof(uint64_t));
  } else {
    memset(outC, 0, dim1 * dim2 * dim3 * sizeof(uint64_t));
  }
  if (!row_batching) {
    // inplace transposing is fine because inS is a copy if row_batching = false
    matrix_transpose(inS, dimS1, dimS2);
  }

  for (int i = 0; i < num_ot; i += batch_size) {
    vector<int> msg_len;
    if (batch_size <= num_ot - i)
      msg_len.resize(batch_size / dim);
    else
      msg_len.resize((num_ot - i) / dim);
    for (int j = i; j < i + batch_size and j < num_ot; j += dim) {
      int bit_offset = i / dim;
      int bit_idx = j / dim;
      msg_len[bit_idx - bit_offset] = bwC - bit_idx;
      for (int k = j; k < j + dim and k < i + batch_size; k++) {
        int inp_idx = k - j;
        int row_idx_R = inp_idx / dimR2;
        int col_idx_R = inp_idx % dimR2;
        for (int h = 0; h < msgs_per_ot; h++) {
          uint64_t elemS;
          if (row_batching)
            elemS = inS[col_idx_R * dimS2 + h];
          // To preserve locality, inS is tranposed if row_batching = false
          else
            elemS = inS[row_idx_R * dimS1 + h];
          // else elemS = inS[h*dimS2 + row_idx_R];
          corr[(k - i) * msgs_per_ot + h] =
              ((elemS << bit_idx) & maskC) >> bit_idx;
        }
        choice[k - i] = (bool)(tmpR[inp_idx] & 1);
        tmpR[inp_idx] >>= 1;
      }
    }
    if (use_straight_ot) {
      if (party == sci::ALICE) {
        if (batch_size <= num_ot - i) {
          otpack->iknp_straight->send_batched_cot(ABs, corr, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_straight->send_batched_cot(ABs, corr, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      } else { // party == sci::BOB
        if (batch_size <= num_ot - i) {
          otpack->iknp_straight->recv_batched_cot(ABr, choice, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_straight->recv_batched_cot(ABr, choice, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      }
    }
    if (use_reversed_ot) {
      if (party == sci::ALICE) {
        if (batch_size <= num_ot - i) {
          otpack->iknp_reversed->recv_batched_cot(ABr, choice, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_reversed->recv_batched_cot(ABr, choice, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      } else { // party == sci::BOB
        if (batch_size <= num_ot - i) {
          otpack->iknp_reversed->send_batched_cot(ABs, corr, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_reversed->send_batched_cot(ABs, corr, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      }
    }
    for (int h = 0; h < dim2; h++) {
      for (int j = i; j < i + batch_size and j < num_ot; j += dim) {
        int bit_idx = j / dim;
        for (int k = 0; k < dim1 * dim3; k++) {
          int row_idx = (row_batching ? k / dim3 : k / dim1);
          int col_idx = (row_batching ? k % dim3 : k % dim1);
          int idx;
          if (row_batching)
            idx = (j - i + row_idx * dimR2 + h) * msgs_per_ot + col_idx;
          // To preserve locality, outC-transpose is computed if row_batching =
          // false
          else
            idx = (j - i + h * dimR2 + row_idx) * msgs_per_ot + col_idx;
          if (use_straight_ot) {
            uint64_t temp = (party == ALICE ? -ABs[idx] : ABr[idx]) << bit_idx;
            if (accumulate) {
              outC[k] += temp;
            } else {
              outC[k * dim2 + h] += temp;
            }
          }
          if (use_reversed_ot) {
            uint64_t temp = (party == BOB ? -ABs[idx] : ABr[idx]) << bit_idx;
            if (accumulate) {
              outC[k] += temp;
            } else {
              outC[k * dim2 + h] += temp;
            }
          }
        }
      }
    }
  }
  if (!row_batching) {
    if (accumulate) {
      matrix_transpose(outC, dim3, dim1);
    } else {
      matrix_transpose(outC, dim3, dim1, dim2);
    }
  }

  if (accumulate) {
    for (int k = 0; k < dim1 * dim3; k++) {
      outC[k] &= maskC;
    }
  } else {
    for (int k = 0; k < dim1 * dim2 * dim3; k++) {
      outC[k] &= maskC;
    }
  }

  delete[] corr;
  delete[] ABs;
  delete[] ABr;
  delete[] choice;
  delete[] tmpR;
  if (!row_batching)
    delete[] inS;
}

void LinearOT::matmul_multiplexer(int32_t dim1, int32_t dim2, int32_t dim3,
                                  uint64_t *inA, uint64_t *inB, uint64_t *outC,
                                  int32_t bwA, int32_t bwB, int32_t bwC,
                                  bool accumulate, MultMode mode) {
  assert(bwA == 1 || bwB == 1);
  assert(bwC < (bwA + bwB));

  bool use_straight_ot = false, use_reversed_ot = false;
  uint64_t maskC = (bwC == 64 ? -1 : ((1ULL << bwC) - 1));
  uint64_t *inS, *inR;
  int32_t dimS1, dimS2, dimR1, dimR2;
  // A whole row of values is multiplied to a bit
  bool row_batching;
  if (bwB == 1) {
    inS = new uint64_t[dim1 * dim2];
    memcpy(inS, inA, dim1 * dim2 * sizeof(uint64_t));
    inR = inB;
    dimS1 = dim1;
    dimS2 = dim2;
    dimR1 = dim2;
    dimR2 = dim3;
    if (mode == MultMode::Alice_has_A)
      use_straight_ot = true;
    else if (mode == MultMode::Bob_has_A)
      use_reversed_ot = true;
    else {
      use_straight_ot = true;
      use_reversed_ot = true;
    }
    row_batching = false;
  } else { // bwA == 1
    inS = inB;
    inR = inA;
    dimS1 = dim2;
    dimS2 = dim3;
    dimR1 = dim1;
    dimR2 = dim2;
    if (mode == MultMode::Alice_has_B)
      use_straight_ot = true;
    else if (mode == MultMode::Bob_has_B)
      use_reversed_ot = true;
    else {
      use_straight_ot = true;
      use_reversed_ot = true;
    }
    row_batching = true;
  }

  int32_t dim = dimR1 * dimR2;
  int32_t msgs_per_ot = (row_batching ? dimS2 : dimS1);

  PRG128 prg;
  uint64_t *data = new uint64_t[2 * dim * msgs_per_ot];
  uint64_t *ABs = new uint64_t[dim * msgs_per_ot];
  uint64_t *ABr = new uint64_t[dim * msgs_per_ot];
  uint8_t *choice = new uint8_t[dim];

  if (accumulate) {
    for (int k = 0; k < dim1 * dim3; k++) {
      outC[k] = 0;
    }
  } else {
    for (int k = 0; k < dim1 * dim2 * dim3; k++) {
      outC[k] = 0;
    }
  }
  memset(ABs, 0, dim * msgs_per_ot * sizeof(uint64_t));
  memset(ABr, 0, dim * msgs_per_ot * sizeof(uint64_t));

  if (!row_batching) {
    // inplace transposing is fine because inS is a copy if row_batching = false
    matrix_transpose(inS, dimS1, dimS2);
  }

  if (use_straight_ot && party == ALICE) {
    prg.random_data(ABs, dim * msgs_per_ot * sizeof(uint64_t));
  }
  if (use_reversed_ot && party == BOB) {
    prg.random_data(ABs, dim * msgs_per_ot * sizeof(uint64_t));
  }

  for (int i = 0; i < dim; i++) {
    int row_idx_R = i / dimR2;
    int col_idx_R = i % dimR2;

    choice[i] = (bool)(inR[i] & 1);
    uint64_t choice_bit = choice[i];
    for (int h = 0; h < msgs_per_ot; h++) {
      uint64_t elemS;
      if (row_batching)
        elemS = inS[col_idx_R * dimS2 + h];
      // To preserve locality, inS is tranposed if row_batching = false
      else
        elemS = inS[row_idx_R * dimS1 + h];
      int idx = i * msgs_per_ot + h;
      data[idx * 2] = (ABs[idx] + elemS * choice_bit) & maskC;
      data[idx * 2 + 1] = (ABs[idx] + elemS * (1 - choice_bit)) & maskC;
    }
  }
  if (party == sci::ALICE) {
    if (use_straight_ot) {
      otpack->iknp_straight->send_batched_got(data, dim, bwC, msgs_per_ot);
    }
    if (use_reversed_ot) {
      otpack->iknp_reversed->recv_batched_got(ABr, choice, dim, bwC,
                                              msgs_per_ot);
    }
  } else { // party == sci::BOB
    if (use_straight_ot) {
      otpack->iknp_straight->recv_batched_got(ABr, choice, dim, bwC,
                                              msgs_per_ot);
    }
    if (use_reversed_ot) {
      otpack->iknp_reversed->send_batched_got(data, dim, bwC, msgs_per_ot);
    }
  }
  for (int h = 0; h < dim2; h++) {
    for (int k = 0; k < dim1 * dim3; k++) {
      int row_idx = (row_batching ? k / dim3 : k / dim1);
      int col_idx = (row_batching ? k % dim3 : k % dim1);
      int idx;
      if (row_batching)
        idx = (row_idx * dimR2 + h) * msgs_per_ot + col_idx;
      // To preserve locality, outC-transpose is computed if row_batching =
      // false
      else
        idx = (h * dimR2 + row_idx) * msgs_per_ot + col_idx;
      if (use_straight_ot) {
        uint64_t temp = (party == ALICE ? -ABs[idx] : ABr[idx]);
        if (accumulate) {
          outC[k] += temp;
        } else {
          outC[k * dim2 + h] += temp;
        }
      }
      if (use_reversed_ot) {
        uint64_t temp = (party == BOB ? -ABs[idx] : ABr[idx]);
        if (accumulate) {
          outC[k] += temp;
        } else {
          outC[k * dim2 + h] += temp;
        }
      }
    }
  }
  if (!row_batching) {
    if (accumulate) {
      matrix_transpose(outC, dim3, dim1);
    } else {
      matrix_transpose(outC, dim3, dim1, dim2);
    }
  }

  if (accumulate) {
    for (int k = 0; k < dim1 * dim3; k++) {
      outC[k] &= maskC;
    }
  } else {
    for (int k = 0; k < dim1 * dim2 * dim3; k++) {
      outC[k] &= maskC;
    }
  }

  delete[] data;
  delete[] ABs;
  delete[] ABr;
  delete[] choice;
  if (!row_batching)
    delete[] inS;
}

void LinearOT::matrix_multiplication(int32_t dim1, int32_t dim2, int32_t dim3,
                                     uint64_t *inA, uint64_t *inB,
                                     uint64_t *outC, int32_t bwA, int32_t bwB,
                                     int32_t bwC, bool signed_arithmetic,
                                     bool signed_B, bool accumulate,
                                     MultMode mode, uint8_t *msbA,
                                     uint8_t *msbB) {
  assert(bwC <= 64);
  assert((bwC <= (bwA + bwB)) && (bwC >= bwA) && (bwC >= bwB));
  int32_t extra_bits = (accumulate ? ceil(log2(dim2)) : 0);
  uint64_t *tmpA = new uint64_t[dim1 * dim2];
  uint64_t *tmpB = new uint64_t[dim2 * dim3];

  bool sender_A = false;
  bool sender_B = false;
  if (mode == MultMode::Alice_has_A || mode == MultMode::Alice_has_B) {
    sender_A = true;
  } else if (mode == MultMode::Bob_has_A || mode == MultMode::Bob_has_B) {
    sender_B = true;
  } else {
    if (bwA > bwB) {
      sender_A = true;
    } else {
      sender_B = true;
    }
  }
  if (sender_A) {
    if (mode == MultMode::Alice_has_A || mode == MultMode::Bob_has_A) {
      for (int i = 0; i < dim1 * dim2; i++) {
        tmpA[i] = (signed_arithmetic ? signed_val(inA[i], bwA) : inA[i]);
      }
    } else {
      if (signed_arithmetic) {
        xt->s_extend(dim1 * dim2, inA, tmpA, bwA, bwA + extra_bits, msbA);
      } else {
        xt->z_extend(dim1 * dim2, inA, tmpA, bwA, bwA + extra_bits, msbA);
      }
    }
    memcpy(tmpB, inB, dim2 * dim3 * sizeof(uint64_t));
    bwA = bwA + extra_bits;
  } else if (sender_B) {
    if (mode == MultMode::Alice_has_B || mode == MultMode::Bob_has_B) {
      for (int i = 0; i < dim2 * dim3; i++) {
        tmpB[i] = ((signed_arithmetic && signed_B) ? signed_val(inB[i], bwB)
                                                   : inB[i]);
      }
    } else {
      if (signed_arithmetic && signed_B) {
        xt->s_extend(dim2 * dim3, inB, tmpB, bwB, bwB + extra_bits, msbB);
      } else {
        xt->z_extend(dim2 * dim3, inB, tmpB, bwB, bwB + extra_bits, msbB);
      }
    }
    memcpy(tmpA, inA, dim1 * dim2 * sizeof(uint64_t));
    bwB = bwB + extra_bits;
  }
  bwC = bwC + extra_bits;
  uint64_t maskA = (bwA == 64 ? -1 : ((1ULL << bwA) - 1));
  uint64_t maskB = (bwB == 64 ? -1 : ((1ULL << bwB) - 1));
  uint64_t maskC = (bwC == 64 ? -1 : ((1ULL << bwC) - 1));
  uint64_t pow_A = 1ULL << bwA;
  uint64_t pow_B = 1ULL << bwB;
  uint64_t pow_A_2 = (1ULL << (bwA - 1));
  uint64_t pow_B_2 = (1ULL << (bwB - 1));
  int32_t dim;
  if (accumulate)
    dim = dim1 * dim3;
  else
    dim = dim1 * dim2 * dim3;

  for (int i = 0; i < dim1 * dim2; i++) {
    if (signed_arithmetic) {
      if (mode == MultMode::Alice_has_A) {
        tmpA[i] = (party == ALICE ? tmpA[i] + pow_A_2 : 0) & maskA;
      } else if (mode == MultMode::Bob_has_A) {
        tmpA[i] = (party == BOB ? tmpA[i] + pow_A_2 : 0) & maskA;
      } else {
        tmpA[i] = (tmpA[i] + (party == BOB ? pow_A_2 : 0)) & maskA;
      }
    } else {
      tmpA[i] = tmpA[i] & maskA;
    }
  }
  for (int i = 0; i < dim2 * dim3; i++) {
    if (signed_arithmetic && signed_B) {
      if (mode == MultMode::Alice_has_B) {
        tmpB[i] = (party == ALICE ? tmpB[i] + pow_B_2 : 0) & maskB;
      } else if (mode == MultMode::Bob_has_B) {
        tmpB[i] = (party == BOB ? tmpB[i] + pow_B_2 : 0) & maskB;
      } else {
        tmpB[i] = (tmpB[i] + (party == BOB ? pow_B_2 : 0)) & maskB;
      }
    } else {
      tmpB[i] = tmpB[i] & maskB;
    }
  }
  uint64_t *cross_terms = new uint64_t[dim];
  matmul_cross_terms(dim1, dim2, dim3, tmpA, tmpB, cross_terms, bwA, bwB, bwC,
                     accumulate, mode);

  uint64_t *local_terms = new uint64_t[dim];
  if (party == ALICE &&
      (mode == MultMode::Alice_has_A || mode == MultMode::Alice_has_B)) {
    matmul_cleartext(dim1, dim2, dim3, tmpA, tmpB, local_terms, accumulate);
  } else if (party == BOB &&
             (mode == MultMode::Bob_has_A || mode == MultMode::Bob_has_B)) {
    matmul_cleartext(dim1, dim2, dim3, tmpA, tmpB, local_terms, accumulate);
  } else if (mode == MultMode::None) {
    matmul_cleartext(dim1, dim2, dim3, tmpA, tmpB, local_terms, accumulate);
  } else {
    memset(local_terms, 0, dim * sizeof(uint64_t));
  }

  uint8_t *wA = new uint8_t[dim1 * dim2];
  uint8_t *wB = new uint8_t[dim2 * dim3];
  uint64_t *wA_B = new uint64_t[dim];
  uint64_t *wB_A = new uint64_t[dim];

  if (bwC > bwA) {
    if (mode == MultMode::Alice_has_A || mode == MultMode::Bob_has_A) {
      memset(wA, 0, dim1 * dim2 * sizeof(uint8_t));
      memset(wA_B, 0, dim * sizeof(uint64_t));
    } else {
      if (msbA != nullptr) {
        uint8_t *tmp_msbA = new uint8_t[dim1 * dim2];
        if (signed_arithmetic) {
          for (int i = 0; i < dim1 * dim2; i++) {
            tmp_msbA[i] = (party == ALICE ? msbA[i] ^ 1 : msbA[i]);
          }
        } else {
          for (int i = 0; i < dim1 * dim2; i++) {
            tmp_msbA[i] = (sender_A && (extra_bits > 0) ? 0 : msbA[i]);
          }
        }
        aux->MSB_to_Wrap(tmpA, tmp_msbA, wA, dim1 * dim2, bwA);
        delete[] tmp_msbA;
      } else {
        aux->wrap_computation(tmpA, wA, dim1 * dim2, bwA);
      }
      uint64_t *wA64 = new uint64_t[dim1 * dim2];
      for (int i = 0; i < dim1 * dim2; i++) {
        wA64[i] = uint64_t(wA[i]);
      }
      matmul_multiplexer(dim1, dim2, dim3, wA64, tmpB, wA_B, 1, bwB, bwC - bwA,
                         accumulate, mode);
      delete[] wA64;
    }
  }
  if (bwC > bwB) {
    if (mode == MultMode::Alice_has_B || mode == MultMode::Bob_has_B) {
      memset(wB, 0, dim2 * dim3 * sizeof(uint8_t));
      memset(wB_A, 0, dim * sizeof(uint64_t));
    } else {
      if (msbB != nullptr) {
        uint8_t *tmp_msbB = new uint8_t[dim2 * dim3];
        if (signed_arithmetic && signed_B) {
          for (int i = 0; i < dim2 * dim3; i++) {
            tmp_msbB[i] = (party == ALICE ? msbB[i] ^ 1 : msbB[i]);
          }
        } else {
          for (int i = 0; i < dim2 * dim3; i++) {
            tmp_msbB[i] = (sender_B && (extra_bits > 0) ? 0 : msbB[i]);
          }
        }
        aux->MSB_to_Wrap(tmpB, tmp_msbB, wB, dim2 * dim3, bwB);
        delete[] tmp_msbB;
      } else {
        aux->wrap_computation(tmpB, wB, dim2 * dim3, bwB);
      }
      uint64_t *wB64 = new uint64_t[dim2 * dim3];
      for (int i = 0; i < dim2 * dim3; i++) {
        wB64[i] = uint64_t(wB[i]);
      }
      matmul_multiplexer(dim1, dim2, dim3, tmpA, wB64, wB_A, bwA, 1, bwC - bwB,
                         accumulate, mode);
      delete[] wB64;
    }
  }

  uint64_t *tmpC = new uint64_t[dim];
  int inner_loop_size = (accumulate ? dim2 : 1);
  for (int i = 0; i < dim; i++) {
    tmpC[i] =
        (local_terms[i] + cross_terms[i] - pow_A * wA_B[i] - pow_B * wB_A[i]) &
        maskC;
    if (signed_arithmetic) {
      for (int j = 0; j < inner_loop_size; j++) {
        int idx = (accumulate ? i : i / dim2);
        int common_idx = (accumulate ? j : i % dim2);
        int row_idx = idx / dim3;
        int col_idx = idx % dim3;
        int A_idx = row_idx * dim2 + common_idx;
        int B_idx = common_idx * dim3 + col_idx;
        if (signed_B) {
          if (party == ALICE) {
            tmpC[i] = (tmpC[i] - pow_A_2 * (tmpB[B_idx] - pow_B * wB[B_idx]) -
                       pow_B_2 * (tmpA[A_idx] - pow_A * wA[A_idx])) &
                      maskC;
          } else { // party == BOB
            tmpC[i] = (tmpC[i] - pow_A_2 * (tmpB[B_idx] - pow_B * wB[B_idx]) -
                       pow_B_2 * (tmpA[A_idx] - pow_A * wA[A_idx]) +
                       pow_A_2 * pow_B_2) &
                      maskC;
          }
        } else {
          tmpC[i] =
              (tmpC[i] - pow_A_2 * (tmpB[B_idx] - pow_B * wB[B_idx])) & maskC;
        }
      }
    }
  }
  if (accumulate) {
    trunc->truncate_and_reduce(dim1 * dim3, tmpC, outC, extra_bits, bwC);
  } else {
    memcpy(outC, tmpC, dim * sizeof(uint64_t));
  }

  delete[] cross_terms;
  delete[] local_terms;
  delete[] wA;
  delete[] wB;
  delete[] wA_B;
  delete[] wB_A;
  delete[] tmpA;
  delete[] tmpB;
  delete[] tmpC;
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/essential_matrix_utils.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <algorithm>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/types.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// Decomposes the essential matrix into the rotation R and translation t such
// that E can be any of the four candidate solutions: [rotation1 | translation],
// [rotation1 | -translation], [rotation2 | translation], [rotation2 |
// -translation].
void DecomposeEssentialMatrix(const Matrix3d& essential_matrix,
                              Matrix3d* rotation1,
                              Matrix3d* rotation2,
                              Vector3d* translation) {
  Matrix3d d;
  d << 0, 1, 0, -1, 0, 0, 0, 0, 1;

  const Eigen::JacobiSVD<Matrix3d> svd(
      essential_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  if (U.determinant() < 0) {
    U.col(2) *= -1.0;
  }

  if (V.determinant() < 0) {
    V.col(2) *= -1.0;
  }

  // Possible configurations.
  *rotation1 = U * d * V.transpose();
  *rotation2 = U * d.transpose() * V.transpose();
  *translation = U.col(2).normalized();
}

// x = (R2 * X + t2)
// x = (R2 * R1^t * X + t2)
// x = (R2 * (R1^t * X - t1) + t2)
// x = R2 * R1^t * X - R2 * t1 + t2
void EssentialMatrixFromTwoProjectionMatrices(
    const Matrix3x4d& pose1,
    const Matrix3x4d& pose2,
    Eigen::Matrix3d* essential_matrix) {
  // Create the Ematrix from the poses.
  const Eigen::Matrix3d R1 = pose1.leftCols<3>();
  const Eigen::Matrix3d R2 = pose2.leftCols<3>();
  const Eigen::Vector3d t1 = pose1.rightCols<1>();
  const Eigen::Vector3d t2 = pose2.rightCols<1>();

  // Pos1 = -R1^t * t1.
  // Pos2 = -R2^t * t2.
  // t = R1 * (pos2 - pos1).
  // t = R1 * (-R2^t * t2 + R1^t * t1)
  // t = t1 - R1 * R2^t * t2;

  // Relative transformation between to cameras.
  const Eigen::Matrix3d relative_rotation = R1 * R2.transpose();
  const Eigen::Vector3d translation = (t1 - relative_rotation * t2).normalized();
  *essential_matrix = CrossProductMatrix(translation) * relative_rotation;
}

int GetBestPoseFromEssentialMatrix(
    const Matrix3d& essential_matrix,
    const std::vector<FeatureCorrespondence>& normalized_correspondences,
    Matrix3d* rotation,
    Vector3d* position) {
  // Decompose ematrix.
  Matrix3d rotation1, rotation2;
  Vector3d translation;
  DecomposeEssentialMatrix(
      essential_matrix, &rotation1, &rotation2, &translation);
  const std::vector<Matrix3d> rotations = {
      rotation1, rotation1, rotation2, rotation2};
  const std::vector<Vector3d> positions = {
      -rotations[0].transpose() * translation,
      -rotations[1].transpose() * -translation,
      -rotations[2].transpose() * translation,
      -rotations[3].transpose() * -translation};

  // From the 4 candidate poses, find the one with the most triangulated points
  // in front of the camera.
  std::vector<int> points_in_front_of_cameras(4, 0);
  for (int i = 0; i < 4; i++) {
    for (const auto& correspondence : normalized_correspondences) {
      if (IsTriangulatedPointInFrontOfCameras(
              correspondence, rotations[i], positions[i])) {
        ++points_in_front_of_cameras[i];
      }
    }
  }

  // Find the pose with the most points in front of the camera.
  const auto& max_element = std::max_element(points_in_front_of_cameras.begin(),
                                             points_in_front_of_cameras.end());
  const int max_index =
      std::distance(points_in_front_of_cameras.begin(), max_element);

  // Set the pose.
  *rotation = rotations[max_index];
  *position = positions[max_index];
  return *max_element;
}

}  // namespace theia