#ifdef FUTILITY_HAVE_PETSC
#include <petsc.h>
#include <petscsys.h>
#include <petscfix.h>
#include <petscksp.h>
#if ((PETSC_VERSION_MAJOR<=3) && (PETSC_VERSION_MINOR<=5))
#include <petsc-private/fortranimpl.h>
#include <petsc-private/pcimpl.h>
#else
#include <petsc/private/fortranimpl.h>
#include <petsc/private/pcmgimpl.h>
#endif

#ifdef PETSC_USE_POINTER_CONVERSION
extern void *PetscToPointer(void*);
extern int PetscFromPointer(void *);
extern void PetscRmPointer(void*);
#else
#define PetscToPointer(a) (*(PetscFortranAddr *)(a))
#define PetscFromPointer(a) (PetscFortranAddr)(a)
#define PetscRmPointer(a)
#endif

#if ((PETSC_VERSION_MAJOR<=3) && (PETSC_VERSION_MINOR<=5))
//This is taken from petsc-3.5/src/ksp/pc/impls/mg/mgimpl.h
//There is no version of petsc/private/pcmgimpl.h that is includable in older
//  versions of PETSc, so we just have to manually redefine the typdefs defined
//  in mgimpl.h here.
typedef struct {
  PetscInt cycles;                             /* Type of cycle to run: 1 V 2 W */
  PetscInt level;                              /* level = 0 coarsest level */
  PetscInt levels;                             /* number of active levels used */
  Vec      b;                                  /* Right hand side */
  Vec      x;                                  /* Solution */
  Vec      r;                                  /* Residual */

  PetscErrorCode (*residual)(Mat,Vec,Vec,Vec);

  Mat           A;                             /* matrix used in forming residual*/
  KSP           smoothd;                       /* pre smoother */
  KSP           smoothu;                       /* post smoother */
  Mat           interpolate;
  Mat           restrct;                       /* restrict is a reserved word in C99 and on Cray */
  Vec           rscale;                        /* scaling of restriction matrix */
  PetscLogEvent eventsmoothsetup;              /* if logging times for each level */
  PetscLogEvent eventsmoothsolve;
  PetscLogEvent eventresidual;
  PetscLogEvent eventinterprestrict;
} PC_MG_Levels;
typedef struct {
  PCMGType  am;                               /* Multiplicative, additive or full */
  PetscInt  cyclesperpcapply;                 /* Number of cycles to use in each PCApply(), multiplicative only*/
  PetscInt  maxlevels;                        /* total number of levels allocated */
  PetscInt  galerkin;                         /* use Galerkin process to compute coarser matrices, 0=no, 1=yes, 2=yes but computed externally */
  PetscBool usedmfornumberoflevels;           /* sets the number of levels by getting this information out of the DM */

  PetscInt     nlevels;
  PC_MG_Levels **levels;
  PetscInt     default_smoothu;               /* number of smooths per level if not over-ridden */
  PetscInt     default_smoothd;               /*  with calls to KSPSetTolerances() */
  PetscReal    rtol,abstol,dtol,ttol;         /* tolerances for when running with PCApplyRichardson_MG */

  void          *innerctx;                   /* optional data for preconditioner, like PCEXOTIC that inherits off of PCMG */
  PetscLogStage stageApply;
} PC_MG;
#endif

#ifdef PETSC_HAVE_FORTRAN_CAPS
#define pcmgsoftreset_ PCMGSOFTRESET
#elif !defined(PETSC_HAVE_FORTRAN_UNDERSCORE) && !defined(FORTRANDOUBLEUNDERSCORE)
#define pcmgsoftreset_ pcmgsoftreset
#endif

#undef __FUNCT__
#define __FUNCT__ "PCMGSoftReset"

PetscErrorCode PCMGSoftReset(PC pc)
{
  PC_MG          *mg        = (PC_MG*)pc->data;
  PC_MG_Levels   **mglevels = mg->levels;
  PetscErrorCode ierr;
  PetscInt       i,n;

  PetscFunctionBegin;
  if (mglevels) {
    n = mglevels[0]->levels;
    for (i=0; i<n; i++) {
      ierr = MatDestroy(&mglevels[i]->A);CHKERRQ(ierr);
      if (mglevels[i]->smoothd != mglevels[i]->smoothu) {
        ierr = KSPReset(mglevels[i]->smoothd);CHKERRQ(ierr);
      }
      ierr = KSPReset(mglevels[i]->smoothu);CHKERRQ(ierr);
    }
  }
  //This may not work if DM's are used to define the MG structure.
  pc->setupcalled = PETSC_FALSE;
  PetscFunctionReturn(0);
}

PETSC_EXTERN void PETSC_STDCALL  pcmgsoftreset_(PC pc, int *__ierr ){
*__ierr = PCMGSoftReset(
  (PC)PetscToPointer((pc) ));
}
#endif


//           $Id: pfc_libraries.h 37984 2018-10-27 15:50:30Z p20068 $
//          $URL: https://svn01.fh-hagenberg.at/bin/cepheiden/vocational/teaching/ESD/SPS3/2018-WS/ILV/src/Snippets/bitmap-gsl/pfc_libraries.h $
//     $Revision: 37984 $
//         $Date: 2018-10-27 17:50:30 +0200 (Sa., 27 Okt 2018) $
//       $Author: p20068 $
//
//       Creator: Peter Kulczycki (peter.kulczycki<AT>fh-hagenberg.at)
// Creation Date:
//     Copyright: (c) 2018 Peter Kulczycki (peter.kulczycki<AT>fh-hagenberg.at)
//
//       License: This document contains proprietary information belonging to
//                University of Applied Sciences Upper Austria, Campus
//                Hagenberg. It is distributed under the Boost Software License,
//                Version 1.0 (see http://www.boost.org/LICENSE_1_0.txt).
//
//    Annotation: This file is part of the code snippets handed out during one
//                of my HPC lessons held at the University of Applied Sciences
//                Upper Austria, Campus Hagenberg.

#pragma once

#include "./pfc_macros.h"

// -------------------------------------------------------------------------------------------------

#if defined PFC_DETECTED_COMPILER_NVCC
   #define PFC_DO_NOT_USE_BOOST_UNITS
   #define PFC_DO_NOT_USE_GSL
   #define PFC_DO_NOT_USE_VLD
   #define PFC_DO_NOT_USE_WINDOWS
#endif

// -------------------------------------------------------------------------------------------------

#undef PFC_HAVE_VLD
#undef PFC_VLD_INCLUDED

#if __has_include (<vld.h>) && !defined PFC_DO_NOT_USE_VLD   // Visual Leak Detector (https://kinddragon.github.io/vld)
   #include <vld.h>

   #define PFC_HAVE_VLD
   #define PFC_VLD_INCLUDED

   #pragma message ("PFC: using 'Visual Leak Detector'")
#else
   #pragma message ("PFC: not using 'Visual Leak Detector'")
#endif

// -------------------------------------------------------------------------------------------------

#undef PFC_HAVE_GSL
#undef PFC_GSL_INCLUDED

#if __has_include (<gsl/gsl>) && !defined PFC_DO_NOT_USE_GSL   // Guideline Support Library (https://github.com/Microsoft/GSL)
   #include <gsl/gsl>

   #define PFC_HAVE_GSL
   #define PFC_GSL_INCLUDED

   #pragma message ("PFC: using 'Guideline Support Library'")
#else
   #pragma message ("PFC: not using 'Guideline Support Library'")
#endif

// -------------------------------------------------------------------------------------------------

#undef PFC_HAVE_BOOST_UNITS
#undef PFC_BOOST_UNITS_INCLUDED

#if __has_include (<boost/units/io.hpp>)
#if __has_include (<boost/units/systems/si/length.hpp>)
#if __has_include (<boost/units/systems/si/prefixes.hpp>) && !defined PFC_DO_NOT_USE_BOOST_UNITS
   #include <boost/units/io.hpp>                    // http://www.boost.org
   #include <boost/units/systems/si/length.hpp>     // https://sourceforge.net/projects/boost/files/boost-binaries
   #include <boost/units/systems/si/prefixes.hpp>   //

   #define PFC_HAVE_BOOST_UNITS
   #define PFC_BOOST_UNITS_INCLUDED

   #pragma message ("PFC: using 'Boost.Units'")
#else
   #pragma message ("PFC: not using 'Boost.Units'")
#endif
#endif
#endif

// -------------------------------------------------------------------------------------------------

#undef PFC_HAVE_WINDOWS
#undef PFC_WINDOWS_INCLUDED

#if __has_include (<windows.h>) && !defined PFC_DO_NOT_USE_WINDOWS
   #undef  NOMINMAX
   #define NOMINMAX

   #undef  STRICT
   #define STRICT

   #undef  VC_EXTRALEAN
   #define VC_EXTRALEAN

   #undef  WIN32_LEAN_AND_MEAN
   #define WIN32_LEAN_AND_MEAN

   #include <windows.h>

   #define PFC_HAVE_WINDOWS
   #define PFC_WINDOWS_INCLUDED

   #pragma message ("PFC: using 'windows.h'")
#else
   #pragma message ("PFC: not using 'windows.h'")
#endif


#ifndef AMICI_MISC_H
#define AMICI_MISC_H

#include "amici/defines.h"
#include "amici/exception.h"
#include "amici/vector.h"
#include <sunmatrix/sunmatrix_sparse.h> // SUNMatrixContent_Sparse

#include <algorithm>
#include <vector>
#include <memory>
#include <regex>

#include <gsl/gsl-lite.hpp>

