#include<iostream>
#include<fstream>
#include<cstdlib>
#include<iomanip>
#include<unistd.h>
#include<cstdio>
#include<string.h>
#include<sstream>
#include<math.h>
#include<complex>
#include<stdlib.h>     /* srand, rand */
#include<time.h>
#include<iomanip>
#include<ctime>

using namespace std;

void transformDtoC(double* array, double a, double z, double angle){
// [x] =[a      a*cos(gamma)] [u]
// [y] =[0      a*sin(gamma)] [v]
  double* dummy;        dummy = new double[3];
  dummy[0] = a*array[0] + a*cos(angle*3.14159265/180)*array[1];
  dummy[1] = 0*array[0] + a*sin(angle*3.14159265/180)*array[1];
  dummy[2] = array[2]*z;
  for( int i=0; i<3; i++) array[i] = dummy[i];
}
void transformCtoD(double* array, double a, double z, double angle){
// [u] =[1/a    -cos(gamma)/a*sin(gamma)][x]
// [v] =[0      1/(a*sin(gamma)         ][y]
  double* dummy;        dummy = new double[3];
  dummy[0] = ((1./a)*array[0]) -(cos(angle*3.14159265/180)/(a*sin(angle*3.14159265/180)))*array[1];
  dummy[1] = 0*array[0] + (1./(a*sin(angle*3.14159265/180)))*array[1];
  dummy[2] = array[2]/z;
  for( int i=0; i<3; i++) array[i] = dummy[i];
}
void transformMtoC(double* pos_pol, double* pos_cart){
  
  //HERE 
  double sin_t = sin(pos_pol[4]*3.14159265/180);
  double cos_t = cos(pos_pol[4]*3.14159265/180);
  double sin_p = sin(pos_pol[5]*3.14159265/180);
  double cos_p = cos(pos_pol[5]*3.14159265/180);

  double fac1 = 0.5;//MASS_B/(MASS_A+MASS_B);
  double fac2 = 0.5;//MASS_A/(MASS_A+MASS_B);

//  double fac1 = 0.97235024; double fac2 = 0.027649686;

  //poscart 1 - 3 is for H1-ATOM
  //poscart 4 - 6 is for H2-ATOM
  pos_cart[0] = pos_pol[0] + fac1*pos_pol[3]   * sin_t*cos_p;
  pos_cart[3] = pos_pol[0] - fac2*pos_pol[3]   * sin_t*cos_p;
  pos_cart[1] = pos_pol[1] + fac1*pos_pol[3]   * sin_t*sin_p;
  pos_cart[4] = pos_pol[1] - fac2*pos_pol[3]   * sin_t*sin_p;
  pos_cart[2] = pos_pol[2] + fac1*pos_pol[3]   * cos_t;
  pos_cart[5] = pos_pol[2] - fac2*pos_pol[3]   * cos_t;

  //if(pos_cart[2] < -3.627185) pos_cart[2] = -3.627185;
  //if(pos_cart[5] < -3.627185) pos_cart[5] = -3.627185; 

  //cout << "CART ";
  //for(int i = 0; i < 6; i++) cout << pos_cart[i] << " ";
  //cout << "    \n\n";

};
void transformCtoM(double* pos_pol, double* pos_cart){
   //THIS ROUTINE CONVERTS FROM CARTESIAN COORDINATES
   //TO MOLECULAR COORDINATES. 
   double x1 = pos_cart[0]; double x2 = pos_cart[3];
   double y1 = pos_cart[1]; double y2 = pos_cart[4];
   double z1 = pos_cart[2]; double z2 = pos_cart[5];
 
   //double fac1 = MASS_A/(MASS_A+MASS_B); double fac2 = MASS_B/(MASS_A+MASS_B);
   double fac1 = 0.5;	double fac2 = 0.5;

   double dx = x1 - x2; double dy = y1 - y2; double dz = z1 - z2;
 
   double X = x1*fac1 + x2*fac2;
   double Y = y1*fac1 + y2*fac2;
   double Z = z1*fac1 + z2*fac2;
   double r = sqrt(dx*dx + dy*dy + dz*dz);
   //CALCULATED THETA AND PHI IN THE DEFINITION OF THE SURFACE NORMAL
   double cos_theta = (z1-z2)/r;
   double theta = acos(cos_theta);
   double sin_phi = 0.; double cos_phi = 1.;
   double phi = 0.;
   //sin_theta = sqrt(1-cos(theta)*cos(theta)) because theta [0,pi]
   double sin_theta = sqrt(1. - cos_theta*cos_theta);
   double rfi = r*sin_theta;
   if(sin_theta > 1e-10){
     sin_phi = (y1-y2)/rfi;
     cos_phi = (x1-x2)/rfi;
     if(fabs(cos_phi) > 1.0) cos_phi = (int)cos_phi; // to avoid numerical issues
     phi = acos(cos_phi);
     if(sin_phi < 0.) phi = 2.*M_PI - phi;
   }
  //  set_int_pol_coord(tprime, pprime);
   //calulate phi
   pos_pol[0] = X     ;
   pos_pol[1] = Y     ;
   pos_pol[2] = Z     ;
   pos_pol[3] = r     ;
   pos_pol[4] = theta/(3.14159265/180);
   pos_pol[5] = phi/(3.14159265/180);
 
};

