/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY AUTHORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/

/**
 * \file equation.c
 * \brief Source file with the equation data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include "config.h"
#include "utils.h"
#include "equation.h"

#define DEBUG_EQUATION 0        ///< macro to debug the equation functions.

long double r0[3];              ///< position vector.
long double r1[3];              ///< velocity vector.
long double r2[3];              ///< acceleration vector.
long double ro0[3];             ///< backup of the position vector.
long double ro1[3];             ///< backup of the velocity vector.
long double ro2[3];             ///< backup of the acceleration vector.
void (*equation_acceleration) (Equation * eq, long double *r0,
                               long double *r1, long double *r2, long double t);
///< pointer to the function to calculate the acceleration.
void (*equation_solution) (Equation * eq, long double *r0, long double *r1,
                           long double t);
///< pointer to the function to calculate the analytical solution.
long double (*equation_step_size) (Equation * eq);
///< pointer to the function to calculate the time step size.
int (*equation_land) (Equation * eq, long double, long double *t,
                      long double *dt);
///< pointer to the function to finalize the trajectory.
long double kt;
///< stability time step size coefficient.
long double dt;
///< time step size.
unsigned long int nevaluations;
///< number of evaluations of the acceleration function.

/**
 * Function to calculate the acceleration on non-resitance model.
 *
 * This function calculates the acceleration vector on a non-resistance
 * model. The movement equation is:
 * \f{equation}\ddot{\vec{r}}=\vec{g}\f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector.
 */
static void
equation_acceleration_0 (Equation * eq __attribute__ ((unused)),
                         ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1 __attribute__ ((unused)),
                         ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t __attribute__ ((unused)))
  ///< actual time.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_0: start\n");
#endif
  r2[0] = r2[1] = 0.L;
  r2[2] = -G;
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_0: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_0: end\n");
#endif
}

/**
 * Function to solve the non-resistance model.
 *
 * This function solves the movement on a resistance model characterized by the
 * movement equation:
 * \f{equation}{\ddot{\vec{r}}=\vec{g},\f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector.
 * The analytical solution is:
 * \f{equation}\dot{\vec{r}}=\dot{\vec{r}}_0+\vec{g}\,t,\f}
 * \f{equation}\vec{r}=\vec{r}_0+\dot{\vec{r}}_0\,t+\frac12\,\vec{g}\,t^2.\f}
 */
static void
equation_solution_0 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< time.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_0: start\n");
#endif
  r1[0] = eq->v[0];
  r1[1] = eq->v[1];
  r1[2] = eq->v[2] - eq->g * t;
  r0[0] = eq->r[0] + eq->v[0] * t;
  r0[1] = eq->r[1] + eq->v[1] * t;
  r0[2] = eq->r[2] + t * (eq->v[2] - t * 0.5L * G);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_0: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_0: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_0: end\n");
#endif
}

/**
 * Function to calculate the acceleration on the 1st resistance model.
 *
 * This function calculates the acceleration vector on a resistance model
 * model characterized by the movement equation:
 * \f[\ddot{\vec{r}}=\vec{g}-\lambda\,\left(\dot{\vec{r}}-\vec{w}\right)\f]
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector,
 * \f$\vec{w}=\left(w_x,\;w_y\;0\right)\f$ the wind velocity vector and
 * \f$\lambda\f$ a resistance coefficient.
 */
static void
equation_acceleration_1 (Equation * eq, ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1,       ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t __attribute__ ((unused)))
  ///< actual time.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_1: start\n");
#endif
  r2[0] = -eq->lambda * (r1[0] - eq->w[0]);
  r2[1] = -eq->lambda * (r1[1] - eq->w[1]);
  r2[2] = -eq->g - eq->lambda * r1[2];
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_1: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_1: end\n");
#endif
}

/**
 * Function to solve the 1st resistance model.
 *
 * This function solves the movement on a resistance model characterized by the
 * movement equation:
 * \f{equation}
 * \ddot{\vec{r}}=\vec{g}-\lambda\,\left(\dot{\vec{r}}-\vec{w}\right),
 * \f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector,
 * \f$\vec{w}=(w_x,\;w_y,\;0)\f$ the wind velocity vector and
 * \f$\lambda\f$ a resistance coefficient.
 * The analytical solution is:
 * \f{equation}
 * \dot{\vec{r}}=\dot{\vec{r}}_0\,\exp\left(-\lambda\,t\right)
 * +\left(\vec{w}+\frac{\vec{g}}{\lambda}\right)
 * \,\left[1-\exp\left(-\lambda\,t\right)\right],
 * \f}
 * \f{equation}
 * \vec{r}=\vec{r}_0+\left(\vec{w}+\frac{\vec{g}}{\lambda}\right)\,t
 * +\frac{\dot{\vec{r}}_0-\vec{w}-\vec{g}/\lambda}{\lambda}
 * \,\left[1-\exp\left(-\lambda\,t\right)\right].
 * \f}
 */
static void
equation_solution_1 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< time.
{
  long double v[2];
  long double li, gl, elt, k;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_1: start\n");
#endif
  v[0] = eq->v[0] - eq->w[0];
  v[1] = eq->v[1] - eq->w[1];
  elt = expl (-eq->lambda * t);
  r1[0] = eq->w[0] + v[0] * elt;
  r1[1] = eq->w[1] + v[1] * elt;
  li = 1.L / eq->lambda;
  gl = eq->g * li;
  r1[2] = (eq->v[2] + gl) * elt - gl;
  k = li * (1.L - elt);
  r0[0] = eq->r[0] + eq->w[0] * t + v[0] * k;
  r0[1] = eq->r[1] + eq->w[1] * t + v[1] * k;
  r0[2] = eq->r[2] - gl * t + (eq->v[2] + gl) * k;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_1: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_1: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_1: end\n");
#endif
}

/**
 * Function to calculate the acceleration on 2nd resistance model.
 *
 * This function calculates the acceleration vector on a resistance model
 * model characterized by the movement equations:
 * \f[\left.\begin{array}{r}
 * \ddot{x}=-\lambda\,\left|\dot{x}-w_x\right|\,\left(\dot{x}-w_x\right),\\
 * \ddot{y}=-\lambda\,\left|\dot{y}-w_y\right|\,\left(\dot{y}-w_y\right),\\
 * \ddot{z}=-g-\lambda\,\left|\dot{z}\right|\,\dot{z},
 * \end{array}\right\}\f]
 * with g the gravitational constant, \f$w_x\f$ and \f$w_y\f$ the wind velocity
 * vector components and \f$\lambda\f$ a resistance coefficient.
 */
static void
equation_acceleration_2 (Equation * eq, ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1,       ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t __attribute__ ((unused)))
  ///< actual time.
{
  long double v[2];
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_2: start\n");
#endif
  v[0] = r1[0] - eq->w[0];
  v[1] = r1[1] - eq->w[1];
  r2[0] = -eq->lambda * fabsl (v[0]) * v[0];
  r2[1] = -eq->lambda * fabsl (v[1]) * v[1];
  r2[2] = -eq->g - eq->lambda * fabsl (r1[2]) * r1[2];
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_2: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_2: end\n");
#endif
}

/**
 * Function to solve the 2nd resistance mode.
 *
 * This function solves the movement on a resistance model characterized by the
 * movement equations:
 * \f{equation}\left.\begin{array}{r}
 * \ddot{x}=-\lambda\,\left|\dot{x}-w_x\right|\,\left(\dot{x}-w_x\right)\\
 * \ddot{y}=-\lambda\,\left|\dot{y}-w_y\right|\,\left(\dot{y}-w_y\right)\\
 * \ddot{z}=-g-\lambda\,\left|\dot{z}\right|\,\dot{z}
 * \end{array}\right\}\f}
 * with g the gravitational constant, \f$w_x\f$ and \f$w_y\f$ the wind velocity
 * vector components and \f$\lambda\f$ a resistance coefficient.
 * The analytical solution is:
 * \f{equation}
 * \dot{x}=w_x+\frac{\dot{x}_0-w_x}{1+\lambda\,\left|\dot{x}_0-w_x\right|\,t},
 * \f}
 * \f{equation}
 * \dot{y}=w_y+\frac{\dot{y}_0-w_y}{1+\lambda\,\left|\dot{y}_0-w_y\right|\,t},
 * \f}
 * \f{equation}
 * \dot{z}=\left\{\begin{array}{cl}
 * \dot{z}_0\leq 0\Rightarrow & \sqrt{\frac{g}{\lambda}}\,
 * \frac{\dot{z}_0\,\cosh\left(\sqrt{g\,\lambda}\,t\right)
 * -\sqrt{g/\lambda}\,\sinh\left(\sqrt{g\,\lambda}\,t\right)}
 * {\sqrt{g/\lambda}\,\cosh\left(\sqrt{g\,\lambda}\,t\right)
 * -\dot{z}_0\,\sinh\left(\sqrt{g\,\lambda}\,t\right)},\\
 * \dot{z}_0>0,\;
 * t\leq\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * \sqrt{\frac{g}{\lambda}}
 * \,\tan\left[\arctan\left(\dot{z}_0\,\sqrt{\frac{\lambda}{g}}\right)
 * -\sqrt{g\,\lambda}\,t\right],\\
 * \dot{z}_0>0,\;
 * t>\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * -\sqrt{\frac{g}{\lambda}}\,\tanh\left[\sqrt{g\,\lambda}\,t
 *  -\arctan\left(\dot{z}_0\,\sqrt{\frac{\lambda}{g}}\right)\right],
 * \end{array}\right.
 * \f}
 * \f{equation}
 * x=x_0+w_x\,t+\frac{\dot{x}_0-w_x}{\lambda\,\left|\dot{x}_0-w_x\right|}
 * \,\ln\left(1+\lambda\,\left|\dot{x}_0-w_x\right|\,t\right),
 * \f}
 * \f{equation}
 * y=y_0+w_y\,t+\frac{\dot{y}_0-w_y}{\lambda\,\left|\dot{y}_0-w_y\right|}
 * \,\ln\left(1+\lambda\,\left|\dot{y}_0-w_y\right|\,t\right),
 * \f}
 * \f{equation}
 * z=\left\{\begin{array}{cl}
 * \dot{z}_0\leq 0\Rightarrow & z_0
 * -\frac{\ln\left[\cosh\left(\sqrt{g\,\lambda}\,t\right)
 * -\dot{z}_0\,\sqrt{\lambda/g}\,
 * \sinh\left(\sqrt{g\,\lambda}\,t\right)\right]}{\lambda},\\
 * \dot{z}_0>0,\;
 * t\leq\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * z_0+\frac{1}{\lambda}\,\ln\left\{\frac{\cos\left[
 * \arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)-\sqrt{g\,\lambda}\,t\right]}
 * {\cos\left[\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)\right]}\right\},\\
 * \dot{z}_0>0,\;
 * t>\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * z_0-\frac{\ln\left\{
 * \cos\left[\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)\right]
 * \,\cosh\left[\sqrt{g\,\lambda}\,t
 * -\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)\right]\right\}}{\lambda}
 * \end{array}\right.
 * \f}
 */
