// Molecule.cpp: implementation of the CMolecule class.
//
//////////////////////////////////////////////////////////////////////

#include "molecule.h"
#include <math.h>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include "ccmath.h"

static const int NATOMS=86;
static std::string atomnames[NATOMS]={"H","He","Li","Be","B","C","N","O","F","Ne","Na","Mg",
                                      "Al","Si","P","S","Cl","Ar","K","Ca","Sc","Ti","V","Cr","Mn","Fe","Co","Ni","Cu","Zn","Ga",
                                      "Ge","As","Se","Br","Kr","Rb","Sr","Y","Zr","Nb","Mo","Tc","Ru","Rh","Pd","Ag","Cd","In","Sn",
                                      "Sb","Te","I","Xe","Cs","Ba","La","Hf","Ta","W","Re","Os","Ir","Pt","Au","Hg","Tl","Pb","Bi"
                                      "Po","At","Rn"};
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CMolecule::CMolecule()
{
  m_S2=0; //rhf by default
  m_maximumdisplacement=m_rmsdisplacement=m_maximumforce=m_rmsforce=-1;
}

CMolecule::~CMolecule()
{}


void CMolecule::SelectAtom(int i,bool b)
{
  m_atom[i].Select(b);
}
/** No descriptions */
float CMolecule::CalculateDihedral(int i, int j)
{


  return coordinate::dihedral( m_atom[i].c,
                               m_atom[m_atom[i].conexion[0].index].c,
                               m_atom[m_atom[j].conexion[0].index].c,
                               m_atom[j].c);

}

/** No descriptions */
bool CMolecule::AreSp3Connected(int i, int j)
{


  CAtom& H1=m_atom[i];
  CAtom& H2=m_atom[j];
  CAtom& C1=this->m_atom[H1.conexion[0].index];
  CAtom& C2=this->m_atom[H2.conexion[0].index];

  if (C1.conexion.size()!=4 || C2.conexion.size()!=4)
  {
    std::cout << "No sp3 atoms";
    return false;
  }

  register int k;
  bool b=false;

  for (k=0;k<4;k++)
  {

    if(C1.conexion[k].index==H2.conexion[0].index)
      b=true;
  }



  return b;
}
/** No descriptions */
float CMolecule::GetDistance(int atom1, int atom2)
{

  return coordinate::distance(m_atom[atom1].c, m_atom[atom2].c);

}
/** No descriptions */
float CMolecule::GetHCdistance(int h)
{

  CAtom& heavyatom=m_atom[m_atom[h].conexion[0].index];
  return coordinate::distance(m_atom[h].c,heavyatom.c);


}
/** Calculate Molecular Weight */
float CMolecule::CalculateWeight()
{

  std::vector<CAtom>::iterator it;
  float sum=0.0f;
  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    float weight=it->GetAtomicMass();
    if(weight <0)
      return -1.0f;
    else sum+=weight;
  }

  m_molecularweight=sum;

  return m_molecularweight;
}
/** returns molecular weight */
double CMolecule::GetMolecularWeight()
{

  return m_molecularweight;
}
/** Calculate an approximate center of masses for better rotation handling
it is not really CM cause all atoms have the same weight */
void CMolecule::CalculateMassCenter(bool real)
{

  std::vector<CAtom>::iterator it;
  coordinate cm;
  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    cm.x+=it->c.x;
    cm.y+=it->c.y;
    cm.z+=it->c.z;
  }

  cm.x/=m_atom.size();
  cm.y/=m_atom.size();
  cm.z/=m_atom.size();

  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    it->c-=cm;
  }


}

coordinate CMolecule::GetMassCenter()
{
  std::vector<CAtom>::iterator it;
  coordinate cm;
  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    cm.x+=it->c.x*it->GetAtomicMass();
    cm.y+=it->c.y*it->GetAtomicMass();
    cm.z+=it->c.z*it->GetAtomicMass();
  }

  cm.x/=m_atom.size();
  cm.y/=m_atom.size();
  cm.z/=m_atom.size();

  return cm;
}