namespace amici {

/**
 * @brief creates a slice from existing data
 *
 * @param data to be sliced
 * @param index slice index
 * @param size slice size
 * @return span of the slice
 */

template <class T>
gsl::span<T> slice(std::vector<T> &data, int index, unsigned size) {
    if ((index + 1) * size > data.size())
        throw std::out_of_range("requested slice is out of data range");
    if (size > 0)
        return gsl::make_span(&data.at(index*size), size);

    return gsl::make_span(static_cast<T*>(nullptr), 0);
}

/**
 * @brief creates a constant slice from existing constant data
 *
 * @param data to be sliced
 * @param index slice index
 * @param size slice size
 * @return span of the slice
 */

template <class T>
const gsl::span<const T> slice(const std::vector<T> &data,
                               int index, unsigned size) {
    if ((index + 1) * size > data.size())
        throw std::out_of_range("requested slice is out of data range");
    if (size > 0)
        return gsl::make_span(&data.at(index*size), size);

    return gsl::make_span(static_cast<T*>(nullptr), 0);
}

/**
 * @brief local helper to check whether the provided buffer has the expected
 * size
 * @param buffer buffer to which values are to be written
 * @param expected_size expected size of the buffer
 */
template <class T>
void checkBufferSize(gsl::span<T> buffer,
                     typename gsl::span<T>::index_type expected_size) {
    if (buffer.size() != expected_size)
        throw AmiException("Incorrect buffer size! Was %u, expected %u.",
                           buffer.size(), expected_size);
}

/* TODO: templating writeSlice breaks implicit conversion between vector & span
 not sure whether this is fixable */

/**
 * @brief local helper function to write computed slice to provided buffer (span)
 * @param slice computed value
 * @param buffer buffer to which values are to be written
 */
template <class T>
void writeSlice(const gsl::span<const T> slice, gsl::span<T> buffer) {
    checkBufferSize(buffer, slice.size());
    std::copy(slice.begin(), slice.end(), buffer.data());
};

/**
 * @brief local helper function to write computed slice to provided buffer (vector)
 * @param s computed value
 * @param b buffer to which values are to be written
 */
template <class T>
void writeSlice(const std::vector<T> &s, std::vector<T> &b) {
    writeSlice(gsl::make_span(s.data(), s.size()),
               gsl::make_span(b.data(), b.size()));
};

/**
 * @brief local helper function to write computed slice to provided buffer (vector/span)
 * @param s computed value
 * @param b buffer to which values are to be written
 */
template <class T>
void writeSlice(const std::vector<T> &s, gsl::span<T> b) {
    writeSlice(gsl::make_span(s.data(), s.size()), b);
};

/**
 * @brief local helper function to write computed slice to provided buffer (AmiVector/span)
 * @param s computed value
 * @param b buffer to which values are to be written
 */
void writeSlice(const AmiVector &s, gsl::span<realtype> b);

/**
  * @brief Remove parameter scaling according to the parameter scaling in pscale
  *
  * All vectors must be of same length.
  *
  * @param bufferScaled scaled parameters
  * @param pscale parameter scaling
  * @param bufferUnscaled unscaled parameters are written to the array
  */
void unscaleParameters(gsl::span<const realtype> bufferScaled,
                       gsl::span<const ParameterScaling> pscale,
                       gsl::span<realtype> bufferUnscaled);

/**
  * @brief Remove parameter scaling according to `scaling`
  *
  * @param scaledParameter scaled parameter
  * @param scaling parameter scaling
  *
  * @return Unscaled parameter
  */
double getUnscaledParameter(double scaledParameter, ParameterScaling scaling);

/**
 * @brief Apply parameter scaling according to `scaling`
 * @param unscaledParameter
 * @param scaling parameter scaling
 * @return Scaled parameter
 */
double getScaledParameter(double unscaledParameter, ParameterScaling scaling);

/**
 * @brief Apply parameter scaling according to `scaling`
 * @param bufferUnscaled
 * @param pscale parameter scaling
 * @param bufferScaled destination
 */
void scaleParameters(gsl::span<const realtype> bufferUnscaled,
                     gsl::span<const ParameterScaling> pscale,
                     gsl::span<realtype> bufferScaled);

/**
 * @brief Returns the current backtrace as std::string
 * @param maxFrames Number of frames to include
 * @return Backtrace
 */
std::string backtraceString(int maxFrames);

/**
 * @brief Convert std::regex_constants::error_type to string
 * @param err_type error type
 * @return Error type as string
 */
std::string regexErrorToString(std::regex_constants::error_type err_type);

/**
 * @brief Format printf-style arguments to std::string
 * @param fmt Format string
 * @param ap Argument list pointer
 * @return Formatted String
 */
std::string printfToString(const char *fmt, va_list ap);

/**
 * @brief Generic implementation for a context manager, explicitly deletes copy
 * and move operators for derived classes
 */
class ContextManager{
  public:
    ContextManager() = default;
    ContextManager(ContextManager &other) = delete;
    ContextManager(ContextManager &&other) = delete;
};

} // namespace amici

#endif // AMICI_MISC_H


/*
 Copyright (c) 2015, Mina Kamel, ASL, ETH Zurich, Switzerland

 You can contact the author at <mina.kamel@mavt.ethz.ch>

 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright
 notice, this list of conditions and the following disclaimer.
 * 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.
 * Neither the name of ETHZ-ASL nor the
 names of its contributors may be used to endorse or promote products
 derived from this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 ETHZ-ASL 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.
 */

#ifndef INCLUDE_MAV_NONLINEAR_MPC_NONLINEAR_MPC_H_
#define INCLUDE_MAV_NONLINEAR_MPC_NONLINEAR_MPC_H_

#include <ros/ros.h>
#include <Eigen/Eigen>
#include <mav_msgs/conversions.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include <stdio.h>
#include <mav_control_interface/mpc_queue.h>
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <mav_disturbance_observer/KF_disturbance_observer.h>
#include <std_srvs/Empty.h>
#include <lapacke.h>

ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;

namespace mav_control {

lapack_logical select_lhp(const double *real, const double *imag)
{
  return *real < 0.0;
}

class NonlinearModelPredictiveControl
{
 public:
  NonlinearModelPredictiveControl(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
  ~NonlinearModelPredictiveControl();

  // Dynamic parameters
  void setPositionPenality(const Eigen::Vector3d& q_position)
  {
    q_position_ = q_position;
  }
  void setVelocityPenality(const Eigen::Vector3d& q_velocity)
  {
    q_velocity_ = q_velocity;
  }
  void setAttitudePenality(const Eigen::Vector2d& q_attitude)
  {
    q_attitude_ = q_attitude;
  }
  void setCommandPenality(const Eigen::Vector3d& r_command)
  {
    r_command_ = r_command;
  }
  void setYawGain(double K_yaw)
  {
    K_yaw_ = K_yaw;
  }

  void setAltitudeIntratorGain(double Ki_altitude)
  {
    Ki_altitude_ = Ki_altitude;
  }

  void setXYIntratorGain(double Ki_xy)
  {
    Ki_xy_ = Ki_xy;
  }

  void setEnableOffsetFree(bool enable_offset_free)
  {
    enable_offset_free_ = enable_offset_free;
  }

  void setEnableIntegrator(bool enable_integrator)
  {
    enable_integrator_ = enable_integrator;
  }

  void setControlLimits(const Eigen::VectorXd& control_limits)
  {
    //roll_max, pitch_max, yaw_rate_max, thrust_min and thrust_max
    roll_limit_ = control_limits(0);
    pitch_limit_ = control_limits(1);
    yaw_rate_limit_ = control_limits(2);
    thrust_min_ = control_limits(3);
    thrust_max_ = control_limits(4);
  }

  void applyParameters();

  double getMass() const
  {
    return mass_;
  }

  // get reference and predicted state
  bool getCurrentReference(mav_msgs::EigenTrajectoryPoint* reference) const;
  bool getCurrentReference(mav_msgs::EigenTrajectoryPointDeque* reference) const;
  bool getPredictedState(mav_msgs::EigenTrajectoryPointDeque* predicted_state) const;

  // set odom and commands
  void setOdometry(const mav_msgs::EigenOdometry& odometry);
  void setCommandTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory);
  void setCommandTrajectory(const mav_msgs::EigenTrajectoryPointDeque& command_trajectory);

  // compute control input
  void calculateRollPitchYawrateThrustCommand(Eigen::Vector4d* ref_attitude_thrust);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 private:

  // constants
  static constexpr double kGravity = 9.8066;
  static constexpr int kDisturbanceSize = 3;

  // ros node handles
  ros::NodeHandle nh_, private_nh_;

  // reset integrator service
  ros::ServiceServer reset_integrator_service_server_;
  bool resetIntegratorServiceCallback(std_srvs::Empty::Request &req,
                                      std_srvs::Empty::Response &res);

  // sampling time parameters
  void initializeParameters();
  bool initialized_parameters_;

  // sampling time parameters
  double sampling_time_;
  double prediction_sampling_time_;

  // system model parameters
  double mass_;
  double roll_time_constant_;
  double roll_gain_;
  double pitch_time_constant_;
  double pitch_gain_;
  Eigen::Vector3d drag_coefficients_;

  // controller parameters
  // state penalty
  Eigen::Vector3d q_position_;
  Eigen::Vector3d q_velocity_;
  Eigen::Vector2d q_attitude_;

  // control penalty
  Eigen::Vector3d r_command_;

  // yaw P gain
  double K_yaw_;

  // error integrator
  bool enable_integrator_;
  double Ki_altitude_;
  double Ki_xy_;
  double antiwindup_ball_;
  Eigen::Vector3d position_error_integration_;
  double position_error_integration_limit_;

  // control input limits
  double roll_limit_;
  double pitch_limit_;
  double yaw_rate_limit_;
  double thrust_min_;
  double thrust_max_;

  // reference queue
  MPCQueue mpc_queue_;
  Vector3dDeque position_ref_, velocity_ref_, acceleration_ref_;
  std::deque<double> yaw_ref_, yaw_rate_ref_;

  // solver matrices
  Eigen::Matrix<double, ACADO_NY, ACADO_NY> W_;
  Eigen::Matrix<double, ACADO_NYN, ACADO_NYN> WN_;
  Eigen::Matrix<double, ACADO_N + 1, ACADO_NX> state_;
  Eigen::Matrix<double, ACADO_N, ACADO_NU> input_;
  Eigen::Matrix<double, ACADO_N, ACADO_NY> reference_;
  Eigen::Matrix<double, 1, ACADO_NYN> referenceN_;
  Eigen::Matrix<double, ACADO_N + 1, ACADO_NOD> acado_online_data_;

  // disturbance observer
  bool enable_offset_free_;
  KFDisturbanceObserver disturbance_observer_;

  // commands
  Eigen::Vector4d command_roll_pitch_yaw_thrust_;

  // debug info
  bool verbose_;
  double solve_time_average_;

  // most recent odometry information
  mav_msgs::EigenOdometry odometry_;
  bool received_first_odometry_;

  // initilize solver
  void initializeAcadoSolver(Eigen::VectorXd x0);

  // solve continuous time Riccati equation
  Eigen::MatrixXd solveCARE(Eigen::MatrixXd Q, Eigen::MatrixXd R);

};

}

#endif /* INCLUDE_MAV_NONLINEAR_MPC_NONLINEAR_MPC_H_ */


/* spmatrix.c
 *
 * Copyright (C) 2012 Patrick Alken
 *
 * 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 3 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <math.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spmatrix.h>

#include "avl.c"

static int compare_triplet(const void *pa, const void *pb, void *param);
static void *avl_spmalloc (size_t size, void *param);
static void avl_spfree (void *block, void *param);

static struct libavl_allocator avl_allocator_spmatrix =
{
  avl_spmalloc,
  avl_spfree
};

/*
gsl_spmatrix_alloc()
  Allocate a sparse matrix in triplet representation

Inputs: n1 - number of rows
        n2 - number of columns

Notes: if (n1,n2) are not known at allocation time, they can each be
set to 1, and they will be expanded as elements are added to the matrix
*/

gsl_spmatrix *
gsl_spmatrix_alloc(const size_t n1, const size_t n2)
{
  const double density = 0.1; /* estimate */
  size_t nzmax = (size_t) floor(n1 * n2 * density);

  if (nzmax == 0)
    nzmax = 10;

  return gsl_spmatrix_alloc_nzmax(n1, n2, nzmax, GSL_SPMATRIX_TRIPLET);
} /* gsl_spmatrix_alloc() */

/*
gsl_spmatrix_alloc_nzmax()
  Allocate a sparse matrix with given nzmax

Inputs: n1     - number of rows
        n2     - number of columns
        nzmax  - maximum number of matrix elements
        sptype - type of matrix (triplet, CCS, CRS)

Notes: if (n1,n2) are not known at allocation time, they can each be
set to 1, and they will be expanded as elements are added to the matrix
*/