static void
equation_solution_2 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< actual time.
{
  long double v[2], k[2];
  long double tc, g_l, gl, glt, lt, li, alpha;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_2: start\n");
#endif
  v[0] = eq->v[0] - eq->w[0];
  v[1] = eq->v[1] - eq->w[1];
  lt = eq->lambda * t;
  k[0] = 1.L + lt * fabsl (v[0]);
  k[1] = 1.L + lt * fabsl (v[1]);
  r1[0] = eq->w[0] + v[0] / k[0];
  r1[1] = eq->w[1] + v[1] / k[1];
  li = 1.L / eq->lambda;
  k[0] = li * logl (k[0]);
  k[1] = li * logl (k[1]);
  r0[0] = eq->r[0] + eq->w[0] * t;
  r0[1] = eq->r[1] + eq->w[1] * t;
  if (v[0] >= 0.L)
    r0[0] += k[0];
  else
    r0[0] -= k[0];
  if (v[1] >= 0.L)
    r0[1] += k[1];
  else
    r0[1] -= k[1];
  gl = sqrtl (eq->g * eq->lambda);
  glt = gl * t;
  g_l = sqrtl (eq->g / eq->lambda);
  r0[2] = eq->r[2];
  if (eq->v[2] <= 0.L)
    {
      k[0] = coshl (glt);
      k[1] = sinhl (glt);
      r1[2] = g_l * (eq->v[2] * k[0] - g_l * k[1])
        / (g_l * k[0] - eq->v[2] * k[1]);
      r0[2] -= li * logl (k[0] - eq->v[2] * k[1] / g_l);
    }
  else
    {
      alpha = atanl (eq->v[2] / g_l);
      tc = alpha / gl;
      if (t <= tc)
        {
          r1[2] = g_l * tanl (alpha - glt);
          r0[2] += li * logl (cosl (alpha - glt) / cosl (alpha));
        }
      else
        {
          t -= tc;
          glt = gl * t;
          r1[2] = -g_l * tanhl (glt);
          r0[2] -= li * logl (cosl (alpha) * coshl (glt));
        }
    }
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_2: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_2: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_2: end\n");
#endif
}

/**
 * Function to calculate the acceleration on a forced model.
 *
 * This function calculates the acceleration vector on a forced model. The
 * movement equation is:
 * \f{equation}
 * \ddot{\vec{r}}=\vec{g}+\vec{w}\,\exp\left(-\lambda\,t\right)
 * \f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector, \f$\lambda\f$ the
 * force decay factor and \f$\vec{w}=\left(w_x,\;w_y\;0\right)\f$ the force
 * vector.
 */
static void
equation_acceleration_3 (Equation * eq, ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1 __attribute__ ((unused)),
                         ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t) ///< actual time.
{
  long double elt;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_3: start\n");
#endif
  elt = expl (-eq->lambda * t);
  r2[0] = eq->w[0] * elt;
  r2[1] = eq->w[1] * elt;
  r2[2] = -G;
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_3: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_3: end\n");
#endif
}

/**
 * Function to solve the forced model.
 *
 * This function solves the movement on a forced model characterized by the
 * movement equation:
 * \f{equation}
 * \ddot{\vec{r}}=\vec{g}+\vec{w}\,\exp\left(-\lambda\,t\right)
 * \f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector, \f$\lambda\f$ the
 * force decay factor and \f$\vec{w}=\left(w_x,\;w_y\;0\right)\f$ the force
 * vector.
 * The analytical solution is:
 * \f{equation}
 * \dot{\vec{r}}=\dot{\vec{r}}_0+\vec{g}\,t
 * +\frac{\vec{w}}{\lambda}\left[1-\exp\left(-\lambda\,t\right)\right],\f}
 * \f{equation}\vec{r}=\vec{r}_0
 * +\left(\dot{\vec{r}}_0+\frac{\vec{w}}{\lambda}\right)\,t
 * +\frac12\,\vec{g}\,t^2
 * +\frac{\vec{w}}{\lambda^2}\,\left[1-\exp\left(-\lambda\,t\right)\right].
 * \f}
 */
static void
equation_solution_3 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< time.
{
  long double li, k;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_3: start\n");
#endif
  li = 1.L / eq->lambda;
  k = li * (1.L - expl (-eq->lambda * t));
  r1[0] = eq->v[0] + eq->w[0] * k;
  r1[1] = eq->v[1] + eq->w[1] * k;
  r1[2] = eq->v[2] - eq->g * t;
  k *= li;
  r0[0] = eq->r[0] + (eq->v[0] + eq->w[0] * li) * t - eq->w[0] * k;
  r0[1] = eq->r[1] + (eq->v[1] + eq->w[1] * li) * t - eq->w[1] * k;
  r0[2] = eq->r[2] + t * (eq->v[2] - t * 0.5L * G);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_3: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_3: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_3: end\n");
#endif
}

/**
 * Function to solve numerically the equation by the mean point method.
 *
 * \return solution time.
 */
long double
equation_solve (Equation * eq,  ///< Equation struct.
                long double *r0,        ///< position vector solution.
                long double *r1)        ///< velocity vector solution.
{
  long double r02[3], r12[3];
  long double t1, t2, t3;
  unsigned int i;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solve: start\n");
#endif
  t1 = 0.L;
  t2 = 1.L;
  equation_solution (eq, r02, r12, t2);
  while (r02[2] > 0.L)
    {
      t2 *= 2.L;
      equation_solution (eq, r02, r12, t2);
    }
  for (i = 0; i < 64; ++i)
    {
      t3 = 0.5L * (t1 + t2);
      equation_solution (eq, r02, r12, t3);
      if (r02[2] > 0.L)
        t1 = t3;
      else
        t2 = t3;
    }
  memcpy (r0, r02, 3 * sizeof (long double));
  memcpy (r1, r12, 3 * sizeof (long double));
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solve: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solve: x=%Lg y=%Lg z=%Lg\n", r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solve: t=%Lg\n", t3);
  fprintf (stderr, "equation_solve: end\n");
#endif
  return t3;
}

/**
 * Function to set a constant time step size.
 *
 * \return time step size.
 */
static long double
equation_step_size_0 (Equation * eq __attribute__ ((unused)))
///< Equation struct.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_0: start\n");
  fprintf (stderr, "equation_step_size_0: dt=%Lg\n", dt);
  fprintf (stderr, "equation_step_size_0: end\n");
#endif
  return dt;
}

/**
 * Function to set the time step size based on stability condition for the 1st
 * resistance model.
 *
 * \return time step size.
 */
static long double
equation_step_size_1 (Equation * eq)    ///< Equation struct.
{
  long double dt;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_1: start\n");
#endif
  dt = kt / fabsl (eq->lambda);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_1: dt=%Lg\n", dt);
  fprintf (stderr, "equation_step_size_1: end\n");
#endif
  return dt;
}

/**
 * Function to set the time step size based on stability condition for the 2nd
 * resistance model.
 *
 * \return time step size.
 */
static long double
equation_step_size_2 (Equation * eq)    ///< Equation struct.
{
  long double dt;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_2: start\n");
#endif
  dt = kt / (fabsl (eq->lambda) *
             fmaxl (fabsl (r1[0] - eq->w[0]),
                    fmaxl (fabsl (r1[1] - eq->w[1]), fabsl (r1[2]))));
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_2: dt=%Lg\n", dt);
  fprintf (stderr, "equation_step_size_2: end\n");
#endif
  return dt;
}

