/*   EXTRAITS DE LA LICENCE
	Copyright CEA, contributeurs : Luc BILLARD et Damien
	CALISTE, laboratoire L_Sim, (2001-2005)
  
	Adresse mèl :
	BILLARD, non joignable par mèl ;
	CALISTE, damien P caliste AT cea P fr.

	Ce logiciel est un programme informatique servant à visualiser des
	structures atomiques dans un rendu pseudo-3D. 

	Ce logiciel est régi par la licence CeCILL soumise au droit français et
	respectant les principes de diffusion des logiciels libres. Vous pouvez
	utiliser, modifier et/ou redistribuer ce programme sous les conditions
	de la licence CeCILL telle que diffusée par le CEA, le CNRS et l'INRIA 
	sur le site "http://www.cecill.info".

	Le fait que vous puissiez accéder à cet en-tête signifie que vous avez 
	pris connaissance de la licence CeCILL, et que vous en avez accepté les
	termes (cf. le fichier Documentation/licence.fr.txt fourni avec ce logiciel).
*/

/*   LICENCE SUM UP
	Copyright CEA, contributors : Luc BILLARD et Damien
	CALISTE, laboratoire L_Sim, (2001-2005)

	E-mail address:
	BILLARD, not reachable any more ;
	CALISTE, damien P caliste AT cea P fr.

	This software is a computer program whose purpose is to visualize atomic
	configurations in 3D.

	This software is governed by the CeCILL  license under French law and
	abiding by the rules of distribution of free software.  You can  use, 
	modify and/ or redistribute the software under the terms of the CeCILL
	license as circulated by CEA, CNRS and INRIA at the following URL
	"http://www.cecill.info". 

	The fact that you are presently reading this means that you have had
	knowledge of the CeCILL license and that you accept its terms. You can
	find a copy of this licence shipped with this software at Documentation/licence.en.txt.
*/

#include "toolMatrix.h"

#include <visu_tools.h>
#include <math.h>

gboolean matrix_reducePrimitiveVectors(float reduced[6], double full[3][3])
{
  float X[3];
  float Y[3];
  float Z[3];
  double u[3], x[3];
  int i, j, k;
  double deltaIJ;
  float norm;

  g_return_val_if_fail(reduced && full, FALSE);

  DBG_fprintf(stderr, "Matrix : transform full to reduced matrix.\n");
  /* Compute the X vector of the new basis, colinear with old x. */
  for (i = 0; i < 3; i++)
    {
      X[i] = (float)full[0][i];
      x[i] = full[0][i];
    }

  /* Compute the Y vector of the new basis, orthogonal to X and
     coplanar with X and old y vector. */
  u[0] = full[0][1] * full[1][2] - full[0][2] * full[1][1];
  u[1] = full[0][2] * full[1][0] - full[0][0] * full[1][2];
  u[2] = full[0][0] * full[1][1] - full[0][1] * full[1][0];
/*   DBG_fprintf(stderr, "x        : %f %f %f\n", x[0], x[1], x[2]); */
/*   DBG_fprintf(stderr, "x vect y : %f %f %f\n", u[0], u[1], u[2]); */
  deltaIJ = x[0] * u[1] - x[1] * u[0];
  if (deltaIJ != 0.)
    {
      i = 0;
      j = 1;
      k = 2;
      DBG_fprintf(stderr, " Using deltaIJ scheme with (i, j, k)"
		  " = (%d, %d, %d)\n", i, j, k);
    }
  else
    {
      deltaIJ = x[0] * u[2] - x[2] * u[0];
      if (deltaIJ != 0.)
	{
	  i = 0;
	  j = 2;
	  k = 1;
	  DBG_fprintf(stderr, " Using deltaIJ scheme with (i, j, k)"
		      " = (%d, %d, %d)\n", i, j, k);
	}
      else
	{
	  deltaIJ = x[1] * u[2] - x[2] * u[1];
	  if (deltaIJ != 0.)
	    {
	      i = 1;
	      j = 2;
	      k = 0;
	      DBG_fprintf(stderr, " Using deltaIJ scheme with (i, j, k)"
			  " = (%d, %d, %d)\n", i, j, k);
	    }
	  else
	    {
	      g_warning("The input axes are not in 3D.");
	      return FALSE;
	    }
	}
    }
  Y[k] = -1.;
  Y[i] = (float)((x[k] * u[j] - x[j] * u[k]) / deltaIJ);
  Y[j] = (float)((x[i] * u[k] - x[k] * u[i]) / deltaIJ);
  /* We need to turn Y if y.Y is negative. */
  norm = 0.;
  for (i = 0; i < 3; i++)
    norm += full[1][i] * Y[i];
  if (norm < 0.)
  for (i = 0; i < 3; i++)
    Y[i] *= -1.;
    
  /* Compute the new Z vector in order to form a direct orthogonal
     basis with X and Y. */
  Z[0] = X[1] * Y[2] - X[2] * Y[1];
  Z[1] = X[2] * Y[0] - X[0] * Y[2];
  Z[2] = X[0] * Y[1] - X[1] * Y[0];

  /* Normalise the new basis (X, Y, Z). */
  norm = 0.;
  for (i = 0; i < 3; i++)
    norm += X[i] * X[i];
  norm = sqrt(norm);
  for (i = 0; i < 3; i++)
    X[i] /= norm;
  norm = 0.;
  for (i = 0; i < 3; i++)
    norm += Y[i] * Y[i];
  norm = sqrt(norm);
  for (i = 0; i < 3; i++)
    Y[i] /= norm;
  norm = 0.;
  for (i = 0; i < 3; i++)
    norm += Z[i] * Z[i];
  norm = sqrt(norm);
  for (i = 0; i < 3; i++)
    Z[i] /= norm;

/*   DBG_fprintf(stderr, "X : %f %f %f\n", X[0], X[1], X[2]); */
/*   DBG_fprintf(stderr, "Y : %f %f %f\n", Y[0], Y[1], Y[2]); */
/*   DBG_fprintf(stderr, "Z : %f %f %f\n", Z[0], Z[1], Z[2]); */

  /* Compute the reduce value for the basis. */
  reduced[0] = 0.;
  for (i = 0; i < 3; i++)
    reduced[0] += X[i] * full[0][i];

  reduced[1] = 0.;
  for (i = 0; i < 3; i++)
    reduced[1] += X[i] * full[1][i];

  reduced[2] = 0.;
  for (i = 0; i < 3; i++)
    reduced[2] += Y[i] * full[1][i];

  reduced[3] = 0.;
  for (i = 0; i < 3; i++)
    reduced[3] += X[i] * full[2][i];

  reduced[4] = 0.;
  for (i = 0; i < 3; i++)
    reduced[4] += Y[i] * full[2][i];

  reduced[5] = 0.;
  for (i = 0; i < 3; i++)
    reduced[5] += Z[i] * full[2][i];

  return TRUE;
}