gsl_spmatrix *
gsl_spmatrix_alloc_nzmax(const size_t n1, const size_t n2,
                         const size_t nzmax, const size_t sptype)
{
  gsl_spmatrix *m;

  if (n1 == 0)
    {
      GSL_ERROR_NULL ("matrix dimension n1 must be positive integer",
                      GSL_EINVAL);
    }
  else if (n2 == 0)
    {
      GSL_ERROR_NULL ("matrix dimension n2 must be positive integer",
                      GSL_EINVAL);
    }

  m = calloc(1, sizeof(gsl_spmatrix));
  if (!m)
    {
      GSL_ERROR_NULL("failed to allocate space for spmatrix struct",
                     GSL_ENOMEM);
    }

  m->size1 = n1;
  m->size2 = n2;
  m->nz = 0;
  m->nzmax = GSL_MAX(nzmax, 1);
  m->sptype = sptype;

  m->i = malloc(m->nzmax * sizeof(size_t));
  if (!m->i)
    {
      gsl_spmatrix_free(m);
      GSL_ERROR_NULL("failed to allocate space for row indices",
                     GSL_ENOMEM);
    }

  if (sptype == GSL_SPMATRIX_TRIPLET)
    {
      m->tree_data = malloc(sizeof(gsl_spmatrix_tree));
      if (!m->tree_data)
        {
          gsl_spmatrix_free(m);
          GSL_ERROR_NULL("failed to allocate space for AVL tree struct",
                         GSL_ENOMEM);
        }

      m->tree_data->n = 0;

      /* allocate tree data structure */
      m->tree_data->tree = avl_create(compare_triplet, (void *) m,
                                      &avl_allocator_spmatrix);
      if (!m->tree_data->tree)
        {
          gsl_spmatrix_free(m);
          GSL_ERROR_NULL("failed to allocate space for AVL tree",
                         GSL_ENOMEM);
        }

      /* preallocate nzmax tree nodes */
      m->tree_data->node_array = malloc(m->nzmax * sizeof(struct avl_node));
      if (!m->tree_data->node_array)
        {
          gsl_spmatrix_free(m);
          GSL_ERROR_NULL("failed to allocate space for AVL tree nodes",
                         GSL_ENOMEM);
        }

      m->p = malloc(m->nzmax * sizeof(size_t));
      if (!m->p)
        {
          gsl_spmatrix_free(m);
          GSL_ERROR_NULL("failed to allocate space for column indices",
                         GSL_ENOMEM);
        }
    }
  else if (sptype == GSL_SPMATRIX_CCS)
    {
      m->p = malloc((n2 + 1) * sizeof(size_t));
      m->work = malloc(GSL_MAX(n1, n2) *
                       GSL_MAX(sizeof(size_t), sizeof(double)));
      if (!m->p || !m->work)
        {
          gsl_spmatrix_free(m);
          GSL_ERROR_NULL("failed to allocate space for column pointers",
                         GSL_ENOMEM);
        }
    }
  else if (sptype == GSL_SPMATRIX_CRS)
    {
      m->p = malloc((n1 + 1) * sizeof(size_t));
      m->work = malloc(GSL_MAX(n1, n2) *
                       GSL_MAX(sizeof(size_t), sizeof(double)));
      if (!m->p || !m->work)
        {
          gsl_spmatrix_free(m);
          GSL_ERROR_NULL("failed to allocate space for row pointers",
                         GSL_ENOMEM);
        }
    }

  m->data = malloc(m->nzmax * sizeof(double));
  if (!m->data)
    {
      gsl_spmatrix_free(m);
      GSL_ERROR_NULL("failed to allocate space for data",
                     GSL_ENOMEM);
    }

  return m;
} /* gsl_spmatrix_alloc_nzmax() */

/*
gsl_spmatrix_free()
  Free sparse matrix object
*/

void
gsl_spmatrix_free(gsl_spmatrix *m)
{
  if (m->i)
    free(m->i);

  if (m->p)
    free(m->p);

  if (m->data)
    free(m->data);

  if (m->work)
    free(m->work);

  if (m->tree_data)
    {
      if (m->tree_data->tree)
        avl_destroy(m->tree_data->tree, NULL);

      if (m->tree_data->node_array)
        free(m->tree_data->node_array);

      free(m->tree_data);
    }

  free(m);
} /* gsl_spmatrix_free() */

/*
gsl_spmatrix_realloc()
  As elements are added to the sparse matrix, its possible that they
will exceed the previously specified nzmax - reallocate the matrix
with a new nzmax
*/

int
gsl_spmatrix_realloc(const size_t nzmax, gsl_spmatrix *m)
{
  int s = GSL_SUCCESS;
  void *ptr;

  if (nzmax < m->nz)
    {
      GSL_ERROR("new nzmax is less than current nz", GSL_EINVAL);
    }

  ptr = realloc(m->i, nzmax * sizeof(size_t));
  if (!ptr)
    {
      GSL_ERROR("failed to allocate space for row indices", GSL_ENOMEM);
    }

  m->i = (size_t *) ptr;

  if (GSL_SPMATRIX_ISTRIPLET(m))
    {
      ptr = realloc(m->p, nzmax * sizeof(size_t));
      if (!ptr)
        {
          GSL_ERROR("failed to allocate space for column indices", GSL_ENOMEM);
        }

      m->p = (size_t *) ptr;
    }

  ptr = realloc(m->data, nzmax * sizeof(double));
  if (!ptr)
    {
      GSL_ERROR("failed to allocate space for data", GSL_ENOMEM);
    }

  m->data = (double *) ptr;

  /* rebuild binary tree */
  if (GSL_SPMATRIX_ISTRIPLET(m))
    {
      size_t n;

      /* reset tree to empty state, but don't free root tree ptr */
      avl_empty(m->tree_data->tree, NULL);
      m->tree_data->n = 0;

      ptr = realloc(m->tree_data->node_array, nzmax * sizeof(struct avl_node));
      if (!ptr)
        {
          GSL_ERROR("failed to allocate space for AVL tree nodes", GSL_ENOMEM);
        }

      m->tree_data->node_array = ptr;

      /*
       * need to reinsert all tree elements since the m->data addresses
       * have changed
       */
      for (n = 0; n < m->nz; ++n)
        {
          ptr = avl_insert(m->tree_data->tree, &m->data[n]);
          if (ptr != NULL)
            {
              GSL_ERROR("detected duplicate entry", GSL_EINVAL);
            }
        }
    }

  /* update to new nzmax */
  m->nzmax = nzmax;

  return s;
} /* gsl_spmatrix_realloc() */

int
gsl_spmatrix_set_zero(gsl_spmatrix *m)
{
  m->nz = 0;

  if (GSL_SPMATRIX_ISTRIPLET(m))
    {
      /* reset tree to empty state and node index pointer to 0 */
      avl_empty(m->tree_data->tree, NULL);
      m->tree_data->n = 0;
    }

  return GSL_SUCCESS;
} /* gsl_spmatrix_set_zero() */

size_t
gsl_spmatrix_nnz(const gsl_spmatrix *m)
{
  return m->nz;
} /* gsl_spmatrix_nnz() */

/*
gsl_spmatrix_compare_idx()
  Comparison function for searching binary tree in triplet
representation.

To detect duplicate elements in the tree, we want to determine
if there already exists an entry for (i,j) in the tree. Since
the actual tree node stores only the data elements data[n],
we will do pointer arithmetic to get from the given data[n]
to the row/column indices i[n] and j[n].

This compare function will sort the tree first by row i,
and for equal rows, it will then sort by column j

Inputs: ia - row index of element a
        ja - column index of element a
        ib - row index of element b
        jb - column index of element b

Return:
  -1 if pa < pb: (ia,ja) < (ib,jb)
  +1 if pa > pb: (ia,ja) > (ib,jb)
   0 if pa = pb: (ia,ja) == (ib,jb)
*/

int
gsl_spmatrix_compare_idx(const size_t ia, const size_t ja,
                         const size_t ib, const size_t jb)
{
  if (ia < ib)
    return -1;
  else if (ia > ib)
    return 1;
  else
    {
      /* row indices are equal, sort by column index */
      if (ja < jb)
        return -1;
      else if (ja > jb)
        return 1;
      else
        return 0; /* row and column indices are equal */
    }
}

/*
gsl_spmatrix_tree_rebuild()
  When reading a triplet matrix from disk, or when
copying a triplet matrix, it is necessary to rebuild the
binary tree for element searches.

Inputs: m - triplet matrix
*/

int
gsl_spmatrix_tree_rebuild(gsl_spmatrix * m)
{
  if (!GSL_SPMATRIX_ISTRIPLET(m))
    {
      GSL_ERROR("m must be in triplet format", GSL_EINVAL);
    }
  else
    {
      size_t n;

      /* reset tree to empty state, but don't free root tree ptr */
      avl_empty(m->tree_data->tree, NULL);
      m->tree_data->n = 0;

      /* insert all tree elements */
      for (n = 0; n < m->nz; ++n)
        {
          void *ptr = avl_insert(m->tree_data->tree, &m->data[n]);
          if (ptr != NULL)
            {
              GSL_ERROR("detected duplicate entry", GSL_EINVAL);
            }
        }

      return GSL_SUCCESS;
    }
}

/*
compare_triplet()
  Comparison function for searching binary tree in triplet
representation.

To detect duplicate elements in the tree, we want to determine
if there already exists an entry for (i,j) in the tree. Since
the actual tree node stores only the data elements data[n],
we will do pointer arithmetic to get from the given data[n]
to the row/column indices i[n] and j[n].

This compare function will sort the tree first by row i,
and for equal rows, it will then sort by column j

Inputs: pa    - element 1 for comparison (double *)
        pb    - element 2 for comparison (double *)
        param - parameter (gsl_spmatrix)

Return:
  -1 if pa < pb: (ia,ja) < (ib,jb)
  +1 if pa > pb: (ia,ja) > (ib,jb)
   0 if pa = pb: (ia,ja) == (ib,jb)
*/

static int
compare_triplet(const void *pa, const void *pb, void *param)
{
  gsl_spmatrix *m = (gsl_spmatrix *) param;

  /* pointer arithmetic to find indices in data array */
  const size_t idxa = (const double *) pa - m->data;
  const size_t idxb = (const double *) pb - m->data;

  return gsl_spmatrix_compare_idx(m->i[idxa], m->p[idxa],
                                  m->i[idxb], m->p[idxb]);
} /* compare_triplet() */

static void *
avl_spmalloc (size_t size, void *param)
{
  gsl_spmatrix *m = (gsl_spmatrix *) param;

  if (size != sizeof(struct avl_node))
    {
      GSL_ERROR_NULL("attemping to allocate incorrect node size", GSL_EBADLEN);
    }

  /*
   * return the next available avl_node slot; index
   * m->tree_data->n keeps track of next open slot
   */
  if (m->tree_data->n < m->nzmax)
    {
      /* cast to char* for pointer arithmetic */
      unsigned char *node_ptr = (unsigned char *) m->tree_data->node_array;

      /* offset in bytes for next node slot */
      size_t offset = (m->tree_data->n)++ * sizeof(struct avl_node);

      return node_ptr + offset;
    }
  else
    {
      /*
       * we should never get here - gsl_spmatrix_realloc() should
       * be called before exceeding nzmax nodes
       */
      GSL_ERROR_NULL("attemping to allocate tree node past nzmax", GSL_EINVAL);
    }
}

static void
avl_spfree (void *block, void *param)
{
  (void)block;
  (void)param;
  /*
   * do nothing - instead of allocating/freeing individual nodes,
   * we malloc and free nzmax nodes at a time
   */
}


#include <stdio.h>
#include <gsl/gsl_matrix.h>

