#include "UtsusemiSqeCalc.hh"

// added by VIC
// omp.h must be included regardless of whether MULTH is defined or not
// because "omp_get_thread_num" is called outside of the #ifdef MULTH block in this file
#include <omp.h>
// end add

//////////////////////////////////////////////////////////
UtsusemiSqeCalc::
UtsusemiSqeCalc(){
    _Initialize();
}
//////////////////////////////////////////////////////////
UtsusemiSqeCalc::
UtsusemiSqeCalc( bool isDebug ){
    _Initialize();
    isDEBUG = isDebug;
}
//////////////////////////////////////////////////////////
UtsusemiSqeCalc::
UtsusemiSqeCalc( std::string _instType ){
    _Initialize();
    SetInstGeometry( _instType );
}
//////////////////////////////////////////////////////////
UtsusemiSqeCalc::
~UtsusemiSqeCalc(){
    delete stool;
    delete UCC;
    delete _XP;
}
const Int4 UtsusemiSqeCalc::ROTATE_AXIS_X = 0;
const Int4 UtsusemiSqeCalc::ROTATE_AXIS_Y = 1;
const Int4 UtsusemiSqeCalc::ROTATE_AXIS_Z = 2;
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_Initialize(){
    stool = new StringTools();
    isDEBUG = UtsusemiEnvGetDebugMode();
    //MASKVALUE = 100000000.0;
    MASKVALUE = UTSUSEMIMASKVALUE64;
    MessageTag = "UtsusemiSqeCalc >> ";
    UCC = new UtsusemiUnitConverter();
    isDirectGeometry = true;

    _NumOfMulTh = UtsusemiGetNumOfMulTh();
    _XP = new UtsusemiSqeCalcXtalParams();
    isSetXtalParams = false;
}
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
SetInstGeometry( std::string _instType ){
    if ((_instType!="Direct") and (_instType!="Inverted")){
        return;
    }
    if (_instType=="Direct") isDirectGeometry=true;
    if (_instType=="Inverted") isDirectGeometry=false;
}
//////////////////////////////////////////////////////////
std::vector<gsl_vector*> UtsusemiSqeCalc::
_MakeEmpty3DMatrix(){
    std::vector<gsl_vector*> ret;
    for (UInt4 i=0; i<3; i++){
        gsl_vector* tmp = gsl_vector_alloc(3);
        gsl_vector_set( tmp, 0, 0.0 );
        gsl_vector_set( tmp, 1, 0.0 );
        gsl_vector_set( tmp, 2, 0.0 );
        ret.push_back( tmp );
    }
    return ret;
}
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_Delete3DMatrix( std::vector<gsl_vector*> *A ){
    for (UInt4 i=0; i<(A->size()); i++)
        gsl_vector_free( A->at(i) );
    A->clear();
}
//////////////////////////////////////////////////////////
bool UtsusemiSqeCalc::
_Calc3DReversedMatrix( std::vector<gsl_vector*> *A, std::vector<gsl_vector*> *C ){
    if ((A->size()!=3)||(C->size()!=3)) return false;

    Double a11 = gsl_vector_get(A->at(0),0);
    Double a12 = gsl_vector_get(A->at(0),1);
    Double a13 = gsl_vector_get(A->at(0),2);
    Double a21 = gsl_vector_get(A->at(1),0);
    Double a22 = gsl_vector_get(A->at(1),1);
    Double a23 = gsl_vector_get(A->at(1),2);
    Double a31 = gsl_vector_get(A->at(2),0);
    Double a32 = gsl_vector_get(A->at(2),1);
    Double a33 = gsl_vector_get(A->at(2),2);
    Double detA= a11*a22*a33 + a21*a32*a13 + a31*a12*a23 - a11*a32*a23 - a31*a22*a13 - a21*a12*a33;

    if (detA == 0){
        return false;
    }else{

        gsl_vector_set( C->at(0), 0, (a22*a33-a23*a32)/detA );
        gsl_vector_set( C->at(0), 1, (a13*a32-a12*a33)/detA );
        gsl_vector_set( C->at(0), 2, (a12*a23-a13*a22)/detA );

        gsl_vector_set( C->at(1), 0, (a23*a31-a21*a33)/detA );
        gsl_vector_set( C->at(1), 1, (a11*a33-a13*a31)/detA );
        gsl_vector_set( C->at(1), 2, (a13*a21-a11*a23)/detA );

        gsl_vector_set( C->at(2), 0, (a21*a32-a12*a31)/detA );
        gsl_vector_set( C->at(2), 1, (a12*a31-a11*a32)/detA );
        gsl_vector_set( C->at(2), 2, (a11*a22-a12*a21)/detA );

        return true;
    }
}
//////////////////////////////////////////////////////////
bool UtsusemiSqeCalc::
_CalcRotateUVWMatrix( UInt4 rot_axis, Double angle, std::vector<gsl_vector*> *A ){

    Double **r = new Double*[3];
    for (UInt4 i=0;i<3;i++) r[i] = new Double[3];

    bool isValidAxis = true;
    if (rot_axis==ROTATE_AXIS_X){       // Xaxis
        double ang_rad = -angle*M_PI/180.0;  // clockwise from axis perpendicular of Y and Z axes  > 0

        r[0][0] = cos(ang_rad);
        r[0][1] = 0.0;
        r[0][2] = sin(ang_rad);
        r[1][0] = 0.0;
        r[1][1] = 1.0;
        r[1][2] = 0.0;
        r[2][0] = -sin(ang_rad);
        r[2][1] = 0.0;
        r[2][2] = cos(ang_rad);

    }else if (rot_axis==ROTATE_AXIS_Y){ // Yaxis
        double ang_rad = -angle*M_PI/180.0;  // clockwise from bird view > 0

        r[0][0] = cos(ang_rad);
        r[0][1] = -sin(ang_rad);
        r[0][2] = 0.0;
        r[1][0] = sin(ang_rad);
        r[1][1] = cos(ang_rad);
        r[1][2] = 0.0;
        r[2][0] = 0.0;
        r[2][1] = 0.0;
        r[2][2] = 1.0;

    }else if (rot_axis==ROTATE_AXIS_Z){ // Zaxis
        double ang_rad = -angle*M_PI/180.0;  // clockwise from upstream > 0

        r[0][0] = 1.0;
        r[0][1] = 0.0;
        r[0][2] = 0.0;
        r[1][0] = 0.0;
        r[1][1] = cos(ang_rad);
        r[1][2] = -sin(ang_rad);
        r[2][0] = 0.0;
        r[2][1] = sin(ang_rad);
        r[2][2] = cos(ang_rad);

    }else{
        UtsusemiError(MessageTag+"_CalcRotateUVWMatrix :: axis is invarid.");
        isValidAxis = false;
    }

    if (isValidAxis){
        std::vector<gsl_vector*> T = _MakeEmpty3DMatrix();
        for (UInt4 i=0;i<3;i++){
            for (UInt4 j=0;j<3;j++){
                gsl_vector* tmp = gsl_vector_alloc(3);
                gsl_vector_memcpy( tmp, A->at(j) );
                gsl_blas_dscal( r[j][i], tmp );
                gsl_vector_add( T[i], tmp );
                gsl_vector_free( tmp );
            }
        }
        for (UInt4 i=0;i<3;i++)
            gsl_vector_memcpy( A->at(i), T[i] );

        _Delete3DMatrix( &T );
    }

    delete [] r;

    return isValidAxis;
}
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_CalcReciprocalLatticeMatrix( std::vector<Double> latticeConst, std::vector<gsl_vector*> *M ){
    Double alpha = latticeConst[3]/180.0*M_PI;
    Double beta =  latticeConst[4]/180.0*M_PI;
    Double gamma = latticeConst[5]/180.0*M_PI;

    std::vector<gsl_vector*> ML = _MakeEmpty3DMatrix();
    gsl_vector_set( ML[0], 0, latticeConst[0] );
    gsl_vector_set( ML[0], 1, 0.0 );
    gsl_vector_set( ML[0], 2, 0.0 );
    gsl_vector_set( ML[1], 0, latticeConst[1]*cos(gamma) );
    gsl_vector_set( ML[1], 1, latticeConst[1]*sin(gamma) );
    gsl_vector_set( ML[1], 2, 0.0 );
    gsl_vector_set( ML[2], 0, latticeConst[2]*cos(alpha) );
    gsl_vector_set( ML[2], 1, latticeConst[2]*cos(beta)*sin(gamma) );
    Double c3 = fabs(latticeConst[2])*sqrt( (1.0 - cos(alpha)*cos(alpha) + (cos(beta)*cos(beta))*(sin(beta)*sin(beta))) );
    gsl_vector_set( ML[2], 2, c3 );
    if (isDEBUG){
        std::cout << "vectora =" << gsl_vector_get(ML[0],0) << "," << gsl_vector_get(ML[0],1) << "," << gsl_vector_get(ML[0],2) << std::endl;
        std::cout << "vectorb =" << gsl_vector_get(ML[1],0) << "," << gsl_vector_get(ML[1],1) << "," << gsl_vector_get(ML[1],2) << std::endl;
        std::cout << "vectorb =" << gsl_vector_get(ML[2],0) << "," << gsl_vector_get(ML[2],1) << "," << gsl_vector_get(ML[2],2) << std::endl;
    }

    _CalcCross3d( ML[1], ML[2], M->at(0) );
    _CalcCross3d( ML[2], ML[0], M->at(1) );
    _CalcCross3d( ML[0], ML[1], M->at(2) );

    Double v0;
    gsl_blas_ddot( ML[0], M->at(0), &v0 );
    if (isDEBUG) std::cout << "v0=" << v0 << std::endl;

    gsl_blas_dscal( (2.0*M_PI/v0), M->at(0) );
    gsl_blas_dscal( (2.0*M_PI/v0), M->at(1) );
    gsl_blas_dscal( (2.0*M_PI/v0), M->at(2) );

    if (isDEBUG){
        std::cout << "vecRa ";
        std::cout << gsl_vector_get(M->at(0),0) <<","<<gsl_vector_get(M->at(0),1) <<","<<gsl_vector_get(M->at(0),2) <<std::endl;
        std::cout << "vecRb ";
        std::cout << gsl_vector_get(M->at(1),0) <<","<<gsl_vector_get(M->at(1),1) <<","<<gsl_vector_get(M->at(1),2) <<std::endl;
        std::cout << "vecRc ";
        std::cout << gsl_vector_get(M->at(2),0) <<","<<gsl_vector_get(M->at(2),1) <<","<<gsl_vector_get(M->at(2),2) <<std::endl;
    }

    _Delete3DMatrix( &ML );

}
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_CalcUVWMatrix( std::vector<Double> Uvec, std::vector<Double> Vvec, std::vector<gsl_vector*> *M, std::vector<gsl_vector*> *N ){

    // copy Reciprocal Lattice Matrix
    std::vector<gsl_vector*> T = _MakeEmpty3DMatrix();
    for (UInt4 i=0; i<3; i++)
        gsl_vector_memcpy( T[i], M->at(i) );
    gsl_blas_dscal( Uvec[0], T[0] );
    gsl_blas_dscal( Uvec[1], T[1] );
    gsl_blas_dscal( Uvec[2], T[2] );

    for (UInt4 i=0; i<3; i++)
        gsl_vector_set_all( N->at(i), 0.0 );

    gsl_vector_add( N->at(0), T[0] );
    gsl_vector_add( N->at(0), T[1] );
    gsl_vector_add( N->at(0), T[2] );

    for (UInt4 i=0; i<3; i++)
        gsl_vector_memcpy( T[i], M->at(i) );
    gsl_blas_dscal( Vvec[0], T[0] );
    gsl_blas_dscal( Vvec[1], T[1] );
    gsl_blas_dscal( Vvec[2], T[2] );

    gsl_vector_add( N->at(1), T[0] );
    gsl_vector_add( N->at(1), T[1] );
    gsl_vector_add( N->at(1), T[2] );

    gsl_blas_dscal((1.0/gsl_blas_dnrm2( N->at(0) )), N->at(0));

    Double v1;
    gsl_vector *tmp = gsl_vector_alloc(3);
    gsl_blas_ddot( N->at(0), N->at(1), &v1 );
    gsl_vector_memcpy( tmp, N->at(0) ); // vecU -> tmp
    gsl_blas_dscal( v1, tmp );

    gsl_vector_sub( N->at(1), tmp );

    gsl_blas_dscal((1.0/gsl_blas_dnrm2( N->at(1) )), N->at(1) );

    _CalcCross3d( N->at(0), N->at(1), N->at(2) );
    _Delete3DMatrix( &T );

    gsl_vector_free(tmp);
    if (isDEBUG){
        std::cout << "VecU norm";
        std::cout << gsl_vector_get(N->at(0),0) <<","<<gsl_vector_get(N->at(0),1) <<","<<gsl_vector_get(N->at(0),2) <<std::endl;
        std::cout << "VecV norm";
        std::cout << gsl_vector_get(N->at(1),0) <<","<<gsl_vector_get(N->at(1),1) <<","<<gsl_vector_get(N->at(1),2) <<std::endl;
    }

}
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_CalcReciproLattice2( std::vector<Double> latticeConst, std::vector<Double> Uvec, std::vector<Double> Vvec, std::vector<Double> rotSteps, std::vector<gsl_vector*> *ret ){
    if (ret->size()!=3){
        return;
    }
    std::vector<gsl_vector*> M = _MakeEmpty3DMatrix();
    std::vector<gsl_vector*> N = _MakeEmpty3DMatrix();
    _CalcReciprocalLatticeMatrix( latticeConst, &M );
    _CalcUVWMatrix( Uvec, Vvec, &M, &N );

    for (UInt4 i=0;i<(rotSteps.size()/2);i++){
        _CalcRotateUVWMatrix( (UInt4)(rotSteps[2*i]), rotSteps[2*i+1], &N );
    }
    Double a0,a1,a2;
    Double b0,b1,b2;
    Double c0,c1,c2;
    gsl_blas_ddot( M[0], N[0], &a0 );
    gsl_blas_ddot( M[0], N[1], &a1 );
    gsl_blas_ddot( M[0], N[2], &a2 );
    gsl_blas_ddot( M[1], N[0], &b0 );
    gsl_blas_ddot( M[1], N[1], &b1 );
    gsl_blas_ddot( M[1], N[2], &b2 );
    gsl_blas_ddot( M[2], N[0], &c0 );
    gsl_blas_ddot( M[2], N[1], &c1 );
    gsl_blas_ddot( M[2], N[2], &c2 );
    if (isDEBUG){
        std::cout << "-------- Matrix UVW---- ------------------------------" << std::endl;
        std::cout << "U-vec ";
        std::cout << gsl_vector_get(N[0],0) <<","<<gsl_vector_get(N[0],1) <<","<<gsl_vector_get(N[0],2)<<std::endl;
        std::cout << "V-vec ";
        std::cout << gsl_vector_get(N[1],0) <<","<<gsl_vector_get(N[1],1) <<","<<gsl_vector_get(N[1],2)<<std::endl;
        std::cout << "W-vec ";
        std::cout << gsl_vector_get(N[2],0) <<","<<gsl_vector_get(N[2],1) <<","<<gsl_vector_get(N[2],2)<<std::endl;
        std::cout << "------------------------------------------------------" << std::endl;
    }
    _Delete3DMatrix( &M );
    _Delete3DMatrix( &N );

    gsl_vector_set( ret->at(0), 0, a0 );
    gsl_vector_set( ret->at(0), 1, a1 );
    gsl_vector_set( ret->at(0), 2, a2 );
    gsl_vector_set( ret->at(1), 0, b0 );
    gsl_vector_set( ret->at(1), 1, b1 );
    gsl_vector_set( ret->at(1), 2, b2 );
    gsl_vector_set( ret->at(2), 0, c0 );
    gsl_vector_set( ret->at(2), 1, c1 );
    gsl_vector_set( ret->at(2), 2, c2 );

    if (isDEBUG){
        std::cout << "-------- Matrix [L][UVW] -----------------------------" << std::endl;
        std::cout << "U-vec " << a0 <<","<<a1<<","<<a2<<std::endl;
        std::cout << "V-vec " << b0 <<","<<b1<<","<<b2<<std::endl;
        std::cout << "W-vec " << c0 <<","<<c1<<","<<c2<<std::endl;
        std::cout << "------------------------------------------------------" << std::endl;
    }

}
//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_CalcReciproLattice( std::vector<Double> latticeConst, std::vector<Double> Uvec, std::vector<Double> Vvec, Double phi, std::vector<gsl_vector*> *ret ){

    Double alpha = latticeConst[3]/180.0*M_PI;
    Double beta =  latticeConst[4]/180.0*M_PI;
    Double gamma = latticeConst[5]/180.0*M_PI;

    gsl_vector *uva = gsl_vector_alloc(3);
    gsl_vector *uvb = gsl_vector_alloc(3);
    gsl_vector *uvc = gsl_vector_alloc(3);
    gsl_vector *tmp = gsl_vector_alloc(3);

    gsl_vector_set( uva, 0, latticeConst[0] );
    gsl_vector_set( uva, 1, 0.0 );
    gsl_vector_set( uva, 2, 0.0 );
    gsl_vector_set( uvb, 0, latticeConst[1]*cos(gamma) );
    gsl_vector_set( uvb, 1, latticeConst[1]*sin(gamma) );
    gsl_vector_set( uvb, 2, 0.0 );
    gsl_vector_set( uvc, 0, latticeConst[2]*cos(alpha) );
    gsl_vector_set( uvc, 1, latticeConst[2]*cos(beta)*sin(gamma) );
    Double c3 = fabs(latticeConst[2])*sqrt( (1.0 - cos(alpha)*cos(alpha) + (cos(beta)*cos(beta))*(sin(beta)*sin(beta))) );
    gsl_vector_set( uvc, 2, c3 );
    if (isDEBUG){
        std::cout << "vectora =" << gsl_vector_get(uva,0) << "," << gsl_vector_get(uva,1) << "," << gsl_vector_get(uva,2) << std::endl;
        std::cout << "vectorb =" << gsl_vector_get(uvb,0) << "," << gsl_vector_get(uvb,1) << "," << gsl_vector_get(uvb,2) << std::endl;
        std::cout << "vectorb =" << gsl_vector_get(uvc,0) << "," << gsl_vector_get(uvc,1) << "," << gsl_vector_get(uvc,2) << std::endl;
    }
    gsl_vector* vecRa = gsl_vector_alloc(3);
    gsl_vector* vecRb = gsl_vector_alloc(3);
    gsl_vector* vecRc = gsl_vector_alloc(3);
    _CalcCross3d( uvb, uvc, vecRa );
    _CalcCross3d( uvc, uva, vecRb );
    _CalcCross3d( uva, uvb, vecRc );

    Double v0;
    gsl_blas_ddot( uva, vecRa, &v0 );

    if (isDEBUG) std::cout << "v0=" << v0 << std::endl;

    gsl_blas_dscal( (2.0*M_PI/v0), vecRa );
    gsl_blas_dscal( (2.0*M_PI/v0), vecRb );
    gsl_blas_dscal( (2.0*M_PI/v0), vecRc );

    if (isDEBUG){
        std::cout << "vecRa ";
        std::cout << gsl_vector_get(vecRa,0) <<","<<gsl_vector_get(vecRa,1) <<","<<gsl_vector_get(vecRa,2) <<std::endl;
        std::cout << "vecRb ";
        std::cout << gsl_vector_get(vecRb,0) <<","<<gsl_vector_get(vecRb,1) <<","<<gsl_vector_get(vecRb,2) <<std::endl;
        std::cout << "vecRc ";
        std::cout << gsl_vector_get(vecRc,0) <<","<<gsl_vector_get(vecRc,1) <<","<<gsl_vector_get(vecRc,2) <<std::endl;
    }

    gsl_vector *UV0 = gsl_vector_alloc(3);
    gsl_vector *UV1 = gsl_vector_alloc(3);
    gsl_vector *UV2 = gsl_vector_alloc(3);

    gsl_vector_memcpy( UV0, vecRa );
    gsl_vector_memcpy( UV1, vecRb );
    gsl_vector_memcpy( UV2, vecRc );

    gsl_blas_dscal( Uvec[0], UV0 );
    gsl_blas_dscal( Uvec[1], UV1 );
    gsl_blas_dscal( Uvec[2], UV2 );

    gsl_vector *vecU = gsl_vector_alloc(3);
    gsl_vector_set_all( vecU, 0.0 );

    gsl_vector_add( vecU, UV0 );
    gsl_vector_add( vecU, UV1 );
    gsl_vector_add( vecU, UV2 );

    gsl_vector_memcpy( UV0, vecRa );
    gsl_vector_memcpy( UV1, vecRb );
    gsl_vector_memcpy( UV2, vecRc );
    gsl_blas_dscal( Vvec[0], UV0 );
    gsl_blas_dscal( Vvec[1], UV1 );
    gsl_blas_dscal( Vvec[2], UV2 );

    gsl_vector *vecV = gsl_vector_alloc(3);
    gsl_vector_set_all( vecV, 0.0 );
    gsl_vector_add( vecV, UV0 );
    gsl_vector_add( vecV, UV1 );
    gsl_vector_add( vecV, UV2 );
    if (isDEBUG){
        std::cout << "vecV ";
        std::cout << gsl_vector_get(vecV,0) <<","<<gsl_vector_get(vecV,1) <<","<<gsl_vector_get(vecV,2) <<std::endl;
    }
    gsl_blas_dscal((1.0/gsl_blas_dnrm2( vecU )), vecU);

    Double v1;
    gsl_blas_ddot( vecU, vecV, &v1 );
    gsl_vector_memcpy( tmp, vecU ); // vecU -> tmp
    gsl_blas_dscal( v1, tmp );

    gsl_vector_sub( vecV, tmp );

    gsl_blas_dscal((1.0/gsl_blas_dnrm2( vecV )), vecV );

    gsl_vector *vecW = gsl_vector_alloc(3);
    _CalcCross3d( vecU, vecV, vecW );

    if (phi!=0.0){
        double phi_rad = -phi*M_PI/180.0;
        Double **R;
        R = new Double*[3];
        for (UInt4 i=0;i<3;i++) R[i] = new Double[3];
        R[0][0] = cos(phi_rad);
        R[0][1] = sin(phi_rad);
        R[0][2] = 0.0;
        R[1][0] = -sin(phi_rad);
        R[1][1] = cos(phi_rad);
        R[1][2] = 0.0;
        R[2][0] = 0.0;
        R[2][1] = 0.0;
        R[2][2] = 1.0;

        std::vector< gsl_vector* > vecUVWo,vecUVWr;
        for (UInt4 i=0;i<3;i++) {
            vecUVWo.push_back( gsl_vector_alloc(3) );
            vecUVWr.push_back( gsl_vector_alloc(3) );
        }

        for (UInt4 i=0;i<3;i++){
            gsl_vector_memcpy( vecUVWo[0], vecU );
            gsl_vector_memcpy( vecUVWo[1], vecV );
            gsl_vector_memcpy( vecUVWo[2], vecW );
            gsl_vector_set_all( vecUVWr[i], 0.0 );
            for (UInt4 j=0;j<3;j++){
                gsl_blas_dscal( R[i][j], vecUVWo[j] );
                gsl_vector_add( vecUVWr[i], vecUVWo[j] );
            }
        }

        gsl_vector_memcpy( vecU, vecUVWr[0] );
        gsl_vector_memcpy( vecV, vecUVWr[1] );
        gsl_vector_memcpy( vecW, vecUVWr[2] );

        for (UInt4 i=0;i<3;i++){
            gsl_vector_free( vecUVWo[i] );
            gsl_vector_free( vecUVWr[i] );
            delete [] R[i];
        }
        delete [] R;
    }
    if (isDEBUG){
        std::cout << "VecU norm";
        std::cout << gsl_vector_get(vecU,0) <<","<<gsl_vector_get(vecU,1) <<","<<gsl_vector_get(vecU,2) <<std::endl;
        std::cout << "VecV norm";
        std::cout << gsl_vector_get(vecV,0) <<","<<gsl_vector_get(vecV,1) <<","<<gsl_vector_get(vecV,2) <<std::endl;
    }

    Double a0,a1,a2;
    Double b0,b1,b2;
    Double c0,c1,c2;
    gsl_blas_ddot( vecRa, vecU, &a0 );
    gsl_blas_ddot( vecRa, vecV, &a1 );
    gsl_blas_ddot( vecRa, vecW, &a2 );
    gsl_blas_ddot( vecRb, vecU, &b0 );
    gsl_blas_ddot( vecRb, vecV, &b1 );
    gsl_blas_ddot( vecRb, vecW, &b2 );
    gsl_blas_ddot( vecRc, vecU, &c0 );
    gsl_blas_ddot( vecRc, vecV, &c1 );
    gsl_blas_ddot( vecRc, vecW, &c2 );

    gsl_vector_free( uva );
    gsl_vector_free( uvb );
    gsl_vector_free( uvc );
    gsl_vector_free( tmp );
    gsl_vector_free( vecRa );
    gsl_vector_free( vecRb );
    gsl_vector_free( vecRc );
    gsl_vector_free( UV0 );
    gsl_vector_free( UV1 );
    gsl_vector_free( UV2 );
    gsl_vector_free( vecU );
    gsl_vector_free( vecV );
    gsl_vector_free( vecW );

    if (ret->size()!=3){
        return;
    }

    gsl_vector_set( ret->at(0), 0, a0 );
    gsl_vector_set( ret->at(0), 1, a1 );
    gsl_vector_set( ret->at(0), 2, a2 );
    gsl_vector_set( ret->at(1), 0, b0 );
    gsl_vector_set( ret->at(1), 1, b1 );
    gsl_vector_set( ret->at(1), 2, b2 );
    gsl_vector_set( ret->at(2), 0, c0 );
    gsl_vector_set( ret->at(2), 1, c1 );
    gsl_vector_set( ret->at(2), 2, c2 );

}

