/*
 * TesselationHelpers.cpp
 *
 *  Created on: Aug 3, 2009
 *      Author: heber
 */

#include "tesselationhelpers.hpp"

double det_get(gsl_matrix *A, int inPlace) {
  /*
  inPlace = 1 => A is replaced with the LU decomposed copy.
  inPlace = 0 => A is retained, and a copy is used for LU.
  */

  double det;
  int signum;
  gsl_permutation *p = gsl_permutation_alloc(A->size1);
  gsl_matrix *tmpA;

  if (inPlace)
  tmpA = A;
  else {
  gsl_matrix *tmpA = gsl_matrix_alloc(A->size1, A->size2);
  gsl_matrix_memcpy(tmpA , A);
  }


  gsl_linalg_LU_decomp(tmpA , p , &signum);
  det = gsl_linalg_LU_det(tmpA , signum);
  gsl_permutation_free(p);
  if (! inPlace)
  gsl_matrix_free(tmpA);

  return det;
};

void get_sphere(Vector *center, Vector &a, Vector &b, Vector &c, double RADIUS)
{
  gsl_matrix *A = gsl_matrix_calloc(3,3);
  double m11, m12, m13, m14;

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a.x[i]);
    gsl_matrix_set(A, i, 1, b.x[i]);
    gsl_matrix_set(A, i, 2, c.x[i]);
  }
  m11 = det_get(A, 1);

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a.x[i]*a.x[i] + b.x[i]*b.x[i] + c.x[i]*c.x[i]);
    gsl_matrix_set(A, i, 1, b.x[i]);
    gsl_matrix_set(A, i, 2, c.x[i]);
  }
  m12 = det_get(A, 1);

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a.x[i]*a.x[i] + b.x[i]*b.x[i] + c.x[i]*c.x[i]);
    gsl_matrix_set(A, i, 1, a.x[i]);
    gsl_matrix_set(A, i, 2, c.x[i]);
  }
  m13 = det_get(A, 1);

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a.x[i]*a.x[i] + b.x[i]*b.x[i] + c.x[i]*c.x[i]);
    gsl_matrix_set(A, i, 1, a.x[i]);
    gsl_matrix_set(A, i, 2, b.x[i]);
  }
  m14 = det_get(A, 1);

  if (fabs(m11) < MYEPSILON)
    cerr << "ERROR: three points are colinear." << endl;

  center->x[0] =  0.5 * m12/ m11;
  center->x[1] = -0.5 * m13/ m11;
  center->x[2] =  0.5 * m14/ m11;

  if (fabs(a.Distance(center) - RADIUS) > MYEPSILON)
    cerr << "ERROR: The given center is further way by " << fabs(a.Distance(center) - RADIUS) << " from a than RADIUS." << endl;

  gsl_matrix_free(A);
};



/**
 * Function returns center of sphere with RADIUS, which rests on points a, b, c
 * @param Center this vector will be used for return
 * @param a vector first point of triangle
 * @param b vector second point of triangle
 * @param c vector third point of triangle
 * @param *Umkreismittelpunkt new cneter point of circumference
 * @param Direction vector indicates up/down
 * @param AlternativeDirection vecotr, needed in case the triangles have 90 deg angle
 * @param Halfplaneindicator double indicates whether Direction is up or down
 * @param AlternativeIndicator doube indicates in case of orthogonal triangles which direction of AlternativeDirection is suitable
 * @param alpha double angle at a
 * @param beta double, angle at b
 * @param gamma, double, angle at c
 * @param Radius, double
 * @param Umkreisradius double radius of circumscribing circle
 */