int
main (void)
{
  int i, j, k = 0;
  gsl_matrix * m = gsl_matrix_alloc (100, 100);
  gsl_matrix * a = gsl_matrix_alloc (100, 100);

  for (i = 0; i < 100; i++)
    for (j = 0; j < 100; j++)
      gsl_matrix_set (m, i, j, 0.23 + i + j);

  {
     FILE * f = fopen ("test.dat", "wb");
     gsl_matrix_fwrite (f, m);
     fclose (f);
  }

  {
     FILE * f = fopen ("test.dat", "rb");
     gsl_matrix_fread (f, a);
     fclose (f);
  }

  for (i = 0; i < 100; i++)
    for (j = 0; j < 100; j++)
      {
        double mij = gsl_matrix_get (m, i, j);
        double aij = gsl_matrix_get (a, i, j);
        if (mij != aij) k++;
      }

  gsl_matrix_free (m);
  gsl_matrix_free (a);

  printf ("differences = %d (should be zero)\n", k);
  return (k > 0);
}


/* ode-initval/control.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_odeiv.h"

gsl_odeiv_control *
gsl_odeiv_control_alloc(const gsl_odeiv_control_type * T)
{
  gsl_odeiv_control * c =
    (gsl_odeiv_control *) malloc(sizeof(gsl_odeiv_control));

  if(c == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for control struct",
                      GSL_ENOMEM);
    };

  c->type = T;
  c->state = c->type->alloc();

  if (c->state == 0)
    {
      free (c);		/* exception in constructor, avoid memory leak */

      GSL_ERROR_NULL ("failed to allocate space for control state",
                      GSL_ENOMEM);
    };

  return c;
}

int
gsl_odeiv_control_init(gsl_odeiv_control * c,
                       double eps_abs, double eps_rel,
                       double a_y, double a_dydt)
{
  return c->type->init (c->state, eps_abs, eps_rel, a_y, a_dydt);
}

void
gsl_odeiv_control_free(gsl_odeiv_control * c)
{
  c->type->free(c->state);
  free(c);
}

const char *
gsl_odeiv_control_name(const gsl_odeiv_control * c)
{
  return c->type->name;
}

int
gsl_odeiv_control_hadjust (gsl_odeiv_control * c, gsl_odeiv_step * s, const double y0[], const double yerr[], const double dydt[], double * h)
{
  return c->type->hadjust(c->state, s->dimension, s->type->order(s->state),
                          y0, yerr, dydt, h);
}


#include "cwc.h"
#include "../ccv_internal.h"
#ifdef HAVE_GSL
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#endif
#include "cwc_ext.h"
#ifdef USE_DISPATCH
#include <dispatch/dispatch.h>
#endif

#ifdef HAVE_GSL
static void _cwc_convnet_random_image_manipulation(gsl_rng* rng, ccv_dense_matrix_t* image, float image_manipulation)
{
	assert(rng && CCV_GET_CHANNEL(image->type) == CCV_C3 && image_manipulation > 0 && image_manipulation <= 1);
	int ord[3] = {0, 1, 2};
	gsl_ran_shuffle(rng, ord, 3, sizeof(int));
	int i;
	for (i = 0; i < 3; i++)
		// change the applying order
		switch (ord[i])
		{
			case 0:
				// introduce some brightness changes to the original image
				ccv_scale(image, (ccv_matrix_t**)&image, 0, gsl_rng_uniform_pos(rng) * image_manipulation * 2 + (1 - image_manipulation));
				break;
			case 1:
				// introduce some saturation changes to the original image
				ccv_saturation(image, &image, 0, gsl_rng_uniform_pos(rng) * image_manipulation * 2 + (1 - image_manipulation));
				break;
			case 2:
				// introduce some contrast changes to the original image
				ccv_contrast(image, &image, 0, gsl_rng_uniform_pos(rng) * image_manipulation * 2 + (1 - image_manipulation));
				break;
		}
}

void cwc_convnet_batch_formation(gsl_rng* rng, ccv_array_t* categorizeds, ccv_dense_matrix_t* mean_activity, ccv_dense_matrix_t* eigenvectors, ccv_dense_matrix_t* eigenvalues, float image_manipulation, float color_gain, int* idx, ccv_size_t dim, int min_dim, int max_dim, int rows, int cols, int channels, int category_count, int symmetric, int batch, int offset, int size, float* b, int* c)
{
	assert(size > 0 && size <= batch);
	assert(min_dim >= rows && min_dim >= cols);
	assert(max_dim >= min_dim);
	float* channel_gains = (float*)alloca(sizeof(float) * channels * size);
	memset(channel_gains, 0, sizeof(float) * channels * size);
	int i;
	gsl_rng** rngs = (gsl_rng**)alloca(sizeof(gsl_rng*) * size);
	memset(rngs, 0, sizeof(gsl_rng*) * size);
	if (rng)
		for (i = 0; i < size; i++)
		{
			rngs[i] = gsl_rng_alloc(gsl_rng_default);
			gsl_rng_set(rngs[i], gsl_rng_get(rng));
		}
	parallel_for(i, size) {
		int j, k;
		assert(offset + i < categorizeds->rnum);
		ccv_categorized_t* categorized = (ccv_categorized_t*)ccv_array_get(categorizeds, idx ? idx[offset + i] : offset + i);
		assert(categorized->c < category_count && categorized->c >= 0); // now only accept classes listed
		if (c)
			c[i] = categorized->c;
		ccv_dense_matrix_t* image = 0;
		switch (categorized->type)
		{
			case CCV_CATEGORIZED_DENSE_MATRIX:
				image = categorized->matrix;
				break;
			case CCV_CATEGORIZED_FILE:
				image = 0;
				ccv_read(categorized->file.filename, &image, CCV_IO_ANY_FILE | CCV_IO_RGB_COLOR);
				break;
		}
		if (image)
		{
			// first resize to between min_dim and max_dim
			ccv_dense_matrix_t* input = 0;
			ccv_size_t resize = dim;
			if (rngs[i]) // randomize the resized dimensions
			{
				int d = gsl_rng_uniform_int(rngs[i], max_dim - min_dim + 1) + min_dim;
				resize = ccv_size(d, d);
			}
			// if neither side is the same as expected, we have to resize first
			if (resize.height != image->rows && resize.width != image->cols)
				ccv_convnet_input_formation(resize, image, &input);
			else
				input = image;
			if (rngs[i] && image_manipulation > 0)
				_cwc_convnet_random_image_manipulation(rngs[i], input, image_manipulation);
			ccv_dense_matrix_t* patch = 0;
			if (input->cols != cols || input->rows != rows)
			{
				int x = rngs[i] ? gsl_rng_uniform_int(rngs[i], input->cols - cols + 1) : (input->cols - cols + 1) / 2;
				int y = rngs[i] ? gsl_rng_uniform_int(rngs[i], input->rows - rows + 1) : (input->rows - rows + 1) / 2;
				ccv_slice(input, (ccv_matrix_t**)&patch, CCV_32F, y, x, rows, cols);
			} else
				ccv_shift(input, (ccv_matrix_t**)&patch, CCV_32F, 0, 0); // converting to 32f
			if (input != image) // only unload if we created new input
				ccv_matrix_free(input);
			// we loaded image in, deallocate it now
			if (categorized->type != CCV_CATEGORIZED_DENSE_MATRIX)
				ccv_matrix_free(image);
			// random horizontal reflection
			if (symmetric && rngs[i] && gsl_rng_uniform_int(rngs[i], 2) == 0)
				ccv_flip(patch, &patch, 0, CCV_FLIP_X);
			int x = rngs[i] ? gsl_rng_uniform_int(rngs[i], mean_activity->cols - cols + 1) : (mean_activity->cols - cols + 1) / 2;
			int y = rngs[i] ? gsl_rng_uniform_int(rngs[i], mean_activity->rows - rows + 1) : (mean_activity->rows - rows + 1) / 2;
			ccv_dense_matrix_t mean_patch = ccv_reshape(mean_activity, y, x, rows, cols);
			ccv_subtract(patch, &mean_patch, (ccv_matrix_t**)&patch, 0);
			assert(channels == CCV_GET_CHANNEL(patch->type));
			if (color_gain > 0 && rngs[i] && eigenvectors && eigenvalues)
			{
				assert(channels == 3); // only support RGB color gain
				memset(channel_gains + channels * i, 0, sizeof(float) * channels);
				for (j = 0; j < channels; j++)
				{
					float alpha = gsl_ran_gaussian(rngs[i], color_gain) * eigenvalues->data.f64[j];
					for (k = 0; k < channels; k++)
						channel_gains[k + i * channels] += eigenvectors->data.f64[j * channels + k] * alpha;
				}
			}
			for (j = 0; j < channels; j++)
				for (k = 0; k < rows * cols; k++)
					b[(j * rows * cols + k) * batch + i] = patch->data.f32[k * channels + j] + channel_gains[j + i * channels];
			ccv_matrix_free(patch);
		} else
			PRINT(CCV_CLI_ERROR, "cannot load %s.\n", categorized->file.filename);
	} parallel_endfor
	if (rng)
		for (i = 0; i < size; i++)
			gsl_rng_free(rngs[i]);
}
#endif

void cwc_convnet_mean_formation(ccv_array_t* categorizeds, ccv_size_t dim, int channels, int symmetric, ccv_dense_matrix_t** b)
{
	int i, count = 0;
	ccv_dense_matrix_t* c = ccv_dense_matrix_new(dim.height, dim.width, channels | CCV_64F, 0, 0);
	ccv_zero(c);
	ccv_dense_matrix_t* db = *b = ccv_dense_matrix_renew(*b, dim.height, dim.width, channels | CCV_32F, channels | CCV_32F, 0);
	for (i = 0; i < categorizeds->rnum; i++)
	{
		if (i % 23 == 0 || i == categorizeds->rnum - 1)
			FLUSH(CCV_CLI_INFO, " - compute mean activity %d / %d", i + 1, categorizeds->rnum);
		ccv_categorized_t* categorized = (ccv_categorized_t*)ccv_array_get(categorizeds, i);
		ccv_dense_matrix_t* image = 0;
		switch (categorized->type)
		{
			case CCV_CATEGORIZED_DENSE_MATRIX:
				image = categorized->matrix;
				break;
			case CCV_CATEGORIZED_FILE:
				ccv_read(categorized->file.filename, &image, CCV_IO_ANY_FILE | CCV_IO_RGB_COLOR);
				break;
		}
		if (!image)
		{
			PRINT(CCV_CLI_ERROR, "cannot load %s.\n", categorized->file.filename);
			continue;
		}
		ccv_dense_matrix_t* patch = 0;
		if (image->cols != dim.width || image->rows != dim.height)
		{
			int x = (image->cols - dim.width + 1) / 2;
			int y = (image->rows - dim.height + 1) / 2;
			assert(x == 0 || y == 0);
			ccv_slice(image, (ccv_matrix_t**)&patch, CCV_32F, y, x, dim.height, dim.width);
		} else
			ccv_shift(image, (ccv_matrix_t**)&patch, CCV_32F, 0, 0); // converting to 32f
		if (categorized->type != CCV_CATEGORIZED_DENSE_MATRIX)
			ccv_matrix_free(image);
		ccv_add(patch, c, (ccv_matrix_t**)&c, CCV_64F);
		++count;
		ccv_matrix_free(patch);
	}
	if (symmetric)
	{
		int j, k;
		double p = 0.5 / count;
		double* cptr = c->data.f64;
		float* dbptr = db->data.f32;
		for (i = 0; i < db->rows; i++)
		{
			for (j = 0; j < db->cols; j++)
				for (k = 0; k < channels; k++)
					dbptr[j * channels + k] = p * (cptr[j * channels + k] + cptr[(c->cols - j - 1) * channels + k]);
			dbptr += db->cols * channels;
			cptr += c->cols * channels;
		}
	} else {
		double p = 1.0 / count;
		for (i = 0; i < dim.height * dim.width * channels; i++)
			db->data.f32[i] = p * c->data.f64[i];
	}
	ccv_matrix_free(c);
	PRINT(CCV_CLI_INFO, "\n");
}