int main(int argc, char* argv[]){
/* program for coordinate transformations                                     */

  double a3D;
  double angle;
  double Z;

  double* C1;	C1 = new double[3];
//  double* C2;     C2 = new double[3];

  //array to hold molecular and cartesian coordinates
  double* pos_mol;	pos_mol = new double[6];
  double* pos_cart;	pos_cart = new double[6];

  if ( argc == 1 ){
    cout << "Coordinate transform program \n"
         << "          N.B.: Angle refers to the angle between the u (x) axis and the v axis\n"
         << " 5 modes: --> DtoC [a3D] [angle (deg)] [Z unitcell] [u] [v] [z]                    out: [X]  [Y]  [Z]\n"
         << "          --> CtoD [a3D] [angle (deg)] [Z unitcell] [X] [Y] [Z]                    out: [u]  [v]  [z/(Z unitcell)]\n"
         << "          --> MOLtoCART [X] [Y] [Z] [r] [theta (deg)] [phi (deg)]                  out: [X1] [Y1] [Z1] [X2] [Y2] [Z2]\n"
         << "          --> CARTtoMOL [X] [Y] [Z] [X] [Y] [Z]                                    out: [X]  [Y]  [Z]  [r]  [theta(deg)] [phi(deg)]\n"  
         << "          --> sites (shows the 10 sites for the (111) pes in my 120deg system      \n";
    return 0;
  }
  //fix cout precision en length
  std::cout.precision(9);
  std::cout.setf( std::ios::fixed, std:: ios::floatfield );
  if( !strcmp(argv[1],"DtoC") ){
    a3D   = atof(argv[2]) / sqrt(2);
    angle = atof(argv[3]);
    Z     = atof(argv[4]);
    C1[0] = atof(argv[5]);
    C1[1] = atof(argv[6]);
    C1[2] = atof(argv[7]); 
//    cout << "before " << C1[0] << " " << C1[1] << " " << C1[2] << "\n";
    transformDtoC( C1, a3D, Z, angle);
    cout << C1[0] << " " << C1[1] << " " << C1[2] << "\n";
  }
  else if( !strcmp(argv[1],"CtoD") ){
    a3D   = atof(argv[2]) / sqrt(2);
    angle = atof(argv[3]);
    Z     = atof(argv[4]);
    C1[0] = atof(argv[5]);
    C1[1] = atof(argv[6]);
    C1[2] = atof(argv[7]); 
    transformCtoD( C1, a3D, Z, angle);
    cout << C1[0] << " " << C1[1] << " " << C1[2] << "\n";
  }
  else if( !strcmp(argv[1],"MOLtoCART") ){
    pos_mol[0] = atof(argv[2]);
    pos_mol[1] = atof(argv[3]);
    pos_mol[2] = atof(argv[4]);
    pos_mol[3] = atof(argv[5]);
    pos_mol[4] = atof(argv[6]);
    pos_mol[5] = atof(argv[7]); 
    transformMtoC( pos_mol, pos_cart);
    for( int i =0; i < 6; i++ ) cout << pos_cart[i] << " ";
    cout << "\n";
  }
  else if ( !strcmp(argv[1],"CARTtoMOL") ){
     pos_cart[0] = atof(argv[2]);
     pos_cart[1] = atof(argv[3]);
     pos_cart[2] = atof(argv[4]);
     pos_cart[3] = atof(argv[5]);
     pos_cart[4] = atof(argv[6]);
     pos_cart[5] = atof(argv[7]); 
     transformCtoM( pos_mol, pos_cart);
     for( int i =0; i < 6; i++ ) cout << pos_mol[i] << " ";
     cout << "\n";
  }
  else if ( !strcmp(argv[1],"sites") ){
    cout << "                      60 degrees                      120 degrees \n";
    cout << "sites:	u		v             u                    v \n";
    //TOP
    C1[0] = 0.;		C1[1] = 0.;		C1[2]  = 0.;
    cout << "TOP    " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //BRIDGE
    C1[0] = 1./2.;         C1[1] = 0.;             C1[2]  = 0.;
    cout << "BRIDGE " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //HCP
    C1[0] = 1./3.;         C1[1] = 1./3.;             C1[2]  = 0.;
    cout << "HCP    " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //TtH
    C1[0] = 1./6.;         C1[1] = 1./6.;             C1[2]  = 0.;
    cout << "TtH    " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //e
    C1[0] = 1./3.;         C1[1] = 1./6.;             C1[2]  = 0.;
    cout << "e      " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //t
    C1[0] = 1./6.;         C1[1] = 0.;             C1[2]  = 0.;
    cout << "t      " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //eta
    C1[0] = 1./3.;         C1[1] = 0.;             C1[2]  = 0.;
    cout << "eta    " << C1[0] << "  " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << "  " << C1[1] << "\n";

    //TtF
    C1[0] = 1./3.;         C1[1] = -(1./6.);             C1[2]  = 0.;
    cout << "TtF    " << C1[0] << " " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << " " << C1[1] << "\n";

    //e'
    C1[0] = 1./2.;         C1[1] = -(1./6.);             C1[2]  = 0.;
    cout << "e'     " << C1[0] << " " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << " " << C1[1] << "\n";

    //FCC
    C1[0] = 2./3.;         C1[1] = -(1./3.);             C1[2]  = 0.;
    cout << "FCC    " << C1[0] << " " << C1[1] << "      "; 
    transformDtoC(C1, 1, 1, 60);
    transformCtoD(C1, 1, 1, 120);
    cout << C1[0] << " " << C1[1] << "\n";
  }  
  return 0;
}