coordinate CMolecule::Centroid() const
{
  std::vector<CAtom>::const_iterator it;
  coordinate cm;
  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    cm.x+=it->c.x;
    cm.y+=it->c.y;
    cm.z+=it->c.z;
  }

  cm.x/=m_atom.size();
  cm.y/=m_atom.size();
  cm.z/=m_atom.size();

  return cm;
}
/** Get the a->b vector */
coordinate CMolecule::GetVector(int& a,int& b)
{

  return m_atom[b].c-m_atom[a].c;
}
/** No descriptions */
void CMolecule::ResetSelection()
{

  std::vector<CAtom>::iterator it;

  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    it->Select(false);
  }
}
/** No descriptions */
void CMolecule::GetHydrogenList(std::vector<int>& list)
{

  std::vector<CAtom>::iterator it;

  int counter=0;
  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    if(it->name=="H")
      list.push_back(counter);
    counter++;
  }
}
/** No descriptions */
void CMolecule::SetAtomPair(int i, int j)
{


  m_pairs.push_back(pair(i,j,"kk"));
}
/** No descriptions */
void CMolecule::ResetAtomPairs()
{
  m_pairs.clear();
}

/** No descriptions */
void CMolecule::GetEnviroment(int s1, int s2,std::vector< std::vector<std::string> >& subs_str)
{

  //find the two carbons
  int c1=m_atom[s1].conexion[0].index;

  int c2=m_atom[s2].conexion[0].index;

  std::vector<std::string> str(4);


  std::vector<int> alpha(4);

  std::vector<bond>::iterator it;

  /*for(it1=m_atom[c1].conexion.begin();it1!=m_atom[c1].conexion.end();it1++)
  {

    bool found=false;
     for(it2=m_atom[c2].conexion.begin();it2!=m_atom[c2].conexion.end();it2++)
        if( (*it1)==(*it2) ) found=true;

     if(found)
      break;
        
  } */

  //int common=(*it1);

  int counter=0;


  //locate the alpha centers
  for(it=m_atom[c1].conexion.begin();it!=m_atom[c1].conexion.end();it++)
  {
    if( (*it).index != s1 && (*it).index!=c2 )
    {
      alpha[counter]=(*it).index;
      counter++;
    }
  }


  for(it=m_atom[c2].conexion.begin();it!=m_atom[c2].conexion.end();it++)
  {
    if( (*it).index!=s2 && (*it).index!=c1 )
    {
      alpha[counter]=(*it).index;
      counter++;
    }
  }
  //Ok, now we have numbers of the 4 S1, S2, S3 and S4 substituents
  // now I compare the direction of the cross products in order to get the order
  //S1,S2,S3,S4
  /*                        S4  H1  S3
                             \  |  /
                              \ | /    
                                C
                              / | \
                             /  |  \
                           S2   H2  S1    */

  coordinate H1C1=m_atom[s1].c-m_atom[c1].c;
  coordinate S1C1=m_atom[alpha[0]].c-m_atom[c1].c;

  coordinate H2C2=m_atom[s2].c-m_atom[c2].c;
  coordinate S3C3=m_atom[alpha[2]].c-m_atom[c2].c;

  coordinate C1C2=m_atom[c2].c-m_atom[c1].c;


  //Get croos products
  coordinate p1=H1C1^S1C1;
  coordinate p2=H2C2^S3C3;

  //now the core of this algorithm, if position is right scalar product
  //p1*p2 will be positive

  if( (p1*C1C2) < 0 )
    std::swap(alpha[0],alpha[1]);
  if( (p2*C1C2) > 0)
    std::swap(alpha[2],alpha[3]);

  //Well, now get the strings
  for(int i=0;i<4;i++)
  {
    subs_str[i].push_back(m_atom[alpha[i]].name);
    bool baromaticfound=false;
    for(it=m_atom[alpha[i]].conexion.begin();it!=m_atom[alpha[i]].conexion.end();it++)
    {
      if( !( it->index==c1 || it->index==c2) )
      {
        subs_str[i].push_back(m_atom[it->index].name);

        if(it->order==eDOUBLE)
          subs_str[i].push_back(m_atom[it->index].name);
        if(it->order==eTRIPLE)
          subs_str[i].push_back(m_atom[it->index].name+m_atom[it->index].name);
        if(it->order==eAROMATIC && !baromaticfound)
        {
          subs_str[i].push_back(m_atom[it->index].name);
          baromaticfound=true;
        }

      }
    }

  }

  std::vector< std::vector <std::string> >::iterator st;

  for(st=subs_str.begin();st!=subs_str.end();st++)
  {
    std::vector<std::string>::iterator sst;
    for(sst=st->begin();sst!=st->end();sst++)
    {
      std::cout << (*sst);
    }
    std::cout <<std::endl;
  }

}