void cwc_convnet_channel_eigen(ccv_array_t* categorizeds, ccv_dense_matrix_t* mean_activity, ccv_size_t dim, int channels, ccv_dense_matrix_t** eigenvectors, ccv_dense_matrix_t** eigenvalues)
{
	assert(channels == 3); // this function cannot handle anything other than 3x3 covariance matrix
	double* mean_value = (double*)alloca(sizeof(double) * channels);
	memset(mean_value, 0, sizeof(double) * channels);
	assert(CCV_GET_CHANNEL(mean_activity->type) == channels);
	assert(mean_activity->rows == dim.height);
	assert(mean_activity->cols == dim.width);
	int i, j, k, c, count = 0;
	for (i = 0; i < dim.height * dim.width; i++)
		for (k = 0; k < channels; k++)
			mean_value[k] += mean_activity->data.f32[i * channels + k];
	for (i = 0; i < channels; i++)
		mean_value[i] = mean_value[i] / (dim.height * dim.width);
	double* covariance = (double*)alloca(sizeof(double) * channels * channels);
	memset(covariance, 0, sizeof(double) * channels * channels);
	for (c = 0; c < categorizeds->rnum; c++)
	{
		if (c % 23 == 0 || c == categorizeds->rnum - 1)
			FLUSH(CCV_CLI_INFO, " - compute covariance matrix for data augmentation (color gain) %d / %d", c + 1, categorizeds->rnum);
		ccv_categorized_t* categorized = (ccv_categorized_t*)ccv_array_get(categorizeds, c);
		ccv_dense_matrix_t* image = 0;
		switch (categorized->type)
		{
			case CCV_CATEGORIZED_DENSE_MATRIX:
				image = categorized->matrix;
				break;
			case CCV_CATEGORIZED_FILE:
				ccv_read(categorized->file.filename, &image, CCV_IO_ANY_FILE | CCV_IO_RGB_COLOR);
				break;
		}
		if (!image)
		{
			PRINT(CCV_CLI_ERROR, "cannot load %s.\n", categorized->file.filename);
			continue;
		}
		ccv_dense_matrix_t* patch = 0;
		if (image->cols != dim.width || image->rows != dim.height)
		{
			int x = (image->cols - dim.width + 1) / 2;
			int y = (image->rows - dim.height + 1) / 2;
			assert(x == 0 || y == 0);
			ccv_slice(image, (ccv_matrix_t**)&patch, CCV_32F, y, x, dim.height, dim.width);
		} else
			ccv_shift(image, (ccv_matrix_t**)&patch, CCV_32F, 0, 0); // converting to 32f
		if (categorized->type != CCV_CATEGORIZED_DENSE_MATRIX)
			ccv_matrix_free(image);
		for (i = 0; i < dim.width * dim.height; i++)
			for (j = 0; j < channels; j++)
				for (k = j; k < channels; k++)
					covariance[j * channels + k] += (patch->data.f32[i * channels + j] - mean_value[j]) * (patch->data.f32[i * channels + k] - mean_value[k]);
		++count;
		ccv_matrix_free(patch);
	}
	for (i = 0; i < channels; i++)
		for (j = 0; j < i; j++)
			covariance[i * channels + j] = covariance[j * channels + i];
	double p = 1.0 / ((double)count * dim.height * dim.width);
	for (i = 0; i < channels; i++)
		for (j = 0; j < channels; j++)
			covariance[i * channels + j] *= p; // scale down
	ccv_dense_matrix_t covm = ccv_dense_matrix(3, 3, CCV_64F | CCV_C1, covariance, 0);
	ccv_eigen(&covm, eigenvectors, eigenvalues, CCV_64F, 1e-8);
	PRINT(CCV_CLI_INFO, "\n");
}


/*
 This file forms part of hpGEM. This package has been developed over a number of
 years by various people at the University of Twente and a full list of
 contributors can be found at http://hpgem.org/about-the-code/team

 This code is distributed using BSD 3-Clause License. A copy of which can found
 below.

 Copyright (c) 2020, University of Twente
 All rights reserved.

 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.

 3. Neither the name of the copyright holder nor the names of its contributors
 may be used to endorse or promote products derived from this software without
 specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER 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.
 */
#ifndef HPGEM_MATRIXBLOCKS_H
#define HPGEM_MATRIXBLOCKS_H

#include <vector>
#include <petsc.h>

#include "LinearAlgebra/MiddleSizeMatrix.h"

namespace DGMax {

using namespace hpgem;

/// A block from the global matrix, or a pair of blocks that are placed
/// symmetrically.
class MatrixBlocks {
   public:
    MatrixBlocks(std::vector<PetscInt> rowIndices,
                 std::vector<PetscInt> columnIndices,
                 LinearAlgebra::MiddleSizeMatrix block)
        : rowIndices_(std::move(rowIndices)),
          columnIndices_(std::move(columnIndices)),
          block1_(std::move(block)),
          pair_(false){};

    MatrixBlocks(std::vector<PetscInt> rowIndices,
                 std::vector<PetscInt> columnIndices,
                 LinearAlgebra::MiddleSizeMatrix block1,
                 LinearAlgebra::MiddleSizeMatrix block2)
        : rowIndices_(std::move(rowIndices)),
          columnIndices_(std::move(columnIndices)),
          block1_(std::move(block1)),
          block2_(std::move(block2)),
          pair_(true){};

    /// Number of entries in the matrix block. If this is pair, then this gives
    /// the size of a single block.
    std::size_t getBlockSize() const {
        return rowIndices_.size() * columnIndices_.size();
    }

    /// Whether this is a pair of blocks or a single block.
    bool isPair() const { return pair_; }

    /// \brief Load blocks into temporary storage for insertion.
    ///
    /// This stores the raw entries of the block in the storage space for use
    /// with insertBlocks(). The first getBlockSize() entries of the storage are
    /// used for storing the first matrix block. If this object is a pair then
    /// the subsequent getBlockSize() entries will be used for the second block.
    ///
    /// \param storage The storage to use, will be resized if necessary.
    void loadBlocks(std::vector<PetscScalar>& storage) const {
        // Make sure we have enough space
        std::size_t storageSize = getBlockSize();
        std::size_t storageMultiplier = pair_ ? 2 : 1;
        if (storage.size() < storageMultiplier * storageSize) {
            storage.resize(storageMultiplier * storageSize);
        }
        // Copy data
        for (std::size_t i = 0; i < storageSize; ++i) {
            storage[i] = block1_[i];
        }
        if (pair_) {
            for (std::size_t i = 0; i < storageSize; ++i) {
                storage[i + storageSize] = block2_[i];
            }
        }
    }

    /// \brief Insert the block or blocks in a PetscMatrix.
    void insertBlocks(std::vector<PetscScalar>& storage, Mat mat) const {
        insertBlock(storage, false, mat);
        if (pair_) {
            insertBlock(storage, true, mat);
        }
    }

   private:
    /// The global indices for the rows
    std::vector<PetscInt> rowIndices_;
    /// The global indices for the columns
    std::vector<PetscInt> columnIndices_;

    /// The primary block
    LinearAlgebra::MiddleSizeMatrix block1_;
    /// The symmetrically placed block
    LinearAlgebra::MiddleSizeMatrix block2_;

    /// Whether this is a single block or a symmetrically placed pair.
    bool pair_;

    /// Helper function inserting a single block.
    void insertBlock(std::vector<PetscScalar>& storage, bool hermitianPart,
                     Mat mat) const;

    /// Validate the correctnees
    void validate() const;
};

}  // namespace DGMax

#endif  // HPGEM_MATRIXBLOCKS_H


#pragma once

#include "mexObjectiveFunction.h"

#include <nlopt.h>
#include <mex.h>

#include <string>
#include <unordered_map>
#include <utility>

#ifndef MX_CURRENT_API_VER
#define mxIsScalar(A) (mxGetNumberOfElements(A)==1)
#endif

#define MEX_ACTION_ARGUMENTS const mxArray *mxObj, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// The class that we are interfacing to
class mexNLopt
{
public:
  mexNLopt(const mxArray *mxObj, int nrhs, const mxArray *prhs[]) : opt(NULL) { init(prhs[0], prhs[1]); }

  /**
   * \brief Copy constructor
   */
  mexNLopt(const mexNLopt &src) : opt(nlopt_copy(src.opt)) {}

  /**
   * \brief Move constructor
   */
  mexNLopt(mexNLopt &&src) : opt(std::move(src.opt)) {}

  /**
   * \brief Destructor
   */
  ~mexNLopt() { if (opt) nlopt_destroy(opt); }

  /**
   * \brief Copy assignment
   */
  mexNLopt &operator=(const mexNLopt &src);

  /**
   * \brief Copy assignment
   */
  mexNLopt &operator=(mexNLopt &&src) noexcept;

  /**
   * \brief Returns the wrapping MATLAB class name
   */
  static std::string get_classname() { return "nlopt.options"; }; // must match the Matlab classname

  /**
   * \brief Performs static (object-independent) actions
   */
  static bool static_handler(std::string command, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);

  /**
   * \brief Performs object-dependent actions
   */
  bool action_handler(const mxArray *mxObj, const std::string &command, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);

private:
  /**
 * \brief Return NLopt version string
 */
  static mxArray *getNLoptVersion();

  /**
 * \brief Return algorithm name given algorithm enum
 */
  static std::string get_algorithm_name_string(nlopt_algorithm a);

  /**
 * \brief Return algorithm name given algorithm enum
 */
  static mxArray *get_algorithm_name(nlopt_algorithm a);

  /**
 * \brief Return algorithm description given algorithm enum
 */
  static mxArray *get_algorithm_desc(nlopt_algorithm a);

  /**
 * \brief Iterate over algorithms performing unary function on each element
 *
 * \note signature for f: \code bool f(const nlopt_algorithm a) \endcode
 * \note if f returns false, iteration stops immediately
 */
  template <class UnaryFunction>
  static void for_each_algorithm(UnaryFunction f)
  {
    for (int n = 0; n < (int)NLOPT_NUM_ALGORITHMS && f((nlopt_algorithm)n); ++n)
      ;
  }

  /**
 * \brief Returns a cellstr array of all available algorithms
 */
  static mxArray *getAlgorithms(const mxArray *incl_desc);

  /**
 * \brief Returns algorithm given by mxString
 */
  static nlopt_algorithm find_algorithm_by_name(const mxArray *mxStr);

  nlopt_opt opt; /** nlopt_opt "object" (an opaque pointer) */

  /**
 * \brief Initialize nlopt object
 */
  void init(const mxArray *mxAlgorithm, const mxArray *mxDim);

  // GET ROUTINES

  void getAlgorithm(MEX_ACTION_ARGUMENTS) const { plhs[0] = mexNLopt::get_algorithm_name(nlopt_get_algorithm(opt)); }
  void getDimension(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar((double)nlopt_get_dimension(opt)); }