void matrix_productMatrix(float matRes[3][3], float matA[3][3], float matB[3][3])
{
  int i, j, k;

  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++)
      {
	matRes[i][j] = 0.;
	for (k = 0; k < 3; k++)
	  matRes[i][j] += matA[i][k] * matB[k][j];
      }
}
void matrix_productVector(float vectRes[3], float mat[3][3], float vect[3])
{
  int i, j;

  for (i = 0; i < 3; i++)
    {
      vectRes[i] = 0.;
      for (j = 0; j < 3; j++)
	vectRes[i] += mat[i][j] * vect[j];
    }
}

#ifndef PI
#define PI 3.1415926
#endif

#ifndef RAD2DEG
#define RAD2DEG(x) ((180./PI)*x)
#endif

#ifndef DEG2RAD
#define DEG2RAD(x) ((PI/180.)*x)
#endif

/* s[0] = pho, s[1] = theta, s[2] = phi
   c[0] = x, c[1] = y, c[2] = z */
void matrix_cartesianToSpherical(float *spherical, float *cartesian)
{
  const float *c = cartesian;
  float *s = spherical;

  double rho;
  double theta; 
  double phi;

  if(c[0] == 0 && c[1] == 0 && c[2] == 0)
    {
      s[0] = 0;
      s[1] = 0;
      s[2] = 0;
      return;
    }

  rho = sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);

  if(c[0] == 0 && c[1] == 0)
    theta = (c[2] > 0) ? 0 : PI;
  else 
    theta = acos(c[2]/rho);

  if(c[0] != 0)
    {
      phi = atan(c[1]/c[0]) + PI*((c[0] < 0) ? 1 : 0);
    }
  else
    {
      if(c[1] == 0) /* facultatif*/
	phi = 0;    /* facultatif*/
      else if(c[1] > 0)
	phi = PI/2;
      else
	phi = -PI/2;
    }

  s[0] = rho;
  s[1] = /*fModulo(RAD2DEG(theta), 180);*/ RAD2DEG(theta);
  s[2] = fModulo(RAD2DEG(phi), 360);
}

void matrix_sphericalToCartesian(float *cartesian, float *spherical)
{
  cartesian[0] = spherical[0] * sin(DEG2RAD(spherical[1]))*cos(DEG2RAD(spherical[2]));
  cartesian[1] = spherical[0] * sin(DEG2RAD(spherical[1]))*sin(DEG2RAD(spherical[2]));
  cartesian[2] = spherical[0] * cos(DEG2RAD(spherical[1]));

  return;
}