/** Get Hybridization type for atom i */
std::string CMolecule::GetHybridization(int i)
{

  CAtom& atom=this->m_atom[i];
  size_t n=atom.conexion.size();

  std::vector<bond>::iterator it;
  std::cout <<"conexion=";

  for(it=atom.conexion.begin();it!=atom.conexion.end();it++)
  {
    std::cout << it->index <<std::endl;
  }

  if(atom.name=="C")
  {
    switch(atom.conexion.size())
    {
    case 4:
      return "sp3";
    case 3:
      return "sp2";
      break;
    case 2:
      return "sp";
      break;
    default:
      return "abnormal";
    }
  }

  else
    return "not from IV Group";





}
/** A simple function to set the bond orders */
void CMolecule::SetBondOrders()
{

  //This is quite simple and do not contain all cases
  //maybe this could be improved with the use of OpenBabel Library

  std::vector<CAtom>::iterator at;

  for(at=m_atom.begin();at!=m_atom.end();at++)
  {
    std::vector<bond>::iterator ct;
    std::sort(at->conexion.begin(),at->conexion.end());
    ct=std::unique(at->conexion.begin(),at->conexion.end());
    at->conexion.erase(ct,at->conexion.end());
    for(ct=at->conexion.begin();ct!=at->conexion.end();ct++)
    {
      if(at->name=="H")
        ct->order=eSINGLE;

      if(at->name=="C")
      {
        float d=coordinate::distance(at->c,m_atom[ct->index].c);
        if(m_atom[ct->index].name=="C")
        {

          if(fabs(d-1.500f) <0.025f)
            ct->order=eSINGLE;
          if(fabs(d-1.400f) <0.025f)
            ct->order=eAROMATIC;
          if(fabs(d-1.350f) <0.025f)
            ct->order=eDOUBLE;
          if(fabs(d-1.200f) <0.025f )
            ct->order=eTRIPLE;

        }

        if(m_atom[ct->index].name=="O")
        {
          if(fabs(d-1.200f) <0.025f)
            ct->order=eDOUBLE;
          else
            ct->order=eSINGLE;
        }
      }
    }
  }
}
/** Get Angles to apply in Barfield Equation */
void CMolecule::GetHCCAngles(int s1,int s2,float* angle)
{

  int c1=m_atom[s1].conexion[0].index;

  int c2=m_atom[s2].conexion[0].index;

  coordinate v1=m_atom[s1].c-m_atom[c1].c;
  coordinate v2=m_atom[c2].c-m_atom[c1].c;

  angle[0]=coordinate::GetAngleVector(v1,v2);

  coordinate v3=m_atom[s2].c-m_atom[c2].c;
  coordinate v4=m_atom[c1].c-m_atom[c2].c;

  angle[1]=coordinate::GetAngleVector(v3,v4);


}
/** No descriptions */
void CMolecule::ResetIrradiations()
{

  std::vector<CAtom>::iterator it;

  for(it=m_atom.begin();it!=m_atom.end();it++)
  {
    if(it->IsIrradiated())
    {
      it->Irradiate(false);
      goto label;
    }
  }
label :
  return;
}
/** No descriptions */
bool CMolecule::IsMethyl(int i)
{

  if(GetHybridization(i)!="sp3") return false;
  std::vector<bond>::iterator it;
  int sum=0;
  for(it=m_atom[i].conexion.begin();it!=m_atom[i].conexion.end();it++)
  {
    if(m_atom[it->index].name=="H")
      sum++;
  }

  return sum>=3;
}
/** returns the number of substituents different from hydrogen */
int CMolecule::GetNSubstituents(int i)
{

  std::vector<bond>::iterator it;
  CAtom& atom=m_atom[i];
  int sum=0;
  for(it=atom.conexion.begin();it!=atom.conexion.end();it++)
  {
    if(m_atom[it->index].name!="H")
      sum++;
  }
  return sum;
}
/** No descriptions */
float CMolecule::GetAngle(int a1, int a2, int a3,bool degrees /*=false*/)
{

  coordinate v1=m_atom[a1].c-m_atom[a2].c;
  coordinate v2=m_atom[a3].c-m_atom[a2].c;

  float angle=coordinate::GetAngleVector(v1,v2);

  if(degrees)
    return angle*180/M_PI;
  else
    return angle;
}