  /* stopping criteria: */
  void getStopVal(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar(nlopt_get_stopval(opt)); }
  void getFTolRel(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar(nlopt_get_ftol_rel(opt)); }
  void getXTolRel(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar(nlopt_get_xtol_rel(opt)); }
  void getFTolAbs(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar(nlopt_get_ftol_abs(opt)); }
  void getXTolAbs(MEX_ACTION_ARGUMENTS) const;
  void getMaxEval(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar((double)nlopt_get_maxeval(opt)); }
  void getMaxTime(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar(nlopt_get_maxtime(opt)); }
  void getPopulation(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar((double)nlopt_get_population(opt)); }
  void getVectorStorage(MEX_ACTION_ARGUMENTS) const { plhs[0] = mxCreateDoubleScalar((double)nlopt_get_vector_storage(opt)); }
  void getInitialStep(MEX_ACTION_ARGUMENTS) const;
  mxArray *getInitialStep(const mxArray *x0) const;

  // SET ROUTINES
  void setStopVal(MEX_ACTION_ARGUMENTS)
  {
    double sval = mxGetScalar(prhs[0]);
    nlopt_set_stopval(opt, isinf(sval) ? (sval > 0) ? HUGE_VAL : -HUGE_VAL : sval);
  }
  void setFTolRel(MEX_ACTION_ARGUMENTS) { nlopt_set_ftol_rel(opt, mxGetScalar(prhs[0])); }
  void setXTolRel(MEX_ACTION_ARGUMENTS) { nlopt_set_xtol_rel(opt, mxGetScalar(prhs[0])); }
  void setFTolAbs(MEX_ACTION_ARGUMENTS) { nlopt_set_ftol_abs(opt, mxGetScalar(prhs[0])); }
  void setXTolAbs(MEX_ACTION_ARGUMENTS)
  {
    if (mxIsScalar(prhs[0]))
      nlopt_set_xtol_abs1(opt, mxGetScalar(prhs[0]));
    else
      nlopt_set_xtol_abs(opt, mxGetPr(prhs[0]));
  }
  void setMaxEval(MEX_ACTION_ARGUMENTS) { nlopt_set_maxeval(opt, (int)mxGetScalar(prhs[0])); }
  void setMaxTime(MEX_ACTION_ARGUMENTS) { nlopt_set_maxtime(opt, mxGetScalar(prhs[0])); }
  void setPopulation(MEX_ACTION_ARGUMENTS) { nlopt_set_population(opt, (unsigned)mxGetScalar(prhs[0])); }
  void setVectorStorage(MEX_ACTION_ARGUMENTS) { nlopt_set_vector_storage(opt, (unsigned)mxGetScalar(prhs[0])); }
  void setInitialStep(MEX_ACTION_ARGUMENTS)
  {
    if (mxIsChar(prhs[0])) // 'auto'
      nlopt_set_initial_step(opt, NULL);
    else if (mxIsScalar(prhs[0]))
      nlopt_set_initial_step1(opt, mxGetScalar(prhs[0]));
    else
      nlopt_set_initial_step(opt, mxGetPr(prhs[0]));
  }

  // OPTIMIZATION ROUTINES
  void fminunc(MEX_ACTION_ARGUMENTS);
  void fmincon(MEX_ACTION_ARGUMENTS);
  void fminbnd(MEX_ACTION_ARGUMENTS);

  // common routine to run and gather the outcome
  static void config_obj_fun(nlopt_opt opt, mexObjectiveFunction &data);
  static void run_n_report(int nlhs, mxArray *plhs[], nlopt_opt opt, mexObjectiveFunction &data);

  // routine to set subproblem options
  static void set_local_optimizer(nlopt_opt opt, const mxArray *mxObj);

  static void set_bounds(nlopt_opt opt, const mxArray *mxLB, const mxArray *mxUB);

  // map of actions
  typedef void (mexNLopt::*action_fcn)(MEX_ACTION_ARGUMENTS);
  static const std::unordered_map<std::string, action_fcn> action_map;

  typedef void (mexNLopt::*const_action_fcn)(MEX_ACTION_ARGUMENTS) const;
  static const std::unordered_map<std::string, const_action_fcn> const_action_map;
};


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#ifndef __BSSCR_MG_h__
#define __BSSCR_MG_h__

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>
#include <petscksp.h>
#include <petscpc.h>
#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>
#include <StgFEM/StgFEM.h>
#include <PICellerator/PICellerator.h>
#include <Underworld/Underworld.h>
#include "Solvers/KSPSolvers/KSPSolvers.h"

#include "common-driver-utils.h"
#include "BSSCR.h" /* includes StokesBlockKSPInterface.h */

#define KSPBSSCR         "bsscr"

typedef struct {
  KSP ksp;
  PC pc;

  PetscTruth useAcceleratingSmoothingMG;
  PetscTruth acceleratingSmoothingMGView;

  /* mg_accelerating_smoothing options */

  PetscInt smoothsMax;
  PetscInt smoothsToStartWith;
  PetscInt currentNumberOfSmooths;
  PetscInt smoothingIncrement;
  PetscInt targetCyclesForTenfoldReduction;

  PetscInt smoothingCountThisSolve;
  PetscInt totalSmoothingCount;
  PetscInt totalMgCycleCount;
} MGContext;

PetscErrorCode KSPCycleEffectivenessMonitorAndAdjust(KSP ksp, PetscInt n, PetscReal rnorm, void *_mgctx );
//PetscErrorCode MG_inner_solver_mgContext_initialise(MGContext *mgCtx);
PetscErrorCode MG_inner_solver_pcmg_shutdown( PC pc_MG );
//PetscErrorCode BSSCR_mgPCApply( void *ctx, Vec x, Vec y );
//PetscErrorCode BSSCR_mgPCAccelerating( void *ctx, Vec x, Vec y );
double setupMG( KSP_BSSCR * bsscrp_self, KSP ksp_inner, PC pc_MG, Mat K, MGContext *mgCtx );
#endif


/*! @copyright (c) 2017 King Abdullah University of Science and
 *                      Technology (KAUST). All rights reserved.
 *
 * STARS-H is a software package, provided by King Abdullah
 *             University of Science and Technology (KAUST)
 *
 * @file include/common.h
 *
 * @cond
 * This command in pair with endcond will prevent file from being documented.
 *
 * @version 0.3.0
 * @author Aleksandr Mikhalev
 * @date 2020-06-09
 * */

#ifndef __COMMON_H__
#define __COMMON_H__

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <complex.h>
#include <limits.h>
#include <stdint.h>

#ifdef MKL
    #include <mkl.h>
#else
    #include <cblas.h>
    #include <lapacke.h>
#endif

#ifdef OPENMP
    #include <omp.h>
#endif

#ifdef MPI
    #include <mpi.h>
    #ifndef SIZE_MAX
        #error "SIZE_MAX not defined"
    #endif
    #if SIZE_MAX == UCHAR_MAX
       #define my_MPI_SIZE_T MPI_UNSIGNED_CHAR
    #elif SIZE_MAX == USHRT_MAX
       #define my_MPI_SIZE_T MPI_UNSIGNED_SHORT
    #elif SIZE_MAX == UINT_MAX
       #define my_MPI_SIZE_T MPI_UNSIGNED
    #elif SIZE_MAX == ULONG_MAX
       #define my_MPI_SIZE_T MPI_UNSIGNED_LONG
    #elif SIZE_MAX == ULLONG_MAX
       #define my_MPI_SIZE_T MPI_UNSIGNED_LONG_LONG
    #else
       #error "No MPI data type fits size_t"
    #endif
#endif

#ifdef STARPU
    #include <starpu.h>
#endif

#ifdef GSL
    #include <gsl/gsl_sf_bessel.h>
    #include <gsl/gsl_sf_gamma.h>
#endif

