/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010 University of Bonn. All rights reserved.
 * Please see the LICENSE file or "Copyright notice" in builder.cpp for details.
 */

/*
 * RotateToPrincipalAxisSystemAction.cpp
 *
 *  Created on: May 10, 2010
 *      Author: heber
 */

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/RealSpaceMatrix.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "element.hpp"
#include "molecule.hpp"


#include <iostream>
#include <fstream>
#include <string>

using namespace std;

#include "Actions/MoleculeAction/RotateToPrincipalAxisSystemAction.hpp"

// and construct the stuff
#include "RotateToPrincipalAxisSystemAction.def"
#include "Action_impl_pre.hpp"
/** =========== define the function ====================== */
Action::state_ptr MoleculeRotateToPrincipalAxisSystemAction::performCall() {
  molecule *mol = NULL;

  // obtain information
  getParametersfromValueStorage();

  for (World::MoleculeSelectionIterator iter = World::getInstance().beginMoleculeSelection(); iter != World::getInstance().endMoleculeSelection(); ++iter) {
    mol = iter->second;
    DoLog(0) && (Log() << Verbose(0) << "Converting to prinicipal axis system." << endl);
    RealSpaceMatrix InertiaTensor;
    Vector *CenterOfGravity = mol->DetermineCenterOfGravity();

    // reset inertia tensor
    InertiaTensor.setZero();

    // sum up inertia tensor
    for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
      Vector x = (*iter)->getPosition();
      x -= *CenterOfGravity;
      const double mass = (*iter)->getType()->getMass();
      InertiaTensor.at(0,0) += mass*(x[1]*x[1] + x[2]*x[2]);
      InertiaTensor.at(0,1) += mass*(-x[0]*x[1]);
      InertiaTensor.at(0,2) += mass*(-x[0]*x[2]);
      InertiaTensor.at(1,0) += mass*(-x[1]*x[0]);
      InertiaTensor.at(1,1) += mass*(x[0]*x[0] + x[2]*x[2]);
      InertiaTensor.at(1,2) += mass*(-x[1]*x[2]);
      InertiaTensor.at(2,0) += mass*(-x[2]*x[0]);
      InertiaTensor.at(2,1) += mass*(-x[2]*x[1]);
      InertiaTensor.at(2,2) += mass*(x[0]*x[0] + x[1]*x[1]);
    }
    // print InertiaTensor for debugging
    DoLog(0) && (Log() << Verbose(0) << "The inertia tensor is:" << InertiaTensor << endl);

    // diagonalize to determine principal axis system
    Vector Eigenvalues = InertiaTensor.transformToEigenbasis();

    for(int i=0;i<NDIM;i++)
      DoLog(0) && (Log() << Verbose(0) << "eigenvalue = " << Eigenvalues[i] << ", eigenvector = " << InertiaTensor.column(i) << endl);

    // check whether we rotate or not
    DoLog(0) && (Log() << Verbose(0) << "Transforming molecule into PAS ... ");

    // obtain first column, eigenvector to biggest eigenvalue
    Vector BiggestEigenvector(InertiaTensor.column(Eigenvalues.SmallestComponent()));
    Vector DesiredAxis(params.Axis);

    // Creation Line that is the rotation axis
    DesiredAxis.VectorProduct(BiggestEigenvector);
    Line RotationAxis(Vector(0.,0.,0.), DesiredAxis);

    // determine angle
    const double alpha = BiggestEigenvector.Angle(params.Axis);

    DoLog(0) && (Log() << Verbose(0) << alpha << endl);

    for (molecule::iterator iter = mol->begin(); iter != mol->end(); ++iter) {
      *(*iter) -= *CenterOfGravity;
      (*iter)->setPosition(RotationAxis.rotateVector((*iter)->getPosition(), alpha));
      *(*iter) += *CenterOfGravity;
    }
    DoLog(0) && (Log() << Verbose(0) << "done." << endl);

    // summing anew for debugging (resulting matrix has to be diagonal!)
    // reset inertia tensor
    InertiaTensor.setZero();

    // sum up inertia tensor
    for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
      Vector x = (*iter)->getPosition();
      x -= *CenterOfGravity;
      const double mass = (*iter)->getType()->getMass();
      InertiaTensor.at(0,0) += mass*(x[1]*x[1] + x[2]*x[2]);
      InertiaTensor.at(0,1) += mass*(-x[0]*x[1]);
      InertiaTensor.at(0,2) += mass*(-x[0]*x[2]);
      InertiaTensor.at(1,0) += mass*(-x[1]*x[0]);
      InertiaTensor.at(1,1) += mass*(x[0]*x[0] + x[2]*x[2]);
      InertiaTensor.at(1,2) += mass*(-x[1]*x[2]);
      InertiaTensor.at(2,0) += mass*(-x[2]*x[0]);
      InertiaTensor.at(2,1) += mass*(-x[2]*x[1]);
      InertiaTensor.at(2,2) += mass*(x[0]*x[0] + x[1]*x[1]);
      // print InertiaTensor for debugging
      DoLog(0) && (Log() << Verbose(0) << "The inertia tensor is:" << InertiaTensor << endl);
    }

    // free everything
    delete(CenterOfGravity);
  }
  return Action::success;
}

Action::state_ptr MoleculeRotateToPrincipalAxisSystemAction::performUndo(Action::state_ptr _state) {
//  MoleculeRotateToPrincipalAxisSystemState *state = assert_cast<MoleculeRotateToPrincipalAxisSystemState*>(_state.get());

//  string newName = state->mol->getName();
//  state->mol->setName(state->lastName);

  return Action::failure;
}

Action::state_ptr MoleculeRotateToPrincipalAxisSystemAction::performRedo(Action::state_ptr _state){
  // Undo and redo have to do the same for this action
  return performUndo(_state);
}

bool MoleculeRotateToPrincipalAxisSystemAction::canUndo() {
  return false;
}

bool MoleculeRotateToPrincipalAxisSystemAction::shouldUndo() {
  return false;
}
/** =========== end of function ====================== */