/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <stdio.h>
#include <string.h>
#include "fix_smd_move_triangulated_surface.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "atom.h"
#include "comm.h"
#include "force.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "update.h"
#include "integrate.h"
#include "respa.h"
#include "memory.h"
#include "error.h"
#include "pair.h"
#include "domain.h"
#include <Eigen/Eigen>
#include "math_const.h"

using namespace Eigen;
using namespace LAMMPS_NS;
using namespace FixConst;
using namespace MathConst;
using namespace std;

/* ---------------------------------------------------------------------- */

FixSMDMoveTriSurf::FixSMDMoveTriSurf(LAMMPS *lmp, int narg, char **arg) :
		Fix(lmp, narg, arg) {

	if (atom->smd_flag != 1) {
		error->all(FLERR, "fix fix smd/move_tri_surf command requires atom_style smd");
	}

	if (narg < 3)
		error->all(FLERR, "Illegal number of arguments for fix fix smd/move_tri_surf command");

	rotateFlag = linearFlag = wiggleFlag = false;
	wiggle_direction  = 1.0;
	wiggle_max_travel = 0.0;

	int iarg = 3;

	if (comm->me == 0) {
		printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
		printf("fix fix smd/move_tri_surf is active for group: %s \n", arg[1]);
	}
	while (true) {

		if (iarg >= narg) {
			break;
		}

		if (strcmp(arg[iarg], "*LINEAR") == 0) {
			linearFlag = true;
			if (comm->me == 0) {
				printf("... will move surface in a linear fashion\n");
			}

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected three floats for velocity following *LINEAR");
			}
			vx = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected three floats for velocity following *LINEAR");
			}
			vy = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected three floats for velocity following *LINEAR");
			}
			vz = force->numeric(FLERR, arg[iarg]);

		} else if (strcmp(arg[iarg], "*WIGGLE") == 0) {
			wiggleFlag = true;
			if (comm->me == 0) {
				printf("... will move surface in wiggle fashion\n");
			}

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
			}
			vx = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
			}
			vy = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
			}
			vz = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
			}
			wiggle_max_travel = force->numeric(FLERR, arg[iarg]);

		} else if (strcmp(arg[iarg], "*ROTATE") == 0) {
			rotateFlag = true;

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			origin(0) = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			origin(1) = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			origin(2) = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			rotation_axis(0) = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			rotation_axis(1) = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			rotation_axis(2) = force->numeric(FLERR, arg[iarg]);

			iarg++;
			if (iarg == narg) {
				error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
			}
			rotation_period = force->numeric(FLERR, arg[iarg]);

			/*
			 * construct rotation matrix
			 */

			u_cross(0, 0) = 0.0;
			u_cross(0, 1) = -rotation_axis(2);
			u_cross(0, 2) = rotation_axis(1);

			u_cross(1, 0) = rotation_axis(2);
			u_cross(1, 1) = 0.0;
			u_cross(1, 2) = -rotation_axis(0);

			u_cross(2, 0) = -rotation_axis(1);
			u_cross(2, 1) = rotation_axis(0);
			u_cross(2, 2) = 0.0;

			uxu = rotation_axis * rotation_axis.transpose();

			if (comm->me == 0) {
				printf("will rotate with period %f\n", rotation_period);
			}

		} else {
			char msg[128];
			sprintf(msg, "Illegal keyword for fix smd/move_tri_surf: %s\n", arg[iarg]);
			error->all(FLERR, msg);
		}

		iarg++;

	}

	if (comm->me == 0) {
		printf(">>========>>========>>========>>========>>========>>========>>========>>========\n\n");
	}

	// set comm sizes needed by this fix
	comm_forward = 12;

	//atom->add_callback(0);
	//atom->add_callback(1);

	time_integrate = 1;
}

/* ---------------------------------------------------------------------- */

FixSMDMoveTriSurf::~FixSMDMoveTriSurf()
{
  // unregister callbacks to this fix from Atom class
  //atom->delete_callback(id,0);
  //atom->delete_callback(id,1);
}

/* ---------------------------------------------------------------------- */

int FixSMDMoveTriSurf::setmask() {
	int mask = 0;
	mask |= INITIAL_INTEGRATE;
	//mask |= PRE_EXCHANGE;
	return mask;
}

/* ---------------------------------------------------------------------- */

void FixSMDMoveTriSurf::init() {
	dtv = update->dt;
}