//////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
CalcReciproLattice( std::vector<Double> latticeConst, std::vector<Double> Uvec, std::vector<Double> Vvec, Double phi ){
    std::vector<gsl_vector*> vec;
    for (UInt4 i=0;i<3;i++){
        vec.push_back( gsl_vector_alloc(3) );
    }
    _CalcReciproLattice( latticeConst, Uvec, Vvec, phi, &vec );

    std::vector<Double> ret;
    ret.clear();
    for (UInt4 i=0;i<vec.size();i++){
        ret.push_back( gsl_vector_get( vec[i],0 ) );
        ret.push_back( gsl_vector_get( vec[i],1 ) );
        ret.push_back( gsl_vector_get( vec[i],2 ) );
    }

    for (UInt4 i=0;i<3;i++){
        gsl_vector_free( vec[i] );
    }

    return ret;
}

//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_Make4DVect( gsl_vector* vec, std::vector< gsl_vector*> latticeVec, gsl_vector* ret ){
    UInt4 n = 4;

    std::vector< gsl_vector* > v;
    v.clear();
    for (UInt4 i=0;i<n;i++){
        v.push_back( gsl_vector_alloc(4) );
        gsl_vector_set_all( v[i],0.0 );
    }
    for (UInt4 i=0;i<latticeVec.size();i++){
        for (UInt4 j=0;j<3;j++){
            gsl_vector_set( v[i], j, gsl_vector_get( latticeVec[i], j ) );
        }
    }
    gsl_vector_set( v[3], 3, 1.0 );

    if (isDEBUG){
        std::cout << "in _Make4DVect" <<std::endl;
        std::cout << gsl_vector_get( v[0], 0 )<<","<<gsl_vector_get( v[0], 1 )<<","<<gsl_vector_get( v[0], 2 )<<","<<gsl_vector_get( v[0], 3 )<<std::endl;
        std::cout << gsl_vector_get( v[1], 0 )<<","<<gsl_vector_get( v[1], 1 )<<","<<gsl_vector_get( v[1], 2 )<<","<<gsl_vector_get( v[1], 3 )<<std::endl;
        std::cout << gsl_vector_get( v[2], 0 )<<","<<gsl_vector_get( v[2], 1 )<<","<<gsl_vector_get( v[2], 2 )<<","<<gsl_vector_get( v[2], 3 )<<std::endl;
        std::cout << gsl_vector_get( v[3], 0 )<<","<<gsl_vector_get( v[3], 1 )<<","<<gsl_vector_get( v[3], 2 )<<","<<gsl_vector_get( v[3], 3 )<<std::endl;
        std::cout << "view vector" << std::endl;
        std::cout << gsl_vector_get(vec,0)<<","<<gsl_vector_get(vec,1)<<","<<gsl_vector_get(vec,2)<<","<<gsl_vector_get(vec,3)<<std::endl;
    }

    gsl_vector *tmp = gsl_vector_alloc(4);
    gsl_vector_set_all( ret, 0.0 );

    for (UInt4 i=0;i<4;i++){
        gsl_vector_set_all( tmp, 0.0 );
        gsl_vector_memcpy( tmp, v[i] );
        gsl_blas_dscal( gsl_vector_get( vec, i ), tmp );
        gsl_vector_add( ret, tmp );
    }

    if (isDEBUG){
        std::cout << "-----" << std::endl;
        std::cout << "In _Make4DVect" << std::endl;
        std::cout << "u=" << gsl_vector_get( ret,0 ) <<","<<gsl_vector_get( ret,1 )<<",";
        std::cout << gsl_vector_get( ret,2 ) <<","<< gsl_vector_get( ret,3 ) << std::endl;
    }

    Double vecdot;
    gsl_blas_ddot( ret, ret, &vecdot );
    gsl_blas_dscal((1.0/vecdot), ret );

    if (isDEBUG){
        std::cout << "uu=" << gsl_vector_get( ret,0 ) <<","<<gsl_vector_get( ret,1 )<<",";
        std::cout << gsl_vector_get( ret,2 ) <<","<< gsl_vector_get( ret,3 ) << std::endl;
        std::cout << "-----" << std::endl;
    }

    gsl_vector_free( tmp );
    for (UInt4 i=0;i<n;i++){
        gsl_vector_free( v[i] );
    }

}