/**
 * Function to finish the trajectory based on final time.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_0 (Equation * eq, ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double tf;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_0: start\n");
  fprintf (stderr, "equation_land_0: to=%Lg\n", to);
#endif
  tf = eq->tf;
  if (to >= tf)
    {
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_0: landing\n");
      fprintf (stderr, "equation_land_0: end\n");
#endif
      return 1;
    }
  *t = to + *dt;
  if (*t >= tf)
    {
      *dt = tf - to;
      *t = tf;
    }
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_0: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_0: no landing\n");
  fprintf (stderr, "equation_land_0: end\n");
#endif
  return 0;
}

/**
 * Function to finish the trajectory based on 1st order landing.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_1 (Equation * eq __attribute__ ((unused)),
                 ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_1: start\n");
  fprintf (stderr, "equation_land_1: to=%Lg\n", to);
#endif
  if (r0[2] > 0.)
    {
      *t = to + *dt;
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_1: t=%Lg dt=%Lg\n", *t, *dt);
      fprintf (stderr, "equation_land_1: no landing\n");
      fprintf (stderr, "equation_land_1: end\n");
#endif
      return 0;
    }
  h = r0[2] / r1[2];
  r0[0] -= h * r1[0];
  r0[1] -= h * r1[1];
  r0[2] -= h * r1[2];
  r1[0] -= h * r2[0];
  r1[1] -= h * r2[1];
  r1[2] -= h * r2[2];
  *t = to - h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_1: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_1: landing\n");
  fprintf (stderr, "equation_land_1: end\n");
#endif
  return 1;
}

/**
 * Function to finish the trajectory based on 2nd order landing.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_2 (Equation * eq __attribute__ ((unused)),
                 ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_2: start\n");
  fprintf (stderr, "equation_land_2: to=%Lg\n", to);
#endif
  if (r0[2] > 0.)
    {
      *t = to + *dt;
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_2: t=%Lg dt=%Lg\n", *t, *dt);
      fprintf (stderr, "equation_land_2: no landing\n");
      fprintf (stderr, "equation_land_2: end\n");
#endif
      return 0;
    }
  h = solve_quadratic (0.5L * r2[2], -r1[2], r0[2], 0.L, *dt);
  r0[0] -= h * (r1[0] - h * 0.5L * r2[0]);
  r0[1] -= h * (r1[1] - h * 0.5L * r2[1]);
  r0[2] -= h * (r1[2] - h * 0.5L * r2[2]);
  r1[0] -= h * r2[0];
  r1[1] -= h * r2[1];
  r1[2] -= h * r2[2];
  *t = to - h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_2: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_2: landing\n");
  fprintf (stderr, "equation_land_2: end\n");
#endif
  return 1;
}

/**
 * Function to finish the trajectory based on 3rd order landing.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_3 (Equation * eq __attribute__ ((unused)),
                 ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double h, r3[3];
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_3: start\n");
  fprintf (stderr, "equation_land_3: to=%Lg\n", to);
#endif
  if (r0[2] > 0.)
    {
      *t = to + *dt;
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_3: t=%Lg dt=%Lg\n", *t, *dt);
      fprintf (stderr, "equation_land_3: no landing\n");
      fprintf (stderr, "equation_land_3: end\n");
#endif
      return 0;
    }
  r3[0] = (r2[0] - ro2[0]) / *dt;
  r3[1] = (r2[1] - ro2[1]) / *dt;
  r3[2] = (r2[2] - ro2[2]) / *dt;
  h = solve_cubic (-1.L / 6.L * r3[2], 0.5L * r2[2], -r1[2], r0[2], 0.L, *dt);
  r0[0] -= h * (r1[0] - h * (0.5L * r2[0] - h * 1.L / 6.L * r3[0]));
  r0[1] -= h * (r1[1] - h * (0.5L * r2[1] - h * 1.L / 6.L * r3[1]));
  r0[2] -= h * (r1[2] - h * (0.5L * r2[2] - h * 1.L / 6.L * r3[2]));
  r1[0] -= h * (r2[0] - h * 0.5L * r3[0]);
  r1[1] -= h * (r2[1] - h * 0.5L * r3[1]);
  r1[2] -= h * (r2[2] - h * 0.5L * r3[2]);
  *t = to - h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_3: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_3: landing\n");
  fprintf (stderr, "equation_land_3: end\n");
#endif
  return 1;
}

/**
 * Function to init the equation variables.
 */
void
equation_init (Equation * eq,   ///< Equation struct.
               gsl_rng * rng)   ///< gsl_rng struct.
{
  long double v, ha, va;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_init: start\n");
#endif
  switch (eq->type)
    {
    case 1:
    case 2:
    case 3:
      eq->lambda = eq->min_lambda
        + (eq->max_lambda - eq->min_lambda) * gsl_rng_uniform (rng);
    }
  v = eq->min_velocity
    + (eq->max_velocity - eq->min_velocity) * gsl_rng_uniform (rng);
  ha = 2.L * M_PIl * gsl_rng_uniform (rng);
  eq->r[0] = eq->r[1] = 0.L;
  va = eq->vertical_angle * M_PIl / 180.L;
  eq->v[2] = v * sinl (va);
  eq->v[1] = v * cosl (va);
  eq->v[0] = eq->v[1] * cosl (ha);
  eq->v[1] *= sinl (ha);
  v = eq->max_wind * gsl_rng_uniform (rng);
  ha = 2.L * M_PIl * gsl_rng_uniform (rng);
  eq->w[0] = v * cosl (ha);
  eq->w[1] = v * sinl (ha);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_init: end\n");
#endif
}

/**
 * Function to read the equation data on a XML node.
 *
 * \return 1 on success, 0 on error.
 */
int
equation_read_xml (Equation * eq,       ///< Equation struct.
                   xmlNode * node,      ///< XML node.,
									 unsigned int initial)   ///< type of initial conditions.
{
  const char *message[] = {
    "Bad XML node",
    "Bad type",
    "Unknown type",
    "Bad x",
    "Bad y",
    "Bad z",
    "Bad vx",
    "Bad vy",
    "Bad vz",
    "Bad wx",
    "Bad wy",
    "Bad lambda",
		"Bad minimum velocity",
		"Bad maximum velocity",
		"Bad vertical angle",
		"Bad maximum wind",
		"Bad minimum lambda",
		"Bad maximum lambda",
    "Bad g",
    "Bad time step type",
    "Bad dt",
    "Bad kt",
    "Unknown time step type",
    "Bad land type",
    "Bad t",
    "Unknown land type"
  };
  int e, error_code;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: start\n");
  fprintf (stderr, "equation_read_xml: name=%s\n", node->name);
#endif
  if (xmlStrcmp (node->name, XML_EQUATION))
    {
      e = 0;
      goto exit_on_error;
    }
  eq->type = xml_node_get_uint (node, XML_TYPE, &error_code);
  if (error_code)
    {
      e = 1;
      goto exit_on_error;
    }
  switch (eq->type)
    {
    case 0:
      equation_acceleration = equation_acceleration_0;
      equation_solution = equation_solution_0;
      break;
    case 1:
      equation_acceleration = equation_acceleration_1;
      equation_solution = equation_solution_1;
      break;
    case 2:
      equation_acceleration = equation_acceleration_2;
      equation_solution = equation_solution_2;
      break;
    case 3:
      equation_acceleration = equation_acceleration_3;
      equation_solution = equation_solution_3;
      break;
    default:
      e = 2;
      goto exit_on_error;
    }
  eq->r[0] = xml_node_get_float_with_default (node, XML_X, 0.L, &error_code);
  if (error_code)
    {
      e = 3;
      goto exit_on_error;
    }
  eq->r[1] = xml_node_get_float_with_default (node, XML_Y, 0.L, &error_code);
  if (error_code)
    {
      e = 4;
      goto exit_on_error;
    }
  eq->r[2] = xml_node_get_float (node, XML_Z, &error_code);
  if (error_code || eq->r[2] < 0.L)
    {
      e = 5;
      goto exit_on_error;
    }
	if (initial)
	  {
      eq->v[0]
				= xml_node_get_float_with_default (node, XML_VX, 0.L, &error_code);
      if (error_code)
        {
          e = 6;
          goto exit_on_error;
        }
      eq->v[1]
				= xml_node_get_float_with_default (node, XML_VY, 0.L, &error_code);
      if (error_code)
        {
          e = 7;
          goto exit_on_error;
        }
      eq->v[2]
				= xml_node_get_float_with_default (node, XML_VZ, 0.L, &error_code);
      if (error_code)
        {
          e = 8;
          goto exit_on_error;
        }
      eq->w[0]
				= xml_node_get_float_with_default (node, XML_WX, 0.L, &error_code);
      if (error_code)
        {
          e = 9;
          goto exit_on_error;
        }
      eq->w[1]
			 	= xml_node_get_float_with_default (node, XML_WY, 0.L, &error_code);
      if (error_code)
        {
          e = 10;
          goto exit_on_error;
        }
      switch (eq->type)
        {
        case 1:
        case 2:
        case 3:
          eq->lambda = xml_node_get_float_with_default (node, XML_LAMBDA, 0.L,
							                                          &error_code);
          if (error_code)
            {
              e = 11;
              goto exit_on_error;
            }
				}
		}
	else
	  {
      eq->min_velocity
				= xml_node_get_float_with_default (node, XML_VMIN, 0.L, &error_code);
			if (error_code || eq->min_velocity < 0.L)
			  {
					e = 12;
					goto exit_on_error;
				}
      eq->max_velocity = xml_node_get_float (node, XML_VMAX, &error_code);
			if (error_code || eq->max_velocity <= 0.L)
			  {
					e = 13;
					goto exit_on_error;
				}
      eq->vertical_angle
				= xml_node_get_float (node, XML_VERTICAL_ANGLE, &error_code);
			if (error_code)
			  {
					e = 14;
					goto exit_on_error;
				}
      eq->max_wind
				= xml_node_get_float_with_default (node, XML_WMAX, 0.L, &error_code);
			if (error_code || eq->max_wind < 0.L)
			  {
					e = 15;
					goto exit_on_error;
				}
      switch (eq->type)
        {
        case 1:
        case 2:
        case 3:
          eq->min_lambda
						= xml_node_get_float_with_default (node, XML_LAMBDA_MIN, 0.L,
								                               &error_code);
				  if (error_code)
					  {
						  e = 16;
						  goto exit_on_error;
						}
          eq->max_lambda
						= xml_node_get_float_with_default (node, XML_LAMBDA_MAX, 0.L,
								                               &error_code);
				  if (error_code || eq->max_lambda < eq->min_lambda)
					  {
						  e = 17;
						  goto exit_on_error;
						}
			   }
		}
  eq->g = xml_node_get_float_with_default (node, XML_G, G, &error_code);
  if (error_code)
    {
      e = 18;
      goto exit_on_error;
    }
  eq->size_type = xml_node_get_uint (node, XML_TIME_STEP, &error_code);
  if (error_code)
    {
      e = 19;
      goto exit_on_error;
    }
  switch (eq->size_type)
    {
    case 0:
      equation_step_size = equation_step_size_0;
      dt = xml_node_get_float (node, XML_DT, &error_code);
      if (error_code)
        {
          e = 20;
          goto exit_on_error;
        }
      break;
    case 1:
      switch (eq->type)
        {
        case 1:
          equation_step_size = equation_step_size_1;
          break;
        case 2:
          equation_step_size = equation_step_size_2;
        }
      kt = xml_node_get_float (node, XML_KT, &error_code);
      if (error_code)
        {
          e = 21;
          goto exit_on_error;
        }
      break;
    default:
      e = 22;
      goto exit_on_error;
    }
  eq->land_type = xml_node_get_uint (node, XML_LAND, &error_code);
  if (error_code)
    {
      e = 23;
      goto exit_on_error;
    }
  switch (eq->land_type)
    {
    case 0:
      equation_land = equation_land_0;
      eq->tf = xml_node_get_float (node, XML_T, &error_code);
      if (error_code || eq->tf < 0.)
        {
          e = 24;
          goto exit_on_error;
        }
      break;
    case 1:
      equation_land = equation_land_1;
      break;
    case 2:
      equation_land = equation_land_2;
      break;
    case 3:
      equation_land = equation_land_3;
      break;
    default:
      e = 25;
      goto exit_on_error;
    }
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: success\n");
  fprintf (stderr, "equation_read_xml: end\n");
#endif
  return 1;
exit_on_error:
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: error\n");
#endif
  error_add (message[e]);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: end\n");
#endif
  return 0;
}


