#!/bin/sh
# autopkgtest check: Build and run a program against eigen3
# (C) 2014 Anton Gladky
# Author: Anton Gladky <gladk@debian.org>

set -e

WORKDIR=$(mktemp -d)
trap "rm -rf $WORKDIR" 0 INT QUIT ABRT PIPE TERM
cd $WORKDIR
cat <<EOF > demo.cpp
// This is the set of some expressions to check 
// Eigen3 correctness

#include <iostream>
#include <algorithm>
#include <iterator>
#include <memory>
#include <Eigen/Dense>

Eigen::Vector3d cart_to_sph(Eigen::Vector3d cart) {                //Input = [rho, z, phi], return [Theta, Psi, rho]
  double const& x = cart(2);
  double const& y = cart(0);
  double const& z = cart(1);
  
  double const rho = sqrt(x*x + y*y + z*z);
  double Theta = atan2(y,x);
  double const Psi = acos(z/rho);
  
  if (Theta < 0.0) {
    Theta+=2*M_PI;
  }
  return Eigen::Vector3d(Theta, Psi, rho);
}

Eigen::Vector3d sph_to_cart(Eigen::Vector3d sph) {                //Input = [Theta(0<=Theta<=2Pi), Psi(0<=Psi<=Pi), R], return = [X, Y, Z]
  double const& Theta = sph(0);
  double const& Psi   = sph(1);
  double const& rho   = sph(2);
  
  double const x = rho * sin (Psi) * cos(Theta);
  double const y = rho * sin (Psi) * sin(Theta);
  double const z = rho * cos (Psi);

  return Eigen::Vector3d(x, y, z);
}