float CMolecule::GetDihedral(int a1, int a2, int a3,int a4,bool degrees /*=false*/)
{

  float d= coordinate::dihedral(m_atom[a1].c,m_atom[a2].c,m_atom[a3].c,m_atom[a4].c);

  if(degrees)
    return d*180/M_PI;
  else
    return d;
}


const float rotbondpass=0.3f;
void CMolecule::RotateBond(int i, int j, int sense)
{
  m_rotaxis[0]=m_atom[i].c;
  m_rotaxis[1]=m_atom[j].c;
  float pass=rotbondpass;
  if (sense < 0 ) pass*=-1;
  RotateNeighbours(i,j,sense,pass);
  m_rotatedatoms.clear();

}


void CMolecule::RotateNeighbours(int i, int j, int sense,double pass)
{
  CAtom& atom=m_atom[j];
  std::vector<bond>::iterator it;
  //rotate atom
  std::cout << "Rotate[i,j]=" << i  << " " << j << std::endl;

  //if( sense < 0) pass*=-1;

  std::vector<int>::iterator pos=std::find( m_rotatedatoms.begin(), m_rotatedatoms.end() , j );
  if( pos == m_rotatedatoms.end() )
  {
    atom.c=coordinate::RotAroundAxis(m_rotaxis[0],m_rotaxis[1],atom.c,pass);
    m_rotatedatoms.push_back(j);
  }
  else return;

  for(it=atom.conexion.begin();it!=atom.conexion.end();it++)
  {

    if(it->index != i)
      RotateNeighbours(j, it->index, sense,pass);
  }


}

void CMolecule::SetDihedral(int i, int j, int k, int l,float dihedral)
{
 float oldd= coordinate::dihedral(m_atom[i].c,m_atom[j].c,m_atom[k].c,m_atom[l].c);
 float pass=dihedral*M_PI/180.-oldd;
 
  m_rotaxis[0]=m_atom[j].c;
  m_rotaxis[1]=m_atom[k].c;
  RotateNeighbours(j,k,1,pass);
  m_rotatedatoms.clear();
 
}

const char* CMolecule::GetAtomName(int i)
{
  if ( i>86 )
    return "unknown";
  if ( i == 0 ) //Ghost Atom
    return "X";

  return atomnames[i-1].c_str();

}

int CMolecule::GetZ(const char* name)
{
  for(int i=0;i<NATOMS;i++)
  {
    if(atomnames[i]==name)
      return i;
  }
}

void CMolecule::SetConexions()
{

  std::vector<CAtom>::iterator at,at1;
  int counter1,counter2;
  for(at=m_atom.begin(),counter1=0;at!=m_atom.end();at++,counter1++)
  {
    at->conexion.clear();
    if( at->name != "X") //no conexions for ghost atoms
      for(at1=m_atom.begin(),counter2=0;at1!=m_atom.end();at1++,counter2++)
      {
        if( at1->name != "X")
        {
          if(counter1 != counter2)
          {
            float d=coordinate::distance(at->c,at1->c);

            if(at->name=="H" || at1->name=="H")
            {
              if (d <1.4f)
              {
                at->conexion.push_back(bond(counter2));
                // at1->conexion.push_back(bond(counter1));
              }
            }

            else if ( GetZ(at->name.c_str()) <11 && GetZ(at1->name.c_str()) < 11 )
            {
              if(d< 2.0f)
              {
                at->conexion.push_back(bond(counter2));
                // at1->conexion.push_back(bond(counter1));
              }
            }

            else if( ( GetZ(at->name.c_str()) <11 && GetZ(at1->name.c_str()) >= 11 ) ||
                     ( (GetZ(at1->name.c_str()) <11 && GetZ(at->name.c_str()) >= 11 ) ) )
            {
              if(d < 2.4f)
                at->conexion.push_back(bond(counter2));
            }

            else
            {
              if(d < 3.0f)
              {
                at->conexion.push_back(bond(counter2));
                //  at1->conexion.push_back(bond(counter1));
              }
            }
          }
        }
      }
  }


}