#define STARSH_ERROR(format, ...)\
{\
    fprintf(stderr, "STARSH ERROR: %s(): ", __func__);\
    fprintf(stderr, format, ##__VA_ARGS__);\
    fprintf(stderr, "\n");\
}

#ifdef SHOW_WARNINGS
    #define STARSH_WARNING(format, ...)\
    {\
        fprintf(stderr, "STARSH WARNING: %s(): ", __func__);\
        fprintf(stderr, format, ##__VA_ARGS__);\
        fprintf(stderr, "\n");\
    }
#else
    #define STARSH_WARNING(...)
#endif

#define STARSH_MALLOC(var, expr_nitems)\
{\
    var = malloc(sizeof(*var)*(expr_nitems));\
    if(!var)\
    {\
        STARSH_ERROR("line %d: malloc() failed", __LINE__);\
        return STARSH_MALLOC_ERROR;\
    }\
}

#define STARSH_REALLOC(var, expr_nitems)\
{\
    var = realloc(var, sizeof(*var)*(expr_nitems));\
    if(!var)\
    {\
        STARSH_ERROR("malloc() failed");\
        return STARSH_MALLOC_ERROR;\
    }\
}

#define STARSH_PMALLOC(var, expr_nitems, var_info)\
{\
    var = malloc(sizeof(*var)*(expr_nitems));\
    if(!var)\
    {\
        STARSH_ERROR("malloc() failed");\
        var_info = STARSH_MALLOC_ERROR;\
    }\
}

#define STARSH_PREALLOC(var, expr_nitems, var_info)\
{\
    var = realloc(var, sizeof(*var)*(expr_nitems));\
    if(!var)\
    {\
        STARSH_ERROR("malloc() failed");\
        var_info = STARSH_MALLOC_ERROR;\
    }\
}

int cmp_size_t(const void *a, const void *b);

#endif // __COMMON_H__

//! @endcond


#ifndef CONSTANTS_H
#define CONSTANTS_H

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <limits.h>
#include <math.h>
#include <time.h>
#include <float.h>
#include <utils/version.h>
#include <utils/help.h>
#include <utils/error.h>
#include <gsl/gsl_statistics_double.h>

/*
 * MIN macro
 */
#define MIN(a,b) (((a)<(b))?(a):(b))

/*
 * MAX macro
 */
#define MAX(a,b) (((a)>(b))?(a):(b))

/*
 * STR macro
 */
#define STR(a) ((a > 0)?("-"):("+"))

/*
 * Default value for read_minlen parameter
 */
#define MIN_READ_LEN 0

/*
 * Default value for minlen parameter
 */
#define MIN_LEN 15

/*
 * Default value for maxlen parameter
 */
#define MAX_LEN 30

/*
 * Default value for spacing parameter
 */
#define SPACING 20

/*
 * Default value for minheight parameter
 */
#define MIN_READS 10.0f

/*
 * Default value for trimming threshold parameter
 */
#define TRIM_THRESHOLD 0.05

/*
 * Default value for minimum trimming parameter
 */
#define TRIM_MIN 2

/*
 * Default value for maximum trimming parameter
 */
#define TRIM_MAX 20

/*
 * Default value for IDR cutoff
 */
#define CUTOFF 2.0f

/*
 * Default value for Replicate number
 */
#define REPLICATE_NUMBER 1

/*
 * Replicate treatment options
 */
#define REPLICATE_POOL_STR "pool" // default value
#define REPLICATE_MEAN_STR "mean"
#define REPLICATE_REPLICATE_STR "replicate"
#define REPLICATE_POOL 0
#define REPLICATE_MEAN 1
#define REPLICATE_REPLICATE 2

/*
 * IDR method options
 */
#define IDR_COMMON_STR "common" // default value
#define IDR_NONE_STR "none"
#define IDR_SERE_STR "sere"
#define IDR_IDR_STR "idr"
#define IDR_COMMON 0
#define IDR_NONE 1
#define IDR_SERE 2
#define IDR_IDR 3

/*
 * Maximum length of a contig
 */
#define MAX_CONTIG_LENGTH 200

/*
 * Maximum number of contigs
 */
#define MAX_CONTIGS 10000000

/*
 * Maximum size, in chars, of an error message
 */
#define MAX_ERR_MSG 100

/*
 * Maximum size, in chars, of a given path
 */
#define MAX_PATH 500

/*
 * Maximum size, in chars, of the 4th field (name) in the query BED file
 */
#define MAX_FEATURE 50

/*
 * Maximum number of replicates
 */
#define MAX_REPLICATES 10

/*
 * Maximum number of alignments per heap
 */
#define MAX_ALIGN_HEAP 50000000

/*
 * Maximum read length
 */
#define MAX_READ_LENGTH 200

/*
 * Maximum profile length
 */
#define MAX_PROFILE_LENGTH 500

/*
 * Maximum number of block base pairs
 */
#define MAX_BLOCK 3000

/*
 * Alignment strand
 */
#define FWD_STRAND 0 // forward/watson
#define REV_STRAND 1 // reverse/crick

/*
 * Alignment validity
 */
#define VALID_ALIGNMENT 1
#define INVALID_ALIGNMENT 0

/*
 * Constants for npIDR method
 */
#define ABSOLUTE 0
#define CONDITIONAL 1

/*
 * Path separator
 */
#ifdef __unix__
  #define PATH_SEPARATOR "/"
#else
  #define PATH_SEPARATOR "\\"
#endif

/*
 * Output file suffixes
 */
#define PROFILES_SUFFIX "profiles.dat"
#define CONTIGS_SUFFIX "contigs.dat"
#define CROSSCOR_SUFFIX "crosscor.dat"
#define CLUSTERS_SUFFIX "clusters.neWick"
#define ANNOTATION_O_SUFFIX "annotation.bed"
#define TMPROFILES_SUFFIX "tmprofiles.dat"

/*
 * Maximum N limit for gaussian white noise generation
 */
#define MAX_GNOISE_N 20

/*
 * Cluster cutoff default value
 */
#define CLUSTER_CUTOFF -1.0f

/*
 * Condition for existence of annotation file
 */
#define ANNOTATION_CONDITION 0

/*
 * Condition for existence of additional profiles file
 */
#define ADDITIONAL_P_CONDITION 0

/*
 * Default value for the feature to profile overlap percentage
 */
#define OVERLAP_FTOP 0.9

/*
 * Default value for the profile to feature overlap percentage
 */
#define OVERLAP_PTOF 0.5

/*
 * Maximum number of annotation files
 */
#define MAX_ANNOTATIONS 10

/*
 * Condition for existence of correlations file
 */
#define CORRELATIONS_CONDITION 0

/*
 * Constants for profile category
 */
#define NOVEL 0
#define KNOWN 1

/*
 * Differential processing default p-value
 */
#define P_VALUE 0.01

/*
 * Differential processing default overlap
 */
#define DP_FOLD_CHANGE 7

/*
 * Suffix for differentially processed profiles
 */
#define DIFFPROC_PROFILE_O_SUFFIX "diffprofiles.dat"
/*
 * Suffix for differentially processed clusters
 */
#define DIFFPROC_CLUSTER_O_SUFFIX "diffclusters.dat"
#endif


#include <pygsl/solver.h>
#include <pygsl/rng.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>

const char  * filename = __FILE__;
PyObject *module = NULL;
static const char monte_f_type_name[] = "F-monte";
static const char module_doc[] = "XXX Missing";
static const char PyGSL_monte_type[] = "monte";

enum PyGSL_gsl_monte_type{
     PyGSL_MONTE_plain = 1,
     PyGSL_MONTE_miser = 2,
     PyGSL_MONTE_vegas = 3
};

struct _monte_csys{
     enum PyGSL_gsl_monte_type type;
};

typedef struct _monte_csys monte_csys;

static double
PyGSL_g_test(double *k, size_t dim, void *params)
{
     double A = 1.0 / (M_PI * M_PI * M_PI);
     return A / (1.0 - cos (k[0]) * cos (k[1]) * cos (k[2]));
}

static double
PyGSL_monte_function_wrap(double *x,  size_t dim, void *params)
{
     double tmp, *dresult;
     int dimension;
     PyGSL_solver *s;
     PyArrayObject *a_array = NULL;
     PyObject *result = NULL, *arglist=NULL, *callback, *retval;
     PyGSL_error_info  info;
     /* the line number to appear in the traceback */
     int trb_lineno = -1, i;
     struct pygsl_array_cache * cache_ptr;
     gsl_vector_view view;

     FUNC_MESS_BEGIN();
     s = (PyGSL_solver  *) params;

     /*
     flag = PyGSL_function_wrap_On_O(&view.vector, s->cbs[0], s->args,
                                     &tmp, NULL, view.vector.size,
                                     "monte_wrap");
     */

     FUNC_MESS_BEGIN();
     callback = s->cbs[0];
     assert(s->args != NULL);
     assert(callback != NULL);

     /* Do I need to copy the array ??? */
     for(i = 2; i < PyGSL_SOLVER_N_ARRAYS; ++i){
	  cache_ptr = &(s->cache[i]);
	  if(cache_ptr->ref == NULL)
	       /* no array found */
	       break;
	  if(x == cache_ptr->data){
	       a_array = cache_ptr->ref;
	       break;
	  }
     }
     if(i >= PyGSL_SOLVER_N_ARRAYS){
	  trb_lineno = __LINE__ -1;
	  pygsl_error("Not enough space to cache arrays...", filename, trb_lineno, GSL_ESANITY);
	  goto fail;
     }
     if(a_array == NULL){
	  dimension = dim;
	  a_array = (PyArrayObject *) PyArray_FromDimsAndData(1, &dimension, PyArray_DOUBLE, (char *)x);
	  cache_ptr->ref = a_array;
	  cache_ptr->data = x;
	  /*
	   * Required for deallocation.
	   * Tuples steal the references. But this array will be dereferenced by the
	   * normal procedure!
	   */
	  Py_INCREF(a_array);
     }
     if (a_array == NULL){
	  trb_lineno = __LINE__ - 2;
	  goto fail;
     }
     /* arglist was already set up */
     result = (PyObject *) s->cache[1].ref;
     dresult = (double *) s->cache[1].data;
     arglist = (PyObject *) s->cache[0].ref;

     Py_INCREF(s->args);
     PyTuple_SET_ITEM(arglist, 0, (PyObject *) a_array);
     PyTuple_SET_ITEM(arglist, 1, s->args);

     /*
     arglist = Py_BuildValue("(OOO)", a_array, s->args, result);
     if(DEBUG > 2){
	  fprintf(stderr, "callback = %p, arglist = %p\n", callback, arglist);
     }
     */
     FUNC_MESS("    Call Python Object BEGIN");
     retval = PyEval_CallObject(callback, arglist);
     FUNC_MESS("    Call Python Object END");

     /*
     info.callback = callback;
     info.message  = __FUNCTION__;
     if(PyGSL_CHECK_PYTHON_RETURN(retval, 0, &info) != GSL_SUCCESS){
	  trb_lineno = __LINE__ - 1;
	  goto fail;
     }
     info.argnum = 1;
     DEBUG_MESS(3, "result was %e", *dresult);
     */
     FUNC_MESS_END();
     return *dresult;

 fail:
     PyGSL_add_traceback(NULL, __FILE__, __FUNCTION__, trb_lineno);
     FUNC_MESS("Failure");

     if(s->isset == 1){
	  longjmp(s->buffer, GSL_EFAILED);
	  FUNC_MESS("\t\t Using jump buffer");
     } else {
	  FUNC_MESS("\t\t Jump buffer was not defined!");
     }
     tmp = gsl_nan();
     return tmp;
}

static PyObject *
PyGSL_monte_init(PyGSL_solver *self, PyObject *args)
{
     int flag = GSL_EFAILED;
     monte_csys * csys;

     FUNC_MESS_BEGIN();
     assert(PyGSL_solver_check(self));
     csys = (monte_csys *)self->c_sys;
     assert(csys);
     switch(csys->type){
     case PyGSL_MONTE_plain:
	  flag = gsl_monte_plain_init(self->solver);
	  break;
     case PyGSL_MONTE_miser:
	  flag = gsl_monte_miser_init(self->solver);
	  break;
     case PyGSL_MONTE_vegas:
	  flag = gsl_monte_vegas_init(self->solver);
	  break;
     default:
	  DEBUG_MESS(2, "Monte type %d unknown",flag);
	  PyGSL_ERROR_NULL("Unknown monte type!", GSL_ESANITY);
     }

     if(PyGSL_ERROR_FLAG(flag) != GSL_SUCCESS){
	  PyGSL_add_traceback(module, filename, __FUNCTION__, __LINE__);
	  return NULL;
     }
     Py_INCREF(Py_None);
     FUNC_MESS_END();
     return Py_None;
}

typedef int (*pygsl_monte_fptr_t)(gsl_monte_function * F, double XL[], double XU[], size_t DIM,
		     size_t CALLS, gsl_rng * R, gsl_monte_plain_state * S,
		     double * RESULT, double * ABSERR);

static PyObject *
PyGSL_monte_integrate(PyGSL_solver *self, PyObject *args)
{

     int flag = GSL_EFAILED, line, dim;
     unsigned int ncalls;
     monte_csys * csys;
     gsl_rng * rng;
     PyObject  *func=NULL, *xlo=NULL, *xuo=NULL, *rng_obj=NULL, *ncallso=NULL, *argso=NULL;
     PyArrayObject *xla=NULL;
     PyArrayObject *xua=NULL;
     double result, abserr;
     gsl_monte_function mfunc;
     pygsl_monte_fptr_t fptr;

     FUNC_MESS_BEGIN();
     assert(PyGSL_solver_check(self));

     if(!PyArg_ParseTuple(args, "OOOOO|O", &func, &xlo, &xuo, &ncallso, &rng_obj, &argso)){
	  line = __LINE__ - 1;
	  goto fail;
     }

     if(!PyCallable_Check(func)){
	  line = __LINE__ - 1;
	  pygsl_error("The function argument must be callable!", filename, line, GSL_EINVAL);
	  goto fail;
     }

     if(PyGSL_PYLONG_TO_UINT(ncallso, &ncalls, NULL) != GSL_SUCCESS){
	  line = __LINE__ - 1;
	  goto fail;
     }

     if(!PyGSL_RNG_Check(rng_obj)){
	  line = __LINE__ - 1;
	  pygsl_error("The rng object must be a rng!", filename, line, GSL_EINVAL);
	  goto fail;
     }

     rng = ((PyGSL_rng *) rng_obj)->rng;

     xla = PyGSL_vector_check(xlo, -1, PyGSL_DARRAY_CINPUT(2), NULL, NULL);
     if(xla == NULL){
	  line = __LINE__ - 2;
	  goto fail;
     }

     dim = xla->dimensions[0];
     xua = PyGSL_vector_check(xuo, dim, PyGSL_DARRAY_CINPUT(3), NULL, NULL);
     if(xua == NULL){
	  line = __LINE__ - 2;
	  goto fail;
     }

     if(argso == NULL){
	  Py_INCREF(Py_None);
	  argso = Py_None;
     }
     Py_XDECREF(self->cbs[0]);
     Py_INCREF(func);
     self->cbs[0] = func;
     self->args = argso;
     csys = (monte_csys *)self->c_sys;

     switch(csys->type){
     case PyGSL_MONTE_plain:	  fptr = (pygsl_monte_fptr_t)&gsl_monte_plain_integrate; break;
     case PyGSL_MONTE_miser:	  fptr = (pygsl_monte_fptr_t)&gsl_monte_miser_integrate; break;
     case PyGSL_MONTE_vegas:	  fptr = (pygsl_monte_fptr_t)&gsl_monte_vegas_integrate; break;
     default:
	  line = __LINE__ - 5;
	  DEBUG_MESS(2, "Monte type %d unknown",flag);
	  pygsl_error("Unknown monte type!", filename, line, GSL_ESANITY);
	  goto fail;
     }

     assert(fptr);
     mfunc.f = &PyGSL_monte_function_wrap;
     mfunc.f = &PyGSL_g_test;
     mfunc.dim = dim;
     mfunc.params = self;

     if(setjmp(self->buffer) != 0){
	  self->isset = 0;
	  line = __LINE__ - 2;
	  goto fail;
     }else{
	  self->isset = 1;
     }
     flag = fptr(&mfunc, (double *) xla->data, (double *)xua->data,
		 dim, ncalls, rng, self->solver, &result, &abserr);
     self->isset = 0;
     if(PyGSL_ERROR_FLAG(flag) != GSL_SUCCESS){
	  line = __LINE__ - 2;
	  return NULL;
     }

     Py_DECREF(xla);     xla = NULL;
     Py_DECREF(xua);     xua = NULL;
     Py_DECREF(self->cbs[0]);
     Py_DECREF(self->args);
     self->cbs[0] = NULL;
     self->args = NULL;
     FUNC_MESS_END();

     return Py_BuildValue("dd", result, abserr);

 fail:
     FUNC_MESS("FAIL");
     Py_XDECREF(xla);
     Py_XDECREF(xua);
     Py_XDECREF(self->cbs[0]);
     Py_XDECREF(self->args);
     self->cbs[0] = NULL;
     self->args = NULL;
     PyGSL_add_traceback(module, filename, __FUNCTION__, line);
     return NULL;
}

#define GET_SET(montetype, name, mode) \
static PyObject * GetSet_ ## montetype ## _ ## name(PyGSL_solver *self, PyObject *args) \
{ \
     gsl_monte_## montetype ## _state *state = self->solver; \
     return PyGSL_solver_GetSet((PyObject *)self, args, &(state->name), mode); \
}

#define GET_SET_DOUBLE(mintype, name) GET_SET(mintype, name, PyGSL_MODE_DOUBLE)
#define GET_SET_SIZE_T(mintype, name) GET_SET(mintype, name, PyGSL_MODE_SIZE_T)
#define GET_SET_INT(mintype, name)    GET_SET(mintype, name, PyGSL_MODE_INT)

GET_SET_DOUBLE(miser, estimate_frac)
GET_SET_SIZE_T(miser, min_calls)
GET_SET_SIZE_T(miser, min_calls_per_bisection)
GET_SET_DOUBLE(miser, alpha)
GET_SET_DOUBLE(miser, dither)

GET_SET_DOUBLE(vegas, result)
GET_SET_DOUBLE(vegas, sigma)
GET_SET_DOUBLE(vegas, chisq)
GET_SET_DOUBLE(vegas, alpha)
GET_SET_SIZE_T(vegas, iterations)
GET_SET_INT(vegas, stage)
GET_SET_INT(vegas, mode)
GET_SET_INT(vegas, verbose)

#undef GET_SET_DOUBLE
#undef GET_SET_SIZE_T
#undef GET_SET_INT
#undef GET_SET
#define GETSET(montetype, name, mode) {#name, (PyCFunction) GetSet_ ## montetype ## _ ## name, METH_VARARGS, NULL},
#define GET_SET_DOUBLE(montetype, name) GETSET(montetype, name, 0)
#define GET_SET_INT(montetype, name) GETSET(montetype, name, 0)
#define GET_SET_SIZE_T(montetype, name) GETSET(montetype, name, 0)

#define MONTE_STANDARD_METHODS \
     {"init",      (PyCFunction)PyGSL_monte_init,      METH_VARARGS, NULL}, \
     {"integrate", (PyCFunction)PyGSL_monte_integrate, METH_VARARGS, NULL}, \

static PyMethodDef PyGSL_monte_plain_methods[] = {
     MONTE_STANDARD_METHODS
     {NULL, NULL, 0, NULL}
};

static PyMethodDef PyGSL_monte_miser_methods[] = {
     MONTE_STANDARD_METHODS
     GET_SET_DOUBLE(miser, estimate_frac)
     GET_SET_SIZE_T(miser, min_calls)
     GET_SET_SIZE_T(miser, min_calls_per_bisection)
     GET_SET_DOUBLE(miser, alpha)
     GET_SET_DOUBLE(miser, dither)
     {NULL, NULL, 0, NULL}
};

static PyMethodDef PyGSL_monte_vegas_methods[] = {
     MONTE_STANDARD_METHODS
     GET_SET_DOUBLE(vegas, result)
     GET_SET_DOUBLE(vegas, sigma)
     GET_SET_DOUBLE(vegas, chisq)
     GET_SET_DOUBLE(vegas, alpha)
     GET_SET_SIZE_T(vegas, iterations)
     GET_SET_INT(vegas, stage)
     GET_SET_INT(vegas, mode)
     GET_SET_INT(vegas, verbose)
     {NULL, NULL, 0, NULL}
};

/* make the init look similar to the one */
static void *
PyGSL_gsl_monte_alloc(void * type, int n)
{
     enum PyGSL_gsl_monte_type flag = (enum PyGSL_gsl_monte_type) type;
     void * result = NULL;

     FUNC_MESS_BEGIN();
     switch(flag){
     case PyGSL_MONTE_plain:
	  result = gsl_monte_plain_alloc(n);
	  break;
     case PyGSL_MONTE_miser:
	  result = gsl_monte_miser_alloc(n);
	  break;
     case PyGSL_MONTE_vegas:
	  result = gsl_monte_vegas_alloc(n);
	  break;
     default:
	  DEBUG_MESS(2, "Monte type %d unknown",flag);
	  PyGSL_ERROR_NULL("Unknown monte type!", GSL_ESANITY);
     }
     FUNC_MESS_END();
     return result;
}

const struct _SolverStatic
monte_plain_solver_f   = {{(void_m_t) gsl_monte_plain_free,
			  (void_m_t) NULL,
			  NULL,
			  NULL},
			  1, PyGSL_monte_plain_methods,  PyGSL_monte_type},
monte_miser_solver_f   = {{(void_m_t) gsl_monte_miser_free,
			  (void_m_t) NULL,
			  NULL,
			   NULL},
			  1, PyGSL_monte_miser_methods,  PyGSL_monte_type},
monte_vegas_solver_f   = {{(void_m_t) gsl_monte_vegas_free,
			  (void_m_t) NULL,
			  NULL,
			  NULL},
			  1, PyGSL_monte_vegas_methods,  PyGSL_monte_type};

static PyObject*
PyGSL_init_monte(PyObject *self, PyObject *args, int type, const struct _SolverStatic *sost)
{
     PyGSL_solver *result = NULL;
     struct pygsl_array_cache *tmp;
     int line = -1, dims;
     monte_csys * csys;
     PyObject *mytuple;

     FUNC_MESS_BEGIN();
     const solver_alloc_struct alloc = {(const void *) type, (void *) PyGSL_gsl_monte_alloc, sost};
     result = (PyGSL_solver *) PyGSL_solver_dn_init(self, args, &alloc, 1);
     if(result == NULL){
	  line = __LINE__ - 2;
	  goto fail;
     }

     csys = (monte_csys *) calloc(1, sizeof(monte_csys));
     if(csys == NULL){
	  PyErr_NoMemory();
	  line = __LINE__ - 1;
	  goto fail;
     }

     dims = 1;
     tmp = result->cache;

     tmp[1].ref = PyGSL_New_Array(1, &dims, PyArray_DOUBLE);
     tmp[1].data = (double *) tmp[1].ref->data;
     mytuple =  PyTuple_New(3);
     PyTuple_SetItem(mytuple, 2, (PyObject *) tmp[1].ref);
     /*
      * Required for deallocation.
      * Tuples steal the references. But this array will be dereferenced by the
      * normal procedure!
      */
     Py_INCREF(tmp[1].ref);
     Py_INCREF(tmp[1].ref);

     /*
      * Initalise the others
      */
     Py_INCREF(Py_None);
     Py_INCREF(Py_None);
     Py_INCREF(Py_None);
     Py_INCREF(Py_None);
     PyTuple_SetItem(mytuple, 0, Py_None);
     PyTuple_SetItem(mytuple, 1, Py_None);

     tmp[0].ref = (PyArrayObject*) mytuple;

     result->cache = tmp;
     csys->type = type;
     result->c_sys = csys;

     FUNC_MESS_END();
     return (PyObject *) result;

 fail:
     FUNC_MESS("Fail");
     Py_XDECREF(result);
     PyGSL_add_traceback(module, filename, __FUNCTION__, __LINE__);
     return NULL;
}

#define MONTE_INIT(name) \
static PyObject* PyGSL_init_ ## name(PyObject * self, PyObject *args) \
{ return PyGSL_init_monte(self, args, PyGSL_MONTE_ ##name, &monte_ ## name ## _solver_f); }

MONTE_INIT(plain)
MONTE_INIT(miser)
MONTE_INIT(vegas)

static PyMethodDef mMethods[] = {
     {"plain", PyGSL_init_plain, METH_VARARGS, NULL},
     {"miser", PyGSL_init_miser, METH_VARARGS, NULL},
     {"vegas", PyGSL_init_vegas, METH_VARARGS, NULL},
     {NULL, NULL, 0, NULL}
};

void
initmonte(void)
{
     PyObject* m, *dict, *item;
     FUNC_MESS_BEGIN();

     m=Py_InitModule("monte", mMethods);
     module = m;
     assert(m);
     dict = PyModule_GetDict(m);
     if(!dict)
	  goto fail;

     init_pygsl()
     import_pygsl_solver();
     assert(PyGSL_API);

     if (!(item = PyString_FromString((char*)module_doc))){
	  PyErr_SetString(PyExc_ImportError,
			  "I could not generate module doc string!");
	  goto fail;
     }
     if (PyDict_SetItemString(dict, "__doc__", item) != 0){
	  PyErr_SetString(PyExc_ImportError,
			  "I could not init doc string!");
	  goto fail;
     }
     PyModule_AddIntConstant(m, "GSL_VEGAS_MODE_IMPORTANCE",      GSL_VEGAS_MODE_IMPORTANCE);
     PyModule_AddIntConstant(m, "GSL_VEGAS_MODE_IMPORTANCE_ONLY", GSL_VEGAS_MODE_IMPORTANCE_ONLY);
     PyModule_AddIntConstant(m, "GSL_VEGAS_MODE_STRATIFIED",      GSL_VEGAS_MODE_STRATIFIED);

     FUNC_MESS_END();
     return;

 fail:
     FUNC_MESS("FAIL");
     return;

}