int main(int ac, char* av[])
{
  std::cout<<"Check Eigen3 dyadic product: "<<std::endl; 
  
  Eigen::Vector3f x(1,2,3);
  Eigen::Vector3f y(4,5,6);

  Eigen::Matrix3f dProduct = x*y.transpose();
  std::cout<<"dProduct: "<<dProduct<<std::endl; 
  
  
  Eigen::Vector3f df(1,0,0);// df.normalize();
  Eigen::Vector3f dr(0,1,0);// dr.normalize();
  Eigen::Vector3f dz(0,0,1);// dz.normalize();
  
  Eigen::Vector3f ForceVec(-1,-2,-3);// dz.normalize();

  Eigen::Matrix3f axisMatrix; axisMatrix << df, dr, dz;
  axisMatrix.transposeInPlace();
  
  std::cout<<"axisMatrix: "<<axisMatrix<<std::endl;
  
  Eigen::Matrix3f forceMatrix; forceMatrix << ForceVec, ForceVec, ForceVec;
  forceMatrix.transposeInPlace();
  
  std::cout<<"forceMatrix: "<<forceMatrix<<std::endl;
  
  std::cout<<"forceMatrix*axisMatrix: "<<axisMatrix.cwiseProduct(forceMatrix)<<std::endl;
  
  std::cout<<"forceMatrix*axisMatrix, row[0]: "<<axisMatrix.cwiseProduct(forceMatrix).row(1)<<std::endl;
  
  std::cout<<"ForceVec length "<<ForceVec.norm()<<std::endl;
  std::cout<<"forceMatrix length "<<sqrt(forceMatrix.col(0).norm()*forceMatrix.col(0).norm() + forceMatrix.col(1).norm()*forceMatrix.col(1).norm() + forceMatrix.col(2).norm()*forceMatrix.col(2).norm())<<std::endl;
  
  Eigen::Vector3f fc(7,8,9);
  Eigen::Vector3f xc(1,1,1);
  
  std::cout<<"fc: "<<fc<<std::endl; 
  std::cout<<"xc: "<<xc<<std::endl; 
  Eigen::Matrix3f dProduct3 = fc*xc.transpose();
  std::cout<<"dProduct3: "<<dProduct3<<std::endl; 
  std::cout<<"dProduct3.trace(): "<<dProduct3.trace()<<std::endl; 
  
  
  
  
  Eigen::Vector3f fc2(-0.0000266509,4.83335e-6,0.0000201031);
  Eigen::Vector3f xc2(-0.006362649999999999,-0.0440067,0.0018367079999999998);
  
  std::cout<<"fc2: "<<fc2<<std::endl; 
  std::cout<<"xc2: "<<xc2<<std::endl; 
  Eigen::Matrix3f dProduct32 = fc2*xc2.transpose();
  std::cout<<"dProduct32: "<<dProduct32<<std::endl; 
  
  Eigen::Vector3f fc3(0.0,0.0,-10.0);
  Eigen::Vector3f xc3(0.0,0.0,0.5);
  
  std::cout<<"fc3: "<<fc3<<std::endl; 
  std::cout<<"xc3: "<<xc3<<std::endl; 
  xc3.normalize();
  Eigen::Matrix3f dProduct33 = fc3*xc3.transpose();
  std::cout<<"dProduct33: "<<dProduct33<<std::endl; 
  
  Eigen::Vector3f fc4(1.0,2.0,3.0);
  Eigen::Vector3f xc4(4.0,5.0,6.0);
  
  std::cout<<"fc4: "<<fc4<<std::endl; 
  std::cout<<"xc4: "<<xc4<<std::endl; 
  
  Eigen::Matrix3f dProduct44 = fc4*xc4.transpose();
  std::cout<<"dProduct44: "<<dProduct44<<std::endl; 
  std::cout<<"dProduct44.trace(): "<<dProduct44.trace()<<std::endl; 
  double dPr44 = dProduct44.trace()/3.0;
  
  double SigmaMax = dProduct44.diagonal().maxCoeff();
  double SigmaMin = dProduct44.diagonal().minCoeff();
  double SigmaNul = dProduct44.trace() - dProduct44.diagonal().maxCoeff() - dProduct44.diagonal().minCoeff();
  std::cout<<"dProduct44.diagonal(): "<<dProduct44.diagonal()<<std::endl; 
  std::cout<<"dProduct44.diagonal().maxCoeff(): "<<dProduct44.diagonal().maxCoeff()<<std::endl; 
  std::cout<<"dProduct44.diagonal().minCoeff(): "<<dProduct44.diagonal().minCoeff()<<std::endl; 
  
  std::cout<<"SigmaMax: "<<SigmaMax<<std::endl; 
  std::cout<<"SigmaNul: "<<SigmaNul<<std::endl; 
  std::cout<<"SigmaMin: "<<SigmaMin<<std::endl; 
  
  
  std::cout<<"dProduct44: "<<dProduct44<<std::endl; 
  std::cout<<"dProduct44*dProduct44: "<<dProduct44*dProduct44<<std::endl; 
  
  fc3 = Eigen::Vector3f(1.0,2.0,3.0);
  xc3 = Eigen::Vector3f(1.0,2.0,3.0);
  
  std::cout<<"fc3: "<<fc3<<std::endl; 
  std::cout<<"xc3: "<<xc3<<std::endl; 
  //xc3.normalize();
  dProduct33 = fc3*xc3.transpose();
  std::cout<<"dProduct33: "<<dProduct33<<std::endl; 
  
  fc3 = Eigen::Vector3f(1.0,2.0,3.0);
  xc3 = Eigen::Vector3f(1.0,2.0,3.0);
  std::cout<<"2.0*dProduct33: "<<2.0*fc3*xc3.transpose()<<std::endl; 
  std::cout<<"fc3: "<<fc3<<std::endl; 
  std::cout<<"xc3: "<<xc3<<std::endl<<std::endl<<std::endl; 
  
  
  double c0 = 0.96;
  double c1 = 1.1;
  double R = 0.1;
  double s = 0.001;
  double Vb = 0.00001;
  double Theta = 0.3;
  double Gamma = 0.3;
  
  
  //Capillary Law=====================================================================
  
  double beta = asin(pow(Vb/((c0*R*R*R*(1+3*s/R)*(1+c1*sin(Theta)))), 1.0/4.0));
  double r1 = (R*(1-cos(beta)) + s/2.0)/(cos(beta+Theta));
  double r2 = R*sin(beta) + r1*(sin(beta+Theta)-1);
  double Pc = Gamma*(1/r1 - 1/r2);
  double Scrit = (1+0.5*Theta)*pow(Vb,1/3.0);
  double fC = 2*M_PI*Gamma*R*sin(beta)*sin(beta+Theta) + M_PI*R*R*Pc*sin(beta)*sin(beta);
  
  
  std::cout<<"beta: "<<beta<<std::endl; 
  std::cout<<"r1: "<<r1<<std::endl; 
  std::cout<<"r2: "<<r2<<std::endl; 
  std::cout<<"Pc: "<<Pc<<std::endl; 
  std::cout<<"Scrit: "<<Scrit<<std::endl; 
  std::cout<<"fC: "<<fC<<std::endl; 
  
  //Capillary Law=====================================================================
  
  Eigen::Vector3f fc8 = Eigen::Vector3f(1.0,2.0,3.0);
  Eigen::Vector3f xc8 = Eigen::Vector3f(4.0,5.0,6.0);
  std::cout<<std::endl<<"fc8: "<<fc8(0)<<" "<<fc8(1)<<" "<<fc8(2)<<std::endl; 
  std::cout<<std::endl<<"xc8: "<<xc8(0)<<" "<<xc8(1)<<" "<<xc8(2)<<std::endl; 
  
  Eigen::Matrix3f dProduct_ = fc8*xc8.transpose();
  std::cout<<std::endl<<std::endl<<std::endl<<"dProduct: Fc Xc: "<<std::endl<<dProduct_<<std::endl; 
  dProduct_.transposeInPlace();
  std::cout<<std::endl<<std::endl<<std::endl<<"dProduct: Fc Xc: "<<std::endl<<dProduct_<<std::endl; 
  
  dProduct_ << fc8, fc8, fc8;
  
  std::cout<<std::endl<<std::endl<<std::endl<<"dProduct: Fc Xc: "<<std::endl<<dProduct_<<std::endl; 
  dProduct_.transposeInPlace();
  std::cout<<std::endl<<std::endl<<std::endl<<"dProduct: Fc Xc: "<<std::endl<<dProduct_<<std::endl; 
  
  std::cout<<std::endl<<std::endl<<std::endl<<"dProduct: Fc Xc: "<<std::endl<<dProduct_.row(0).sum()<<std::endl; 
  
  
  Eigen::Vector3d ddr = Eigen::Vector3d::UnitX();
  
  Eigen::Vector3d dvl = Eigen::Vector3d(0.2,0.1,-0.2);
  
  Eigen::Quaternion<double> dTheta  = Eigen::Quaternion<double>::FromTwoVectors(ddr, Eigen::Vector3d(dvl(0), dvl(1), 0.0)); dTheta.normalize();
  Eigen::AngleAxis<double>  AaTheta = Eigen::AngleAxis<double>(dTheta);
  double angleAaTheta = AaTheta.angle()*AaTheta.axis()(2);
  
  
  Eigen::Quaternion<double> dPsi   = Eigen::Quaternion<double>::FromTwoVectors(ddr, Eigen::Vector3d(dvl(0), 0.0, dvl(2))); dPsi.normalize();
  Eigen::AngleAxis<double>  AaPsi = Eigen::AngleAxis<double>(dPsi);
  double angleAaPsi = AaPsi.angle()*AaPsi.axis()(1);
  
  
  Eigen::Quaternion<double> dPsiTheta = Eigen::Quaternion<double>::FromTwoVectors(ddr, dvl); dPsi.normalize();
  
  std::cout<<angleAaTheta<<"   "<<angleAaPsi<<std::endl;
  std::cout<<dPsiTheta.toRotationMatrix()<<"   "<<std::endl;
  std::cout<<acos(dPsiTheta.toRotationMatrix()(0))<<"   "<<std::endl;
  
  
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>  md1(2, 2); md1 << 1, 2, 3, 4;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>  md2(2, 2); md2 << 5, 6, 7, 8;
  
  
  std::cout<<md1<<std::endl<<std::endl;
  std::cout<<md2<<std::endl<<std::endl;
  
  std::cout<<md1+md2<<std::endl<<std::endl<<std::endl;
  
  
  Eigen::Vector3d lbranch = Eigen::Vector3d(0.0, 1.0, 0.0);  //[rho, z, phi], return [Theta, Psi, rho]
  
  const Eigen::Vector3d convertA = cart_to_sph(lbranch); 
  
  std::cout<<"[rho, z, phi]: ["<< lbranch(0) << ", " << lbranch(1)  << ", " << lbranch(2)<<"] " <<std::endl;
  std::cout<<"Theta: "<< convertA(0)*180.0/M_PI << "; Psi: " << convertA(1)*180.0/M_PI<<std::endl;
  
  unsigned short _sizeori = 10;
  const double dAngle = M_PI/_sizeori;
  const double d2Ang  = dAngle/2.0;
  
  std::cout<<"Theta: "<< floor(convertA(0)/(M_PI/_sizeori)) << "; Psi: " << floor(convertA(1)/(M_PI/_sizeori)) << std::endl;
  
  
  const double ThetaI = floor(convertA(0)/(M_PI/_sizeori))*dAngle + d2Ang;
  const double PsiI   = floor(convertA(1)/(M_PI/_sizeori))*dAngle + d2Ang;
  
  std::cout<<"ThetaI: "<< ThetaI*180.0/M_PI << "; Psi: " << PsiI*180.0/M_PI << std::endl;
  
  Eigen::Vector3d SphXYZ = sph_to_cart(Eigen::Vector3d(ThetaI, PsiI, 1.0));
  std::cout<<"[X, Y, Z]: ["<< SphXYZ(0) << ", " << SphXYZ(1)  << ", " << SphXYZ(2)<<"] " <<std::endl;
  
  /*
  this->addinteraction(floor(convertA(0)/(M_PI/_sizeori)),    // Theta
                       floor(convertA(1)/(M_PI/_sizeori)));   // Psi
  */
  
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> inter(4,2);
  
  inter << 1,2,3,4,5,6,7,8;
  
  std::cout<<inter<<std::endl;
  
  std::cout<<inter.colwise().sum()<<std::endl;
  
  return 0;
}