void Get_center_of_sphere(Vector* Center, Vector a, Vector b, Vector c, Vector *NewUmkreismittelpunkt, Vector* Direction, Vector* AlternativeDirection,
    double HalfplaneIndicator, double AlternativeIndicator, double alpha, double beta, double gamma, double RADIUS, double Umkreisradius)
{
  Vector TempNormal, helper;
  double Restradius;
  Vector OtherCenter;
  cout << Verbose(3) << "Begin of Get_center_of_sphere.\n";
  Center->Zero();
  helper.CopyVector(&a);
  helper.Scale(sin(2.*alpha));
  Center->AddVector(&helper);
  helper.CopyVector(&b);
  helper.Scale(sin(2.*beta));
  Center->AddVector(&helper);
  helper.CopyVector(&c);
  helper.Scale(sin(2.*gamma));
  Center->AddVector(&helper);
  //*Center = a * sin(2.*alpha) + b * sin(2.*beta) + c * sin(2.*gamma) ;
  Center->Scale(1./(sin(2.*alpha) + sin(2.*beta) + sin(2.*gamma)));
  NewUmkreismittelpunkt->CopyVector(Center);
  cout << Verbose(4) << "Center of new circumference is " << *NewUmkreismittelpunkt << ".\n";
  // Here we calculated center of circumscribing circle, using barycentric coordinates
  cout << Verbose(4) << "Center of circumference is " << *Center << " in direction " << *Direction << ".\n";

  TempNormal.CopyVector(&a);
  TempNormal.SubtractVector(&b);
  helper.CopyVector(&a);
  helper.SubtractVector(&c);
  TempNormal.VectorProduct(&helper);
  if (fabs(HalfplaneIndicator) < MYEPSILON)
    {
      if ((TempNormal.ScalarProduct(AlternativeDirection) <0 and AlternativeIndicator >0) or (TempNormal.ScalarProduct(AlternativeDirection) >0 and AlternativeIndicator <0))
        {
          TempNormal.Scale(-1);
        }
    }
  else
    {
      if (TempNormal.ScalarProduct(Direction)<0 && HalfplaneIndicator >0 || TempNormal.ScalarProduct(Direction)>0 && HalfplaneIndicator<0)
        {
          TempNormal.Scale(-1);
        }
    }

  TempNormal.Normalize();
  Restradius = sqrt(RADIUS*RADIUS - Umkreisradius*Umkreisradius);
  cout << Verbose(4) << "Height of center of circumference to center of sphere is " << Restradius << ".\n";
  TempNormal.Scale(Restradius);
  cout << Verbose(4) << "Shift vector to sphere of circumference is " << TempNormal << ".\n";

  Center->AddVector(&TempNormal);
  cout << Verbose(0) << "Center of sphere of circumference is " << *Center << ".\n";
  get_sphere(&OtherCenter, a, b, c, RADIUS);
  cout << Verbose(0) << "OtherCenter of sphere of circumference is " << OtherCenter << ".\n";
  cout << Verbose(3) << "End of Get_center_of_sphere.\n";
};


/** Constructs the center of the circumcircle defined by three points \a *a, \a *b and \a *c.
 * \param *Center new center on return
 * \param *a first point
 * \param *b second point
 * \param *c third point
 */
void GetCenterofCircumcircle(Vector *Center, Vector *a, Vector *b, Vector *c)
{
  Vector helper;
  double alpha, beta, gamma;
  Vector SideA, SideB, SideC;
  SideA.CopyVector(b);
  SideA.SubtractVector(c);
  SideB.CopyVector(c);
  SideB.SubtractVector(a);
  SideC.CopyVector(a);
  SideC.SubtractVector(b);
  alpha = M_PI - SideB.Angle(&SideC);
  beta = M_PI - SideC.Angle(&SideA);
  gamma = M_PI - SideA.Angle(&SideB);
  //cout << Verbose(3) << "INFO: alpha = " << alpha/M_PI*180. << ", beta = " << beta/M_PI*180. << ", gamma = " << gamma/M_PI*180. << "." << endl;
  if (fabs(M_PI - alpha - beta - gamma) > HULLEPSILON)
    cerr << "GetCenterofCircumcircle: Sum of angles " << (alpha+beta+gamma)/M_PI*180. << " > 180 degrees by " << fabs(M_PI - alpha - beta - gamma)/M_PI*180. << "!" << endl;

  Center->Zero();
  helper.CopyVector(a);
  helper.Scale(sin(2.*alpha));
  Center->AddVector(&helper);
  helper.CopyVector(b);
  helper.Scale(sin(2.*beta));
  Center->AddVector(&helper);
  helper.CopyVector(c);
  helper.Scale(sin(2.*gamma));
  Center->AddVector(&helper);
  Center->Scale(1./(sin(2.*alpha) + sin(2.*beta) + sin(2.*gamma)));
};