//////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
Make4DVect( std::vector<Double> vaxis, std::vector<Double> lc ){
    std::vector< gsl_vector* > latticeVec;
    latticeVec.clear();
    for (UInt4 i=0;i<4;i++){
        latticeVec.push_back( gsl_vector_alloc(3) );
        for (UInt4 j=0;j<3;j++){
            gsl_vector_set( latticeVec[i], j, lc[i*3+j] );
        }
    }
    gsl_vector* vec = gsl_vector_alloc(4);
    for (UInt4 i=0;i<vaxis.size();i++){
        gsl_vector_set( vec, i, vaxis[i] );
    }
    gsl_vector* ret_vec = gsl_vector_alloc(3);
    _Make4DVect( vec, latticeVec, ret_vec );

    std::vector<Double> ret;
    ret.clear();
    for (UInt4 i=0;i<4;i++){
        ret.push_back( gsl_vector_get( ret_vec, i ) );
    }
    gsl_vector_free( ret_vec );
    gsl_vector_free( vec );
    for (UInt4 i=0;i<4;i++){
        gsl_vector_free( latticeVec[i] );
    }

    return ret;
}

//////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
MakeProjectionMatrix( std::vector<Double> latticeConst, std::vector<Double> Uvec, std::vector<Double> Vvec, std::vector<Double> rotateSteps, std::vector<Double> viewAxes ){
    std::vector<gsl_vector*> latticeVec = _MakeEmpty3DMatrix();
    _CalcReciproLattice2( latticeConst, Uvec, Vvec, rotateSteps, &latticeVec );

    if (isDEBUG){
        std::cout << "l1=" << gsl_vector_get( latticeVec[0],0 ) << "," << gsl_vector_get( latticeVec[0],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[0],2 ) << std::endl;
        std::cout << "l2=" << gsl_vector_get( latticeVec[1],0 ) << "," << gsl_vector_get( latticeVec[1],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[1],2 ) << std::endl;
        std::cout << "l3=" << gsl_vector_get( latticeVec[2],0 ) << "," << gsl_vector_get( latticeVec[2],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[2],2 ) << std::endl;
    }

    // Make Matrix for (Qx,Qy,Qz,hw) -> (Vx,Vy,Vz,Vw)
    std::vector<Double> A(16);
    gsl_vector* vec1 = gsl_vector_alloc(4);
    gsl_vector *vec2 = gsl_vector_alloc(4);
    for (UInt4 i=0;i<4;i++){
        gsl_vector_set_all( vec1, 0.0 );
        gsl_vector_set_all( vec2, 0.0 );
        for (UInt4 j=0;j<4;j++)
            gsl_vector_set( vec1, j, viewAxes[i*4+j] );

        _Make4DVect( vec1, latticeVec, vec2 );

        for (UInt4 j=0;j<4;j++)
            A[(i*4+j)] = gsl_vector_get( vec2, j );

    }
    gsl_vector_free( vec1 );
    gsl_vector_free( vec2 );
    _Delete3DMatrix( &latticeVec );
    if (isDEBUG){
        std::cout << "-------- Matrix [A]=[V][L][UVW] ----------------------" << std::endl;
        std::cout << A[0] << "," << A[1] << "," << A[2] <<std::endl;
        std::cout << A[4] << "," << A[5] << "," << A[6] <<std::endl;
        std::cout << A[8] << "," << A[9] << "," << A[10] <<std::endl;
        std::cout << "------------------------------------------------------" << std::endl;
    }
    return A;
}
//////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
Projection_QxQyQz( std::vector<Double> vaxis){
    ElementContainerMatrix* ecm = Put();
    if (isSetXtalParams){
        if (_XP->SetSampleInfoToData( ecm )){
        }
    }
    HeaderBase* hh = ecm->PutHeaderPointer();
    std::vector<Double> latticeConst = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_LATTICECONSTS);
    std::vector<Double> Uvec         = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_UVECT);
    std::vector<Double> Vvec         = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_VVECT);
    std::vector<Double> rotateSteps  = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_ROTATESTEPS);

    std::vector<Double> A = MakeProjectionMatrix( latticeConst, Uvec, Vvec, rotateSteps, vaxis );
    /*
    std::vector<gsl_vector*> latticeVec = _MakeEmpty3DMatrix();
    _CalcReciproLattice2( latticeConst, Uvec, Vvec, rotateSteps, &latticeVec );

    if (isDEBUG){
        std::cout << "l1=" << gsl_vector_get( latticeVec[0],0 ) << "," << gsl_vector_get( latticeVec[0],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[0],2 ) << std::endl;
        std::cout << "l2=" << gsl_vector_get( latticeVec[1],0 ) << "," << gsl_vector_get( latticeVec[1],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[1],2 ) << std::endl;
        std::cout << "l3=" << gsl_vector_get( latticeVec[2],0 ) << "," << gsl_vector_get( latticeVec[2],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[2],2 ) << std::endl;
    }

    // Make Matrix for (Qx,Qy,Qz,hw) -> (Vx,Vy,Vz,Vw)
    std::vector<Double> A(16);
    gsl_vector* vec1 = gsl_vector_alloc(4);
    gsl_vector *vec2 = gsl_vector_alloc(4);
    for (UInt4 i=0;i<4;i++){
        gsl_vector_set_all( vec1, 0.0 );
        gsl_vector_set_all( vec2, 0.0 );
        for (UInt4 j=0;j<4;j++)
            gsl_vector_set( vec1, j, vaxis[i*4+j] );

        _Make4DVect( vec1, latticeVec, vec2 );

        for (UInt4 j=0;j<4;j++)
            A[(i*4+j)] = gsl_vector_get( vec2, j );

    }
    gsl_vector_free( vec1 );
    gsl_vector_free( vec2 );
    _Delete3DMatrix( &latticeVec );
    */

    //clock_t t1 = clock();
    for (UInt4 i=0;i<ecm->PutTableSize();i++){
        ElementContainerArray *eca = ecm->PutPointer(i);
        HeaderBase *eca_h = eca->PutHeaderPointer();
        if ((eca_h->CheckKey(UTSUSEMI_KEY_HEAD_DETTYPE)==1)&&(eca_h->PutString(UTSUSEMI_KEY_HEAD_DETTYPE)==UTSUSEMI_KEY_HEAD_DETTYPE_MONITOR)) continue;
        for (UInt4 j=0;j<eca->PutTableSize();j++){
            ElementContainer *ec = eca->PutPointer(j);
            HeaderBase *ec_h = ec->PutHeaderPointer();

            if ( (ec->CheckKey(UTSUSEMI_KEY_QX)==0) || (ec->CheckKey(UTSUSEMI_KEY_QY)==0) || (ec->CheckKey(UTSUSEMI_KEY_QZ)==0) || (ec->CheckKey(UTSUSEMI_KEY_HW)==0) ){
                UtsusemiError(MessageTag+"Projection_QxQyQz > There is no Qx, Qy, Qz information");
                return A;
            }

            std::vector<Double>* Qx = (*ec)(UTSUSEMI_KEY_QX);
            std::vector<Double>* Qy = (*ec)(UTSUSEMI_KEY_QY);
            std::vector<Double>* Qz = (*ec)(UTSUSEMI_KEY_QZ);
            std::vector<Double>* hw = (*ec)(UTSUSEMI_KEY_HW);
            std::vector<Double>* II = (*ec)(UTSUSEMI_KEY_INTENSITY);
            UInt4 size_q = (UInt4)(II->size());
            if ( (Qx->size()!=size_q)||(Qy->size()!=size_q)||(Qz->size()!=size_q) ){
                UtsusemiError(MessageTag+"Projection_QxQyQz > Size of Qx, Qy, Qz is invalid.");
                return A;
            }

            if (ec->CheckKey(UTSUSEMI_KEY_VIEWX)==1) ec->Remove(UTSUSEMI_KEY_VIEWX);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWY)==1) ec->Remove(UTSUSEMI_KEY_VIEWY);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWZ)==1) ec->Remove(UTSUSEMI_KEY_VIEWZ);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWW)==1) ec->Remove(UTSUSEMI_KEY_VIEWW);

            std::vector<Double> tmp;
            ec->Add( UTSUSEMI_KEY_VIEWX,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );
            ec->Add( UTSUSEMI_KEY_VIEWY,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );
            ec->Add( UTSUSEMI_KEY_VIEWZ,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );
            ec->Add( UTSUSEMI_KEY_VIEWW,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );

            std::vector<Double> *Vxi = (*ec)(UTSUSEMI_KEY_VIEWX);
            std::vector<Double> *Vyi = (*ec)(UTSUSEMI_KEY_VIEWY);
            std::vector<Double> *Vzi = (*ec)(UTSUSEMI_KEY_VIEWZ);
            std::vector<Double> *Vwi = (*ec)(UTSUSEMI_KEY_VIEWW);
            Vxi->resize( size_q, 0.0 );
            Vyi->resize( size_q, 0.0 );
            Vzi->resize( size_q, 0.0 );
            Vwi->resize( size_q, 0.0 );

            for (UInt4 k=0;k<size_q;k++){
                Vxi->at(k) = (Qz->at(k))*A[0] + (Qx->at(k))*A[1] + (Qy->at(k))*A[2] + (hw->at(k))*A[3];
                Vyi->at(k) = (Qz->at(k))*A[4] + (Qx->at(k))*A[5] + (Qy->at(k))*A[6] + (hw->at(k))*A[7];
                Vzi->at(k) = (Qz->at(k))*A[8] + (Qx->at(k))*A[9] + (Qy->at(k))*A[10] + (hw->at(k))*A[11];
                Vwi->at(k) = (Qz->at(k))*A[12] + (Qx->at(k))*A[13] + (Qy->at(k))*A[14] + (hw->at(k))*A[15];

            }
        }
    }
    _LastConv3DMatrixElements.clear();
    _LastConv3DMatrixElements.resize(9,0.0);
    for (UInt4 i=0; i<3; i++)
        for (UInt4 j=0; j<3; j++)
            _LastConv3DMatrixElements[i*3+j] = A[i*4+j];

    //std::cout << "Projection time=" << (Double)(clock()-t1) << std::endl;
    return A;
}