/**
 *
 * @file testing_dgels.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @author Hatem Ltaief
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:18 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_dmain.h"

#undef COMPLEX
#define REAL

enum blas_order_type {
            blas_rowmajor = 101,
            blas_colmajor = 102 };

enum blas_uplo_type  {
            blas_upper = 121,
            blas_lower = 122 };

enum blas_cmach_type {
            blas_base      = 151,
            blas_t         = 152,
            blas_rnd       = 153,
            blas_ieee      = 154,
            blas_emin      = 155,
            blas_emax      = 156,
            blas_eps       = 157,
            blas_prec      = 158,
            blas_underflow = 159,
            blas_overflow  = 160,
            blas_sfmin     = 161};

enum blas_norm_type {
            blas_one_norm       = 171,
            blas_real_one_norm  = 172,
            blas_two_norm       = 173,
            blas_frobenius_norm = 174,
            blas_inf_norm       = 175,
            blas_real_inf_norm  = 176,
            blas_max_norm       = 177,
            blas_real_max_norm  = 178 };

static void
BLAS_error(char *rname, int err, int val, int x) {
  fprintf( stderr, "%s %d %d %d\n", rname, err, val, x );
  abort();
}

static
void
BLAS_dsy_norm(enum blas_order_type order, enum blas_norm_type norm,
  enum blas_uplo_type uplo, int n, const double *a, int lda, double *res) {
  int i, j; double anorm, v;
  char rname[] = "BLAS_dsy_norm";

  if (order != blas_colmajor) BLAS_error( rname, -1, order, 0 );

  if (norm == blas_inf_norm) {
    anorm = 0.0;
    if (blas_upper == uplo) {
      for (i = 0; i < n; ++i) {
        v = 0.0;
        for (j = 0; j < i; ++j) {
          v += fabs( a[j + i * lda] );
        }
        for (j = i; j < n; ++j) {
          v += fabs( a[i + j * lda] );
        }
        if (v > anorm)
          anorm = v;
      }
    } else {
      BLAS_error( rname, -3, norm, 0 );
      return;
    }
  } else {
    BLAS_error( rname, -2, norm, 0 );
    return;
  }

  if (res) *res = anorm;
}

static
void
BLAS_dge_norm(enum blas_order_type order, enum blas_norm_type norm,
  int m, int n, const double *a, int lda, double *res) {
  int i, j; float anorm, v;
  char rname[] = "BLAS_dge_norm";

  if (order != blas_colmajor) BLAS_error( rname, -1, order, 0 );

  if (norm == blas_frobenius_norm) {
    anorm = 0.0f;
    for (j = n; j; --j) {
      for (i = m; i; --i) {
        v = a[0];
        anorm += v * v;
        a++;
      }
      a += lda - m;
    }
    anorm = sqrt( anorm );
  } else if (norm == blas_inf_norm) {
    anorm = 0.0f;
    for (i = 0; i < m; ++i) {
      v = 0.0f;
      for (j = 0; j < n; ++j) {
        v += fabs( a[i + j * lda] );
      }
      if (v > anorm)
        anorm = v;
    }
  } else {
    BLAS_error( rname, -2, norm, 0 );
    return;
  }

  if (res) *res = anorm;
}

static
double
BLAS_dpow_di(double x, int n) {
  double rv = 1.0;

  if (n < 0) {
    n = -n;
    x = 1.0 / x;
  }

  for (; n; n >>= 1, x *= x) {
    if (n & 1)
      rv *= x;
  }

  return rv;
}

static
double
BLAS_dfpinfo(enum blas_cmach_type cmach) {
  double eps = 1.0, r = 1.0, o = 1.0, b = 2.0;
  int t = 53, l = 1024, m = -1021;
  char rname[] = "BLAS_dfpinfo";

  if ((sizeof eps) == sizeof(float)) {
    t = 24;
    l = 128;
    m = -125;
  } else {
    t = 53;
    l = 1024;
    m = -1021;
  }

  /* for (i = 0; i < t; ++i) eps *= half; */
  eps = BLAS_dpow_di( b, -t );
  /* for (i = 0; i >= m; --i) r *= half; */
  r = BLAS_dpow_di( b, m-1 );

  o -= eps;
  /* for (i = 0; i < l; ++i) o *= b; */
  o = (o * BLAS_dpow_di( b, l-1 )) * b;

  switch (cmach) {
    case blas_eps: return eps;
    case blas_sfmin: return r;
    default:
      BLAS_error( rname, -1, cmach, 0 );
      break;
  }
  return 0.0;
}

static int check_orthogonality(int, int, int, double*, double);
static int check_factorization(int, int, double*, double*, int, double*, double);
static int check_solution(int, int, int, double*, int, double*, double*, int, double);