/** Returns the parameter "path length" for a given \a NewSphereCenter relative to \a OldSphereCenter on a circle on the plane \a CirclePlaneNormal with center \a CircleCenter and radius \a CircleRadius.
 * Test whether the \a NewSphereCenter is really on the given plane and in distance \a CircleRadius from \a CircleCenter.
 * It calculates the angle, making it unique on [0,2.*M_PI) by comparing to SearchDirection.
 * Also the new center is invalid if it the same as the old one and does not lie right above (\a NormalVector) the base line (\a CircleCenter).
 * \param CircleCenter Center of the parameter circle
 * \param CirclePlaneNormal normal vector to plane of the parameter circle
 * \param CircleRadius radius of the parameter circle
 * \param NewSphereCenter new center of a circumcircle
 * \param OldSphereCenter old center of a circumcircle, defining the zero "path length" on the parameter circle
 * \param NormalVector normal vector
 * \param SearchDirection search direction to make angle unique on return.
 * \return Angle between \a NewSphereCenter and \a OldSphereCenter relative to \a CircleCenter, 2.*M_PI if one test fails
 */
double GetPathLengthonCircumCircle(Vector &CircleCenter, Vector &CirclePlaneNormal, double CircleRadius, Vector &NewSphereCenter, Vector &OldSphereCenter, Vector &NormalVector, Vector &SearchDirection)
{
  Vector helper;
  double radius, alpha;

  helper.CopyVector(&NewSphereCenter);
  // test whether new center is on the parameter circle's plane
  if (fabs(helper.ScalarProduct(&CirclePlaneNormal)) > HULLEPSILON) {
    cerr << "ERROR: Something's very wrong here: NewSphereCenter is not on the band's plane as desired by " <<fabs(helper.ScalarProduct(&CirclePlaneNormal))  << "!" << endl;
    helper.ProjectOntoPlane(&CirclePlaneNormal);
  }
  radius = helper.ScalarProduct(&helper);
  // test whether the new center vector has length of CircleRadius
  if (fabs(radius - CircleRadius) > HULLEPSILON)
    cerr << Verbose(1) << "ERROR: The projected center of the new sphere has radius " << radius << " instead of " << CircleRadius << "." << endl;
  alpha = helper.Angle(&OldSphereCenter);
  // make the angle unique by checking the halfplanes/search direction
  if (helper.ScalarProduct(&SearchDirection) < -HULLEPSILON)  // acos is not unique on [0, 2.*M_PI), hence extra check to decide between two half intervals
    alpha = 2.*M_PI - alpha;
  //cout << Verbose(2) << "INFO: RelativeNewSphereCenter is " << helper << ", RelativeOldSphereCenter is " << OldSphereCenter << " and resulting angle is " << alpha << "." << endl;
  radius = helper.Distance(&OldSphereCenter);
  helper.ProjectOntoPlane(&NormalVector);
  // check whether new center is somewhat away or at least right over the current baseline to prevent intersecting triangles
  if ((radius > HULLEPSILON) || (helper.Norm() < HULLEPSILON)) {
    //cout << Verbose(2) << "INFO: Distance between old and new center is " << radius << " and between new center and baseline center is " << helper.Norm() << "." << endl;
    return alpha;
  } else {
    //cout << Verbose(1) << "INFO: NewSphereCenter " << helper << " is too close to OldSphereCenter" << OldSphereCenter << "." << endl;
    return 2.*M_PI;
  }
};

struct Intersection {
  Vector x1;
  Vector x2;
  Vector x3;
  Vector x4;
};

/**
 * Intersection calculation function.
 *
 * @param x to find the result for
 * @param function parameter
 */