/* ----------------------------------------------------------------------
 ------------------------------------------------------------------------- */

void FixSMDMoveTriSurf::initial_integrate(int vflag) {
	double **x = atom->x;
	double **x0 = atom->x0;
	double **v = atom->v;
	double **vest = atom->vest;
	double **smd_data_9 = atom->smd_data_9;
	tagint *mol = atom->molecule;

	int *mask = atom->mask;
	int nlocal = atom->nlocal;
	double phi;
	int i;
	Matrix3d eye, Rot;
	eye.setIdentity();

	Vector3d v1, v2, v3, n, point, rotated_point, R, vel;

	if (igroup == atom->firstgroup)
		nlocal = atom->nfirst;

	if (linearFlag) { // translate particles
		for (i = 0; i < nlocal; i++) {
			if (mask[i] & groupbit) {

				v[i][0] = vx;
				v[i][1] = vy;
				v[i][2] = vz;

				vest[i][0] = vx;
				vest[i][1] = vy;
				vest[i][2] = vz;

				x[i][0] += dtv * vx;
				x[i][1] += dtv * vy;
				x[i][2] += dtv * vz;

				/*
				 * if this is a triangle, move the vertices as well
				 */

				if (mol[i] >= 65535) {
					smd_data_9[i][0] += dtv * vx;
					smd_data_9[i][1] += dtv * vy;
					smd_data_9[i][2] += dtv * vz;

					smd_data_9[i][3] += dtv * vx;
					smd_data_9[i][4] += dtv * vy;
					smd_data_9[i][5] += dtv * vz;

					smd_data_9[i][6] += dtv * vx;
					smd_data_9[i][7] += dtv * vy;
					smd_data_9[i][8] += dtv * vz;
				}

			}
		}
	}

	if (wiggleFlag) { // wiggle particles forward and backward

		wiggle_travel += sqrt(vx * vx + vy * vy + vz * vz ) * dtv;
		double wiggle_vx = wiggle_direction * vx;
		double wiggle_vy = wiggle_direction * vy;
		double wiggle_vz = wiggle_direction * vz;

		//printf("wiggle vz is %f, wiggle_max_travel is %f, dir=%f\n", wiggle_vz, wiggle_max_travel, wiggle_direction);

		for (i = 0; i < nlocal; i++) {
			if (mask[i] & groupbit) {

				v[i][0] = wiggle_vx;
				v[i][1] = wiggle_vy;
				v[i][2] = wiggle_vz;

				vest[i][0] = wiggle_vx;
				vest[i][1] = wiggle_vy;
				vest[i][2] = wiggle_vz;

				x[i][0] += dtv * wiggle_vx;
				x[i][1] += dtv * wiggle_vy;
				x[i][2] += dtv * wiggle_vz;

				/*
				 * if this is a triangle, move the vertices as well
				 */

				if (mol[i] >= 65535) {
					smd_data_9[i][0] += dtv * wiggle_vx;
					smd_data_9[i][1] += dtv * wiggle_vy;
					smd_data_9[i][2] += dtv * wiggle_vz;

					smd_data_9[i][3] += dtv * wiggle_vx;
					smd_data_9[i][4] += dtv * wiggle_vy;
					smd_data_9[i][5] += dtv * wiggle_vz;

					smd_data_9[i][6] += dtv * wiggle_vx;
					smd_data_9[i][7] += dtv * wiggle_vy;
					smd_data_9[i][8] += dtv * wiggle_vz;
				}

			}
		}

		if (wiggle_travel >= wiggle_max_travel) {
			wiggle_direction *= -1.0;
			wiggle_travel = 0.0;
		}
	}

	if (rotateFlag) { // rotate particles
		Vector3d xnew, R_new, x_correct;

		/*
		 * rotation angle and matrix form of Rodrigues' rotation formula
		 */

		phi = MY_2PI * dtv / rotation_period;
		//printf("dt=%f, phi =%f, T=%f\n", dtv, phi, rotation_period);
		Rot = cos(phi) * eye + sin(phi) * u_cross + (1.0 - cos(phi)) * uxu;

		for (i = 0; i < nlocal; i++) {
			if (mask[i] & groupbit) {

				/*
				 * generate vector R from origin to point which is to be rotated
				 */
				point << x[i][0], x[i][1], x[i][2];
				R = point - origin;

				/*
				 * rotate vector and shift away from origin
				 */
				rotated_point = Rot * R + origin;

				/*
				 * determine velocity
				 */
				vel = (rotated_point - point) / update->dt;

				/*
				 * assign new velocities and coordinates
				 */
				v[i][0] = vel(0);
				v[i][1] = vel(1);
				v[i][2] = vel(2);

				vest[i][0] = vel(0);
				vest[i][1] = vel(1);
				vest[i][2] = vel(2);

				x[i][0] = rotated_point(0);
				x[i][1] = rotated_point(1);
				x[i][2] = rotated_point(2);

				/*
				 * if this is a triangle, rotate the vertices as well
				 */

				if (mol[i] >= 65535) {

					v1 << smd_data_9[i][0], smd_data_9[i][1], smd_data_9[i][2];
					R = v1 - origin;
					rotated_point = Rot * R + origin;
					vel = (rotated_point - v1) / update->dt;
					smd_data_9[i][0] = rotated_point(0);
					smd_data_9[i][1] = rotated_point(1);
					smd_data_9[i][2] = rotated_point(2);
					v1 = rotated_point;

					v2 << smd_data_9[i][3], smd_data_9[i][4], smd_data_9[i][5];
					R = v2 - origin;
					rotated_point = Rot * R + origin;
					vel = (rotated_point - v2) / update->dt;
					smd_data_9[i][3] = rotated_point(0);
					smd_data_9[i][4] = rotated_point(1);
					smd_data_9[i][5] = rotated_point(2);
					v2 = rotated_point;

					v3 << smd_data_9[i][6], smd_data_9[i][7], smd_data_9[i][8];
					R = v3 - origin;
					rotated_point = Rot * R + origin;
					vel = (rotated_point - v3) / update->dt;
					smd_data_9[i][6] = rotated_point(0);
					smd_data_9[i][7] = rotated_point(1);
					smd_data_9[i][8] = rotated_point(2);
					v3 = rotated_point;

					// recalculate triangle normal
					n = (v2 - v1).cross(v2 - v3);
					n /= n.norm();
					x0[i][0] = n(0);
					x0[i][1] = n(1);
					x0[i][2] = n(2);

				}

			}
		}
	}

	// we changed smd_data_9, x0. perform communication to ghosts
	comm->forward_comm_fix(this);

}