int testing_dgels(int argc, char **argv)
{
    int mode = 0;

    if ( argc < 1 ){
        goto usage;
    } else {
        mode = atoi(argv[0]);
    }

    /* Check for number of arguments*/
    if ( ((mode == 0) && (argc != 6)) ||
         ((mode != 0) && (argc != 7)) ){
      usage:
        USAGE("GELS", "MODE M N LDA NRHS LDB [RH]",
              "   - MODE : 0: flat, 1: tree (RH needed)\n"
              "   - M    : number of rows of the matrix A\n"
              "   - N    : number of columns of the matrix A\n"
              "   - LDA  : leading dimension of the matrix A\n"
              "   - NRHS : number of RHS\n"
              "   - LDB  : leading dimension of the matrix B\n"
              "   - RH   : Size of each subdomains\n");
        return -1;
    }

    int M     = atoi(argv[1]);
    int N     = atoi(argv[2]);
    int LDA   = atoi(argv[3]);
    int NRHS  = atoi(argv[4]);
    int LDB   = atoi(argv[5]);
    int rh;

    int K = min(M, N);
    double eps;
    int info_ortho, info_solution, info_factorization;
    int i,j;
    int LDAxN = LDA*N;
    int LDBxNRHS = LDB*NRHS;

    double *A1 = (double *)malloc(LDA*N*sizeof(double));
    double *A2 = (double *)malloc(LDA*N*sizeof(double));
    double *B1 = (double *)malloc(LDB*NRHS*sizeof(double));
    double *B2 = (double *)malloc(LDB*NRHS*sizeof(double));
    double *Q  = (double *)malloc(LDA*N*sizeof(double));
    PLASMA_desc *T;

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)||(!B1)||(!B2)||(!Q)){
        printf("Out of Memory \n ");
        return -2;
    }

    if ( mode ) {
        rh = atoi(argv[6]);

        PLASMA_Set(PLASMA_HOUSEHOLDER_MODE, PLASMA_TREE_HOUSEHOLDER);
        PLASMA_Set(PLASMA_HOUSEHOLDER_SIZE, rh);
    }

    PLASMA_Alloc_Workspace_dgels(M, N, &T);
    eps = BLAS_dfpinfo( blas_eps );

    /*----------------------------------------------------------
    *  TESTING DGELS
    */

    /* Initialize A1 and A2 */
    LAPACKE_dlarnv_work(IONE, ISEED, LDAxN, A1);
    for (i = 0; i < M; i++)
        for (j = 0; j < N; j++)
            A2[LDA*j+i] = A1[LDA*j+i] ;

    /* Initialize B1 and B2 */
    LAPACKE_dlarnv_work(IONE, ISEED, LDBxNRHS, B1);
    for (i = 0; i < M; i++)
        for (j = 0; j < NRHS; j++)
            B2[LDB*j+i] = B1[LDB*j+i] ;

    memset((void*)Q, 0, LDA*N*sizeof(double));
    for (i = 0; i < K; i++)
        Q[LDA*i+i] = 1.0;

    /* PLASMA DGELS */
    PLASMA_dgels(PlasmaNoTrans, M, N, NRHS, A2, LDA, T, B2, LDB);

    /* PLASMA DGELS */
    if (M >= N)
       /* Building the economy-size Q */
       PLASMA_dorgqr(M, N, K, A2, LDA, T, Q, LDA);
    else
       /* Building the economy-size Q */
       PLASMA_dorglq(M, N, K, A2, LDA, T, Q, LDA);

    printf("\n");
    printf("------ TESTS FOR PLASMA DGELS ROUTINE -------  \n");
    printf("            Size of the Matrix %d by %d\n", M, N);
    printf("\n");
    printf(" The matrix A is randomly generated for each test.\n");
    printf("============\n");
    printf(" The relative machine precision (eps) is to be %e \n",eps);
    printf(" Computational tests pass if scaled residuals are less than 60.\n");

    /* Check the orthogonality, factorization and the solution */
    info_ortho = check_orthogonality(M, N, LDA, Q, eps);
    info_factorization = check_factorization(M, N, A1, A2, LDA, Q, eps);
    info_solution = check_solution(M, N, NRHS, A1, LDA, B1, B2, LDB, eps);

    if ((info_solution == 0)&(info_factorization == 0)&(info_ortho == 0)) {
        printf("***************************************************\n");
        printf(" ---- TESTING DGELS ...................... PASSED !\n");
        printf("***************************************************\n");
    }
    else {
        printf("************************************************\n");
        printf(" - TESTING DGELS ... FAILED !\n");
        printf("************************************************\n");
    }

    /*-------------------------------------------------------------
    *  TESTING DGEQRF + DGEQRS or DGELQF + DGELQS
    */

    /* Initialize A1 and A2 */
    LAPACKE_dlarnv_work(IONE, ISEED, LDAxN, A1);
    for (i = 0; i < M; i++)
        for (j = 0; j < N; j++)
            A2[LDA*j+i] = A1[LDA*j+i];

    /* Initialize B1 and B2 */
    LAPACKE_dlarnv_work(IONE, ISEED, LDBxNRHS, B1);
    for (i = 0; i < M; i++)
        for (j = 0; j < NRHS; j++)
             B2[LDB*j+i] = B1[LDB*j+i];

    memset((void*)Q, 0, LDA*N*sizeof(double));
    for (i = 0; i < K; i++)
        Q[LDA*i+i] = 1.0;

    if (M >= N) {
        printf("\n");
        printf("------ TESTS FOR PLASMA DGEQRF + DGEQRS ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", M, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n", eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        /* Plasma routines */
        PLASMA_dgeqrf(M, N, A2, LDA, T);
        PLASMA_dorgqr(M, N, K, A2, LDA, T, Q, LDA);
        PLASMA_dgeqrs(M, N, NRHS, A2, LDA, T, B2, LDB);

        /* Check the orthogonality, factorization and the solution */
        info_ortho = check_orthogonality(M, N, LDA, Q, eps);
        info_factorization = check_factorization(M, N, A1, A2, LDA, Q, eps);
        info_solution = check_solution(M, N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ((info_solution == 0)&(info_factorization == 0)&(info_ortho == 0)) {
            printf("***************************************************\n");
            printf(" ---- TESTING DGEQRF + DGEQRS ............ PASSED !\n");
            printf("***************************************************\n");
        }
        else{
            printf("***************************************************\n");
            printf(" - TESTING DGEQRF + DGEQRS ... FAILED !\n");
            printf("***************************************************\n");
        }
    }
    else  {
        printf("\n");
        printf("------ TESTS FOR PLASMA DGELQF + DGELQS ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", M, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n", eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        /* Plasma routines */
        PLASMA_dgelqf(M, N, A2, LDA, T);
        PLASMA_dorglq(M, N, K, A2, LDA, T, Q, LDA);
        PLASMA_dgelqs(M, N, NRHS, A2, LDA, T, B2, LDB);

       /* Check the orthogonality, factorization and the solution */
       info_ortho = check_orthogonality(M, N, LDA, Q, eps);
       info_factorization = check_factorization(M, N, A1, A2, LDA, Q, eps);
       info_solution = check_solution(M, N, NRHS, A1, LDA, B1, B2, LDB, eps);

       if ( (info_solution == 0) & (info_factorization == 0) & (info_ortho == 0) ) {
          printf("***************************************************\n");
          printf(" ---- TESTING DGELQF + DGELQS ............ PASSED !\n");
          printf("***************************************************\n");
       }
       else {
          printf("***************************************************\n");
          printf(" - TESTING DGELQF + DGELQS ... FAILED !\n");
          printf("***************************************************\n");
        }
    }

    /*----------------------------------------------------------
    *  TESTING DGEQRF + ZORMQR + DTRSM
    */

    /* Initialize A1 and A2 */
    LAPACKE_dlarnv_work(IONE, ISEED, LDAxN, A1);
    for (i = 0; i < M; i++)
        for (j = 0; j < N; j++)
            A2[LDA*j+i] = A1[LDA*j+i];

    /* Initialize B1 and B2 */
    memset(B2, 0, LDB*NRHS*sizeof(double));
    LAPACKE_dlarnv_work(IONE, ISEED, LDBxNRHS, B1);
    for (i = 0; i < M; i++)
        for (j = 0; j < NRHS; j++)
            B2[LDB*j+i] = B1[LDB*j+i];

    /* PLASMA DGEQRF+ DORMQR + DTRSM */
    memset((void*)Q, 0, LDA*N*sizeof(double));
    for (i = 0; i < K; i++)
        Q[LDA*i+i] = 1.0;

    if (M >= N) {
        printf("\n");
        printf("------ TESTS FOR PLASMA DGEQRF + DORMQR + DTRSM  ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", M, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n",eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        PLASMA_dgeqrf(M, N, A2, LDA, T);
        PLASMA_dorgqr(M, N, K, A2, LDA, T, Q, LDA);
        PLASMA_dormqr(PlasmaLeft, PlasmaTrans, M, NRHS, N, A2, LDA, T, B2, LDB);
        PLASMA_dtrsm(PlasmaLeft, PlasmaUpper, PlasmaNoTrans, PlasmaNonUnit, N, NRHS, 1.0, A2, LDA, B2, LDB);
    }
    else {
        printf("\n");
        printf("------ TESTS FOR PLASMA DGELQF + DORMLQ + DTRSM  ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", M, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n",eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        PLASMA_dgelqf(M, N, A2, LDA, T);
        PLASMA_dtrsm(PlasmaLeft, PlasmaLower, PlasmaNoTrans, PlasmaNonUnit, M, NRHS, 1.0, A2, LDA, B2, LDB);
        PLASMA_dorglq(M, N, K, A2, LDA, T, Q, LDA);
        PLASMA_dormlq(PlasmaLeft, PlasmaTrans, N, NRHS, M, A2, LDA, T, B2, LDB);
    }

    /* Check the orthogonality, factorization and the solution */
    info_ortho = check_orthogonality(M, N, LDA, Q, eps);
    info_factorization = check_factorization(M, N, A1, A2, LDA, Q, eps);
    info_solution = check_solution(M, N, NRHS, A1, LDA, B1, B2, LDB, eps);

    if ( (info_solution == 0) & (info_factorization == 0) & (info_ortho == 0) ) {
        if (M >= N) {
            printf("***************************************************\n");
            printf(" ---- TESTING DGEQRF + DORMQR + DTRSM .... PASSED !\n");
            printf("***************************************************\n");
        }
        else {
            printf("***************************************************\n");
            printf(" ---- TESTING DGELQF + DTRSM + DORMLQ .... PASSED !\n");
            printf("***************************************************\n");
        }
    }
    else {
        if (M >= N) {
            printf("***************************************************\n");
            printf(" - TESTING DGEQRF + DORMQR + DTRSM ... FAILED !\n");
            printf("***************************************************\n");
        }
        else {
            printf("***************************************************\n");
            printf(" - TESTING DGELQF + DTRSM + DORMLQ ... FAILED !\n");
            printf("***************************************************\n");
        }
    }

    free(A1); free(A2); free(B1); free(B2); free(Q);
    PLASMA_Dealloc_Handle_Tile( &T );

    return 0;
}

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

static int check_orthogonality(int M, int N, int LDQ, double *Q, double eps)
{
    double alpha, beta;
    double normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);

    double *work = (double *)malloc(minMN*sizeof(double));

    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    double *Id = (double *) malloc(minMN*minMN*sizeof(double));
    memset((void*)Id, 0, minMN*minMN*sizeof(double));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (double)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    BLAS_dsy_norm( blas_colmajor, blas_inf_norm, blas_upper, minMN, Id, minMN, &normQ );

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n", normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || isinf(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 60.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

static int check_factorization(int M, int N, double *A1, double *A2, int LDA, double *Q, double eps )
{
    double Anorm, Rnorm;
    double alpha, beta;
    int info_factorization;
    int i,j;

    double *Ql       = (double *)malloc(M*N*sizeof(double));
    double *Residual = (double *)malloc(M*N*sizeof(double));
    double *work              = (double *)malloc(max(M,N)*sizeof(double));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        double *R = (double *)malloc(N*N*sizeof(double));
        memset((void*)R, 0, N*N*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, (alpha), Q, LDA, R, N, (beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        double *L = (double *)malloc(M*M*sizeof(double));
        memset((void*)L, 0, M*M*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, (alpha), L, M, Q, LDA, (beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    BLAS_dge_norm( blas_colmajor, blas_inf_norm, M, N, Residual, M, &Rnorm );
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, M, N, A2, LDA, &Anorm );

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || isinf(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 60.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}

/*--------------------------------------------------------------
 * Check the solution
 */

static int check_solution(int M, int N, int NRHS, double *A, int LDA, double *B, double *X, int LDB, double eps)
{
    int info_solution;
    double Rnorm, Anorm, Xnorm, Bnorm;
    double zone, mzone, zzero;
    double result;
    double *work = (double *)malloc(max(M, N)* sizeof(double));

    zone  =  1.0;
    mzone = -1.0;
    zzero =  0.0;

    BLAS_dge_norm( blas_colmajor, blas_inf_norm, M, N,    A, LDA, &Anorm );
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, M, NRHS, B, LDB, &Bnorm );
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, NRHS, X, LDB, &Xnorm );

    /* Compute Ax - b */
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, NRHS, N, (zone), A, LDA, X, LDB, (mzone), B, LDB);

    /* Compute A' * (Ax - b) */
    cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, N, NRHS, M, (zone), A, LDA, B, LDB, (zzero), X, LDB);
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, NRHS, X, LDB, &Rnorm );

    if (getenv("PLASMA_TESTING_VERBOSE"))
       printf( "||A||_oo=%f\n||X||_oo=%f\n||B||_oo=%f\n||A X - B||_oo=%e\n", Anorm, Xnorm, Bnorm, Rnorm );

    result = Rnorm / ( (Anorm*Xnorm+Bnorm)*N*eps ) ;
    printf("============\n");
    printf("Checking the Residual of the solution \n");
    printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n", result);

    if (  isnan(Xnorm) || isinf(Xnorm) || isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
     }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }
    free(work);
    return info_solution;
}


/* -*- linux-c -*- */
/* fewbody_int.c

   Copyright (C) 2002-2004 John M. Fregeau

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include "fewbody.h"

/* allocate memory for ks_params */
void fb_malloc_ks_params(fb_ks_params_t *ks_params)
{
	ks_params->m = fb_malloc_vector(ks_params->nstar);
	ks_params->M = fb_malloc_vector(ks_params->kstar);
	ks_params->amat = fb_malloc_matrix(ks_params->nstar, ks_params->kstar);
	ks_params->Tmat = fb_malloc_matrix(ks_params->kstar, ks_params->kstar);
}

/* initialize ks_params; assumes ks_params is already malloc()ed */
void fb_init_ks_params(fb_ks_params_t *ks_params, fb_hier_t hier)
{
	int i, j, k;
	double *y;

	/* exit if hier is not consistent with ks_params */
	if (ks_params->nstar != hier.nobj) {
		fprintf(stderr, "fb_init_ks_params(): ks_params->nstar != hier.nobj: ks_params->nstar=%d  hier.nobj=%d\n", \
			ks_params->nstar, hier.nobj);
		exit(1);
	}

	/* first set the mass matrix */
	for (i=0; i<hier.nobj; i++) {
		ks_params->m[i] = hier.obj[i]->m;
	}

	/* calculate the M_k */
	k = -1;
	for (i=0; i<hier.nobj-1; i++) {
		for (j=i+1; j<hier.nobj; j++) {
			k++;
			ks_params->M[k] = ks_params->m[i] * ks_params->m[j];
		}
	}

	/* calculate and set the a and T matrices */
	fb_calc_amat(ks_params->amat, ks_params->nstar, ks_params->kstar);
	fb_calc_Tmat(ks_params->amat, ks_params->m, ks_params->Tmat, ks_params->nstar, ks_params->kstar);

	/* set Einit */
	y = fb_malloc_vector(8*ks_params->kstar+1);
	fb_euclidean_to_ks(hier.obj, y, ks_params->nstar, ks_params->kstar);
	ks_params->Einit = fb_ks_Einit(y, *ks_params);

	fb_free_vector(y);
}

/* free memory for ks_params */
void fb_free_ks_params(fb_ks_params_t ks_params)
{
	fb_free_vector(ks_params.m);
	fb_free_vector(ks_params.M);
	fb_free_matrix(ks_params.amat);
	fb_free_matrix(ks_params.Tmat);
}

/* allocate memory for nonks_params */
void fb_malloc_nonks_params(fb_nonks_params_t *nonks_params)
{
	nonks_params->m = fb_malloc_vector(nonks_params->nstar);
}

/* initialize nonks_params; assumes nonks_params is already malloc()ed */
void fb_init_nonks_params(fb_nonks_params_t *nonks_params, fb_hier_t hier)
{
	int i;

	/* exit if hier is not consistent with nonks_params */
	if (nonks_params->nstar != hier.nobj) {
		fprintf(stderr, "fb_init_nonks_params(): nonks_params->nstar != hier.nobj: nonks_params->nstar=%d  hier.nobj=%d\n", \
			nonks_params->nstar, hier.nobj);
		exit(1);
	}

	/* set the mass vector */
	for (i=0; i<hier.nobj; i++) {
		nonks_params->m[i] = hier.obj[i]->m;
	}
}

/* free memory for ks_params */
void fb_free_nonks_params(fb_nonks_params_t nonks_params)
{
	fb_free_vector(nonks_params.m);
}


/**
 *
 * @file qwrapper_slaset.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:44:58 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 **/
void QUARK_CORE_slaset(Quark *quark, Quark_Task_Flags *task_flags,
                       PLASMA_enum uplo, int M, int N,
                       float alpha, float beta,
                       float *A, int LDA)
{
    DAG_CORE_LASET;
    QUARK_Insert_Task(quark, CORE_slaset_quark, task_flags,
        sizeof(PLASMA_enum),                &uplo,  VALUE,
        sizeof(int),                        &M,     VALUE,
        sizeof(int),                        &N,     VALUE,
        sizeof(float),         &alpha, VALUE,
        sizeof(float),         &beta,  VALUE,
        sizeof(float)*LDA*N,    A,      OUTPUT,
        sizeof(int),                        &LDA,   VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_slaset_quark = PCORE_slaset_quark
#define CORE_slaset_quark PCORE_slaset_quark
#endif
void CORE_slaset_quark(Quark *quark)
{
    PLASMA_enum uplo;
    int M;
    int N;
    float alpha;
    float beta;
    float *A;
    int LDA;

    quark_unpack_args_7(quark, uplo, M, N, alpha, beta, A, LDA);
    LAPACKE_slaset_work(
        LAPACK_COL_MAJOR,
        lapack_const(uplo),
        M, N, alpha, beta, A, LDA);
}


/** \file AMDiS.h */

#pragma once

// std c++ headers
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#include <cmath>
#include <cstdint>

// AMDiS includes
#include "AMDiS.hpp"
#include "MTL4Types.hpp"
#include "AdaptInfo.hpp"
#include "AdaptInstationary.hpp"
#include "AdaptStationary.hpp"
#include "Assembler.hpp"
#include "BasisFunction.hpp"
#include "Boundary.hpp"
#include "Boundary.hpp"
#include "BoundaryCondition.hpp"
#include "BoundaryManager.hpp"
#include "CoarseningManager.hpp"
#include "CoarseningManager1d.hpp"
#include "CoarseningManager2d.hpp"
#include "CoarseningManager3d.hpp"
#include "CouplingTimeInterface.hpp"
#include "CouplingIterationInterface.hpp"
#include "CreatorInterface.hpp"
#include "CreatorMap.hpp"
#include "Debug.hpp"
#include "DOFAdmin.hpp"
#include "DOFContainer.hpp"
#include "DOFIndexed.hpp"
#include "DOFIterator.hpp"
#include "DOFMatrix.hpp"
#include "DOFVector.hpp"
#include "DOFVectorOperations.hpp"
#include "DirichletBC.hpp"
// #include "DualTraverse.hpp"
#include "ElInfo.hpp"
#include "ElInfo1d.hpp"
#include "ElInfo2d.hpp"
#include "ElInfo3d.hpp"
#include "Element.hpp"
#include "ElementDofIterator.hpp"
#include "FiniteElemSpace.hpp"
#include "FirstOrderTerm.hpp"
#include "FixVec.hpp"
#include "Flag.hpp"
#include "Global.hpp"
#include "Initfile.hpp"
#include "Lagrange.hpp"
#include "LeafData.hpp"
#include "Line.hpp"
#include "MacroElement.hpp"
#include "Marker.hpp"
#include "MatrixVector.hpp"
#include "MatrixVectorOperations.hpp"
#include "Mesh.hpp"
#include "MeshStructure.hpp"
#include "ComponentTraverseInfo.hpp"
#include "Operator.hpp"
#include "OperatorTerm.hpp"
#include "Parametric.hpp"
#include "PeriodicBC.hpp"
#include "ProblemStat.hpp"
#include "ProblemInstat.hpp"
#include "ProblemTimeInterface.hpp"
#include "ProblemStatBase.hpp"
#include "SecondOrderTerm.hpp"
#include "StandardProblemIteration.hpp"
#include "Projection.hpp"
#include "QPsiPhi.hpp"
#include "Quadrature.hpp"
#include "RCNeighbourList.hpp"
#include "RefinementManager.hpp"
#include "RefinementManager1d.hpp"
#include "RefinementManager2d.hpp"
#include "RefinementManager3d.hpp"
#include "RobinBC.hpp"
#include "SurfaceOperator.hpp"
#include "SurfaceQuadrature.hpp"
#include "SystemVector.hpp"
#include "Tetrahedron.hpp"
#include "Traverse.hpp"
#include "Traits.hpp"
#include "Triangle.hpp"
#include "VertexVector.hpp"
#include "ZeroOrderTerm.hpp"

#include "est/Estimator.hpp"

// #include "io/ArhReader.hpp"
// #include "io/Arh2Reader.hpp"
#include "io/Arh3Reader.hpp"
// #include "io/ArhWriter.hpp"
// #include "io/Arh2Writer.hpp"
#include "io/Arh3Writer.hpp"
#include "io/DataCollector.hpp"
#include "io/FileWriter.hpp"
#include "io/GNUPlotWriter.hpp"
#include "io/GridWriter.hpp"
#include "io/MacroWriter.hpp"
#include "io/PngWriter.hpp"
#include "io/PovrayWriter.hpp"
#include "io/Spreadsheet.hpp"
#include "io/ValueReader.hpp"
#include "io/ValueWriter.hpp"
#include "io/VtkWriter.hpp"
#include "io/VtkVectorWriter.hpp"
//#include "io/VtkReader.h"
#include "io/Reader.hpp"
#include "io/Writer.hpp"

// #include "nonlin/ProblemNonLin.h"
// #include "nonlin/NonLinSolver.h"

#include "solver/ITL_Preconditioner.hpp"
#include "solver/ITL_Solver.hpp"
#include "solver/LinearSolverInterface.hpp"

// #include "time/RosenbrockAdaptInstationary.h"
// #include "time/RosenbrockStationary.h"

#if HAVE_PARALLEL_DOMAIN_AMDIS
#include "parallel/InteriorBoundary.hpp"
#include "parallel/MpiHelper.hpp"
#include "parallel/ParallelDebug.hpp"
#include "parallel/StdMpi.hpp"
#include "parallel/ParallelProblemStat.hpp"

#if !HAVE_PARALLEL_MTL4
#include "parallel/PetscSolver.hpp"
#include "parallel/PetscSolverNavierStokes.hpp"
#endif

#endif

#if HAVE_PETSC
#include <petsc.h>
#endif


#ifndef CTETRA_DOS_H
#define CTETRA_DOS_H

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "input.h"
#include "ecache.h"
#include "tetra.h"
#include "submesh.h"
#include "fermi.h"

double* Tetra_AllDosList(InputFn Efn, int na, int nb, int nc, int num_bands, gsl_matrix *R, double *Es, int num_dos);
double* Tetra_DosList(InputFn Efn, int na, int nb, int nc, int num_bands, gsl_matrix *R, double *Es, int num_dos);
double* Tetra_DosEnergyDerivList(InputFn Efn, int na, int nb, int nc, int num_bands, gsl_matrix *R, double *Es, int num_dos, double num_electrons, double *fermi, double *dos_fermi, double *dos_deriv_fermi);
double Tetra_TotalDos(double E, EnergyCache *Ecache);
double Tetra_TotalDosEnergyDeriv(double E, EnergyCache *Ecache);
double DosContrib(double E, double E1, double E2, double E3, double E4, double num_tetra);
double DosEnergyDerivContrib(double E, double E1, double E2, double E3, double E4, double num_tetra);

#endif // CTETRA_DOS_H


/**
 *
 * @file testing_zhegv.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Azzam Haidar
 * @author Hatem Ltaief
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_zmain.h"

#undef REAL
#define COMPLEX

static int check_orthogonality(int itype, int uplo, int N,
                               PLASMA_Complex64_t *Z, int LDZ,
                               PLASMA_Complex64_t *B, PLASMA_Complex64_t *CHOLB, int LDB,
                               double eps);
static int check_reduction(int itype, int uplo, int N, double *D,
                           PLASMA_Complex64_t *A, int LDA,
                           PLASMA_Complex64_t *B, int LDB,
                           PLASMA_Complex64_t *Z, int LDZ,
                           double eps );
static int check_solution(int N, double *E1, double *E2, double eps);

int testing_zhegv(int argc, char **argv)
{
    /* Check for number of arguments*/
    if (argc != 3) {
        USAGE("HEGV", "N LDA LDB",
              "   - N    : size of the matrices A and B\n"
              "   - LDA  : leading dimension of the matrix A\n"
              "   - LDB  : leading dimension of the matrix B\n");
        return -1;
    }

    double      eps = LAPACKE_dlamch_work('e');
    PLASMA_enum vec = PlasmaVec;
    int    N        = atoi(argv[0]);
    int    LDA      = atoi(argv[1]);
    int    LDB      = atoi(argv[2]);
    int    LDQ      = LDA;
    int    LDAxN    = LDA*N;
    int    LDBxN    = LDB*N;
    int    LDQxN    = LDQ*N;

    int info_ortho     = 0;
    int info_solution  = 0;
    int info_reduction = 0;
    int i, u;

    PLASMA_Complex64_t *A1    = (PLASMA_Complex64_t *)malloc(LDAxN*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *A2    = (PLASMA_Complex64_t *)malloc(LDAxN*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *B1    = (PLASMA_Complex64_t *)malloc(LDBxN*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *B2    = (PLASMA_Complex64_t *)malloc(LDBxN*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *Q     = (PLASMA_Complex64_t *)malloc(LDQxN*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *Ainit = (PLASMA_Complex64_t *)malloc(LDAxN*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *Binit = (PLASMA_Complex64_t *)malloc(LDBxN*sizeof(PLASMA_Complex64_t));
    double *W1 = (double *)malloc(N*sizeof(double));
    double *W2 = (double *)malloc(N*sizeof(double));
    PLASMA_Complex64_t *work = (PLASMA_Complex64_t *)malloc(3*N* sizeof(PLASMA_Complex64_t));
    PLASMA_desc *T;

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)||(!B1)||(!B2)||(!Q)||(!Ainit)||(!Binit)){
        printf("Out of Memory \n ");
        return -2;
    }

    PLASMA_Disable(PLASMA_AUTOTUNING);
    PLASMA_Set(PLASMA_TILE_SIZE, 120);
    PLASMA_Set(PLASMA_INNER_BLOCK_SIZE, 20);

    PLASMA_Enable(PLASMA_WARNINGS);
    PLASMA_Enable(PLASMA_ERRORS);
    PLASMA_Alloc_Workspace_zhegv(N, N, &T);

    /*----------------------------------------------------------
    *  TESTING ZHEGV
    */
    /* Initialize A1 and Ainit */
    PLASMA_zplghe(0., N, A1, LDA, 5198);
    PLASMA_zlacpy(PlasmaUpperLower, N, N, A1, LDA, Ainit, LDA);

    /* Initialize B1 and Binit */
    PLASMA_zplghe((double)N, N, B1, LDB, 4321 );
    PLASMA_zlacpy(PlasmaUpperLower, N, N, B1, LDB, Binit, LDB);

    printf("\n");
    printf("------ TESTS FOR PLASMA ZHEGV ROUTINE -------  \n");
    printf("        Size of the Matrix %d by %d\n", N, N);
    printf("\n");
    printf(" The matrix A is randomly generated for each test.\n");
    printf("============\n");
    printf(" The relative machine precision (eps) is to be %e \n",eps);
    printf(" Computational tests pass if scaled residuals are less than 60.\n");

    /*----------------------------------------------------------
     *  TESTING ZHEGV
     */

    for (i=0; i<3; i++) {
        for (u=0; u<2; u++) {
            memcpy(A2, Ainit, LDAxN*sizeof(PLASMA_Complex64_t));
            memcpy(B2, Binit, LDBxN*sizeof(PLASMA_Complex64_t));
            /* CALL ZHEGV with itype= 1, 2, 3 and uplo= L,U */
            PLASMA_zhegv(itype[i], vec, uplo[u], N, A2, LDA, B2, LDB, W2, T, Q, LDQ);

            /* CALL LAPACK to compute eigenvalues. */
            memcpy(A1, Ainit, LDAxN*sizeof(PLASMA_Complex64_t));
            memcpy(B1, Binit, LDBxN*sizeof(PLASMA_Complex64_t));
            LAPACKE_zhegv( LAPACK_COL_MAJOR,
                     itype[i], lapack_const(vec), lapack_const(uplo[u]),
                     N, A1, LDA, B1, LDB, W1);

            /* CHECK */
            info_solution = check_solution(N, W1, W2, eps);
            /* Check the orthogonality, reduction and the eigen solutions */
            if (vec == PlasmaVec){
                info_ortho = check_orthogonality(itype[i], uplo[u], N, Q, LDQ, Binit, B1, LDB, eps);
                info_reduction = check_reduction(itype[i], uplo[u], N, W2, Ainit, LDA, Binit, LDB, Q, LDQ, eps );
            }

            if ( (info_ortho == 0) & (info_reduction == 0) & (info_solution == 0)) {
                printf("***************************************************\n");
                printf(" ---- TESTING ZHEGV (%s, %s) ...................... PASSED !\n", itypestr[i], uplostr[u]);
                printf("***************************************************\n");
            }
            else {
                printf("************************************************\n");
                printf(" - TESTING ZHEGV (%s, %s) ... FAILED !\n", itypestr[i], uplostr[u]);
                printf("************************************************\n");
            }
        }
    }

    PLASMA_Dealloc_Handle_Tile(&T);
    free(A1);
    free(A2);
    free(B1);
    free(B2);
    free(Q);
    free(Ainit);
    free(Binit);
    free(W1);
    free(W2);
    free(work);

    return 0;
}

/*-------------------------------------------------------------------
 * Check the orthogonality of Z
 */
static int check_orthogonality(int itype, int uplo, int N,
                               PLASMA_Complex64_t *Z, int LDZ,
                               PLASMA_Complex64_t *B, PLASMA_Complex64_t *CHOLB, int LDB,
                               double eps)
{
    static PLASMA_Complex64_t zone  =  1.0;
    static PLASMA_Complex64_t mzone = -1.0;
    static PLASMA_Complex64_t zzero =  0.0;
    static double             done  =  1.0;
    static double             mdone = -1.0;
    PLASMA_enum trans;
    double  normQ, result;
    int     info_ortho;
    double *work = (double *)malloc(N*sizeof(double));
    PLASMA_Complex64_t *TEMP    = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *Id = (PLASMA_Complex64_t *) malloc(N*N*sizeof(PLASMA_Complex64_t));
    char *str;

    /* Build the idendity matrix */
    LAPACKE_zlaset_work(LAPACK_COL_MAJOR, 'A', N, N, 0., 1., Id, N);

    /* Perform orth test*/
    if ( itype == 3 ) {
        /*
         * if ITYPE = 3, Z**H*inv(B)*Z = I.
         * inv(B) = inv(L)**H * inv(L) or inv(U)*inv(U)**H
         */
        str = "Z**H*inv(B)*Z";

        if( uplo==PlasmaUpper ) {
            trans = PlasmaConjTrans;
        } else {
            trans = PlasmaNoTrans;
        }

        /* Compute inv(L)*Z or inv(U)**H*Z */
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaUpperLower),
                            N, N, Z, LDZ, TEMP, N);

        cblas_ztrsm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, (CBLAS_TRANSPOSE)trans,
                    CblasNonUnit, N, N,
                    CBLAS_SADDR(zone), CHOLB, LDB,
                                       TEMP,  N);

        /*
         * Compute Z**H*inv(B)*Z-Id = (Z**H*inv(L)**H) * (inv(L)*Z) - Id
         *
         * Note: Z**H*inv(L)**H is the ConjTranspose of the previous result, so we use ZHERK
         */
        cblas_zherk(CblasColMajor, (CBLAS_UPLO)uplo, CblasConjTrans,
                    N, N, done, TEMP, N, mdone, Id, N);
    } else {
        /*
         * if ITYPE = 1 or 2, Z**H*B*Z = I;
         */
        str = "Z**H*B*Z";
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  B,    LDB,
                                        Z,    LDZ,
                    CBLAS_SADDR(zzero), TEMP, N);
        cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, N, N, N,
                    CBLAS_SADDR(zone),  Z,    LDZ,
                                        TEMP, N,
                    CBLAS_SADDR(mzone), Id  , N);
    }

    normQ = LAPACKE_zlanhe_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), 'U', N, Id, N, work);
    result = normQ / (N * eps);
    printf(" ======================================================\n");
    printf(" | Id-%s |_oo / (N*eps)          : %15.3E \n",str, result );
    printf(" ======================================================\n");

    if ( isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id); free(TEMP);
    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the reduction
 */
static int check_reduction(int itype, int uplo, int N, double *D,
                           PLASMA_Complex64_t *A, int LDA,
                           PLASMA_Complex64_t *B, int LDB,
                           PLASMA_Complex64_t *Z, int LDZ,
                           double eps )
{
    PLASMA_Complex64_t zone  =  1.0;
    PLASMA_Complex64_t zzero =  0.0;
    PLASMA_Complex64_t mzone = -1.0;
    double Anorm, Znorm, Rnorm, result;
    int info_reduction;
    int i;
    char *str;
    PLASMA_Complex64_t *TEMP     = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *Residual = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    double *work = (double *)malloc(N*sizeof(double));

    if( itype == 1 ) {
        /*
         * | A Z - B Z D | / ( |A| |Z| n ulp )
         */
        str = " A Z - B Z D ";

        /* Compute TEMP = Z*D */
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaUpperLower),
                            N, N, Z, LDZ, TEMP, N);

        for (i=0; i<N; i++) {
            cblas_zdscal(N, D[i], TEMP + i*N, 1);
        }

        /* Compute Residual = B*Z*D = B*TEMP */
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  B,        LDB,
                                        TEMP,     N,
                    CBLAS_SADDR(zzero), Residual, N);

        /* Compute A*Z - B*Z*D */
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  A,        LDA,
                                        Z,        LDZ,
                    CBLAS_SADDR(mzone), Residual, N);
    }
    else if( itype == 2 ) {
        /*
         * | A B Z - Z D | / ( |A| |Z| n ulp )
         */
        str = " A B Z - Z D ";

        /* Compute Residual = Z*D */
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaUpperLower),
                            N, N, Z, LDZ, Residual, N);

        for (i=0; i<N; i++) {
            cblas_zdscal(N, D[i], Residual + i*N, 1);
        }

        /* Compute TEMP = B*Z */
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  B,    LDB,
                                        Z,    LDZ,
                    CBLAS_SADDR(zzero), TEMP, N);

        /* Compute A*B*Z - Z*D = A*TEMP-Residual */
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  A,        LDA,
                                        TEMP,     N,
                    CBLAS_SADDR(mzone), Residual, N);
    } else {
        /*
         * | B A Z - Z D | / ( |A| |Z| n ulp )
         */
        str = " B A Z - Z D ";

        /* Compute Residual = Z*D */
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaUpperLower),
                            N, N, Z, LDZ, Residual, N);

        for (i=0; i<N; i++) {
            cblas_zdscal(N, D[i], Residual + i*N, 1);
        }

        /* Compute TEMP = A*Z */
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  A,    LDA,
                                        Z,    LDZ,
                    CBLAS_SADDR(zzero), TEMP, N);

        /* Compute B*A*Z - Z*D = B*TEMP-Residual */
        cblas_zhemm(CblasColMajor, CblasLeft, (CBLAS_UPLO)uplo, N, N,
                    CBLAS_SADDR(zone),  B,        LDB,
                                        TEMP,     N,
                    CBLAS_SADDR(mzone), Residual, N);
    }

    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaOneNorm), N, N, Residual, N,   work);
    Anorm = LAPACKE_zlanhe_work(LAPACK_COL_MAJOR, lapack_const(PlasmaOneNorm), lapack_const(uplo), N, A,        LDA, work);
    Znorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaOneNorm), N, N, Z,        LDZ, work);

    result = Rnorm / ( Anorm * Znorm * N * eps);
    printf(" ======================================================\n");
    printf(" | %s |_oo/(|A|_oo.|Z|_oo.N.eps) : %15.3E \n", str, result );
    printf(" ======================================================\n");

    if ( isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- Reduction is suspicious ! \n");
        info_reduction = 1;
    }
    else {
        printf("-- Reduction is CORRECT ! \n");
        info_reduction = 0;
    }

    free(TEMP); free(Residual); free(work);
    return info_reduction;
}
/*--------------------------------------------------------------
 * Check the solution
 */