double MinIntersectDistance(const gsl_vector * x, void *params)
{
  double retval = 0;
  struct Intersection *I = (struct Intersection *)params;
  Vector intersection;
  Vector SideA,SideB,HeightA, HeightB;
  for (int i=0;i<NDIM;i++)
    intersection.x[i] = gsl_vector_get(x, i);

  SideA.CopyVector(&(I->x1));
  SideA.SubtractVector(&I->x2);
  HeightA.CopyVector(&intersection);
  HeightA.SubtractVector(&I->x1);
  HeightA.ProjectOntoPlane(&SideA);

  SideB.CopyVector(&I->x3);
  SideB.SubtractVector(&I->x4);
  HeightB.CopyVector(&intersection);
  HeightB.SubtractVector(&I->x3);
  HeightB.ProjectOntoPlane(&SideB);

  retval = HeightA.ScalarProduct(&HeightA) + HeightB.ScalarProduct(&HeightB);
  //cout << Verbose(2) << "MinIntersectDistance called, result: " << retval << endl;

  return retval;
};


/**
 * Calculates whether there is an intersection between two lines. The first line
 * always goes through point 1 and point 2 and the second line is given by the
 * connection between point 4 and point 5.
 *
 * @param point 1 of line 1
 * @param point 2 of line 1
 * @param point 1 of line 2
 * @param point 2 of line 2
 *
 * @return true if there is an intersection between the given lines, false otherwise
 */
bool existsIntersection(Vector point1, Vector point2, Vector point3, Vector point4)
{
  bool result;

  struct Intersection par;
    par.x1.CopyVector(&point1);
    par.x2.CopyVector(&point2);
    par.x3.CopyVector(&point3);
    par.x4.CopyVector(&point4);

    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
    gsl_multimin_fminimizer *s = NULL;
    gsl_vector *ss, *x;
    gsl_multimin_function minex_func;

    size_t iter = 0;
    int status;
    double size;

    /* Starting point */
    x = gsl_vector_alloc(NDIM);
    gsl_vector_set(x, 0, point1.x[0]);
    gsl_vector_set(x, 1, point1.x[1]);
    gsl_vector_set(x, 2, point1.x[2]);

    /* Set initial step sizes to 1 */
    ss = gsl_vector_alloc(NDIM);
    gsl_vector_set_all(ss, 1.0);

    /* Initialize method and iterate */
    minex_func.n = NDIM;
    minex_func.f = &MinIntersectDistance;
    minex_func.params = (void *)&par;

    s = gsl_multimin_fminimizer_alloc(T, NDIM);
    gsl_multimin_fminimizer_set(s, &minex_func, x, ss);

    do {
        iter++;
        status = gsl_multimin_fminimizer_iterate(s);

        if (status) {
          break;
        }

        size = gsl_multimin_fminimizer_size(s);
        status = gsl_multimin_test_size(size, 1e-2);

        if (status == GSL_SUCCESS) {
          cout << Verbose(2) << "converged to minimum" <<  endl;
        }
    } while (status == GSL_CONTINUE && iter < 100);

    // check whether intersection is in between or not
  Vector intersection, SideA, SideB, HeightA, HeightB;
  double t1, t2;
  for (int i = 0; i < NDIM; i++) {
    intersection.x[i] = gsl_vector_get(s->x, i);
  }

  SideA.CopyVector(&par.x2);
  SideA.SubtractVector(&par.x1);
  HeightA.CopyVector(&intersection);
  HeightA.SubtractVector(&par.x1);

  t1 = HeightA.Projection(&SideA)/SideA.ScalarProduct(&SideA);

  SideB.CopyVector(&par.x4);
  SideB.SubtractVector(&par.x3);
  HeightB.CopyVector(&intersection);
  HeightB.SubtractVector(&par.x3);

  t2 = HeightB.Projection(&SideB)/SideB.ScalarProduct(&SideB);

  cout << Verbose(2) << "Intersection " << intersection << " is at "
    << t1 << " for (" << point1 << "," << point2 << ") and at "
    << t2 << " for (" << point3 << "," << point4 << "): ";

  if (((t1 >= 0) && (t1 <= 1)) && ((t2 >= 0) && (t2 <= 1))) {
    cout << "true intersection." << endl;
    result = true;
  } else {
    cout << "intersection out of region of interest." << endl;
    result = false;
  }

  // free minimizer stuff
    gsl_vector_free(x);
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free(s);

  return result;
}