/* ---------------------------------------------------------------------- */

void FixSMDMoveTriSurf::reset_dt() {
	dtv = update->dt;
}

/* ---------------------------------------------------------------------- */

int FixSMDMoveTriSurf::pack_forward_comm(int n, int *list, double *buf, int pbc_flag, int *pbc) {
	int i, j, m;
	double **x0 = atom->x0;
	double **smd_data_9 = atom->smd_data_9;

	//printf("in FixSMDIntegrateTlsph::pack_forward_comm\n");
	m = 0;
	for (i = 0; i < n; i++) {
		j = list[i];
		buf[m++] = x0[j][0];
		buf[m++] = x0[j][1];
		buf[m++] = x0[j][2];

		buf[m++] = smd_data_9[j][0];
		buf[m++] = smd_data_9[j][1];
		buf[m++] = smd_data_9[j][2];
		buf[m++] = smd_data_9[j][3];
		buf[m++] = smd_data_9[j][4];
		buf[m++] = smd_data_9[j][5];
		buf[m++] = smd_data_9[j][6];
		buf[m++] = smd_data_9[j][7];
		buf[m++] = smd_data_9[j][8];

	}
	return m;
}

/* ---------------------------------------------------------------------- */

void FixSMDMoveTriSurf::unpack_forward_comm(int n, int first, double *buf) {
	int i, m, last;
	double **x0 = atom->x0;
	double **smd_data_9 = atom->smd_data_9;

	//printf("in FixSMDMoveTriSurf::unpack_forward_comm\n");
	m = 0;
	last = first + n;
	for (i = first; i < last; i++) {
		x0[i][0] = buf[m++];
		x0[i][1] = buf[m++];
		x0[i][2] = buf[m++];

		smd_data_9[i][0] = buf[m++];
		smd_data_9[i][1] = buf[m++];
		smd_data_9[i][2] = buf[m++];
		smd_data_9[i][3] = buf[m++];
		smd_data_9[i][4] = buf[m++];
		smd_data_9[i][5] = buf[m++];
		smd_data_9[i][6] = buf[m++];
		smd_data_9[i][7] = buf[m++];
		smd_data_9[i][8] = buf[m++];
	}
}