static int check_solution(int N, double *E1, double *E2, double eps)
{
    int info_solution, i;
    double resid;
    double maxtmp;
    double maxel = fabs( fabs(E1[0]) - fabs(E2[0]) );
    double maxeig = max( fabs(E1[0]), fabs(E2[0]) );

    for (i=1; i<N; i++) {
        resid  = fabs(fabs(E1[i])-fabs(E2[i]));
        maxtmp = max(fabs(E1[i]), fabs(E2[i]));

        /* Update */
        maxeig = max(maxtmp, maxeig);
        maxel  = max(resid,  maxel );
    }

    maxel = maxel / (maxeig * N * eps);
    printf(" ======================================================\n");
    printf(" | D - eigcomputed | / (|D| * N * eps) : %15.3E \n",  maxel );
    printf(" ======================================================\n");

    printf("============\n");
    printf("Checking the eigenvalues of A\n");
    if ( isnan(maxel) || isinf(maxel) || (maxel > 100) ) {
        printf("-- The eigenvalues are suspicious ! \n");
        info_solution = 1;
    }
    else{
        printf("-- The eigenvalues are CORRECT ! \n");
        info_solution = 0;
    }
    return info_solution;
}


#pragma once

#include "zerrors.h"

#include <string>
#include <vector>
#include <numeric>

#include <cwchar>
#include <mk_wcswidth/mk_wcwidth.h>

#include <gsl/span>

namespace terminal_editor {

#if !defined(USE_NATIVE_WCWIDTH)

/// Returns number of columns required to represent given character.
/// Returns 0 for zero width caracters.
/// Throws when non-printable character is passed as argument.
/// @param ucs      UTF-32 code point.
inline int wcwidth(uint32_t ucs) {
    auto width = mk::wcwidth(ucs);
    ZASSERT(width >= 0) << "Non-printable character encountered: " << ucs;
    return width;
}

#else

inline int wcwidth(uint32_t ucs) {
    auto width = ::wcwidth(ucs);
    ZASSERT(width >= 0) << "Non-printable character encountered: " << ucs;
    return width;
}

#endif

/// Returns number of columns required to represent given string.
/// Returns 0 for zero width strings.
/// Throws when non-printable characters are encountered.
/// @param ucs      UTF-32 code point.
inline int wcswidth(gsl::span<const uint32_t> text) {
    return std::accumulate(text.begin(), text.end(), 0, [](int width, const uint32_t ucs) { return width + wcwidth(ucs); });
}

} // namespace terminal_editor