EOF

cat <<EOF > original_result
Check Eigen3 dyadic product: 
dProduct:  4  5  6
 8 10 12
12 15 18
axisMatrix: 1 0 0
0 1 0
0 0 1
forceMatrix: -1 -2 -3
-1 -2 -3
-1 -2 -3
forceMatrix*axisMatrix: -1 -0 -0
-0 -2 -0
-0 -0 -3
forceMatrix*axisMatrix, row[0]: -0 -2 -0
ForceVec length 3.74166
forceMatrix length 6.48074
fc: 7
8
9
xc: 1
1
1
dProduct3: 7 7 7
8 8 8
9 9 9
dProduct3.trace(): 24
fc2: -2.66509e-05
 4.83335e-06
 2.01031e-05
xc2: -0.00636265
 -0.0440067
 0.00183671
dProduct32:   1.6957e-07  1.17282e-06 -4.89499e-08
-3.07529e-08   -2.127e-07  8.87745e-09
-1.27909e-07 -8.84671e-07  3.69235e-08
fc3:   0
  0
-10
xc3:   0
  0
0.5
dProduct33:   0   0   0
  0   0   0
 -0  -0 -10
fc4: 1
2
3
xc4: 4
5
6
dProduct44:  4  5  6
 8 10 12
12 15 18
dProduct44.trace(): 32
dProduct44.diagonal():  4
10
18
dProduct44.diagonal().maxCoeff(): 18
dProduct44.diagonal().minCoeff(): 4
SigmaMax: 18
SigmaNul: 10
SigmaMin: 4
dProduct44:  4  5  6
 8 10 12
