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

#include "Helpers/MemDebug.hpp"

#include "Actions/MoleculeAction/RotateToPrincipalAxisSystemAction.hpp"
#include "Actions/ActionRegistry.hpp"
#include "Helpers/Log.hpp"
#include "Helpers/Verbose.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/Matrix.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "element.hpp"
#include "molecule.hpp"


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

using namespace std;

#include "UIElements/UIFactory.hpp"
#include "UIElements/Dialog.hpp"
#include "Actions/ValueStorage.hpp"

/****** MoleculeRotateToPrincipalAxisSystemAction *****/

// memento to remember the state when undoing

//class MoleculeRotateToPrincipalAxisSystemState : public ActionState {
//public:
//  MoleculeRotateToPrincipalAxisSystemState(molecule* _mol,std::string _lastName) :
//    mol(_mol),
//    lastName(_lastName)
//  {}
//  molecule* mol;
//  std::string lastName;
//};

const char MoleculeRotateToPrincipalAxisSystemAction::NAME[] = "rotate-to-pas";

MoleculeRotateToPrincipalAxisSystemAction::MoleculeRotateToPrincipalAxisSystemAction() :
  Action(NAME)
{}

MoleculeRotateToPrincipalAxisSystemAction::~MoleculeRotateToPrincipalAxisSystemAction()
{}

void MoleculeRotateToPrincipalAxisSystem(Vector &Axis) {
  ValueStorage::getInstance().setCurrentValue(MoleculeRotateToPrincipalAxisSystemAction::NAME, Axis);
  ActionRegistry::getInstance().getActionByName(MoleculeRotateToPrincipalAxisSystemAction::NAME)->call(Action::NonInteractive);
};

Dialog* MoleculeRotateToPrincipalAxisSystemAction::fillDialog(Dialog *dialog) {
  ASSERT(dialog,"No Dialog given when filling action dialog");

  dialog->queryVector(NAME, false, MapOfActions::getInstance().getDescription(NAME));

  return dialog;
}

Action::state_ptr MoleculeRotateToPrincipalAxisSystemAction::performCall() {
  molecule *mol = NULL;
  Vector Axis;

  // obtain axis to rotate to
  ValueStorage::getInstance().queryCurrentValue(NAME, Axis);

  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);
    Matrix InertiaTensor;
    Vector *CenterOfGravity = mol->DetermineCenterOfGravity();

    // reset inertia tensor
    InertiaTensor.zero();

    // sum up inertia tensor
    for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
      Vector x = (*iter)->x;
      x -= *CenterOfGravity;
      InertiaTensor.at(0,0) += (*iter)->type->mass*(x[1]*x[1] + x[2]*x[2]);
      InertiaTensor.at(0,1) += (*iter)->type->mass*(-x[0]*x[1]);
      InertiaTensor.at(0,2) += (*iter)->type->mass*(-x[0]*x[2]);
      InertiaTensor.at(1,0) += (*iter)->type->mass*(-x[1]*x[0]);
      InertiaTensor.at(1,1) += (*iter)->type->mass*(x[0]*x[0] + x[2]*x[2]);
      InertiaTensor.at(1,2) += (*iter)->type->mass*(-x[1]*x[2]);
      InertiaTensor.at(2,0) += (*iter)->type->mass*(-x[2]*x[0]);
      InertiaTensor.at(2,1) += (*iter)->type->mass*(-x[2]*x[1]);
      InertiaTensor.at(2,2) += (*iter)->type->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(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(Axis);

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

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

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

    // sum up inertia tensor
    for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
      Vector x = (*iter)->x;
      x -= *CenterOfGravity;
      InertiaTensor.at(0,0) += (*iter)->type->mass*(x[1]*x[1] + x[2]*x[2]);
      InertiaTensor.at(0,1) += (*iter)->type->mass*(-x[0]*x[1]);
      InertiaTensor.at(0,2) += (*iter)->type->mass*(-x[0]*x[2]);
      InertiaTensor.at(1,0) += (*iter)->type->mass*(-x[1]*x[0]);
      InertiaTensor.at(1,1) += (*iter)->type->mass*(x[0]*x[0] + x[2]*x[2]);
      InertiaTensor.at(1,2) += (*iter)->type->mass*(-x[1]*x[2]);
      InertiaTensor.at(2,0) += (*iter)->type->mass*(-x[2]*x[0]);
      InertiaTensor.at(2,1) += (*iter)->type->mass*(-x[2]*x[1]);
      InertiaTensor.at(2,2) += (*iter)->type->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;
}

const string MoleculeRotateToPrincipalAxisSystemAction::getName() {
  return NAME;
}