void CMolecule::AllocateVectors(bool rottransmodes/*=false*/)
{
  size_t size;
  rottransmodes ? size =3*m_atom.size() : size = 3*m_atom.size()-6;

  m_modes.reserve(size);

  std::vector<CAtom>::iterator at;
  for(at=m_atom.begin();at!=m_atom.end();at++)
  {
    at->mode.resize(size);
  }
}

std::ostream& operator << (std::ostream& s,CMolecule& m)
{
  std::vector<CAtom>::iterator it;
  for (it=m.m_atom.begin();it!=m.m_atom.end();it++)
  {

    s << std::setiosflags(std::ios::fixed) << std::setw(3) << setiosflags(std::ios::left) << it->name
      << std::setw(10)  << std::resetiosflags(std::ios::left)
      << std::setiosflags(std::ios::right) << std::setiosflags(std::ios::fixed) << std::setprecision(6)
        << it->c.x <<
        std::setw(10) << it->c.y
        << std::setw(10) << it->c.z
        << std::resetiosflags(std::ios::right) << std::endl;
  }
  s << std::resetiosflags(std::ios::fixed);
  return s;
}

void CMolecule::CreateBond(int i, int j)
{
  std::vector<bond>::iterator it;
  it=std::find(m_atom[i].conexion.begin(),m_atom[i].conexion.end(),bond(j));
  if (it!=m_atom[i].conexion.end()) return;
  m_atom[i].conexion.push_back(bond(j));
  m_atom[j].conexion.push_back(bond(i));
}

void CMolecule::DeleteBond(int i, int j)
{
  std::vector<bond>::iterator it;
  it=std::find(m_atom[i].conexion.begin(),m_atom[i].conexion.end(),bond(j));
  if ( it!=m_atom[i].conexion.end() )
  {
    m_atom[i].conexion.erase(it);
    m_atom[j].conexion.erase( std::find(m_atom[j].conexion.begin(),m_atom[j].conexion.end(),bond(i)) );
  }


}

unsigned int CMolecule::CountHeavyAtoms() const
{
   std::vector<CAtom>::const_iterator at;
  unsigned int counter=0;
  for(at=m_atom.begin();at!=m_atom.end();++at)
  {
       if ( at->name != "H" )  counter++;
  }
  return counter;
}

float CMolecule::GetAnglePlanes(std::vector<int>& v, bool degrees)
{
  coordinate v1=m_atom[v[2]].c-m_atom[v[1]].c;
  coordinate v2=m_atom[v[1]].c-m_atom[v[0]].c;
  coordinate v3=v1^v2;

  v1=m_atom[v[5]].c-m_atom[v[4]].c;
  v2=m_atom[v[4]].c-m_atom[v[3]].c;

  coordinate v4=v1^v2;

  //Ok lets get the angle between vectors
  float angle=coordinate::GetAngleVector(v3,v4);

  if(degrees)
    return angle*180/M_PI;
  else
    return angle;
}

void CMolecule::AllocateForces()
{

  m_forces.Initialize(3*m_atom.size());
}

void CMolecule::AllocateHessian()
{

  m_hessian.Initialize(3*m_atom.size(),3*m_atom.size());
  m_dipolederivatives.Initialize(3*m_atom.size(),3);

}

IBArray<double> CMolecule::GetInertiaTensor()
{

  IBArray<double> imatrix(3,3,0.);
  std::vector<CAtom>::iterator at;
  std::vector<coordinate> cmcoords;
  cmcoords.resize(m_atom.size());
  std::vector<coordinate>::iterator ct;

  coordinate mc=GetMassCenter();
  for(at=m_atom.begin(),ct=cmcoords.begin();at!=m_atom.end();at++,ct++)
  {
    (*ct) =at->c-mc;
  }
  for(at=m_atom.begin(),ct=cmcoords.begin();at!=m_atom.end();at++,ct++)
  {
    imatrix(0,0)+=at->GetAtomicMass()*(ct->y*ct->y+ct->z*ct->z);
    imatrix(1,1)+=at->GetAtomicMass()*(ct->x*ct->x+ct->z*ct->z);
    imatrix(2,2)+=at->GetAtomicMass()*(ct->x*ct->x+ct->y*ct->y);

    imatrix(0,1)=imatrix(1,0)-=at->GetAtomicMass()*(ct->x*ct->y);
    imatrix(0,2)=imatrix(2,0)-=at->GetAtomicMass()*(ct->x*ct->z);

    imatrix(1,2)=imatrix(2,1)-=at->GetAtomicMass()*(ct->y*ct->z);

  }
  /* for(size_t i=0;i<3;++i)
     for(size_t j=0;j<3;j++)
       std::cout << "imatrix(" << i << "," << j << ")=" <<  imatrix(i,j) << std::endl;*/
  return imatrix;

}