//////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
Projection( std::vector<Double> vaxis){
    ElementContainerMatrix* ecm = Put();
    if (isSetXtalParams){
        if (_XP->SetSampleInfoToData( ecm )){
        }
    }
    HeaderBase* hh = ecm->PutHeaderPointer();
    std::vector<Double> latticeConst = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_LATTICECONSTS);
    std::vector<Double> Uvec         = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_UVECT);
    std::vector<Double> Vvec         = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_VVECT);

    std::vector<Double> rotateSteps;
    if (hh->CheckKey(UTSUSEMI_KEY_HEAD_SAMPLE_ROTATESTEPS)==1)
        rotateSteps  = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_ROTATESTEPS);
    if ( (hh->CheckKey("SampleRotatePhi")==1) && (hh->CheckKey(UTSUSEMI_KEY_HEAD_SAMPLE_ROTATESTEPS)==0) ){
        rotateSteps.resize(2);
        rotateSteps[0]=1.0;
        rotateSteps[1]=hh->PutDouble("SampleRotatePhi");
    }
    //std::vector<Double> rotateSteps  = hh->PutDoubleVector(UTSUSEMI_KEY_HEAD_SAMPLE_ROTATESTEPS);

    std::vector<Double> A = MakeProjectionMatrix( latticeConst, Uvec, Vvec, rotateSteps, vaxis );
    /*
    std::vector<gsl_vector*> latticeVec = _MakeEmpty3DMatrix();
    //_CalcReciproLattice( latticeConst, Uvec, Vvec, phi, &latticeVec );
    _CalcReciproLattice2( latticeConst, Uvec, Vvec, rotateSteps, &latticeVec );

    if (isDEBUG){
        std::cout << "l1=" << gsl_vector_get( latticeVec[0],0 ) << "," << gsl_vector_get( latticeVec[0],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[0],2 ) << std::endl;
        std::cout << "l2=" << gsl_vector_get( latticeVec[1],0 ) << "," << gsl_vector_get( latticeVec[1],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[1],2 ) << std::endl;
        std::cout << "l3=" << gsl_vector_get( latticeVec[2],0 ) << "," << gsl_vector_get( latticeVec[2],1 )<<",";
        std::cout << gsl_vector_get( latticeVec[2],2 ) << std::endl;
    }

    // Make Matrix for (Qx,Qy,Qz,hw) -> (Vx,Vy,Vz,Vw)
    std::vector<Double> A(16);
    gsl_vector* vec1 = gsl_vector_alloc(4);
    gsl_vector *vec2 = gsl_vector_alloc(4);
    for (UInt4 i=0;i<4;i++){
        gsl_vector_set_all( vec1, 0.0 );
        gsl_vector_set_all( vec2, 0.0 );
        for (UInt4 j=0;j<4;j++)
            gsl_vector_set( vec1, j, vaxis[i*4+j] );

        _Make4DVect( vec1, latticeVec, vec2 );

        for (UInt4 j=0;j<4;j++)
            A[(i*4+j)] = gsl_vector_get( vec2, j );

    }
    gsl_vector_free( vec1 );
    gsl_vector_free( vec2 );
    _Delete3DMatrix( &latticeVec );
    */

    Double Ei = 0.0;
    Double ki = 0.0;
    if (isDirectGeometry){
        Ei = hh->PutDouble(UTSUSEMI_KEY_HEAD_EI);
        ki = sqrt( UCC->EtoK2( Ei ) );
    }

    //clock_t t1 = clock();
    for (UInt4 i=0;i<ecm->PutTableSize();i++){
        ElementContainerArray *eca = ecm->PutPointer(i);
        HeaderBase *eca_h = eca->PutHeaderPointer();
        if ((eca_h->CheckKey(UTSUSEMI_KEY_HEAD_DETTYPE)==1)&&(eca_h->PutString(UTSUSEMI_KEY_HEAD_DETTYPE)==UTSUSEMI_KEY_HEAD_DETTYPE_MONITOR)) continue;
        for (UInt4 j=0;j<eca->PutTableSize();j++){
            ElementContainer *ec = eca->PutPointer(j);
            HeaderBase *ec_h = ec->PutHeaderPointer();

            Double Ef = 0.0;
            Double kf = 0.0;
            Double PA = 0.0;
            Double AA = 0.0;
            if (isDirectGeometry){
                std::vector<Double> vPA = ec_h->PutDoubleVector(UTSUSEMI_KEY_HEAD_PIXELPOLARANGLES);
                std::vector<Double> vAA = ec_h->PutDoubleVector(UTSUSEMI_KEY_HEAD_PIXELAZIMANGLES);
                PA = vPA[0]*M_PI/180.0;
                AA = vAA[0]*M_PI/180.0;
            }else{
                Ef = ec_h->PutDouble(UTSUSEMI_KEY_HEAD_EF);
                kf = sqrt( UCC->EtoK2( Ef ) );
                PA = ec_h->PutDouble(UTSUSEMI_KEY_HEAD_POLARANGLE);
                AA = ec_h->PutDouble(UTSUSEMI_KEY_HEAD_AZIMANGLE);
            }

            Double px = sin(PA)*cos(AA);
            Double py = sin(PA)*sin(AA);
            Double pz = cos(PA);
            //std::vector<Double> pv = ec_h->PutDoubleVector(UTSUSEMI_KEY_HEAD_PIXELPOSITION);
            //Double L2 = sqrt( pv[0]*pv[0]+pv[1]*pv[1]+pv[2]*pv[2] );
            //Double px = pv[0]/L2;
            //Double py = pv[1]/L2;
            //Double pz = pv[2]/L2;

            std::vector<Double> xx = ec->PutX();
            UInt4 size_xx = (UInt4)(xx.size());

            if (ec->CheckKey(UTSUSEMI_KEY_VIEWX)==1) ec->Remove(UTSUSEMI_KEY_VIEWX);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWY)==1) ec->Remove(UTSUSEMI_KEY_VIEWY);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWZ)==1) ec->Remove(UTSUSEMI_KEY_VIEWZ);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWW)==1) ec->Remove(UTSUSEMI_KEY_VIEWW);

            std::vector<Double> tmp;
            ec->Add( UTSUSEMI_KEY_VIEWX,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );
            ec->Add( UTSUSEMI_KEY_VIEWY,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );
            ec->Add( UTSUSEMI_KEY_VIEWZ,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );
            ec->Add( UTSUSEMI_KEY_VIEWW,tmp,UTSUSEMI_KEY_MOMENTTRANSFER_UNIT );

            std::vector<Double> *Vxi = (*ec)(UTSUSEMI_KEY_VIEWX);
            std::vector<Double> *Vyi = (*ec)(UTSUSEMI_KEY_VIEWY);
            std::vector<Double> *Vzi = (*ec)(UTSUSEMI_KEY_VIEWZ);
            std::vector<Double> *Vwi = (*ec)(UTSUSEMI_KEY_VIEWW);
            Vxi->resize( (size_xx -1), 0.0 );
            Vyi->resize( (size_xx -1), 0.0 );
            Vzi->resize( (size_xx -1), 0.0 );
            Vwi->resize( (size_xx -1), 0.0 );

            for (UInt4 k=0;k<(xx.size()-1);k++){
                Double hw = (xx[k+1]+xx[k])/2.0;

                if (isDirectGeometry){
                    kf = sqrt( UCC->EtoK2( Ei-hw ) );
                }else{
                    ki = sqrt( UCC->EtoK2( Ef+hw ) );
                }

                Double qx = -kf*px;
                Double qy = -kf*py;
                Double qz = ki-kf*pz;

                Vxi->at(k) = qz*A[0] + qx*A[1] + qy*A[2] + hw*A[3];
                Vyi->at(k) = qz*A[4] + qx*A[5] + qy*A[6] + hw*A[7];
                Vzi->at(k) = qz*A[8] + qx*A[9] + qy*A[10] + hw*A[11];
                Vwi->at(k) = qz*A[12] + qx*A[13] + qy*A[14] + hw*A[15];

            }
        }
    }
    _LastConv3DMatrixElements.clear();
    _LastConv3DMatrixElements.resize(9,0.0);
    for (UInt4 i=0; i<3; i++)
        for (UInt4 j=0; j<3; j++)
            _LastConv3DMatrixElements[i*3+j] = A[i*4+j];

    //std::cout << "Projection time=" << (Double)(clock()-t1) << std::endl;
    return A;
}