12 15 18
dProduct44*dProduct44: 128 160 192
256 320 384
384 480 576
fc3: 1
2
3
xc3: 1
2
3
dProduct33: 1 2 3
2 4 6
3 6 9
2.0*dProduct33:  2  4  6
 4  8 12
 6 12 18
fc3: 1
2
3
xc3: 1
2
3


beta: 0.300054
r1: 0.00601953
r2: 0.0269368
Pc: 38.7006
Scrit: 0.024776
fC: 0.137678

fc8: 1 2 3

xc8: 4 5 6



dProduct: Fc Xc: 
 4  5  6
 8 10 12
12 15 18



dProduct: Fc Xc: 
 4  8 12
 5 10 15
 6 12 18



dProduct: Fc Xc: 
1 1 1
2 2 2
3 3 3



dProduct: Fc Xc: 
1 2 3
1 2 3
1 2 3



dProduct: Fc Xc: 
6
0.463648   0.785398
 0.666667 -0.333333  0.666667
 0.333333  0.933333  0.133333
-0.666667  0.133333  0.733333   
0.841069   
1 2
3 4

5 6
7 8

 6  8
10 12


[rho, z, phi]: [0, 1, 0] 
Theta: 0; Psi: 0
Theta: 0; Psi: 0
ThetaI: 9; Psi: 9
[X, Y, Z]: [0.154508, 0.0244717, 0.987688] 
1 2
3 4
5 6
7 8
16 20
EOF

g++ -I/usr/include -I/usr/include/eigen3  -o demo demo.cpp
echo "build: OK"
[ -x demo ]
./demo
./demo > new_result

DIFFRESULT=`diff new_result original_result`
if [ "$DIFFRESULT" != "" ]; then
	echo 'Regression test FAILED!'
	exit 1
else
  echo "Regression test PASSED!"
	exit 0
fi
echo "run: OK"