void CMolecule::CalculateFrequencies()
{

  //ok diagonalize the hessian matrix
  size_t ncoord=3*m_atom.size();
  //IMArray<double> evalues(ncoord);
  IBArray<double> evectors(m_hessian);
  IMArray<double> evalues(ncoord);

  size_t i,j;
  const double me=1.66053873e-27*6.023e23;
  for(i=0;i<ncoord;i++)
  {
    for(j=0;j <= i ;j++)
    {
      evectors(i,j)/=sqrt( me*me*m_atom[i/3].GetAtomicMass()*m_atom[j/3].GetAtomicMass() );
      evectors(j,i)=evectors(i,j);
      // std::cerr << "evector[" << i << "," << j << "]=" << evectors(i,j) << std::endl;
      // std::cerr << "[" << i << "," << j << "]=(" << m_atom[i/3].GetAtomicMass() << "," << m_atom[j/3].GetAtomicMass() << ")" << std::endl;
    }
  }

  ccmath::eigen(evectors,evalues,3*m_atom.size());
  //dIMArray evalues=IArray::Eigen(evectors);

  AllocateVectors(true);

  for(size_t i=0;i<ncoord;i++)
  {
    const double c=3e8;
    const double hartreetojoule=4.3597482e-18*6.023e23;
    const double bohrtometer=5.291772e-11;

    const double conversionfactor=hartreetojoule/(bohrtometer*bohrtometer);
    double freq=   conversionfactor*fabs(evalues(i)) / (4*M_PI*M_PI*c*c );
    float x= sqrt(freq)/100.;
    std::cerr << "eigenvalue[" << i <<"]=" << x << std::endl;
    m_modes.push_back(frequency(x,1,1));

  }

  for(i=0;i<ncoord;i++)
  {
    for(j=0;j<m_atom.size();j++)
    {
      m_atom[j].mode[i].x=evectors(3*j,i);
      m_atom[j].mode[i].y=evectors(3*j+1,i);
      m_atom[j].mode[i].z=evectors(3*j+2,i);
    }
  }

  //OK and finally intensities //solve problem with intensities
  /* IBArray<double> trdipole=evectors*m_dipolederivatives;

   for(i=0;i<ncoord;i++)
   {
     double sum=0;
     for(j=0;j<3;j++)
       sum+=trdipole(i,j)*trdipole(i,j);
     m_modes[i/3].y=sqrt(sum);
   }*/

  //Determine inertia tensor
  IBArray<double> inertiatensor=GetInertiaTensor();

  dIMArray itvalues(3);

  ccmath::eigen(inertiatensor,itvalues,3);
  return;
  //dIMArray itvalues=IArray::Eigen(inertiatensor);
  for(i=0;i<3;i++)
    std::cout << "inertia tensor is " << itvalues(i) << std::endl;
  //Ok go for Saytzev conditions
  IBArray<double> D(3*m_atom.size(),3*m_atom.size());
  //translational vectors
  for(size_t i=0;i<3;i++)
    for(size_t j=0;j<m_atom.size();j++)
    {
      D(i,3*j+i)=sqrt(m_atom[j].GetAtomicMass());
    }
  //rotational vectors
  dIBArray& X=inertiatensor;
  for(size_t i=0;i<3;i++)
  {
    for(size_t j=0;j<m_atom.size();j++)
    {
      //put coordinates aligned with tensor
      dIMArray r(3);
      r(0)=m_atom[j].c.x;
      r(1)=m_atom[j].c.y;
      r(2)=m_atom[j].c.z;
      dIMArray rp=X*r;
      double sqrmass=sqrt(m_atom[j].GetAtomicMass());

      D(i+3,3*j)=( rp(1)*X(0,2)-rp(2)*X(0,1) ) /sqrmass;
      D(i+3,3*j+1)=( rp(2)*X(1,0)-rp(0)*X(1,2) ) /sqrmass;
      D(i+3,3*j+2)=( rp(0)*X(2,1)-rp(1)*X(2,0) ) /sqrmass;

    }
  }
  //Ok normalize now
  for(size_t i=0;i<6;i++)
  {
    dIMArray row=D.GetRow(i);
    double norm=row.norm();
    for(size_t j=0;j<3*m_atom.size();j++)
    {
      D(i,j)/=norm;

    }
  }

  //OK lets use now schmidt orthogonalization
  dIMArray u(3*m_atom.size(),1/sqrt(3*m_atom.size())); //unitary vector
  dIMArray v;
  for(size_t i=6;i<3*m_atom.size();i++)
  {
    v=u;
    for(size_t j=0;j<i;j++)
    {
      double prj=v*D.GetRow(j);

      v-=prj*D.GetRow(j);
    }
    for(size_t k=0;k<3*m_atom.size();k++)
    {
      D(i,k)=v(k);
    }

  }

  for(size_t i=0;i<3*m_atom.size();i++)
  {
    dIMArray row=D.GetRow(i);
    double norm=row.norm();
    if(i > 5 )
    {
      for(size_t j=0;j<3*m_atom.size();j++)
      {
        D(i,j)/=norm;

      }
    }
    std::cout << "the norm is " << D.GetRow(i).norm() << std::endl;
  }

  //Ok we have D matrix so lets rotate hessian
  dIBArray transposed=D.Transpose();
  dIBArray rothessian=transposed*m_hessian*D;

}