//////////////////////////////////////////////////////////
void UtsusemiSqeCalc::
_CalcCross3d( gsl_vector* v1, gsl_vector* v2, gsl_vector* ret ){

    Double va = gsl_vector_get( v1, 1 )*gsl_vector_get( v2, 2 ) - gsl_vector_get( v1, 2 )*gsl_vector_get( v2, 1 );
    Double vb = gsl_vector_get( v1, 2 )*gsl_vector_get( v2, 0 ) - gsl_vector_get( v1, 0 )*gsl_vector_get( v2, 2 );
    Double vc = gsl_vector_get( v1, 0 )*gsl_vector_get( v2, 1 ) - gsl_vector_get( v1, 1 )*gsl_vector_get( v2, 0 );

    gsl_vector_set( ret, 0, va );
    gsl_vector_set( ret, 1, vb );
    gsl_vector_set( ret, 2, vc );

}

//////////////////////////////////////////////////////////
Int4 UtsusemiSqeCalc::
Slice( std::vector<Double> Ax1range,std::vector<Double> Ax2range,std::vector<Double> Ax3range,std::vector<Double> Ax4range,std::vector<std::string> AxesType, std::vector<Double> Folding, bool isAverageMode ){

    std::vector< std::vector<Double> > ranges;
    ranges.push_back( Ax1range );
    ranges.push_back( Ax2range );
    ranges.push_back( Ax3range );
    ranges.push_back( Ax4range );

    // Set index for each Axis in "ranges"
    UInt4 indX  = 99;
    UInt4 indY  = 99;
    UInt4 indT1 = 99;
    UInt4 indT2 = 99;

    for (UInt4 i=0;i<AxesType.size();i++){
        if ((AxesType[i]=="X")||(AxesType[i]=="x")){
            indX = i;
        }
        if ((AxesType[i]=="Y")||(AxesType[i]=="y")){
            indY = i;
        }
        if ((AxesType[i]=="T")||(AxesType[i]=="t")){
            if (indT1==99){
                indT1 = i;
            }else{
                indT2 = i;
            }
        }
    }

    // Get ranges of each axis for slicing
    std::vector<Double> xrange = CalcRangeAsBinCenterZero( ranges[indX][0],ranges[indX][1],ranges[indX][2] );
    std::vector<Double> yrange = CalcRangeAsBinCenterZero( ranges[indY][0],ranges[indY][1],ranges[indY][2] );
    Double *T1range = new Double[2];
    T1range[0] = ranges[indT1][0];
    T1range[1] = ranges[indT1][1];
    Double *T2range = new Double[2];
    T2range[0] = ranges[indT2][0];
    T2range[1] = ranges[indT2][1];

    UInt4 n_X = (UInt4)xrange[2];
    UInt4 n_Y = (UInt4)yrange[2];

    // Set Folding parameters
    Double FoldingX = Folding[ indX ];
    Double FoldingY = Folding[ indY ];
    Double FoldingT1 = Folding[ indT1 ];
    Double FoldingT2 = Folding[ indT2 ];
    bool isFoldingFYminusFX = false; // Y=X line
    bool isFoldingFYplusFX = false;  // Y=-X line
    UInt4 indFX=0;
    UInt4 indFY=0;

    if ((Folding.size()==7)&&(Folding[4]>0.0)){
            if (Folding[4]==1){
            isFoldingFYminusFX = true;
        }else if (Folding[4]==2){
            isFoldingFYplusFX = true;
        }else if (Folding[4]==3){
            isFoldingFYminusFX = true;
            isFoldingFYplusFX = true;
        }else{
        }
        indFX = (UInt4)Folding[5];
        indFY = (UInt4)Folding[6];
    }

#ifdef MULTH
    omp_set_num_threads( _NumOfMulTh );
#endif

    // Make and initialize temporal matrix for storing sliced data
    Double ***dArray, ***eArray, ***cArray;
    dArray = new Double**[ _NumOfMulTh ];
    eArray = new Double**[ _NumOfMulTh ];
    cArray = new Double**[ _NumOfMulTh ];
    for (UInt4 i=0;i<_NumOfMulTh;i++){
        dArray[i] = new Double*[ n_X ];
        eArray[i] = new Double*[ n_X ];
        cArray[i] = new Double*[ n_X ];

        for (UInt4 j=0;j<n_X;j++){
            dArray[i][j] = new Double[ n_Y ];
            eArray[i][j] = new Double[ n_Y ];
            cArray[i][j] = new Double[ n_Y ];
        }
    }

    for (UInt4 k=0;k<_NumOfMulTh;k++){
        for (UInt4 i=0;i<n_X;i++){
            for (UInt4 j=0;j<n_Y;j++){
                dArray[k][i][j]=eArray[k][i][j]=cArray[k][i][j]=0.0;
            }
        }
    }

    // Slicing
    ElementContainerMatrix *ecm = Put();

    bool isHistogram = true;
    if ((ecm->PutHeaderPointer()->CheckKey(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==1)&&(ecm->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==0))
        isHistogram = false;

    for (UInt4 i_eca=0;i_eca<ecm->PutSize();i_eca++){
        ElementContainerArray* eca = ecm->PutPointer(i_eca);
        // if ECA is masked, skip it.
        if (eca->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_MASKED)!=0) continue;

        if (isHistogram)
            if ((eca->PutHeaderPointer()->CheckKey(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==1)&&(eca->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==0))
                isHistogram = false;

        for (UInt4 i_ec=0;i_ec<eca->PutSize();i_ec++){
            ElementContainer* ec = eca->PutPointer(i_ec);
            // if EC is masked, skip it.
            if (ec->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_MASKED)!=0) continue;

            if (isHistogram)
                if ((ec->PutHeaderPointer()->CheckKey(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==1)&&(ec->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==0))
                    isHistogram = false;

            std::vector<Double> ii, ee;
            if (isHistogram){
                HistogramBinToPoint* HTP = new HistogramBinToPoint();
                HTP->SetTarget(ec);
                HTP->Convert();
                ii = HTP->PutY();
                ee = HTP->PutE();
                delete HTP;
            }else{
                ii = ec->PutY();
                ee = ec->PutE();
            }

            std::vector<std::vector<Double>*> ax(4);
            ax[0] = (*ec)(UTSUSEMI_KEY_VIEWX);
            ax[1] = (*ec)(UTSUSEMI_KEY_VIEWY);
            ax[2] = (*ec)(UTSUSEMI_KEY_VIEWZ);
            ax[3] = (*ec)(UTSUSEMI_KEY_VIEWW);

#pragma omp parallel for
#if (_OPENMP >= 200805)  // OpenMP 3.0 and later
            for (UInt4 ind=0;ind<ax[0]->size();ind++){
#else
            for (Int4 ind=0;ind<ax[0]->size();ind++){
#endif
                UInt4 multh = omp_get_thread_num();
                if (ee[ind]<0) continue;
                //UInt4 multh = 1;
                // Pick up one point data
                std::vector<Double> AX(4,0.0); // X=AX[indX],Y=AX[indY],T1=AX[indT1],T2=AX[indT2]
                AX[indX] = ax[ indX ]->at(ind);
                AX[indY] = ax[ indY ]->at(ind);
                AX[indT1] = ax[ indT1 ]->at(ind);
                AX[indT2] = ax[ indT2 ]->at(ind);

                // Folding along diagonal line
                if ((isFoldingFYminusFX)&&(AX[indFY]<AX[indFX])){
                    Double tmp = AX[indFX];
                    AX[indFX] = AX[indFY];
                    AX[indFY] = tmp;
                }
                if ((isFoldingFYplusFX)&&(AX[indFY]<(-AX[indFX]))){
                    Double tmp = -AX[indFX];
                    AX[indFX] = -AX[indFY];
                    AX[indFY] = tmp;
                }

                // Folding along each axis
                if (FoldingX==0.0) AX[indX] = fabs(AX[indX]);
                if (FoldingX>0.0) {
                    AX[indX] = fabs(AX[indX]) - (floor(fabs(AX[indX])/(2.0*FoldingX))*(2.0*FoldingX));
                    if (AX[indX]>FoldingX) AX[indX]=(2.0*FoldingX)-AX[indX];
                }
                if (FoldingY==0.0) AX[indY] = fabs(AX[indY]);
                if (FoldingY>0.0) {
                    AX[indY] = fabs(AX[indY]) - (floor(fabs(AX[indY])/(2.0*FoldingY))*(2.0*FoldingY));
                    if (AX[indY]>FoldingY) AX[indY]=(2.0*FoldingY)-AX[indY];
                }
                if (FoldingT1==0.0) AX[indT1] = fabs(AX[indT1]);
                if (FoldingT1>0.0) {
                    AX[indT1] = fabs(AX[indT1]) - (floor(fabs(AX[indT1])/(2.0*FoldingT1))*(2.0*FoldingT1));
                    if (AX[indT1]>FoldingT1) AX[indT1]=(2.0*FoldingT1)-AX[indT1];
                }
                if (FoldingT2==0.0) AX[indT2] = fabs(AX[indT2]);
                if (FoldingT2>0.0) {
                    AX[indT2] = fabs(AX[indT2]) - (floor(fabs(AX[indT2])/(2.0*FoldingT2))*(2.0*FoldingT2));
                    if (AX[indT2]>FoldingT2) AX[indT2]=(2.0*FoldingT2)-AX[indT2];
                }

                // Add a data point to matrix if placed in given range
                if ((T1range[0]<=AX[indT1])&&(AX[indT1]<T1range[1])){
                    if ((T2range[0]<=AX[indT2])&&(AX[indT2]<T2range[1])){
                        if ((xrange[0]<=AX[indX])&&(AX[indX]<=xrange[1])){
                            if ((yrange[0]<=AX[indY])&&(AX[indY]<=yrange[1])){
                              UInt4 i_X = (UInt4)( floor((AX[indX]-xrange[0])/ranges[indX][2]) );
                              UInt4 i_Y = (UInt4)( floor((AX[indY]-yrange[0])/ranges[indY][2]) );
                                if ((i_X<n_X)&&(i_Y<n_Y)){
                                    dArray[multh][i_X][i_Y] += ii[ind];
                                    eArray[multh][i_X][i_Y] += ee[ind]*ee[ind];
                                    cArray[multh][i_X][i_Y] += 1.0;
                                }else{
                                  //std::cout << " Out of range " << i_X <<"/"<< n_X <<  "," << i_Y <<"/"<<n_Y<< std::endl;
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    // Make results from temporal matrix
    datArray.clear();
    errArray.clear();
    XArray.clear();
    YArray.clear();
    delete [] T1range;
    delete [] T2range;
    for (UInt4 i=0;i<(n_X+1);i++){
        XArray.push_back( xrange[0] + ranges[indX][2]*(Double(i)) );
    }
    for (UInt4 i=0;i<(n_Y+1);i++){
        YArray.push_back( yrange[0] + ranges[indY][2]*(Double(i)) );
    }

    for (UInt4 i=0;i<n_X;i++){
        std::vector<Double> datY,datE;
        datY.clear();
        datE.clear();
        for (UInt4 j=0;j<n_Y;j++){
            Double Yav = 0.0;
            Double Eav = 0.0;
            Double Cav = 0.0;
            for (UInt4 k=0;k<_NumOfMulTh;k++){
                Yav += dArray[k][i][j];
                Eav += eArray[k][i][j];
                Cav += cArray[k][i][j];
            }

            if (Cav!=0.){
                if (isAverageMode){
                    datY.push_back( Yav/Cav );
                    datE.push_back( sqrt( Eav )/Cav );
                }else{
                    datY.push_back( Yav );
                    datE.push_back( sqrt( Eav ) );
                }
            }else{
                datY.push_back( MASKVALUE );
                datE.push_back( 0.0 );
            }
        }
        datArray.push_back(datY);
        errArray.push_back(datE);
    }

    // Delete temporal matrix
    for (UInt4 i=0;i<_NumOfMulTh;i++){
        for (UInt4 j=0;j<n_X;j++){
            delete [] dArray[i][j];
            delete [] eArray[i][j];
            delete [] cArray[i][j];
        }
        delete [] dArray[i];
        delete [] eArray[i];
        delete [] cArray[i];
    }
    delete [] dArray;
    delete [] eArray;
    delete [] cArray;

    return 0;
}

//////////////////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
PutDArray(){
    std::vector<Double> ret;
    ret.clear();
    for (UInt4 i=0;i<datArray.size();i++){
        for (UInt4 j=0;j<datArray[i].size();j++){
            ret.push_back( datArray[i][j] );
        }
    }
    return ret;
}

//////////////////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
PutEArray(){
    std::vector<Double> ret;
    ret.clear();
    for (UInt4 i=0;i<errArray.size();i++){
        for (UInt4 j=0;j<errArray[i].size();j++){
            ret.push_back( errArray[i][j] );
        }
    }
    return ret;
}

//////////////////////////////////////////////////////////////////////
std::vector<Double> UtsusemiSqeCalc::
GetAxLimit(){

    Double vx_min,vx_max,vy_min,vy_max,vz_min,vz_max,vw_min,vw_max;
    vx_min = vy_min = vz_min = vw_min = 100000000.0;
    vx_max = vy_max = vz_max = vw_max = -100000000.0;
    Double min_tmp,max_tmp;

    ElementContainerMatrix* ecm = Put();
    if (ecm==NULL){
        std::vector<Double> empty_ret(8,0.0);
        return empty_ret;
    }
    for (UInt4 i=0;i<(ecm->PutTableSize());i++){
        ElementContainerArray *eca = ecm->PutPointer(i);
        for (UInt4 j=0;j<(eca->PutTableSize());j++){
            ElementContainer* ec = eca->PutPointer(j);
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWX)==1){
                min_tmp = ec->Min(UTSUSEMI_KEY_VIEWX);
                max_tmp = ec->Max(UTSUSEMI_KEY_VIEWX);
                if (min_tmp<vx_min) vx_min = min_tmp;
                if (max_tmp>vx_max) vx_max = max_tmp;
            }
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWY)==1){
                min_tmp = ec->Min(UTSUSEMI_KEY_VIEWY);
                max_tmp = ec->Max(UTSUSEMI_KEY_VIEWY);
                if (min_tmp<vy_min) vy_min = min_tmp;
                if (max_tmp>vy_max) vy_max = max_tmp;
            }
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWZ)==1){
                min_tmp = ec->Min(UTSUSEMI_KEY_VIEWZ);
                max_tmp = ec->Max(UTSUSEMI_KEY_VIEWZ);
                if (min_tmp<vz_min) vz_min = min_tmp;
                if (max_tmp>vz_max) vz_max = max_tmp;
            }
            if (ec->CheckKey(UTSUSEMI_KEY_VIEWW)==1){
                min_tmp = ec->Min(UTSUSEMI_KEY_VIEWW);
                max_tmp = ec->Max(UTSUSEMI_KEY_VIEWW);
                if (min_tmp<vw_min) vw_min = min_tmp;
                if (max_tmp>vw_max) vw_max = max_tmp;
            }
        }
    }

    std::vector<Double> ret;
    ret.clear();
    ret.push_back( vx_min );
    ret.push_back( vx_max );
    ret.push_back( vy_min );
    ret.push_back( vy_max );
    ret.push_back( vz_min );
    ret.push_back( vz_max );
    ret.push_back( vw_min );
    ret.push_back( vw_max );

    return ret;
}


Int4 UtsusemiSqeCalc::
ConvertToD4Mat(std::string filepath, std::vector< Double> ax1,std::vector< Double> ax2,
               std::vector< Double> ax3,std::vector< Double> ax4, std::vector<Double> foldings, bool isAverageMode, bool useBinCorr){

    std::string::size_type length = filepath.size();
    std::string::size_type i0 = filepath.rfind(".txt");
    std::string::size_type i1 = filepath.rfind(".vbin");
    bool isBinary = true;
    if ((i0!=std::string::npos) && (i0==(length-4))){
        isBinary = false;
    }else if ((i1!=std::string::npos) && (i1==(length-5))){
        isBinary = true;
    }else{
        std::string msg = MessageTag+"ConvertToD4Mat > Error, file name is invalid."+filepath+"\n";
        msg += "                File name was required .txt or .vbin as extention.\n";
        msg += "                length,i0,i1="+stool->Int4ToString((Int4)length)+","+stool->Int4ToString((Int4)i0)+","+stool->Int4ToString((Int4)i1);
        UtsusemiError( msg );
        return -1;
    }

    std::vector< std::vector<Double> > ArList;
    ArList.clear();
    ArList.push_back( ax1 );  // Ax1range = < min of Ax1, max of Ax1, step of Ax1 >
    ArList.push_back( ax2 );
    ArList.push_back( ax3 );
    ArList.push_back( ax4 );

    std::vector<Int4> ArNum;  // ArNum have the number of histogram box.
    for (UInt4 i=0;i<4;i++){
        std::vector< Double > ret = CalcRangeAsBinCenterZero( ArList[i][0], ArList[i][1], ArList[i][2] );
        ArList[i][0] = ret[0];
        ArList[i][1] = ret[1];
        ArNum.push_back( (Int4)ret[2] );
    }

    std::string msg = MessageTag+"ConvertToD4Mat >> \n";
    for (UInt4 i=0;i<4; i++){
        msg += "X"+stool->UInt4ToString(i)+"="+stool->DoubleToString(ArList[i][0])+","+stool->DoubleToString(ArList[i][1])+","+stool->DoubleToString(ArList[i][2])+","+stool->DoubleToString(ArNum[i])+"\n";
    }
    UtsusemiMessage( msg );

    FILE* fp=NULL;
    std::ofstream fo;
    if (isBinary){
        if ((fp=fopen(filepath.c_str(),"wb"))==NULL){
            UtsusemiError(MessageTag+"ConvertToD4Mat >> Cannot open file="+filepath);
            return -1;
        }
    }else{
        fo.open( filepath.c_str() );
        if (!fo){
            UtsusemiError(MessageTag+"ConvertToD4Mat >> Error, cannot open file."+filepath);
            return -1;
        }
    }


    // save memory under about 400MB
    UInt4 partStep = (UInt4)( (400.0*1024.0*1024.0/8.0)/(ArNum[0]*ArNum[1]*ArNum[2]) );
    if (partStep==0) partStep = 1;
    UtsusemiMessage(MessageTag+"ConvertToD4Mat > Step ="+stool->UInt4ToString(partStep));

    ElementContainerMatrix *ecm = Put();

    std::vector<float> cAx1,cAx2,cAx3,cAx4,cII,cEE;
    cAx1.clear();
    cAx2.clear();
    cAx3.clear();
    cAx4.clear();
    cII.clear();
    cEE.clear();
    HistogramBinToPoint hbt;
    for (Int4 pind=0;pind<ArNum[3];pind+=partStep){

        Double partMin=ArList[3][0]+(Double)pind * ArList[3][2];
        if (partMin>ArList[3][1]) break;
        Double partMax=partMin+ArList[3][2]*(Double)partStep;
        if (partMax>ArList[3][1]) partMax = ArList[3][1];
        UtsusemiMessage(MessageTag+"ConvertToD4Mat > hw ="+stool->DoubleToString(partMin)+"-"+stool->DoubleToString(partMax));

        UInt4 total_num = ArNum[0]*ArNum[1]*ArNum[2]*partStep;
        std::vector<Int4> *matrixP = new std::vector<Int4>( total_num, -1 );

        std::vector<float> d_cnt;
        d_cnt.clear();
        Int4 cnt = -1;

        for (UInt4 psd=0;psd<ecm->PutTableSize();psd++){
            ElementContainerArray *eca = ecm->PutPointer(psd);
            if (eca->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_MASKED)!=0) continue;
            for (UInt4 pixel=0;pixel<eca->PutTableSize();pixel++){
                ElementContainer *ec = eca->PutPointer(pixel);
                if (ec->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_MASKED)!=0) continue;

                std::vector<Double> intensity;
                std::vector<Double> err;
                if (useBinCorr){
                    hbt.SetTarget( ec );
                    hbt.Convert();
                    intensity = hbt.PutY();
                    err = hbt.PutE();
                }else{
                    intensity = ec->PutY();
                    err = ec->PutE();
                }
                //std::vector<Double> intensity = ec->PutY();
                //std::vector<Double> err = ec->PutE();
                std::vector< std::vector<Double> > Vax;
                Vax.clear();
                Vax.push_back( ec->Put(UTSUSEMI_KEY_VIEWX) );
                Vax.push_back( ec->Put(UTSUSEMI_KEY_VIEWY) );
                Vax.push_back( ec->Put(UTSUSEMI_KEY_VIEWZ) );
                Vax.push_back( ec->Put(UTSUSEMI_KEY_VIEWW) );
                for (UInt4 i=0;i<4;i++){
                    if (foldings[i]==1){
                        for (UInt4 j=0;j<Vax[i].size();j++){
                            Vax[i][j]=fabs(Vax[i][j]);
                        }
                    }
                }
                for (UInt4 i=0;i<intensity.size();i++){
                    if (err[i]<0.0) continue;
                    if (( Vax[0][i]>=ArList[0][0])&&( Vax[0][i]<=ArList[0][1] )) {
                        if (( Vax[1][i]>=ArList[1][0])&&( Vax[1][i]<=ArList[1][1] )) {
                            if (( Vax[2][i]>=ArList[2][0])&&( Vax[2][i]<=ArList[2][1] )) {
                                if (( Vax[3][i]>=partMin)&&( Vax[3][i]<partMax )) {
                                    UInt4 ix1 = (UInt4)( (Vax[0][i]-ArList[0][0])/ArList[0][2] );
                                    UInt4 ix2 = (UInt4)( (Vax[1][i]-ArList[1][0])/ArList[1][2] );
                                    UInt4 ix3 = (UInt4)( (Vax[2][i]-ArList[2][0])/ArList[2][2] );
                                    //UInt4 ix4 = (UInt4)( (Vax[3][i]-ArList[3][0])/ArList[3][2] );
                                    UInt4 ix4p = (UInt4)( (Vax[3][i]-partMin)/ArList[3][2] );

                                    UInt4 iii = ix4p + partStep*( ix3 + ArNum[2]*( ix2 + ArNum[1]*ix1 ) );
                                    if (matrixP->at(iii)==-1){
                                        cAx1.push_back( (float)(ArList[0][0] + ((float)(ix1)+0.5)*ArList[0][2] ));
                                        cAx2.push_back( (float)(ArList[1][0] + ((float)(ix2)+0.5)*ArList[1][2] ));
                                        cAx3.push_back( (float)(ArList[2][0] + ((float)(ix3)+0.5)*ArList[2][2] ));
                                        //cAx4.push_back( (float)(ArList[3][0] + ((float)(ix4)+0.5)*ArList[3][2] ));
                                        cAx4.push_back( (float)(partMin + ((float)(ix4p)+0.5)*ArList[3][2] ));

                                        cII.push_back((float)intensity[i]);
                                        cEE.push_back((float)(err[i]*err[i]));
                                        d_cnt.push_back(1.0);
                                        cnt++;
                                        matrixP->at(iii)=cnt;

                                    }else{
                                        Int4 ind = matrixP->at(iii);
                                        cII[ ind ] += (float)intensity[i];
                                        cEE[ ind ] += (float)(err[i]*err[i]);
                                        d_cnt[ ind ] += 1.0;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }

        for (UInt4 i=0;i<d_cnt.size();i++){
            if (isAverageMode){
                cII[i] = cII[i]/d_cnt[i];
                cEE[i] = sqrt( cEE[i] )/d_cnt[i];
            }else{
                cEE[i] = sqrt( cEE[i] );
            }
        }
        if (isBinary){
            float data[6];
            for (UInt4 i=0;i<cII.size();i++){
                data[0] = cAx1[i];
                data[1] = cAx2[i];
                data[2] = cAx3[i];
                data[3] = cAx4[i];
                data[4] = cII[i];
                data[5] = cEE[i];
                std::fwrite( data, sizeof(data), 1, fp );
            }
        }else{
            for (UInt4 i=0;i<cII.size();i++){
                fo << cAx1[i] << "," << cAx2[i] << "," << cAx3[i] << ",";
                fo << cAx4[i] << "," << cII[i] << "," << cEE[i] << std::endl;
            }
        }
        cAx1.clear();
        cAx2.clear();
        cAx3.clear();
        cAx4.clear();
        cII.clear();
        cEE.clear();
        delete matrixP;
    }

    if (isBinary){
        fclose( fp );
    }else{
        fo.close();
    }

    UtsusemiMessage(MessageTag+"ConvertToD4Mat > complete.");
    return 0;
}

//[inamura 161114]-->
Int4 UtsusemiSqeCalc::
ConvertToD4Mat(std::string filepath, bool isAverageMode, bool useBinCorr){
    if (isSetXtalParams){
        return ConvertToD4Mat( filepath,
                               _XP->PutAxRange(0,true),
                               _XP->PutAxRange(1,true),
                               _XP->PutAxRange(2,true),
                               _XP->PutAxRange(3,true),
                               _XP->PutFolding(),
                               isAverageMode, useBinCorr );
    }
    std::string::size_type length = filepath.size();
    std::string::size_type i0 = filepath.rfind(".txt");
    std::string::size_type i1 = filepath.rfind(".vbin");
    bool isBinary = true;
    if ((i0!=std::string::npos) && (i0==(length-4))){
        isBinary = false;
    }else if ((i1!=std::string::npos) && (i1==(length-5))){
        isBinary = true;
    }else{
        std::string msg = MessageTag+"ConvertToD4Mat > Error, file name is invalid."+filepath+"\n";
        msg += "                File name was required .txt or .vbin as extention.\n";
        msg += "                length,i0,i1="+stool->Int4ToString((Int4)length)+","+stool->Int4ToString((Int4)i0)+","+stool->Int4ToString((Int4)i1);
        UtsusemiError( msg );
        return -1;
    }

    FILE* fp=NULL;
    std::ofstream fo;
    if (isBinary){
        if ((fp=fopen(filepath.c_str(),"wb"))==NULL){
            UtsusemiError(MessageTag+"ConvertToD4Mat > Cannot open file="+filepath);
            return -1;
        }
    }else{
        fo.open( filepath.c_str() );
        if (!fo){
            UtsusemiError(MessageTag+"ConvertToD4Mat > Cannot open file."+filepath);
            return -1;
        }
    }

    ElementContainerMatrix *ecm = Put();

    HistogramBinToPoint hbt;

    for (UInt4 psd=0;psd<ecm->PutTableSize();psd++){
        ElementContainerArray *eca = ecm->PutPointer(psd);
        if (eca->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_MASKED)!=0) continue;
        for (UInt4 pixel=0;pixel<eca->PutTableSize();pixel++){
            ElementContainer *ec = eca->PutPointer(pixel);
            if (ec->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_MASKED)!=0) continue;

            std::vector<Double> intensity;
            std::vector<Double> err;
            if (useBinCorr){
                hbt.SetTarget( ec );
                hbt.Convert();
                intensity = hbt.PutY();
                err = hbt.PutE();
            }else{
                intensity = ec->PutY();
                err = ec->PutE();
            }

            std::vector<Double>* Vx = (*ec)(UTSUSEMI_KEY_VIEWX);
            std::vector<Double>* Vy = (*ec)(UTSUSEMI_KEY_VIEWY);
            std::vector<Double>* Vz = (*ec)(UTSUSEMI_KEY_VIEWZ);
            std::vector<Double>* Vw = (*ec)(UTSUSEMI_KEY_VIEWW);

            if (isBinary){
                float data[6];
                for (UInt4 i=0;i<intensity.size();i++){
                    if (err[i]<0.0) continue;
                    data[0] = (float)(Vx->at(i));
                    data[1] = (float)(Vy->at(i));
                    data[2] = (float)(Vz->at(i));
                    data[3] = (float)(Vw->at(i));
                    data[4] = (float)(intensity[i]);
                    data[5] = (float)(err[i]);
                    std::fwrite( data, sizeof(data), 1, fp );
                }
            }else{
                for (UInt4 i=0;i<intensity.size();i++){
                    if (err[i]<0.0) continue;
                    fo << Vx->at(i) << "," << Vy->at(i) << "," << Vz->at(i) << ",";
                    fo << Vw->at(i) << "," << intensity[i] << "," << err[i] << std::endl;
                }
            }
        }
    }

    if (isBinary){
        fclose( fp );
    }else{
        fo.close();
    }

    UtsusemiMessage(MessageTag+"ConvertToD4Mat >> complete.");
    return 0;
}
//<--[inamura 161114]

std::vector<Double> UtsusemiSqeCalc::
QConvFromReciprocalToRealSpace( std::vector<Double> Qvec, std::vector<Double> LC, std::vector<Double> Uv, std::vector<Double> Vv, std::vector<Double> RS, std::vector<Double> VA ){
    std::vector<Double> ret;
    if (Qvec.size()<3) return ret;
    if ((Qvec.size()%3)!=0) return ret;
    if (LC.size()<6) return ret;
    if (Uv.size()<3) return ret;
    if (Vv.size()<3) return ret;

    // Make Conversion Matrix 'U' from (Qx,Qy,Qz) -> (Vx,Vy,Vz)
    std::vector<gsl_vector*> M = _MakeEmpty3DMatrix();
    _CalcReciproLattice2( LC, Uv, Vv, RS, &M );

    std::vector<gsl_vector*> U = _MakeEmpty3DMatrix();
    gsl_vector *vec1 = gsl_vector_alloc(3);
    gsl_vector* vec2 = gsl_vector_alloc(3);

    for (UInt4 i=0;i<3;i++){
        gsl_vector_set_all( vec2, 0.0 );
        for (UInt4 j=0;j<3;j++){
            gsl_vector_set_all( vec1, 0.0 );
            gsl_vector_memcpy( vec1, M[j] );
            gsl_blas_dscal( VA[i*4+j], vec1 );
            gsl_vector_add( vec2, vec1 );
        }

        Double vecdot;
        gsl_blas_ddot( vec2, vec2, &vecdot );
        gsl_blas_dscal((1.0/vecdot), vec2 );

        gsl_vector_memcpy( U[i], vec2 );
    }
    gsl_vector_free( vec1 );
    gsl_vector_free( vec2 );


    // Make Reversed Matrix 'C' from 'U'
    std::vector<gsl_vector*> C = _MakeEmpty3DMatrix();
    _Calc3DReversedMatrix( &U, &C );

    // Use 'C' matrix on (Vx,Vy,Vz) to get (Qx,Qy,Qz)
    std::vector<Double> A;
    A.clear();
    for (UInt4 i=0;i<3;i++){
        for (UInt4 j=0;j<3;j++){
            A.push_back( gsl_vector_get( C[i], j ) );
        }
    }

    ret.resize(Qvec.size(), 0.0);
    for (UInt4 i=0; i<(UInt4)(Qvec.size()/3); i++){
        UInt4 x = i*3;
        UInt4 y = i*3+1;
        UInt4 z = i*3+2;
        ret[z] = Qvec[x]*A[0] + Qvec[y]*A[1] + Qvec[z]*A[2];
        ret[x] = Qvec[x]*A[3] + Qvec[y]*A[4] + Qvec[z]*A[5];
        ret[y] = Qvec[x]*A[6] + Qvec[y]*A[7] + Qvec[z]*A[8];
    }

    _Delete3DMatrix( &M );
    _Delete3DMatrix( &U );
    _Delete3DMatrix( &C );

    return ret;
}

void UtsusemiSqeCalc::Test(){
    // Dummy Data of Qx,Qy,Qz in instrument system
    std::vector<Double> Qx(3);
    std::vector<Double> Qy(3);
    std::vector<Double> Qz(3);
    std::vector<Double> hw(3);
    std::vector<Double> ii(3);
    std::vector<Double> ee(3);

    Qx[0] = 1.11;
    Qx[1] = 2.22;
    Qx[2] = 3.33;
    Qy[0] = 0.52;
    Qy[1] = 1.04;
    Qy[2] = 1.56;
    Qz[0] = -3.4;
    Qz[1] = -1.7;
    Qz[2] = 0.0;
    hw[0] = 10.05;
    hw[1] = 12.11;
    hw[2] = 13.22;
    ii[0] = 100.0;
    ii[1] = 101.0;
    ii[2] = 102.0;
    ee[0] = 1.0;
    ee[1] = 1.0;
    ee[2] = 1.0;

    ElementContainer ec;
    ec.Add( UTSUSEMI_KEY_QX, Qx );
    ec.Add( UTSUSEMI_KEY_QY, Qy );
    ec.Add( UTSUSEMI_KEY_QZ, Qz );
    ec.Add( UTSUSEMI_KEY_HW, hw );
    ec.Add( UTSUSEMI_KEY_INTENSITY, ii );
    ec.Add( UTSUSEMI_KEY_ERROR, ee );
    ec.SetKeys(UTSUSEMI_KEY_HW,UTSUSEMI_KEY_INTENSITY,UTSUSEMI_KEY_ERROR);
    HeaderBase* hh_ec = ec.PutHeaderPointer();
    hh_ec->Add(UTSUSEMI_KEY_HEAD_MASKED, 0);

    ElementContainerArray eca;
    eca.Add(ec);
    HeaderBase* hh_eca =eca.PutHeaderPointer();
    hh_eca->Add(UTSUSEMI_KEY_HEAD_MASKED, 0);

    ElementContainerMatrix ecm;
    ecm.Add(eca);

    HeaderBase* hh_ecm = ecm.PutHeaderPointer();
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_EI,10.0);
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_MASKED, 0);
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_ISHISTOGRAM, 0 );

    // Make Sample Lattice parameters
    std::vector<Double> LC(6);
    LC[0] = 4.8;
    LC[1] = 8.4;
    LC[2] = 2.91;
    LC[3] = 90.0;
    LC[4] = 90.0;
    LC[5] = 90.0;

    // Make Sample Orientation parameters
    std::vector<Double> Uv(6),Vv(6);
    Uv[0] = 0.0;
    Uv[1] = 1.0;
    Uv[2] = 0.0;
    Vv[0] = 0.0;
    Vv[1] = 0.0;
    Vv[2] = 1.0;

    std::vector<Double> RS;  // empty.

    // Set Parameters in Header
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_SAMPLE_LATTICECONSTS,LC);
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_SAMPLE_UVECT,Uv);
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_SAMPLE_VVECT,Vv);
    hh_ecm->Add(UTSUSEMI_KEY_HEAD_SAMPLE_ROTATESTEPS,RS);

    // Set Projection Parameters
    std::vector<Double> ViewAx(16);
    ViewAx[0] = 1.;
    ViewAx[1] = 0.;
    ViewAx[2] = 0.;
    ViewAx[3] = 0.;

    ViewAx[4] = 0.;
    ViewAx[5] = 1.;
    ViewAx[6] = 0.;
    ViewAx[7] = 0.;

    ViewAx[8] = 0.;
    ViewAx[9] = 0.;
    ViewAx[10] = 1.;
    ViewAx[11] = 0.;

    ViewAx[12] = 0.;
    ViewAx[13] = 0.;
    ViewAx[14] = 0.;
    ViewAx[15] = 1.;


    // Do Projection to View space : Calculation of Vx,Vy,Vz,Vw
    // Conversion from (Qx,Qy,Qz,hw) in instrument space
    //              to (Vx,Vy,Vz,Vw) in reciprocal lattice space
    SetTarget( &ecm );
    Projection_QxQyQz( ViewAx );

    // Check converted axes in reciprocal lattice space
    ElementContainer ec_ret = ecm.Put(0).Put(0);
    std::vector<Double> Vx = ec_ret.Put(UTSUSEMI_KEY_VIEWX);
    std::vector<Double> Vy = ec_ret.Put(UTSUSEMI_KEY_VIEWY);
    std::vector<Double> Vz = ec_ret.Put(UTSUSEMI_KEY_VIEWZ);
    std::vector<Double> Vw = ec_ret.Put(UTSUSEMI_KEY_VIEWW);

    for (UInt4 i=0; i<Vx.size(); i++){
        std::cout << "["<< Qx[i] << "," << Qy[i] << "," << Qz[i] << "," << hw[i] << "]  ->  [";
        std::cout << Vx[i] << "," << Vy[i] << "," << Vz[i] << "," << Vw[i] << "]" << std::endl;
    }

    //////////////////////////////////////////////////////////////
    // Make Conversion Matrix from (Vx,Vy,Vz) to (Qx,Qy,Qz)
    //////////////////////////////////////////////////////////////
    std::vector<Double> Qvec(3);
    for (UInt4 i=0; i<Vx.size(); i++){
        Qvec[0] = Vx[i];
        Qvec[1] = Vy[i];
        Qvec[2] = Vz[i];
        std::vector<Double> iQvec = QConvFromReciprocalToRealSpace( Qvec, LC, Uv, Vv, RS, ViewAx );
        std::cout << "["<< Qx[i] << "," << Qy[i] << "," << Qz[i] << "]  ->  [";
        std::cout << Vx[i] << "," << Vy[i] << "," << Vz[i] << "]  ->  [";
        std::cout << iQvec[0] << "," << iQvec[1] << "," << iQvec[2] << "]   ";
        if ((Qx[i]-iQvec[0])<0.0000000001)
            std::cout << " X is OK /  ";
        else
            std::cout << " X is Bad Results! /  " << std::endl;
        if ((Qy[i]-iQvec[1])<0.0000000001)
            std::cout << " Y is OK /  ";
        else
            std::cout << " Y is Bad Results! /  " << std::endl;
        if ((Qz[i]-iQvec[2])<0.0000000001)
            std::cout << " Z is OK /  ";
        else
            std::cout << " Z is Bad Results! /  " << std::endl;
        std::cout << std::endl;
    }
    /*
    // Make Conversion Matrix 'U' from (Qx,Qy,Qz) -> (Vx,Vy,Vz)
    std::vector<gsl_vector*> M = _MakeEmpty3DMatrix();
    _CalcReciproLattice2( LC, Uv, Vv, RS, &M );

    std::vector<gsl_vector*> U = _MakeEmpty3DMatrix();
    gsl_vector *tmp = gsl_vector_alloc(3);
    gsl_vector* ret = gsl_vector_alloc(3);

    for (UInt4 i=0;i<3;i++){
        gsl_vector_set_all( ret, 0.0 );
        for (UInt4 j=0;j<3;j++){
            gsl_vector_set_all( tmp, 0.0 );
            gsl_vector_memcpy( tmp, M[j] );
            gsl_blas_dscal( ViewAx[i*4+j], tmp );
            gsl_vector_add( ret, tmp );
        }

        Double vecdot;
        gsl_blas_ddot( ret, ret, &vecdot );
        gsl_blas_dscal((1.0/vecdot), ret );

        gsl_vector_memcpy( U[i], ret );
    }
    gsl_vector_free( ret );
    gsl_vector_free( tmp );


    // Make Reversed Matrix 'C' from 'U'
    std::vector<gsl_vector*> C = _MakeEmpty3DMatrix();
    _Calc3DReversedMatrix( &U, &C );

    // Use 'C' matrix on (Vx,Vy,Vz) to get (Qx,Qy,Qz)
    std::vector<Double> A;
    A.clear();
    for (UInt4 i=0;i<3;i++){
        for (UInt4 j=0;j<3;j++){
            A.push_back( gsl_vector_get( C[i], j ) );
        }
    }

    std::vector<Double> iQx(3);
    std::vector<Double> iQy(3);
    std::vector<Double> iQz(3);

    for (UInt4 i=0; i<Vx.size(); i++){
        iQz[i] = Vx[i]*A[0] + Vy[i]*A[1] + Vz[i]*A[2];
        iQx[i] = Vx[i]*A[3] + Vy[i]*A[4] + Vz[i]*A[5];
               iQy[i] = Vx[i]*A[6] + Vy[i]*A[7] + Vz[i]*A[8];
    }
    _Delete3DMatrix( &M );
    _Delete3DMatrix( &U );
    _Delete3DMatrix( &C );


    // Check Results
    for (UInt4 i=0; i<Vx.size(); i++){
        std::cout << "["<< Qx[i] << "," << Qy[i] << "," << Qz[i] << "]  ->  [";
        std::cout << Vx[i] << "," << Vy[i] << "," << Vz[i] << "]  ->  [";
        std::cout << iQx[i] << "," << iQy[i] << "," << iQz[i] << "]   ";
        if ((Qx[i]-iQx[i])<0.0000000001)
            std::cout << " X is OK /  ";
        else
            std::cout << " X is Bad Results! /  " << std::endl;
        if ((Qy[i]-iQy[i])<0.0000000001)
            std::cout << " Y is OK /  ";
        else
            std::cout << " Y is Bad Results! /  " << std::endl;
        if ((Qz[i]-iQz[i])<0.0000000001)
            std::cout << " Z is OK /  ";
        else
            std::cout << " Z is Bad Results! /  " << std::endl;
        std::cout << std::endl;
    }
    */

}

bool UtsusemiSqeCalc::
SetSlicedElementContainerArray( ElementContainerArray* _eca, std::string _bin_key, std::string _xval_key, std::string _xrange_key ){
    //std::string _bin_key="X", std::string _xval_key="XVAL", _xrange_key=UTSUSEMI_KEY_HEAD_XRANGE
    UInt4 n_X=(UInt4)(XArray.size())-1;

    ElementContainerMatrix *ecm = Put();
    _eca->InputHeader( ecm->PutHeader() );
    HeaderBase* hh = _eca->PutHeaderPointer();
    if (hh->CheckKey(UTSUSEMI_KEY_HEAD_ISHISTOGRAM)==1)
        hh->OverWrite(UTSUSEMI_KEY_HEAD_ISHISTOGRAM,0);
    else
        hh->Add(UTSUSEMI_KEY_HEAD_ISHISTOGRAM,0);
    _eca->Allocate(n_X);
    for (UInt4 i=0;i<n_X;i++){
        ElementContainer* ec = new ElementContainer();
        ec->Add(_bin_key,YArray);
        ec->Add(UTSUSEMI_KEY_INTENSITY, datArray[i]);
        ec->Add(UTSUSEMI_KEY_ERROR, errArray[i]);
        ec->SetKeys(_bin_key,UTSUSEMI_KEY_INTENSITY,UTSUSEMI_KEY_ERROR);

        std::vector<Double> xrange(2);
        xrange[0] = XArray[i];
        xrange[1] = XArray[i+1];
        ec->AddToHeader( _xrange_key, xrange );
        ec->AddToHeader( _xval_key, (xrange[0]+xrange[1])/2.0 );
        _eca->Set(i,ec);
    }
    return true;
}
bool UtsusemiSqeCalc::
LoadXtalParam( std::string file ){
    isSetXtalParams = _XP->LoadFile( file );
    return isSetXtalParams;
}
bool UtsusemiSqeCalc::
SaveXtalParam( std::string file ){
    return _XP->SaveFile( file );
}
bool UtsusemiSqeCalc::
SetLatticeConstants( PyObject* LC ){
    isSetXtalParams = _XP->SetLatticeConstants( LC );
    return isSetXtalParams;
}
bool UtsusemiSqeCalc::
SetUVvector( PyObject* UV, PyObject* VV ){
    isSetXtalParams = _XP->SetUVvector( UV, VV );
    return isSetXtalParams;
}
bool UtsusemiSqeCalc::
SetRotationSteps( std::string steps ){
    isSetXtalParams = _XP->SetRotationSteps( steps );
    return isSetXtalParams;
}
bool UtsusemiSqeCalc::
Projection( PyObject* arg_ax1, PyObject* arg_ax2, PyObject* arg_ax3, PyObject* arg_ax4 ){
    if (_XP->SetProjectionAxes( arg_ax1, arg_ax2, arg_ax3, arg_ax4 )){
    }else{
        return false;
    }
    return Projection();
}
bool UtsusemiSqeCalc::
Projection(){
    std::vector<Double> A =Projection( _XP->PutViewAxes() );
    return true;
}
bool  UtsusemiSqeCalc::
Slice( PyObject* Ax1, PyObject* Ax2, PyObject* Ax3, PyObject* Ax4, PyObject* DiagFolding, bool isAverageMode ){
    if (!(_XP->SetSliceAxes( Ax1, Ax2, Ax3, Ax4 )))
        return false;
    if (!(_XP->SetDiagFolding(  DiagFolding )))
        return false;
    return Slice(isAverageMode);
}
bool  UtsusemiSqeCalc::
Slice(bool isAverageMode){
    std::vector<Double> ax1range = _XP->PutAxRange(0);
    std::vector<Double> ax2range = _XP->PutAxRange(1);
    std::vector<Double> ax3range = _XP->PutAxRange(2);
    std::vector<Double> ax4range = _XP->PutAxRange(3);
    std::vector<std::string> axType = _XP->PutAxType();
    std::vector<Double> Folding = _XP->PutFolding();
    std::vector<UInt4> DFold = _XP->PutDiagFolding();
    for (UInt4 i=0; i<DFold.size(); i++) Folding.push_back( Double(DFold[i]) );
    return Slice( ax1range,ax2range,ax3range,ax4range,axType, Folding, isAverageMode );
}
bool  UtsusemiSqeCalc::
_ConvPyListToDoubleVect( PyObject *ax, std::vector<double> *ret, UInt4 s ){
    UInt4 size = (UInt4) PyList_Size( ax );
    for (UInt4 i=s; i<size; i++){
        if( PyFloat_CheckExact( PyList_GetItem( ax, i ) ) ){
            ret->push_back( PyFloat_AsDouble( PyList_GetItem( ax, i ) ) );
        }
        else if( PyLong_CheckExact( PyList_GetItem( ax, i ) ) ){
            ret->push_back( ( Double )( PyLong_AsLong( PyList_GetItem( ax, i ) ) ) );
        }
#ifdef IS_PY3
#else
        else if( PyInt_CheckExact( PyList_GetItem( ax, i ) ) ){
            ret->push_back( ( Double )( PyInt_AsLong( PyList_GetItem( ax, i ) ) ) );
        }
#endif
        else{
            return false;
        }
    }
    return true;
}