int CMolecule::GetNVib()
{
  int nmodes;
  if( IsLinear() ) nmodes=3*m_atom.size()-5;
  else nmodes=3*m_atom.size()-6;
  return nmodes;

}

bool CMolecule::IsLinear()
{
  if(m_atom.size() == 2 ) return true;

  for(size_t i=2; i < m_atom.size(); i++)
  {
    if( (180. - GetAngle(i-2,i-1,i,true) ) > 1. )
      return false;
  }

  return true;
}

void CMolecule::GetBox(coordinate& flt, coordinate& brb) const
{

  flt=coordinate(0,0,0);
  brb=coordinate(0,0,0);
  std::vector<CAtom>::const_iterator it;
  coordinate c;
  for(it=m_atom.begin();it!=m_atom.end();++it)
  {
    if ( it->c.x  < flt.x ) flt.x=it->c.x;
    if ( it->c.x > brb.x ) brb.x=it->c.x;
    if ( it->c.y > flt.y ) flt.y=it->c.y;
    if ( it->c.y < brb.y ) brb.y=it->c.y;
    if ( it->c.z < brb.z ) brb.z=it->c.z;
    if ( it->c.z > flt.z ) flt.z=it->c.z;
  }
}

std::vector < std::string > CMolecule::GetNuclei()
{
  std::vector < std::string > nuclei;
  std::vector<CAtom>::const_iterator at;
  std::vector <std::string>::iterator it;
  for(at=m_atom.begin();at!=m_atom.end();++at)
  {
    std::find ( nuclei.begin(), nuclei.end(), at->name );
    if ( it!=nuclei.end() )
    {
       nuclei.push_back(at->name);
    }
  }
  return nuclei;
}

std::vector<double>* CMolecule::Charges(CMolecule::Charge type) 
{
  switch( type )
  {
     case ESP:
     return &m_espcharges;
     default:
     return NULL;
  }
  return NULL;
}

const std::vector<double>* CMolecule::Charges(CMolecule::Charge type) const
{
  switch( type )
  {
     case ESP:
     return &m_espcharges;
     default:
     return NULL;
  }
  return NULL;
}

std::vector<size_t> CMolecule::Methyls() const 
{
  std::vector<CAtom>::const_iterator at;
  std::vector<size_t> methyls;
  for (at=m_atom.begin();at!=m_atom.end();++at)
  {
     if ( at->name == "H" && at->conexion.size() == 1 )
     {
       size_t index=at->conexion.at(0).index;
       const CAtom& atom=m_atom.at(index);
       if ( atom.name == "C" )
       {
          std::vector<bond>::const_iterator it;
          short nh=0;
          for(it=atom.conexion.begin();it!=atom.conexion.end();++it)
          {
            if ( m_atom.at(it->index).name == "H" )
            {
              ++nh;
            }
          }
          if ( nh == 3 )
            methyls.push_back(index);
       }
     }
  }
  return methyls;
}
