/*
   Copyright [2019, 2020] [IBM Corporation]
   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at
       http://www.apache.org/licenses/LICENSE-2.0
   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#ifndef __CCPM_AREATOP_ALLOCATOR_H__
#define __CCPM_AREATOP_ALLOCATOR_H__

#include "atomic_word.h"
#include "list_item.h"
#include <ccpm/interfaces.h>
#include <common/byte_span.h>
#include <gsl/pointers>
#include <array>
#include <cstddef>
#include <ios> // ios_base::fmtflags, ostream
#include <vector>

namespace ccpm
{
	struct level_hints
	{
	private:
		/* Each level has a list for every possible number of contiguous free elements.
		 * of contiguous free elements.
		 * Since "contiguous" free elements do not span a word, that is one list
		 * for every possible contiguous size in a word.
		 * The list at element n locates an area_ctl with has maximal runs of exactly
		 * n+1 free elements,
		 */

		using free_ctls_t = std::array<list_item, alloc_states_per_word>;
		free_ctls_t _free_ctls;
		free_ctls_t::size_type find_free_ctl_ix(unsigned min_run_length) const;
		free_ctls_t::size_type tier_ix_from_run_length(unsigned run_length) const;
	public:
		unsigned _ct_alloc_probe_success;
		unsigned _ct_alloc_probe_failure;
		unsigned _ct_subdivision;

	public:
		static constexpr auto size() { return alloc_states_per_word; }

		/* return smallest tier index which has an area with
		 *   run length >= min_run_length
		 * If no such tier, return sub_states_per_word.
		 */
		auto find_free_ctl(unsigned min_run_length_) const -> const list_item *
		{
			return & _free_ctls[find_free_ctl_ix(min_run_length_)];
		}

		auto find_free_ctl(unsigned min_run_length_) -> list_item *
		{
			return & _free_ctls[find_free_ctl_ix(min_run_length_)];
		}

		const auto *tier_from_run_length(unsigned run_length_) const
		{
			return & _free_ctls[tier_ix_from_run_length(run_length_)];
		}

		auto *tier_from_run_length(unsigned run_length_)
		{
			return & _free_ctls[tier_ix_from_run_length(run_length_)];
		}

		const auto *tier_end() const
		{
			return &_free_ctls[alloc_states_per_word];
		}

		/* Find the longest run length less than mac_run_length.
		 * Used for tracing, when find_free_ctl_ix has failed to
		 * find a long enough run
		 */
		free_ctls_t::size_type find_mac_ctl_ix(unsigned mac_run_length) const;

		level_hints()
			: _free_ctls()
			, _ct_alloc_probe_success(0)
			, _ct_alloc_probe_failure(0)
			, _ct_subdivision(0)
		{}

		using level_ix_t = std::uint8_t;
		void print(
			std::ostream &o_
			, level_ix_t level_
			, std::ios_base::fmtflags // size_format_
		) const;
	};

	struct area_ctl;

	/*
	 * Location of non-persisted items for a single "region" managed by the crash-consistent allocator.
	 * Persisted items are keps in an area_ctl, which is in persistent memory.
	 */
	struct area_top
	{
	private:
		using level_ix_t = std::uint8_t;
		using byte_span = common::byte_span;
		using persist_type = gsl::not_null<ccpm::persister *>;
		area_ctl *_ctl;
		std::size_t _bytes_free;
		bool _all_restored;
		unsigned _trace_level;
		std::ostream *_o;
		using level_hints_vec = std::vector<level_hints>;
		level_hints_vec _level;
		unsigned _ct_allocation;
		byte_span _region; /* for get_region only */

		area_top(
			// persist_type persist
			area_ctl *ctl
			, unsigned trace_level
			, byte_span iov
			, std::ostream &o
		);
		area_top(const area_top &) = delete;
		area_top &operator=(const area_top &) = delete;

		void allocate_strategy_1(
			persist_type persist_
			, void * & ptr_
			, std::size_t bytes
			, std::size_t alignment
			, level_hints_vec::iterator level_
			, unsigned run_length
		);

		bool allocate_recovery_1();
		bool allocate_recovery_2(persist_type persist, level_hints_vec::iterator level);
		bool trace_coarse() const { return 0 < _trace_level; }
		bool trace_fine() const { return 1 < _trace_level; }

	public:
		/* Initial area_ctl */
		explicit area_top(
			persist_type persist
			, byte_span iov
			, unsigned trace_level
			, std::ostream &o
		);
		/* Restored area_ctl */
		explicit area_top(
			persist_type persist
			, byte_span iov
			, const ownership_callback_t &resolver
			, unsigned trace_level
			, std::ostream &o
		);
		~area_top();

		bool includes(const void *addr) const;

		/* Free byte count. Required by users */
		std::size_t bytes_free() const;

		byte_span get_region() const { return _region; }

		void allocate(
			persist_type persist
			, void * & ptr, std::size_t bytes
			, std::size_t alignment
		);

		void deallocate(
			persist_type persist
			, void * & ptr, std::size_t bytes
		);

		void print(
			std::ostream &o
			, std::ios_base::fmtflags size_format
		) const;

		void print_ctls(
			std::ostream *o_
			, std::ios_base::fmtflags format_
		) const;

    level_ix_t height() const { return level_ix_t(_level.size()); }

    void set_root(const byte_span & iov, persist_type persist);
    byte_span get_root() const;

		/*
		 * called by area_ctl to add area_ctl a, at level level_ix, with a longest
		 * free run (consecutive free elements) of free_run, to _level, which is the
		 * non-persistent catalog of area_ctl items.
		 */
		void remove_from_chain(area_ctl *a, level_ix_t level_ix, unsigned longest_run);
		void restore_to_chain(area_ctl *a, level_ix_t level_ix, unsigned run_length);

		bool contains(const void *p) const;

		bool is_in_chain(
			const area_ctl *a
			, level_ix_t level_ix
			, unsigned run_length
		) const;
	};
}

#endif


#pragma once

#include <type_traits>

#include <cuda/runtime_api.hpp>
#include <gsl-lite/gsl-lite.hpp>

namespace thrustshift {

namespace kernel {

template <typename SrcT, typename MapT, typename DstT>
__global__ void scatter(gsl_lite::span<const SrcT> src,
                        gsl_lite::span<const MapT> map,
                        gsl_lite::span<DstT> dst) {

	const auto gtid = threadIdx.x + blockIdx.x * blockDim.x;
	if (gtid < src.size()) {
		dst[map[gtid]] = src[gtid];
	}
}

} // namespace kernel

namespace async {

template <class SrcRange, class MapRange, class DstRange>
void scatter(cuda::stream_t& stream,
             SrcRange&& src,
             MapRange&& map,
             DstRange&& dst) {

	gsl_Expects(src.size() == dst.size());
	gsl_Expects(src.size() == map.size());
	gsl_Expects(src.data() != dst.data());

	if (src.empty()) {
		return;
	}

	using src_value_type =
	    typename std::remove_reference<SrcRange>::type::value_type;
	using map_index_type =
	    typename std::remove_reference<MapRange>::type::value_type;
	using dst_value_type =
	    typename std::remove_reference<DstRange>::type::value_type;

	constexpr cuda::grid::block_dimension_t block_dim = 128;
	const cuda::grid::dimension_t grid_dim =
	    (src.size() + block_dim - 1) / block_dim;
	auto c = cuda::make_launch_config(grid_dim, block_dim);
	auto k = kernel::scatter<src_value_type, map_index_type, dst_value_type>;
	cuda::enqueue_launch(k, stream, c, src, map, dst);
}

} // namespace async

} // namespace thrustshift


/* spprop.c
 *
 * Copyright (C) 2014 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 <gsl/gsl_spmatrix.h>
#include <gsl/gsl_errno.h>

/*
gsl_spmatrix_equal()
  Return 1 if a = b, 0 otherwise
*/

int
gsl_spmatrix_equal(const gsl_spmatrix *a, const gsl_spmatrix *b)
{
  const size_t M = a->size1;
  const size_t N = a->size2;

  if (b->size1 != M || b->size2 != N)
    {
      GSL_ERROR_VAL("matrices must have same dimensions", GSL_EBADLEN, 0);
    }
  else if (a->sptype != b->sptype)
    {
      GSL_ERROR_VAL("trying to compare different sparse matrix types", GSL_EINVAL, 0);
    }
  else
    {
      const size_t nz = a->nz;
      size_t n;

      if (nz != b->nz)
        return 0; /* different number of non-zero elements */

      if (GSL_SPMATRIX_ISTRIPLET(a))
        {
          /*
           * triplet formats could be out of order but identical, so use
           * gsl_spmatrix_get() on b for each aij
           */
          for (n = 0; n < nz; ++n)
            {
              double bij = gsl_spmatrix_get(b, a->i[n], a->p[n]);

              if (a->data[n] != bij)
                return 0;
            }
        }
      else if (GSL_SPMATRIX_ISCCS(a))
        {
          /*
           * for CCS, both matrices should have everything
           * in the same order
           */

          /* check row indices and data */
          for (n = 0; n < nz; ++n)
            {
              if ((a->i[n] != b->i[n]) || (a->data[n] != b->data[n]))
                return 0;
            }

          /* check column pointers */
          for (n = 0; n < a->size2 + 1; ++n)
            {
              if (a->p[n] != b->p[n])
                return 0;
            }
        }
      else if (GSL_SPMATRIX_ISCRS(a))
        {
          /*
           * for CRS, both matrices should have everything
           * in the same order
           */

          /* check column indices and data */
          for (n = 0; n < nz; ++n)
            {
              if ((a->i[n] != b->i[n]) || (a->data[n] != b->data[n]))
                return 0;
            }

          /* check row pointers */
          for (n = 0; n < a->size1 + 1; ++n)
            {
              if (a->p[n] != b->p[n])
                return 0;
            }
        }
      else
        {
          GSL_ERROR_VAL("unknown sparse matrix type", GSL_EINVAL, 0);
        }

      return 1;
    }
} /* gsl_spmatrix_equal() */


/* Function in common for mlfit1, mlfitN, etc ...
 */

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <stdint.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

#include "mlfit.h"
#include "blit3.h"

#ifndef verbose
#define verbose 0
#endif

double estimateNphot(double *V, size_t Vm)
  // V is a Vm x Vm matrix
  // The number of photons locally as sum(V-min(V))
{
  double s = 0;
  double minV = INFINITY;

  for(size_t kk=0; kk<Vm*Vm; kk++)
    s+=V[kk];

  for(size_t kk=0; kk<Vm*Vm; kk++)
    minV = GSL_MIN(V[kk], minV);

  s = s-minV*Vm*Vm;

  //printf("estimateNphot: %f\n", s);
  return s;
}

int double_cmp(const void * a, const void * b)
{
  if (*(const double*)a > *(const double*)b) {
    return 1;
  } else {
    if (*(const double*)a < *(const double*)b) {
      return -1;
    } else {
    return 0;
  }
}
}

double estimateBGV(double *V, size_t Vm, size_t Vn, size_t Vp, double * D)
  // estimate the background level in V [VmxVnxVp] around the point D
  // (x,y,z)
{

  int32_t radius = 7;
  double * ePixels = malloc(sizeof(double)*6*(2*radius+1)*(2*radius+1)); // a few more than we need

  int32_t x = nearbyint(D[0]);
  int32_t y = nearbyint(D[1]);
  int32_t z = nearbyint(D[2]);

  // Loop over the box, and copy the edge pixels to ePixels
  size_t nPixels = 0;
  for(int32_t zz = z-radius; zz<=z+radius; zz++)
    for(int32_t yy = y-radius; yy<=y+radius; yy++)
     for(int32_t xx = x-radius; xx<=x+radius; xx++)
       if((abs(zz-z)+abs(yy-y)+abs(xx-x)) == radius)
         if(xx>=0 && yy>=0 && zz>=0 && xx< (int32_t) Vm && yy<(int32_t) Vn && zz<(int32_t) Vp)
           ePixels[nPixels++] = V[xx + yy*Vm + zz*Vm*Vn];

#if verbose > 0
  printf("%lu ePixels\n", nPixels);
#endif

  double median = 0;
  if(nPixels>0)
  {
  qsort(ePixels, nPixels, sizeof(double),  double_cmp);
  gsl_sort(ePixels, 1, nPixels);
  median = gsl_stats_median_from_sorted_data(ePixels, 1, nPixels);
  //median = (double) nPixels;
#if verbose > 0
  printf("Median: %f\n", median);
#endif
  }
  else
  { median = 0;}

  free(ePixels);
  return(median);
  //return((double) q);
}

double estimateBG(double *V, size_t Vm)
  // Estimate the background level as the median of the edge pixels
{

  size_t nEdge = 4*Vm-4;
  double * ePixels = malloc(nEdge*sizeof(double));

  size_t pos = 0;
  for(uint32_t kk = 0; kk<Vm; kk++, pos++) // Left
    ePixels[pos] = V[kk];
  for(uint32_t kk = 0; kk<Vm; kk++, pos++) // Right
    ePixels[pos] = V[kk+(Vm-1)*Vm];
  for(uint32_t kk = 1; kk<Vm-1; kk++, pos++) // Top
    ePixels[pos] = V[Vm*kk];
  for(uint32_t kk = 1; kk<Vm-1; kk++, pos++) // Bottom
    ePixels[pos] = (double) V[Vm-1+Vm*kk];

  assert(pos == 4*Vm-4);

  if(0) {
    for(uint32_t kk = 0; kk<nEdge; kk++)
      printf("%2d :%f\n", kk, ePixels[kk]);
    printf("\n");
  }

//  qsort(ePixels, nEdge, sizeof(double),  double_cmp);

  if(0) {
    for(uint32_t kk = 0; kk<nEdge; kk++)
      printf("%2d %f\n", kk, ePixels[kk]);
    printf("\n");
  }

  gsl_sort(ePixels, 1, nEdge);
  double median = gsl_stats_median_from_sorted_data(ePixels, 1, nEdge);
  // double mean = gsl_stats_mean(ePixels, 1, nEdge);
#if verbose > 0
  printf("Median: %f\n", median);
#endif
  free(ePixels);
  return(median);
}

int getZLine(double *W, size_t Ws,
    double * V, size_t Vm, size_t Vn, size_t Vp,
    double *D)
  /* Copy a line from V into W.
   * The line will be D[x, y, z-hWs:z+hWs]
   * where hWs = (Ws-1)/2 and D = [x,y,z]
   *
   * returns 1 on failure (i.e. line is out of bounds)
   * returns 0 if ok.
   *
   * See also: getRegion
   */
{

  int64_t x = nearbyint(D[0]);
  int64_t y = nearbyint(D[1]);
  int64_t z = nearbyint(D[2]);

  size_t Ws2 = (Ws-1)/2;

  if(z+Ws2 >= Vp)
    return 1;
  if(z<(int64_t) Ws2)
    return 1;

  size_t pos = 0;
  for(size_t zz = z-Ws2; zz<=z+Ws2; zz++)
  {
    W[pos++] = V[x + y*Vm + zz*Vm*Vn];
  }
  return 0;
}

int getRegion(double * W, size_t Ws,
    double * V, size_t Vm, size_t Vn, size_t Vp,
    double * D)
  // Get a 2D region for constant z or returns 0 if out of bounds
  // Vm, Vn, Vp is the centre of the region.
{
  size_t Ws2 = (Ws-1)/2;
  size_t pos = 0;

  int64_t x = nearbyint(D[0]);
  int64_t y = nearbyint(D[1]);
  int64_t z = nearbyint(D[2]);

#if verbose > 0
  printf("getRegion round(D): (%lu %lu %lu)\n", x, y, z);
  printf("getRegion size(V)   (%lu %lu %lu)\n", Vm, Vn, Vp);
#endif

  if(x-(int64_t) Ws2 < 0)
    return 1;
  if(y-(int64_t) Ws2 < 0)
    return 1;
  if(x+(uint64_t) Ws2  >= Vm)
    return 1;
  if(y+(uint64_t) Ws2  >= Vn)
    return 1;
  if(z<0)
    return 1;
  if((size_t) z>=Vp)
    return 1;

#if verbose > 1
  printf("Region valid\n");
  printf("%lu %lu %lu\n", Vm, Vn, Vp);
#endif

  uint32_t zz = z;
  for(uint32_t yy = y-Ws2; yy<=y+Ws2; yy++) {
    for(uint32_t xx = x-Ws2; xx<=x+Ws2; xx++) {
      assert(xx<Vm); assert(yy<Vn); assert(zz<Vp);
    // printf("x %lu y %lu z %lu\n", x, y, z);

      W[pos++] = V[xx + yy*Vm + zz*Vm*Vn];
    }
  }
  //  showRegion(W, Ws);
 return 0;
}

void showRegion(double * W, size_t Ws)
  // Dump some quadratic region to the terminal
{
printf("Show region ...\n");
  for(size_t xx = 0 ; xx<Ws; xx++) {
    for(size_t yy = 0; yy<Ws; yy++) {
      printf("%f ", W[yy*Ws+xx]);
    }
    printf("\n");
  }
}


/*
 * Copyright 2016 Maikel Nadolski
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef HMM_ALGORITHM_H_
#define HMM_ALGORITHM_H_

#include <map>
#include <range/v3/core.hpp>
#include <range/v3/algorithm.hpp>
#include <range/v3/view/map.hpp>
#include <range/v3/view/unique.hpp>
#include <gsl_util.h>

#include "maikel/hmm/algorithm/forward.h"
#include "maikel/hmm/algorithm/backward.h"
#include "maikel/hmm/algorithm/baum_welch.h"

namespace maikel {

  template <class SymbolType, class IndexType>
    // requires UnsignedIntegral<I>
    bool is_bijective_index_map(std::map<SymbolType,IndexType> const& map)
    {
      IndexType max_index = ranges::max( map | ranges::view::values );
      std::vector<std::size_t> histogram(max_index+1);
      for (IndexType index : map|ranges::view::values)
        ++histogram[index];
      return ranges::all_of(histogram, [](std::size_t count){ return count == 1; });
    }

  template <class T>
  using ValueType = typename std::remove_reference<T>::type::value_type;

  template <class Index, class Range, class T = ranges::range_value_t<Range>>
    std::map<T,Index>
    map_from_symbols(Range&& range)
    {
      std::map<T,Index> symbols_to_index;
      Index index { 0 };
      ranges::for_each(range | ranges::view::unique, [&symbols_to_index,&index](T const& s){
          symbols_to_index[s] = index++;
      });
      Ensures(is_bijective_index_map(symbols_to_index));
      return symbols_to_index;
    }

}

#endif /* HMM_ALGORITHM_H_ */


/* matrix/gsl_matrix_double.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_MATRIX_DOUBLE_H__
#define __GSL_MATRIX_DOUBLE_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/block/gsl_check_range.h>
#include <gsl/vector/gsl_vector_double.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size1;		//nr rows
  size_t size2;		//nr cols
  size_t tda;		//space between two consecutive rows
  double * data;	//a pointer to the data
  gsl_block * block;//the block pointer.
  int owner;		//this is 1 if the current object owns the memory block, 0 otherwise
} gsl_matrix;

typedef struct
{
  gsl_matrix matrix;
} _gsl_matrix_view;

typedef _gsl_matrix_view gsl_matrix_view;

typedef struct
{
  gsl_matrix matrix;
} _gsl_matrix_const_view;

typedef const _gsl_matrix_const_view gsl_matrix_const_view;

/* Allocation */

GSL_FUNC gsl_matrix *
gsl_matrix_alloc (const size_t n1, const size_t n2);

GSL_FUNC gsl_matrix *
gsl_matrix_calloc (const size_t n1, const size_t n2);

GSL_FUNC gsl_matrix *
gsl_matrix_alloc_from_block (gsl_block * b,
                                   const size_t offset,
                                   const size_t n1,
                                   const size_t n2,
                                   const size_t d2);

GSL_FUNC gsl_matrix *
gsl_matrix_alloc_from_matrix (gsl_matrix * m,
                                    const size_t k1,
                                    const size_t k2,
                                    const size_t n1,
                                    const size_t n2);

GSL_FUNC gsl_vector *
gsl_vector_alloc_row_from_matrix (gsl_matrix * m,
                                        const size_t i);

GSL_FUNC gsl_vector *
gsl_vector_alloc_col_from_matrix (gsl_matrix * m,
                                        const size_t j);

GSL_FUNC void gsl_matrix_free (gsl_matrix * m);

/* Views */

GSL_FUNC _gsl_matrix_view
gsl_matrix_submatrix (gsl_matrix * m,
                            const size_t i, const size_t j,
                            const size_t n1, const size_t n2);

GSL_FUNC _gsl_vector_view
gsl_matrix_row (gsl_matrix * m, const size_t i);

GSL_FUNC _gsl_vector_view
gsl_matrix_column (gsl_matrix * m, const size_t j);

GSL_FUNC _gsl_vector_view
gsl_matrix_diagonal (gsl_matrix * m);

GSL_FUNC _gsl_vector_view
gsl_matrix_subdiagonal (gsl_matrix * m, const size_t k);

GSL_FUNC _gsl_vector_view
gsl_matrix_superdiagonal (gsl_matrix * m, const size_t k);

GSL_FUNC _gsl_matrix_view
gsl_matrix_view_array (double * base,
                             const size_t n1,
                             const size_t n2);

GSL_FUNC _gsl_matrix_view
gsl_matrix_view_array_with_tda (double * base,
                                      const size_t n1,
                                      const size_t n2,
                                      const size_t tda);

GSL_FUNC _gsl_matrix_view
gsl_matrix_view_vector (gsl_vector * v,
                              const size_t n1,
                              const size_t n2);

GSL_FUNC _gsl_matrix_view
gsl_matrix_view_vector_with_tda (gsl_vector * v,
                                       const size_t n1,
                                       const size_t n2,
                                       const size_t tda);

GSL_FUNC _gsl_matrix_const_view
gsl_matrix_const_submatrix (const gsl_matrix * m,
                                  const size_t i, const size_t j,
                                  const size_t n1, const size_t n2);

GSL_FUNC _gsl_vector_const_view
gsl_matrix_const_row (const gsl_matrix * m,
                            const size_t i);

GSL_FUNC _gsl_vector_const_view
gsl_matrix_const_column (const gsl_matrix * m,
                               const size_t j);

GSL_FUNC _gsl_vector_const_view
gsl_matrix_const_diagonal (const gsl_matrix * m);

GSL_FUNC _gsl_vector_const_view
gsl_matrix_const_subdiagonal (const gsl_matrix * m,
                                    const size_t k);

GSL_FUNC _gsl_vector_const_view
gsl_matrix_const_superdiagonal (const gsl_matrix * m,
                                      const size_t k);

GSL_FUNC _gsl_matrix_const_view
gsl_matrix_const_view_array (const double * base,
                                   const size_t n1,
                                   const size_t n2);

GSL_FUNC _gsl_matrix_const_view
gsl_matrix_const_view_array_with_tda (const double * base,
                                            const size_t n1,
                                            const size_t n2,
                                            const size_t tda);

GSL_FUNC _gsl_matrix_const_view
gsl_matrix_const_view_vector (const gsl_vector * v,
                                    const size_t n1,
                                    const size_t n2);

GSL_FUNC _gsl_matrix_const_view
gsl_matrix_const_view_vector_with_tda (const gsl_vector * v,
                                             const size_t n1,
                                             const size_t n2,
                                             const size_t tda);

/* Operations */

GSL_FUNC double   gsl_matrix_get(const gsl_matrix * m, const size_t i, const size_t j);
GSL_FUNC void    gsl_matrix_set(gsl_matrix * m, const size_t i, const size_t j, const double x);

GSL_FUNC double * gsl_matrix_ptr(gsl_matrix * m, const size_t i, const size_t j);
GSL_FUNC const double * gsl_matrix_const_ptr(const gsl_matrix * m, const size_t i, const size_t j);

GSL_FUNC void gsl_matrix_set_zero (gsl_matrix * m);
GSL_FUNC void gsl_matrix_set_identity (gsl_matrix * m);
GSL_FUNC void gsl_matrix_set_all (gsl_matrix * m, double x);

GSL_FUNC int gsl_matrix_fread (FILE * stream, gsl_matrix * m) ;
GSL_FUNC int gsl_matrix_fwrite (FILE * stream, const gsl_matrix * m) ;
GSL_FUNC int gsl_matrix_fscanf (FILE * stream, gsl_matrix * m);
GSL_FUNC int gsl_matrix_fprintf (FILE * stream, const gsl_matrix * m, const char * format);

GSL_FUNC int gsl_matrix_memcpy(gsl_matrix * dest, const gsl_matrix * src);
GSL_FUNC int gsl_matrix_swap(gsl_matrix * m1, gsl_matrix * m2);

GSL_FUNC int gsl_matrix_swap_rows(gsl_matrix * m, const size_t i, const size_t j);
GSL_FUNC int gsl_matrix_swap_columns(gsl_matrix * m, const size_t i, const size_t j);
GSL_FUNC int gsl_matrix_swap_rowcol(gsl_matrix * m, const size_t i, const size_t j);
GSL_FUNC int gsl_matrix_transpose (gsl_matrix * m);
GSL_FUNC int gsl_matrix_transpose_memcpy (gsl_matrix * dest, const gsl_matrix * src);

GSL_FUNC double gsl_matrix_max (const gsl_matrix * m);
GSL_FUNC double gsl_matrix_min (const gsl_matrix * m);
GSL_FUNC void gsl_matrix_minmax (const gsl_matrix * m, double * min_out, double * max_out);

GSL_FUNC void gsl_matrix_max_index (const gsl_matrix * m, size_t * imax, size_t *jmax);
GSL_FUNC void gsl_matrix_min_index (const gsl_matrix * m, size_t * imin, size_t *jmin);
GSL_FUNC void gsl_matrix_minmax_index (const gsl_matrix * m, size_t * imin, size_t * jmin, size_t * imax, size_t * jmax);

GSL_FUNC int gsl_matrix_isnull (const gsl_matrix * m);
GSL_FUNC int gsl_matrix_ispos (const gsl_matrix * m);
GSL_FUNC int gsl_matrix_isneg (const gsl_matrix * m);

GSL_FUNC int gsl_matrix_add (gsl_matrix * a, const gsl_matrix * b);
GSL_FUNC int gsl_matrix_add_scaled (gsl_matrix * a, const gsl_matrix * b, double scaleA, double scaleB);
GSL_FUNC int gsl_matrix_sub (gsl_matrix * a, const gsl_matrix * b);
GSL_FUNC int gsl_matrix_mul_elements (gsl_matrix * a, const gsl_matrix * b);
GSL_FUNC int gsl_matrix_div_elements (gsl_matrix * a, const gsl_matrix * b);
GSL_FUNC int gsl_matrix_scale (gsl_matrix * a, const double x);
GSL_FUNC int gsl_matrix_add_constant (gsl_matrix * a, const double x);
GSL_FUNC int gsl_matrix_add_diagonal (gsl_matrix * a, const double x);

/***********************************************************************/
/* The functions below are obsolete                                    */
/***********************************************************************/
GSL_FUNC int gsl_matrix_get_row(gsl_vector * v, const gsl_matrix * m, const size_t i);
GSL_FUNC int gsl_matrix_get_col(gsl_vector * v, const gsl_matrix * m, const size_t j);
GSL_FUNC int gsl_matrix_set_row(gsl_matrix * m, const size_t i, const gsl_vector * v);
GSL_FUNC int gsl_matrix_set_col(gsl_matrix * m, const size_t j, const gsl_vector * v);

/* inline functions if you are using GCC */

#ifdef HAVE_INLINE
extern inline
double
gsl_matrix_get(const gsl_matrix * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (i >= m->size1)
    {
      GSL_ERROR_VAL("first index out of range", GSL_EINVAL, 0) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_VAL("second index out of range", GSL_EINVAL, 0) ;
    }
#endif
  return m->data[i * m->tda + j] ;
}

extern inline
void
gsl_matrix_set(gsl_matrix * m, const size_t i, const size_t j, const double x)
{
#if GSL_RANGE_CHECK
  if (i >= m->size1)
    {
      GSL_ERROR_VOID("first index out of range", GSL_EINVAL) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_VOID("second index out of range", GSL_EINVAL) ;
    }
#endif
  m->data[i * m->tda + j] = x ;
}

extern inline
double *
gsl_matrix_ptr(gsl_matrix * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (i >= m->size1)
    {
      GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
    }
#endif
  return (double *) (m->data + (i * m->tda + j)) ;
}

extern inline
const double *
gsl_matrix_const_ptr(const gsl_matrix * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (i >= m->size1)
    {
      GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
    }
#endif
  return (const double *) (m->data + (i * m->tda + j)) ;
}

#endif

__END_DECLS

#endif /* __GSL_MATRIX_DOUBLE_H__ */


#ifndef RL_MDL_NLOPTINVERSEKINEMATICS_H
#define RL_MDL_NLOPTINVERSEKINEMATICS_H

#include <chrono>
#include <nlopt.h>
#include <random>
#include <utility>

#include "InverseKinematics.h"

namespace rl
{
	namespace mdl
	{
		class NloptInverseKinematics : public InverseKinematics
		{
		public:
			NloptInverseKinematics(Kinematic* kinematic);

			virtual ~NloptInverseKinematics();

			bool solve();

			::rl::math::Real delta;

			::std::chrono::nanoseconds duration;

			::rl::math::Real epsilonRotation;

			::rl::math::Real epsilonTranslation;

			double tolerance;

		protected:

		private:
			static void check(const nlopt_result& ret);

			::rl::math::Real error(const ::rl::math::Vector& q);

			static ::rl::math::Real f(unsigned n, const double* x, double* grad, void* data);

			::std::uniform_real_distribution< ::rl::math::Real> randDistribution;

			::std::mt19937 randEngine;
		};
	}
}

#endif // RL_MDL_NLOPTINVERSEKINEMATICS_H


/* matrix/gsl_matrix_float.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough
 *
 * 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.
 */

#ifndef __GSL_MATRIX_FLOAT_H__
#define __GSL_MATRIX_FLOAT_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_inline.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_vector_float.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size1;
  size_t size2;
  size_t tda;
  float * data;
  gsl_block_float * block;
  int owner;
} gsl_matrix_float;

typedef struct
{
  gsl_matrix_float matrix;
} _gsl_matrix_float_view;

typedef _gsl_matrix_float_view gsl_matrix_float_view;

typedef struct
{
  gsl_matrix_float matrix;
} _gsl_matrix_float_const_view;

typedef const _gsl_matrix_float_const_view gsl_matrix_float_const_view;

/* Allocation */

GSL_FUN gsl_matrix_float *
gsl_matrix_float_alloc (const size_t n1, const size_t n2);

GSL_FUN gsl_matrix_float *
gsl_matrix_float_calloc (const size_t n1, const size_t n2);

GSL_FUN gsl_matrix_float *
gsl_matrix_float_alloc_from_block (gsl_block_float * b,
                                   const size_t offset,
                                   const size_t n1,
                                   const size_t n2,
                                   const size_t d2);

GSL_FUN gsl_matrix_float *
gsl_matrix_float_alloc_from_matrix (gsl_matrix_float * m,
                                    const size_t k1,
                                    const size_t k2,
                                    const size_t n1,
                                    const size_t n2);

GSL_FUN gsl_vector_float *
gsl_vector_float_alloc_row_from_matrix (gsl_matrix_float * m,
                                        const size_t i);

GSL_FUN gsl_vector_float *
gsl_vector_float_alloc_col_from_matrix (gsl_matrix_float * m,
                                        const size_t j);

GSL_FUN void gsl_matrix_float_free (gsl_matrix_float * m);

/* Views */

GSL_FUN _gsl_matrix_float_view
gsl_matrix_float_submatrix (gsl_matrix_float * m,
                            const size_t i, const size_t j,
                            const size_t n1, const size_t n2);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_row (gsl_matrix_float * m, const size_t i);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_column (gsl_matrix_float * m, const size_t j);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_diagonal (gsl_matrix_float * m);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_subdiagonal (gsl_matrix_float * m, const size_t k);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_superdiagonal (gsl_matrix_float * m, const size_t k);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_subrow (gsl_matrix_float * m, const size_t i,
                         const size_t offset, const size_t n);

GSL_FUN _gsl_vector_float_view
gsl_matrix_float_subcolumn (gsl_matrix_float * m, const size_t j,
                            const size_t offset, const size_t n);

GSL_FUN _gsl_matrix_float_view
gsl_matrix_float_view_array (float * base,
                             const size_t n1,
                             const size_t n2);

GSL_FUN _gsl_matrix_float_view
gsl_matrix_float_view_array_with_tda (float * base,
                                      const size_t n1,
                                      const size_t n2,
                                      const size_t tda);

GSL_FUN _gsl_matrix_float_view
gsl_matrix_float_view_vector (gsl_vector_float * v,
                              const size_t n1,
                              const size_t n2);

GSL_FUN _gsl_matrix_float_view
gsl_matrix_float_view_vector_with_tda (gsl_vector_float * v,
                                       const size_t n1,
                                       const size_t n2,
                                       const size_t tda);

GSL_FUN _gsl_matrix_float_const_view
gsl_matrix_float_const_submatrix (const gsl_matrix_float * m,
                                  const size_t i, const size_t j,
                                  const size_t n1, const size_t n2);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_row (const gsl_matrix_float * m,
                            const size_t i);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_column (const gsl_matrix_float * m,
                               const size_t j);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_diagonal (const gsl_matrix_float * m);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_subdiagonal (const gsl_matrix_float * m,
                                    const size_t k);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_superdiagonal (const gsl_matrix_float * m,
                                      const size_t k);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_subrow (const gsl_matrix_float * m, const size_t i,
                               const size_t offset, const size_t n);

GSL_FUN _gsl_vector_float_const_view
gsl_matrix_float_const_subcolumn (const gsl_matrix_float * m, const size_t j,
                                  const size_t offset, const size_t n);

GSL_FUN _gsl_matrix_float_const_view
gsl_matrix_float_const_view_array (const float * base,
                                   const size_t n1,
                                   const size_t n2);

GSL_FUN _gsl_matrix_float_const_view
gsl_matrix_float_const_view_array_with_tda (const float * base,
                                            const size_t n1,
                                            const size_t n2,
                                            const size_t tda);

GSL_FUN _gsl_matrix_float_const_view
gsl_matrix_float_const_view_vector (const gsl_vector_float * v,
                                    const size_t n1,
                                    const size_t n2);

GSL_FUN _gsl_matrix_float_const_view
gsl_matrix_float_const_view_vector_with_tda (const gsl_vector_float * v,
                                             const size_t n1,
                                             const size_t n2,
                                             const size_t tda);

/* Operations */

GSL_FUN void gsl_matrix_float_set_zero (gsl_matrix_float * m);
GSL_FUN void gsl_matrix_float_set_identity (gsl_matrix_float * m);
GSL_FUN void gsl_matrix_float_set_all (gsl_matrix_float * m, float x);

GSL_FUN int gsl_matrix_float_fread (FILE * stream, gsl_matrix_float * m) ;
GSL_FUN int gsl_matrix_float_fwrite (FILE * stream, const gsl_matrix_float * m) ;
GSL_FUN int gsl_matrix_float_fscanf (FILE * stream, gsl_matrix_float * m);
GSL_FUN int gsl_matrix_float_fprintf (FILE * stream, const gsl_matrix_float * m, const char * format);

GSL_FUN int gsl_matrix_float_memcpy(gsl_matrix_float * dest, const gsl_matrix_float * src);
GSL_FUN int gsl_matrix_float_swap(gsl_matrix_float * m1, gsl_matrix_float * m2);

GSL_FUN int gsl_matrix_float_swap_rows(gsl_matrix_float * m, const size_t i, const size_t j);
GSL_FUN int gsl_matrix_float_swap_columns(gsl_matrix_float * m, const size_t i, const size_t j);
GSL_FUN int gsl_matrix_float_swap_rowcol(gsl_matrix_float * m, const size_t i, const size_t j);
GSL_FUN int gsl_matrix_float_transpose (gsl_matrix_float * m);
GSL_FUN int gsl_matrix_float_transpose_memcpy (gsl_matrix_float * dest, const gsl_matrix_float * src);

GSL_FUN float gsl_matrix_float_max (const gsl_matrix_float * m);
GSL_FUN float gsl_matrix_float_min (const gsl_matrix_float * m);
GSL_FUN void gsl_matrix_float_minmax (const gsl_matrix_float * m, float * min_out, float * max_out);

GSL_FUN void gsl_matrix_float_max_index (const gsl_matrix_float * m, size_t * imax, size_t *jmax);
GSL_FUN void gsl_matrix_float_min_index (const gsl_matrix_float * m, size_t * imin, size_t *jmin);
GSL_FUN void gsl_matrix_float_minmax_index (const gsl_matrix_float * m, size_t * imin, size_t * jmin, size_t * imax, size_t * jmax);

GSL_FUN int gsl_matrix_float_isnull (const gsl_matrix_float * m);
GSL_FUN int gsl_matrix_float_ispos (const gsl_matrix_float * m);
GSL_FUN int gsl_matrix_float_isneg (const gsl_matrix_float * m);
GSL_FUN int gsl_matrix_float_isnonneg (const gsl_matrix_float * m);

GSL_FUN int gsl_matrix_float_add (gsl_matrix_float * a, const gsl_matrix_float * b);
GSL_FUN int gsl_matrix_float_sub (gsl_matrix_float * a, const gsl_matrix_float * b);
GSL_FUN int gsl_matrix_float_mul_elements (gsl_matrix_float * a, const gsl_matrix_float * b);
GSL_FUN int gsl_matrix_float_div_elements (gsl_matrix_float * a, const gsl_matrix_float * b);
GSL_FUN int gsl_matrix_float_scale (gsl_matrix_float * a, const double x);
GSL_FUN int gsl_matrix_float_add_constant (gsl_matrix_float * a, const double x);
GSL_FUN int gsl_matrix_float_add_diagonal (gsl_matrix_float * a, const double x);

/***********************************************************************/
/* The functions below are obsolete                                    */
/***********************************************************************/
GSL_FUN int gsl_matrix_float_get_row(gsl_vector_float * v, const gsl_matrix_float * m, const size_t i);
GSL_FUN int gsl_matrix_float_get_col(gsl_vector_float * v, const gsl_matrix_float * m, const size_t j);
GSL_FUN int gsl_matrix_float_set_row(gsl_matrix_float * m, const size_t i, const gsl_vector_float * v);
GSL_FUN int gsl_matrix_float_set_col(gsl_matrix_float * m, const size_t j, const gsl_vector_float * v);
/***********************************************************************/

/* inline functions if you are using GCC */

GSL_FUN INLINE_DECL float   gsl_matrix_float_get(const gsl_matrix_float * m, const size_t i, const size_t j);
GSL_FUN INLINE_DECL void    gsl_matrix_float_set(gsl_matrix_float * m, const size_t i, const size_t j, const float x);
GSL_FUN INLINE_DECL float * gsl_matrix_float_ptr(gsl_matrix_float * m, const size_t i, const size_t j);
GSL_FUN INLINE_DECL const float * gsl_matrix_float_const_ptr(const gsl_matrix_float * m, const size_t i, const size_t j);

#ifdef HAVE_INLINE
INLINE_FUN
float
gsl_matrix_float_get(const gsl_matrix_float * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_VAL("first index out of range", GSL_EINVAL, 0) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_VAL("second index out of range", GSL_EINVAL, 0) ;
        }
    }
#endif
  return m->data[i * m->tda + j] ;
}

INLINE_FUN
void
gsl_matrix_float_set(gsl_matrix_float * m, const size_t i, const size_t j, const float x)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_VOID("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_VOID("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  m->data[i * m->tda + j] = x ;
}

INLINE_FUN
float *
gsl_matrix_float_ptr(gsl_matrix_float * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  return (float *) (m->data + (i * m->tda + j)) ;
}

INLINE_FUN
const float *
gsl_matrix_float_const_ptr(const gsl_matrix_float * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  return (const float *) (m->data + (i * m->tda + j)) ;
}

#endif

__END_DECLS

#endif /* __GSL_MATRIX_FLOAT_H__ */


#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_math.h>

#include "allvars.h"
#include "proto.h"

#ifdef MPISENDRECV_SIZELIMIT

#undef MPI_Sendrecv

int MPI_Sizelimited_Sendrecv(void *sendbuf, int sendcount, MPI_Datatype sendtype,
			     int dest, int sendtag, void *recvbuf, int recvcount,
			     MPI_Datatype recvtype, int source, int recvtag, MPI_Comm comm,
			     MPI_Status * status)
{
  int iter = 0, size_sendtype, size_recvtype, send_now, recv_now;
  int count_limit;

  if(dest != source)
    endrun(3);

  MPI_Type_size(sendtype, &size_sendtype);
  MPI_Type_size(recvtype, &size_recvtype);

  if(dest == ThisTask)
    {
      memcpy(recvbuf, sendbuf, recvcount * size_recvtype);
      return 0;
    }

  count_limit = (int) ((((long long) MPISENDRECV_SIZELIMIT) * 1024 * 1024) / size_sendtype);

  while(sendcount > 0 || recvcount > 0)
    {
      if(sendcount > count_limit)
	{
	  send_now = count_limit;
	  if(iter == 0)
	    {
	      printf("imposing size limit on MPI_Sendrecv() on task=%d (send of size=%d)\n",
		     ThisTask, sendcount * size_sendtype);
	      fflush(stdout);
	    }
	  iter++;
	}
      else
	send_now = sendcount;

      if(recvcount > count_limit)
	recv_now = count_limit;
      else
	recv_now = recvcount;

      MPI_Sendrecv(sendbuf, send_now, sendtype, dest, sendtag,
		   recvbuf, recv_now, recvtype, source, recvtag, comm, status);

      sendcount -= send_now;
      recvcount -= recv_now;

      sendbuf += send_now * size_sendtype;
      recvbuf += recv_now * size_recvtype;
    }

  return 0;
}

#endif


/* WidiPadi is a motif discovery tool for DNA sequences */
/* Copyright (C) 2006 Bioinformatics Centre */
/* Author: Eivind Valen and Ole Winther */

/* 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA. */

#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>
#include <gsl/gsl_sf_gamma.h>

#include "error.h"
#include "sesa.h"

#define USAGE "Usage: widipadi [OPTIONS] SEQUENCES"

enum bool {false, true};

/* FUNCTION PROTOTYPES */
PSSM hits_to_count_matrix(SESA sesa, struct HitTable *hits, const double threshold, int length, int alphabetSize);
int compare_hit_scores(const void *hit1, const void *hit2);
int parse_options(int argc, char **argv);
void print_help();

/* Acceptance */
int accept_all(SESA sesa, struct HitTable *hits);
int acceptIf_over_300_hits(SESA sesa, struct HitTable *hits);
int acceptIf_required_presence(SESA sesa, struct HitTable *hits);

/* DEFAULTS */
unsigned int length = 8;
unsigned int alphabetSize = 4;
unsigned int mismatches = 1;
int (*acceptanceFunc)(SESA, struct HitTable *) = acceptIf_required_presence;
double required_presence = 0.7;
unsigned int exhaustive = false;

/**
 * Prints a help message to stdout.
 */
void print_help() {
  printf(USAGE);
  printf("\nOptions:\n");
/*   printf("  -a                 Size of alphabet\n"); */
  printf("  -h                 Prints this help and exits\n");
  printf("  -l LENGTH          Length of WM\n");
  printf("  -m MISMATCHES      Number of mismatches\n");
  printf("  -r RATIO           Sets the ratio of sequences where the PSSM must match\n");
  printf("  -x                 Do exhaustive search\n");
  printf("\n");
}

/**
 * Parses command line options
 */
int parse_options(int argc, char **argv) {
  int count = 1;
  int c;

  while ((c = getopt(argc, argv, "hl:m:r:x")) != -1) {
    count++;
    switch (c) {
    case 'h':
      print_help();
      exit(EXIT_SUCCESS);
    case 'l':
      length = (unsigned int) strtol(optarg, (char **) NULL, 0);
      count++;
      break;
    case 'm':
      mismatches = (unsigned int) strtol(optarg, (char **) NULL, 0);
      count++;
      break;
    case 'r':
      acceptanceFunc = acceptIf_required_presence;
      required_presence = strtod(optarg, (char **) NULL);
      count++;
      break;
    case 'x':
      exhaustive = true;
      break;
    default:
      fprintf(stderr, USAGE);
      exit(EXIT_FAILURE);
    }
  }

  return count;
}

/*
 * Converts all hits above scoring threshold to a count matrix
 *
 * Only 0th order
 */
PSSM hits_to_count_matrix(SESA sesa, struct HitTable *hits, const double threshold, int length, int alphabetSize) {
  PSSM pssm = initMatrix(0, length, alphabetSize);
  ESA esa = sesa->esa;
  struct HitEntry *hit;
  int i, j;

  i = pssm->offsets[pssm->length];
  while (i--) pssm->counts[i] = 0;

  for (i = 0; i < hits->nScores; i++) {
    hit = &hits->pScores[i];

    if (hit->score < threshold)
      break;

    for (j = 0; j < length; j++) {
      int ltr = esa->pStr[hit->position + j];
      /*       fprintf(stderr, "%c", ind2chr(ltr)); */
      pssm->counts[ pssm->offsets[j] + ltr ]++;
    }
/*     fprintf(stderr, " (%f)\n", hit->score); */
  }

  return pssm;
}

/**
 * Comparing function for sort. Sorts on scores from low to high.
 */
int compare_hit_scores(const void *hit1, const void *hit2) {
  return ((struct HitEntry *)hit2)->score - ((struct HitEntry *)hit1)->score;
}

/**
 * Accepts all PSSMs (for testing)
 */
int accept_all(SESA sesa, struct HitTable *hits) {
  return true;
}

/**
 * Accepts PSSMs with more than 300 hits (for testing)
 */
int acceptIf_over_300_hits(SESA sesa, struct HitTable *hits) {
  return (hits->nScores > 300 ? true : false);
}

/**
 * Accepts PSSMs with hits present in at least "required_presence" of
 * the sequences
 */
int acceptIf_required_presence(SESA sesa, struct HitTable *hits) {
  int present[sesa->nSeq];
  int i, j;

  i = sesa->nSeq;
  while(i--) present[i] = 0;

  i = hits->nScores;
  while (i--) present[ sesa->seq[ hits->pScores[i].position ] ]++;

  j = 0;
  i = sesa->nSeq;
  while (i--) if (present[i]) j++;

  return ( ((double) j / (double) sesa->nSeq)  > required_presence ? true : false);
}

/*
 * Log likelihood scoring
 */
double log_likelihood(ESA esa, PSSM pssm, int *count_bg_all, double *pcount, double *pcount_bg) {
  int pcount_tot = 0, pcount_bg_tot = 0, count_tot = 0, count_bg_tot = 0;
  int alphlen = pssm->alphabetSize;
  int count_bg[alphlen];
  int ccount, ltr, pos, ord, i;
  double ll = 0.0;

  i = alphlen;
  while (i--) count_bg[i] = count_bg_all[i];

  pos = pssm->length;
  while (pos--) {
    ltr = alphlen;
    while(ltr--) {
      ccount = pssm->counts[pos * alphlen + ltr];
      count_bg[ltr] -= ccount ;
      ll += gsl_sf_lngamma( ccount + pcount[ltr] );
    }
  }

  ltr = alphlen;
  while (ltr--) {
    pcount_tot += pcount[ltr];
    pcount_bg_tot += pcount_bg[ltr];
    count_tot += pssm->counts[ltr];
    count_bg_tot += count_bg[ltr];
    ll += gsl_sf_lngamma( count_bg[ltr] + pcount_bg[ltr] );
  }

  ll -= alphlen * gsl_sf_lngamma( count_tot + pcount_tot ) + gsl_sf_lngamma(count_bg_tot + pcount_bg_tot );

  return ll;
}

/*
 * Log likelihood scoring
 */
double higher_log_likelihood(ESA esa, PSSM pssm, double *pcount, double *pcount_bg) {
  int pos, lcpi, skip, curSuf, i, j, p;
  int numSufs = getSize(esa);
  int curEsaIndex = 0;
  int order = 2;
  int wcount[order+1];

  i = order+1;
  while (i--) wcount[i] = 0;

  /* Iterate all suffixes */
  while(curEsaIndex < numSufs){
    lcpi = getLcp(esa, curEsaIndex);
    curSuf = getSuf(esa, curEsaIndex);
    skip = getSkip(esa, curEsaIndex);

    /* We can skip if the lcp is longer than the order */
    if (lcpi > order) {

      /* FIX: HACK */
      if (skip == 0) {
	curEsaIndex++;
	continue;
      }

      /* Count all the skipped words */
      i = order+1;
      while (i--) wcount[i] += skip - curEsaIndex;

      curEsaIndex = skip;
      continue;

    } else {

      pos = pssm->length;
      while (pos--) { }

      /* Print the hits */
      p = 1;
      for (j = 0; j <= order; j++)
	if (ind2chr(esa->pStr[getSuf(esa, curEsaIndex-1)+j]) == '-')
	  p = 0;

      if (p) {
	for (i = 0; i <= order; i++) fprintf(stderr, "%c", ind2chr(esa->pStr[getSuf(esa, curEsaIndex-1)+i]));
	fprintf(stderr, ": %5i\n", wcount[2]);
      }

      wcount[order] = 1;

      /* For higher order models the first columns have fewer letters */
      i = order;
      while (i--) {
	if (i >= lcpi) {

	  p = 1;
	  for (j = 0; j <= i; j++)
	    if (ind2chr(esa->pStr[getSuf(esa, curEsaIndex-1)+j]) == '-')
		p = 0;

	  if (p) {
	    for (j = 0; j <= i; j++) fprintf(stderr, "%c", ind2chr(esa->pStr[getSuf(esa, curEsaIndex-1)+j]));
	    for (j = i; j < order; j++) fprintf(stderr, " ");
	    fprintf(stderr, ": %5i\n", wcount[i]);
	  }

	  wcount[i] = 1;
	}
      }

      curEsaIndex++;
    }

  }

  exit(0);
}

void print_esa (ESA esa) {
  int numSufs = getSize(esa);
  int i,j;

  for (i = 0; i < numSufs; i++) {
    fprintf(stderr, "%4i: SUF(%3i) LCP(%3i) SKIP(%4i) ", i, getSuf(esa, i), getLcp(esa, i), getSkip(esa, i));

    for (j = 0; j < 8; j++) {
      fprintf(stderr, "%c", ind2chr(esa->pStr[getSuf(esa, i)+j]));
    }
    fprintf(stderr, "\n");
  }
}

int main(int argc, char **argv) {
  PSSMScoreTable results;
  struct HitTable *hits = NULL;
  PSSM pssm, count;
  SESA sesa;
  unsigned int arg_count, j;
  int i;

  arg_count = parse_options(argc, argv);

  printf("Reading file: %s  (%i)\n", argv[arg_count], argc - arg_count);
  if (!argv[arg_count]) {
    fprintf(stderr, "Need an input file\n");
    exit(EXIT_FAILURE);
  }

  sesa = build_SESA(argc - arg_count, &argv[arg_count], "ACGT", "N", 0);

  if (!sesa) {
    fprintf(stderr, "%s\n", getError());
    exit(EXIT_FAILURE);
  }

  printf("Searching %s for motifs of length %i with %i mismatch%s\n", (exhaustive ? "exhaustively" : "exclusively"), length, mismatches, (mismatches > 1 ? "es" : ""));

  if (exhaustive) {
    /* Exhaustive search searches for all words og length n.  */
    results = SESA_exhaustive_search(sesa, length, mismatches, alphabetSize, acceptanceFunc);
  } else {
    /* Exclusive search only searches for PSSMs representing words present in the sequence.  */
    results = SESA_exclusive_search(sesa, length, mismatches, alphabetSize, acceptanceFunc);
  }

  /**
   * Both the above methods return a list of integers that uniquely
   * identifies pssms.  We do not return the hits of the PSSMs even
   * though we have searched with them since this might be very(!)
   * memory demanding, depending on criteria used for acceptance.
   *
   * If we know the criteria is strict we might considering altering
   * this to avoid re-searching and thus speed up the process.
   */

  pssm = initMatrix(0, length, alphabetSize);

  for (i = 0; i < results->nPssm; i++) {
    /* Sets the pssm to the values corresponding to the ID*/
    set_mismatch_scores(pssm, results->pssmNr[i]);
    calcAndSetThresholds(pssm, -((double)mismatches)-0.5);

    /* Search with the PSSM again */
    init_hittable();                 /* Creates a hittable */
    hits = SESA_search(sesa, pssm);  /* Searches again */

    /* Sort the hits */
    qsort((void*)hits->pScores, hits->nScores, sizeof(struct HitEntry), compare_hit_scores);

    printf("\n\n");
    print_pssm(pssm);
    for (j = 0; j <= mismatches; j++) {
      printf("Count matrix for %i mismatches\n", j);
      count = hits_to_count_matrix(sesa, hits, -((double)j)-0.5, pssm->length, pssm->alphabetSize);

      double ost1[] = {1.0, 1.0, 1.0, 1.0};
      double ost2[] = {1.0, 1.0, 1.0, 1.0};

/*       double foo = log_likelihood(count, sesa->bg, ost1, ost2); */

      double foo = higher_log_likelihood(sesa->esa, count, ost1, ost2);

/*       fprintf(stderr, "LL: %g\n", foo); */
/*       print_counts(count);  */
      printf("\n\n");
    }

    release_hits(hits); /* Frees the allocated hittable*/
  }

  exit(EXIT_SUCCESS);
}


/* vector/gsl_vector_uint.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough
 *
 * 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.
 */

#ifndef __GSL_VECTOR_UINT_H__
#define __GSL_VECTOR_UINT_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_inline.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_block_uint.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size;
  size_t stride;
  unsigned int *data;
  gsl_block_uint *block;
  int owner;
}
gsl_vector_uint;

typedef struct
{
  gsl_vector_uint vector;
} _gsl_vector_uint_view;

typedef _gsl_vector_uint_view gsl_vector_uint_view;

typedef struct
{
  gsl_vector_uint vector;
} _gsl_vector_uint_const_view;

typedef const _gsl_vector_uint_const_view gsl_vector_uint_const_view;

/* Allocation */

gsl_vector_uint *gsl_vector_uint_alloc (const size_t n);
gsl_vector_uint *gsl_vector_uint_calloc (const size_t n);

gsl_vector_uint *gsl_vector_uint_alloc_from_block (gsl_block_uint * b,
                                                     const size_t offset,
                                                     const size_t n,
                                                     const size_t stride);

gsl_vector_uint *gsl_vector_uint_alloc_from_vector (gsl_vector_uint * v,
                                                      const size_t offset,
                                                      const size_t n,
                                                      const size_t stride);

void gsl_vector_uint_free (gsl_vector_uint * v);

/* Views */

_gsl_vector_uint_view
gsl_vector_uint_view_array (unsigned int *v, size_t n);

_gsl_vector_uint_view
gsl_vector_uint_view_array_with_stride (unsigned int *base,
                                         size_t stride,
                                         size_t n);

_gsl_vector_uint_const_view
gsl_vector_uint_const_view_array (const unsigned int *v, size_t n);

_gsl_vector_uint_const_view
gsl_vector_uint_const_view_array_with_stride (const unsigned int *base,
                                               size_t stride,
                                               size_t n);

_gsl_vector_uint_view
gsl_vector_uint_subvector (gsl_vector_uint *v,
                            size_t i,
                            size_t n);

_gsl_vector_uint_view
gsl_vector_uint_subvector_with_stride (gsl_vector_uint *v,
                                        size_t i,
                                        size_t stride,
                                        size_t n);

_gsl_vector_uint_const_view
gsl_vector_uint_const_subvector (const gsl_vector_uint *v,
                                  size_t i,
                                  size_t n);

_gsl_vector_uint_const_view
gsl_vector_uint_const_subvector_with_stride (const gsl_vector_uint *v,
                                              size_t i,
                                              size_t stride,
                                              size_t n);

/* Operations */

void gsl_vector_uint_set_zero (gsl_vector_uint * v);
void gsl_vector_uint_set_all (gsl_vector_uint * v, unsigned int x);
int gsl_vector_uint_set_basis (gsl_vector_uint * v, size_t i);

int gsl_vector_uint_fread (FILE * stream, gsl_vector_uint * v);
int gsl_vector_uint_fwrite (FILE * stream, const gsl_vector_uint * v);
int gsl_vector_uint_fscanf (FILE * stream, gsl_vector_uint * v);
int gsl_vector_uint_fprintf (FILE * stream, const gsl_vector_uint * v,
                              const char *format);

int gsl_vector_uint_memcpy (gsl_vector_uint * dest, const gsl_vector_uint * src);

int gsl_vector_uint_reverse (gsl_vector_uint * v);

int gsl_vector_uint_swap (gsl_vector_uint * v, gsl_vector_uint * w);
int gsl_vector_uint_swap_elements (gsl_vector_uint * v, const size_t i, const size_t j);

unsigned int gsl_vector_uint_max (const gsl_vector_uint * v);
unsigned int gsl_vector_uint_min (const gsl_vector_uint * v);
void gsl_vector_uint_minmax (const gsl_vector_uint * v, unsigned int * min_out, unsigned int * max_out);

size_t gsl_vector_uint_max_index (const gsl_vector_uint * v);
size_t gsl_vector_uint_min_index (const gsl_vector_uint * v);
void gsl_vector_uint_minmax_index (const gsl_vector_uint * v, size_t * imin, size_t * imax);

int gsl_vector_uint_add (gsl_vector_uint * a, const gsl_vector_uint * b);
int gsl_vector_uint_sub (gsl_vector_uint * a, const gsl_vector_uint * b);
int gsl_vector_uint_mul (gsl_vector_uint * a, const gsl_vector_uint * b);
int gsl_vector_uint_div (gsl_vector_uint * a, const gsl_vector_uint * b);
int gsl_vector_uint_scale (gsl_vector_uint * a, const double x);
int gsl_vector_uint_add_constant (gsl_vector_uint * a, const double x);

int gsl_vector_uint_equal (const gsl_vector_uint * u,
                            const gsl_vector_uint * v);

int gsl_vector_uint_isnull (const gsl_vector_uint * v);
int gsl_vector_uint_ispos (const gsl_vector_uint * v);
int gsl_vector_uint_isneg (const gsl_vector_uint * v);
int gsl_vector_uint_isnonneg (const gsl_vector_uint * v);

INLINE_DECL unsigned int gsl_vector_uint_get (const gsl_vector_uint * v, const size_t i);
INLINE_DECL void gsl_vector_uint_set (gsl_vector_uint * v, const size_t i, unsigned int x);
INLINE_DECL unsigned int * gsl_vector_uint_ptr (gsl_vector_uint * v, const size_t i);
INLINE_DECL const unsigned int * gsl_vector_uint_const_ptr (const gsl_vector_uint * v, const size_t i);

#ifdef HAVE_INLINE

INLINE_FUN
unsigned int
gsl_vector_uint_get (const gsl_vector_uint * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_VAL ("index out of range", GSL_EINVAL, 0);
    }
#endif
  return v->data[i * v->stride];
}

INLINE_FUN
void
gsl_vector_uint_set (gsl_vector_uint * v, const size_t i, unsigned int x)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_VOID ("index out of range", GSL_EINVAL);
    }
#endif
  v->data[i * v->stride] = x;
}

INLINE_FUN
unsigned int *
gsl_vector_uint_ptr (gsl_vector_uint * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_NULL ("index out of range", GSL_EINVAL);
    }
#endif
  return (unsigned int *) (v->data + i * v->stride);
}

INLINE_FUN
const unsigned int *
gsl_vector_uint_const_ptr (const gsl_vector_uint * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_NULL ("index out of range", GSL_EINVAL);
    }
#endif
  return (const unsigned int *) (v->data + i * v->stride);
}
#endif /* HAVE_INLINE */

__END_DECLS

#endif /* __GSL_VECTOR_UINT_H__ */


#ifndef H_LASER_DATA_INLINE
#define H_LASER_DATA_INLINE

//#include <gsl/gsl_math.h>
//#include <gsl/gsl_vector.h>
#include <limits>

//#include "csm.h"

/* Simple inline functions */

/*#warning Seen ld_valid_ray*/

INLINE int ld_valid_ray(LDP ld, int i) { return (i >= 0) && (i < ld->nrays) && (ld->valid[i]); }

INLINE int ld_valid_alpha(LDP ld, int i) { return ld->alpha_valid[i] != 0; }

INLINE void ld_set_null_correspondence(LDP ld, int i) {
  ld->corr[i].valid = 0;
  ld->corr[i].j1 = -1;
  ld->corr[i].j2 = -1;
  ld->corr[i].dist2_j1 = std::numeric_limits<double>::quiet_NaN();  // GSL_NAN;
}

INLINE void ld_set_correspondence(LDP ld, int i, int j1, int j2) {
  ld->corr[i].valid = 1;
  ld->corr[i].j1 = j1;
  ld->corr[i].j2 = j2;
}

/** -1 if not found */

INLINE int ld_next_valid(LDP ld, int i, int dir) {
  int j;
  for (j = i + dir; (j < ld->nrays) && (j >= 0) && !ld_valid_ray(ld, j); j += dir)
    ;
  return ld_valid_ray(ld, j) ? j : -1;
}

INLINE int ld_next_valid_up(LDP ld, int i) { return ld_next_valid(ld, i, +1); }

INLINE int ld_next_valid_down(LDP ld, int i) { return ld_next_valid(ld, i, -1); }

INLINE int ld_valid_corr(LDP ld, int i) { return ld->corr[i].valid; }

#endif


#include "stdlib.h"
#include "stdio.h"
#include "/home/lillian/work/install_fpdebug/valgrind-3.7.0/fpdebug/fpdebug.h"
#include <gsl/gsl_sf.h>
int main(int argc, const char * argv[]) {
unsigned long int hexdouble;
double a;
sscanf(argv[1], "%lX", &hexdouble);
a = *(double*)(&hexdouble);
double result = gsl_sf_Si(a);
//printf("%.15f\n", result);
VALGRIND_PRINT_ERROR("result", &result);
return 0;
}


/**
@file

Copyright John Reid 2006

*/

#include "biopsy/defs.h"

#include <gsl/gsl_rng.h>

namespace biopsy {

gsl_rng *
get_gsl_rng();

} //namespace biopsy


///
/// @file
///
/// @author Mirko Myllykoski (mirkom@cs.umu.se), Umeå University
///
/// @internal LICENSE
///
/// Copyright (c) 2019-2020, Umeå Universitet
///
/// 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.
///

#include <starneig_test_config.h>
#include <starneig/configuration.h>
#include "solvers.h"
#include "../common/common.h"
#include "../common/parse.h"
#include "../common/local_pencil.h"
#ifdef STARNEIG_ENABLE_MPI
#include "../common/starneig_pencil.h"
#endif
#include "../common/threads.h"
#include <starneig/starneig.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <cblas.h>
#include <omp.h>

#ifdef MAGMA_FOUND
#include <cuda_runtime_api.h>
#include <cuda.h>
#include <magma_auxiliary.h>
#include <magma_d.h>
#endif

static hook_solver_state_t lapack_prepare(
    int argc, char * const *argv, struct hook_data_env *env)
{
    return (hook_solver_state_t) env->data;
}

static int lapack_finalize(hook_solver_state_t state, struct hook_data_env *env)
{
    return 0;
}

static int lapack_run(hook_solver_state_t state)
{
    pencil_t data = (pencil_t) state;

    extern void dgehrd_(
        int const *,    // the order of the matrix A
        int const *,    // left bound
        int const *,    // right bound
        double *,       // input/output matrix
        int const *,    // input/output matrix leading dimension
        double *,       // tau (scalar factors)
        double *,       // work space
        int const *,    // work space size
        int *);         // info

    extern void dormhr_(
        char const *,   // side
        char const *,   // transpose
        int const *,    // row count
        int const *,    // column count
        int const *,    // left bound
        int const *,    // right bound
        double const *, // elementary reflectors
        int const *,    // elementary reflector leading dimension
        double const *, // scalar factors (tau)
        double *,       // input/output matrix
        int const *,    // input/output matrix leading dimension
        double *,       // work space
        int const *,    // work space size
        int *);         // info

    extern void dgeqrf_(int const *, int const *, double *, int const *,
        double *, double *, int const *, int *);

    extern void dormqr_(char const *, char const *, int const *, int const *,
        int const *, double const *, int const *, double const *, double *,
        int const *, double*, const int *, int *);

    extern void dgghd3_(char const *, char const *, int const *, int const *,
        int const *, double *, int const *, double *, int const *, double *,
        int const *, double *, int const *, double *, int const *, int *);

    int n = LOCAL_MATRIX_N(data->mat_a);
    double *A = LOCAL_MATRIX_PTR(data->mat_a);
    int ldA = LOCAL_MATRIX_LD(data->mat_a);
    double *Q = LOCAL_MATRIX_PTR(data->mat_q);
    int ldQ = LOCAL_MATRIX_LD(data->mat_q);

    double *B = NULL;
    int ldB = 0;
    double *Z = NULL;
    int ldZ = 0;

    double *tau = NULL;
    double *work = NULL;
    int info, ilo = 1, ihi = n;

    threads_set_mode(THREADS_MODE_LAPACK);

    if (data->mat_b != NULL) {
        B = LOCAL_MATRIX_PTR(data->mat_b);
        ldB = LOCAL_MATRIX_LD(data->mat_b);
        Z = LOCAL_MATRIX_PTR(data->mat_z);
        ldZ = LOCAL_MATRIX_LD(data->mat_z);

        //
        // allocate workspace
        //

        int lwork = 0;

        {
            int _lwork = -1;
            double dlwork;

            dgeqrf_(&n, &n, B, &ldB, tau, &dlwork, &_lwork, &info);
            if (info != 0)
                goto cleanup;

            lwork = MAX(lwork, dlwork);
        }

        {
            int _lwork = -1;
            double dlwork;

            dormqr_("L", "T", &n, &n, &n,
                B, &ldB, tau, A, &ldA, &dlwork, &_lwork, &info);
            if (info != 0)
                goto cleanup;

            lwork = MAX(lwork, dlwork);
        }

        {
            int _lwork = -1;
            double dlwork;

            dormqr_("R", "N", &n, &n, &n,
                B, &ldB, tau, Q, &ldQ, &dlwork, &_lwork, &info);
            if (info != 0)
                goto cleanup;

            lwork = MAX(lwork, dlwork);
        }

        {
            int _lwork = -1;
            double dlwork;

            dgghd3_("V", "V", &n, &ilo, &ihi,
                A, &ldA, B, &ldB, Q, &ldQ, Z, &ldZ, &dlwork, &_lwork, &info);
            if (info != 0)
                goto cleanup;

            lwork = MAX(lwork, dlwork);
        }

        tau = malloc(n*sizeof(double));
        work = malloc(lwork*sizeof(double));

        //
        // reduce
        //

        // form B = ~Q * R
        dgeqrf_(&n, &n, B, &ldB, tau, work, &lwork, &info);
        if (info != 0)
            goto cleanup;

        // A <- ~Q^T * A
        dormqr_("L", "T", &n, &n, &n, B, &ldB, tau, A, &ldA, work, &lwork,
            &info);
        if (info != 0)
            goto cleanup;

        // Q <- Q * ~Q
        dormqr_("R", "N", &n, &n, &n, B, &ldB, tau, Q, &ldQ, work, &lwork,
            &info);
        if (info != 0)
            goto cleanup;

        // clean B (B <- R)
        for (int i = 0; i < n; i++)
            for (int j = i+1; j < n; j++)
                B[(size_t)i*ldB+j] = 0.0;

        // reduce (A,B) to Hessenberg-triangular form
        dgghd3_("V", "V", &n, &ilo, &ihi,
            A, &ldA, B, &ldB, Q, &ldQ, Z, &ldZ, work, &lwork, &info);
        if (info != 0)
            goto cleanup;
    }
    else {

        int lwork = -1;
        double dlwork;

        // request optimal work space size
        dgehrd_(&n, &ilo, &ihi, A, &ldA,
            tau, &dlwork, &lwork, &info);

        if (info != 0)
            goto cleanup;

        lwork = dlwork;
        work = malloc(lwork*sizeof(double));
        tau = malloc(n*sizeof(double));

        // reduce
        dgehrd_(&n, &ilo, &ihi, A, &ldA,
            tau, work, &lwork, &info);

        if (info != 0)
            goto cleanup;

        free(work);
        work = NULL;

        // request optimal work space size
        lwork = -1;
        dormhr_("Right", "No transpose", &n, &n, &ilo, &ihi, A, &ldA, tau,
            Q, &ldQ, &dlwork, &lwork, &info);

        if (info != 0)
            goto cleanup;

        lwork = dlwork;
        work = malloc(lwork*sizeof(double));

        // form Q
        dormhr_("Right", "No transpose", &n, &n, &ilo, &ihi, A, &ldA, tau,
            Q, &ldQ, work, &lwork, &info);

        if (info != 0)
            goto cleanup;

        for (int i = 0; i < n; i++)
            for (int j = i+2; j < n; j++)
                A[i*ldA+j] = 0.0;
    }

cleanup:

    threads_set_mode(THREADS_MODE_DEFAULT);

    free(work);
    free(tau);
    return info;
}

const struct hook_solver hessenberg_lapack_solver = {
    .name = "lapack",
    .desc = "LAPACK's dgehrd/dgghrd subroutine",
    .formats = (hook_data_format_t[]) { HOOK_DATA_FORMAT_PENCIL_LOCAL, 0 },
    .prepare = &lapack_prepare,
    .finalize = &lapack_finalize,
    .run = &lapack_run
};

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

#if defined(PDGEHRD_FOUND) && defined(PDORMHR_FOUND) && defined(PDLASET_FOUND)

static hook_solver_state_t scalapack_prepare(
    int argc, char * const *argv, struct hook_data_env *env)
{
    return env;
}

static int scalapack_finalize(
    hook_solver_state_t state, struct hook_data_env *env)
{
    return 0;
}

static int has_valid_descr(
    int matrix_size, int section_size, const starneig_blacs_descr_t *descr)
{
    if (descr->m != matrix_size || descr->n != matrix_size)
        return 0;
    if (descr->sm != section_size || descr->sn != section_size)
        return 0;
    return 1;
}

static int scalapack_run(hook_solver_state_t state)
{
    extern void pdgehrd_(int const *, int const *, int const *, double *,
        int const *, int const *, const starneig_blacs_descr_t *, double *,
        double *, int const *, int *);

    extern void pdormhr_(char const *, char const *, int const *, int const *,
        int const *, int const *, double *, int const *, int const *,
        const starneig_blacs_descr_t *, double *, double *, int const *,
        int const *, const starneig_blacs_descr_t *, double *, int const *,
        int *);

    extern void pdlaset_(char const *, int const *, int const *,
        double const *, double const *, double *, int const *, int const *,
        const starneig_blacs_descr_t *);

    threads_set_mode(THREADS_MODE_SCALAPACK);

    struct hook_data_env *env = state;
    pencil_t pencil = (pencil_t) env->data;

    if (pencil->mat_a == NULL) {
        fprintf(stderr, "Missing matrix A.\n");
        return -1;
    }

    if (pencil->mat_b != NULL) {
        fprintf(stderr, "Solver does not support generalized cases.\n");
        return -1;
    }

    int n = STARNEIG_MATRIX_N(pencil->mat_a);
    int sn = STARNEIG_MATRIX_BN(pencil->mat_a);

    starneig_distr_t distr = STARNEIG_MATRIX_DISTR(pencil->mat_a);
    starneig_blacs_context_t context = starneig_distr_to_blacs_context(distr);

    starneig_blacs_descr_t desc_a, desc_q;
    double *local_a, *local_q;
    STARNEIG_BLACS_MATRIX_DESCR_LOCAL(
        pencil->mat_a, context, &desc_a, (void **)&local_a);
    STARNEIG_BLACS_MATRIX_DESCR_LOCAL(
        pencil->mat_q, context, &desc_q, (void **)&local_q);

    if (!has_valid_descr(n, sn, &desc_a)) {
        fprintf(stderr, "Matrix A has invalid dimensions.\n");
        return -1;
    }

    if (pencil->mat_q != NULL && !has_valid_descr(n, sn, &desc_q)) {
        fprintf(stderr, "Matrix Q has invalid dimension.\n");
        return -1;
    }

    int ilo = 1, ihi = n, ia = 1, ja = 1, ic = 1, jc = 1, lwork, info;
    double *work = NULL, *tau = NULL, _work;

    lwork = -1;
    pdgehrd_(&n, &ilo, &ihi, NULL, &ia, &ja, &desc_a, NULL,
        &_work, &lwork, &info);

    if (info)
        goto cleanup;

    lwork = _work;
    work = malloc(lwork*sizeof(double));
    tau = malloc(n*sizeof(double));

    pdgehrd_(&n, &ilo, &ihi, local_a, &ia, &ja, &desc_a, tau,
        work, &lwork, &info);

    if (info)
        goto cleanup;

    lwork = -1;
    pdormhr_("Right", "No transpose", &n, &n, &ilo, &ihi, NULL,
        &ia, &ja, &desc_a, NULL, NULL, &ic, &jc,
        &desc_q, &_work, &lwork, &info);

    if (info)
        goto cleanup;

    free(work);
    lwork = _work;
    work = malloc(lwork*sizeof(double));

    pdormhr_("Right", "No transpose", &n, &n, &ilo, &ihi, local_a,
        &ia, &ja, &desc_a, tau, local_q, &ic, &jc,
        &desc_q, work, &lwork, &info);

    {
        int nm1 = n-2, one = 1, three = 3;
        double dzero = 0.0;
        pdlaset_("Lower", &nm1, &nm1, &dzero, &dzero, local_a, &three,
            &one, &desc_a);
    }

cleanup:

    threads_set_mode(THREADS_MODE_DEFAULT);
    starneig_blacs_gridexit(context);

    free(work);
    free(tau);

    return info;
}

const struct hook_solver hessenberg_scalapack_solver = {
    .name = "scalapack",
    .desc = "pdgehrd subroutine from scaLAPACK",
    .formats = (hook_data_format_t[]) {
#ifdef STARNEIG_ENABLE_BLACS
        HOOK_DATA_FORMAT_PENCIL_BLACS,
#endif
        0 },
    .prepare = &scalapack_prepare,
    .finalize = &scalapack_finalize,
    .run = &scalapack_run
};

#endif // PDGEHRD_FOUND

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

struct starpu_state {
    int argc;
    char * const *argv;
    struct hook_data_env *env;
};

static void starpu_print_usage(int argc, char * const *argv)
{
    printf(
        "  --cores [default,(num)} -- Number of CPU cores\n"
        "  --gpus [default,(num)} -- Number of GPUS\n"
        "  --tile-size [default,(num)] -- tile size\n"
        "  --panel-width [default,(num)] -- Panel width\n"
    );
}

static int starpu_check_args(int argc, char * const *argv, int *argr)
{
    struct multiarg_t arg_cores = read_multiarg(
        "--cores", argc, argv, argr, "default", NULL);
    struct multiarg_t arg_gpus = read_multiarg(
        "--gpus", argc, argv, argr, "default", NULL);
    struct multiarg_t tile_size = read_multiarg(
        "--tile-size", argc, argv, argr, "default", NULL);
    struct multiarg_t panel_width = read_multiarg(
        "--panel-width", argc, argv, argr, "default", NULL);

    if (arg_cores.type == MULTIARG_INVALID)
        return -1;

    if (arg_gpus.type == MULTIARG_INVALID)
        return -1;

    if (tile_size.type == MULTIARG_INVALID ||
    (tile_size.type == MULTIARG_INT && tile_size.int_value < 1)) {
        fprintf(stderr, "Invalid tile size.\n");
        return -1;
    }

    if (panel_width.type == MULTIARG_INVALID ||
    (panel_width.type == MULTIARG_INT && panel_width.int_value < 1)) {
        fprintf(stderr, "Invalid panel width.\n");
        return -1;
    }

    return 0;
}

static void starpu_print_args(int argc, char * const *argv)
{
    print_multiarg("--cores", argc, argv, "default", NULL);
    print_multiarg("--gpus", argc, argv, "default", NULL);
    print_multiarg("--tile-size", argc, argv, "default", NULL);
    print_multiarg("--panel-width", argc, argv, "default", NULL);
}

static hook_solver_state_t starpu_prepare(
    int argc, char * const *argv, struct hook_data_env *env)
{
    struct starpu_state *state = malloc(sizeof(struct starpu_state));

    state->argc = argc;
    state->argv = argv;
    state->env = env;

    struct multiarg_t arg_cores = read_multiarg(
        "--cores", argc, argv, NULL, "default", NULL);
    struct multiarg_t arg_gpus = read_multiarg(
        "--gpus", argc, argv, NULL, "default", NULL);

    int cores = STARNEIG_USE_ALL;
    if (arg_cores.type == MULTIARG_INT)
        cores = arg_cores.int_value;

    int gpus = STARNEIG_USE_ALL;
    if (arg_gpus.type == MULTIARG_INT)
        gpus = arg_gpus.int_value;

#ifdef STARNEIG_ENABLE_MPI
    if (env->format == HOOK_DATA_FORMAT_PENCIL_STARNEIG ||
    env->format == HOOK_DATA_FORMAT_PENCIL_BLACS)
        starneig_node_init(cores, gpus, STARNEIG_FAST_DM);
    else
#endif
        starneig_node_init(
            cores, gpus, STARNEIG_HINT_SM | STARNEIG_AWAKE_WORKERS);

    return state;
}

static int starpu_finalize(hook_solver_state_t state, struct hook_data_env *env)
{
    if (state == NULL)
        return 0;

    starneig_node_finalize();

    free(state);
    return 0;
}

static int starpu_run(hook_solver_state_t state)
{
    int argc = ((struct starpu_state *) state)->argc;
    char * const *argv = ((struct starpu_state *) state)->argv;
    struct hook_data_env *env = ((struct starpu_state *) state)->env;

    struct starneig_hessenberg_conf conf;
    starneig_hessenberg_init_conf(&conf);

    struct multiarg_t tile_size = read_multiarg(
        "--tile-size", argc, argv, NULL, "default", NULL);
    struct multiarg_t panel_width = read_multiarg(
        "--panel-width", argc, argv, NULL, "default", NULL);

    if (tile_size.type == MULTIARG_INT)
        conf.tile_size = tile_size.int_value;
    if (panel_width.type == MULTIARG_INT)
        conf.panel_width = panel_width.int_value;

    int ret = 0;

    if (env->format == HOOK_DATA_FORMAT_PENCIL_LOCAL) {
        pencil_t pencil = (pencil_t) env->data;
        if (pencil->mat_b != NULL) {
            ret = starneig_GEP_SM_HessenbergTriangular(
                LOCAL_MATRIX_N(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_a), LOCAL_MATRIX_LD(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_b), LOCAL_MATRIX_LD(pencil->mat_b),
                LOCAL_MATRIX_PTR(pencil->mat_q), LOCAL_MATRIX_LD(pencil->mat_q),
                LOCAL_MATRIX_PTR(pencil->mat_z), LOCAL_MATRIX_LD(pencil->mat_z)
            );
        }
        else {
            ret = starneig_SEP_SM_Hessenberg_expert(&conf,
                LOCAL_MATRIX_N(pencil->mat_a), 0, LOCAL_MATRIX_N(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_a), LOCAL_MATRIX_LD(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_q), LOCAL_MATRIX_LD(pencil->mat_q)
            );
        }
    }
#ifdef STARNEIG_ENABLE_MPI
    if (env->format == HOOK_DATA_FORMAT_PENCIL_BLACS) {
        pencil_t pencil = (pencil_t) env->data;
        if (pencil->mat_b != NULL) {
#ifdef STARNEIG_GEP_DM_HESSENBERGTRIANGULAR
            ret = starneig_GEP_DM_HessenbergTriangular(
                STARNEIG_MATRIX_HANDLE(pencil->mat_a),
                STARNEIG_MATRIX_HANDLE(pencil->mat_b),
                STARNEIG_MATRIX_HANDLE(pencil->mat_q),
                STARNEIG_MATRIX_HANDLE(pencil->mat_z));
#else
            fprintf(stderr,
                "Solver does not support generalized cases in distributed "
                "memory.\n");
            return -1;
#endif
        }
        else {
            ret = starneig_SEP_DM_Hessenberg(
                STARNEIG_MATRIX_HANDLE(pencil->mat_a),
                STARNEIG_MATRIX_HANDLE(pencil->mat_q));
        }
    }
#endif

    return ret;
}

const struct hook_solver hessenberg_starpu_solver = {
    .name = "starneig",
    .desc = "StarPU based subroutine",
    .formats = (hook_data_format_t[]) {
        HOOK_DATA_FORMAT_PENCIL_LOCAL,
#ifdef STARNEIG_ENABLE_BLACS
        HOOK_DATA_FORMAT_PENCIL_BLACS,
#endif
        0 },
    .print_usage = &starpu_print_usage,
    .print_args = &starpu_print_args,
    .check_args = &starpu_check_args,
    .prepare = &starpu_prepare,
    .finalize = &starpu_finalize,
    .run = &starpu_run
};

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

static void starpu_simple_print_usage(int argc, char * const *argv)
{
    printf(
        "  --cores [default,(num)} -- Number of CPU cores\n"
        "  --gpus [default,(num)} -- Number of GPUS\n"
    );
}

static void starpu_simple_print_args(int argc, char * const *argv)
{
    print_multiarg("--cores", argc, argv, "default", NULL);
    print_multiarg("--gpus", argc, argv, "default", NULL);
}

static int starpu_simple_check_args(int argc, char * const *argv, int *argr)
{
    struct multiarg_t arg_cores = read_multiarg(
        "--cores", argc, argv, argr, "default", NULL);
    struct multiarg_t arg_gpus = read_multiarg(
        "--gpus", argc, argv, argr, "default", NULL);

    if (arg_cores.type == MULTIARG_INVALID)
        return -1;

    if (arg_gpus.type == MULTIARG_INVALID)
        return -1;

    return 0;
}

static hook_solver_state_t starpu_simple_prepare(
    int argc, char * const *argv, struct hook_data_env *env)
{
    struct starpu_state *state = malloc(sizeof(struct starpu_state));

    state->argc = argc;
    state->argv = argv;
    state->env = env;

    struct multiarg_t arg_cores = read_multiarg(
        "--cores", argc, argv, NULL, "default", NULL);
    struct multiarg_t arg_gpus = read_multiarg(
        "--gpus", argc, argv, NULL, "default", NULL);

    int cores = STARNEIG_USE_ALL;
    if (arg_cores.type == MULTIARG_INT)
        cores = arg_cores.int_value;

    int gpus = STARNEIG_USE_ALL;
    if (arg_gpus.type == MULTIARG_INT)
        gpus = arg_gpus.int_value;

#ifdef STARNEIG_ENABLE_MPI
    if (env->format == HOOK_DATA_FORMAT_PENCIL_STARNEIG ||
    env->format == HOOK_DATA_FORMAT_PENCIL_BLACS)
        starneig_node_init(cores, gpus, STARNEIG_FAST_DM);
    else
#endif
        starneig_node_init(
            cores, gpus, STARNEIG_HINT_SM | STARNEIG_AWAKE_WORKERS);

    return state;
}

static int starpu_simple_finalize(
    hook_solver_state_t state, struct hook_data_env *env)
{
    if (state == NULL)
        return 0;

    starneig_node_finalize();

    free(state);
    return 0;
}

static int starpu_simple_run(hook_solver_state_t state)
{
    struct hook_data_env *env = ((struct starpu_state *) state)->env;

    int ret = 0;

    if (env->format == HOOK_DATA_FORMAT_PENCIL_LOCAL) {
        pencil_t pencil = (pencil_t) env->data;

        if (pencil->mat_b != NULL) {
            ret = starneig_GEP_SM_HessenbergTriangular(
                LOCAL_MATRIX_N(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_a), LOCAL_MATRIX_LD(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_b), LOCAL_MATRIX_LD(pencil->mat_b),
                LOCAL_MATRIX_PTR(pencil->mat_q), LOCAL_MATRIX_LD(pencil->mat_q),
                LOCAL_MATRIX_PTR(pencil->mat_z), LOCAL_MATRIX_LD(pencil->mat_z)
            );
        }
        else {
            ret = starneig_SEP_SM_Hessenberg(LOCAL_MATRIX_N(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_a), LOCAL_MATRIX_LD(pencil->mat_a),
                LOCAL_MATRIX_PTR(pencil->mat_q), LOCAL_MATRIX_LD(pencil->mat_q)
            );
        }
    }
#ifdef STARNEIG_ENABLE_MPI
    if (env->format == HOOK_DATA_FORMAT_PENCIL_BLACS) {
        pencil_t pencil = (pencil_t) env->data;

        if (pencil->mat_b != NULL) {
#ifdef STARNEIG_GEP_DM_HESSENBERGTRIANGULAR
            ret = starneig_GEP_DM_HessenbergTriangular(
                STARNEIG_MATRIX_HANDLE(pencil->mat_a),
                STARNEIG_MATRIX_HANDLE(pencil->mat_b),
                STARNEIG_MATRIX_HANDLE(pencil->mat_q),
                STARNEIG_MATRIX_HANDLE(pencil->mat_z));
#else
            fprintf(stderr,
                "Solver does not support distributed memory in generalized "
                "cases.\n");
            return -1;
#endif
        }
        else {
            ret = starneig_SEP_DM_Hessenberg(
                STARNEIG_MATRIX_HANDLE(pencil->mat_a),
                STARNEIG_MATRIX_HANDLE(pencil->mat_q));
        }
    }
#endif

    return ret;
}

const struct hook_solver hessenberg_starpu_simple_solver = {
    .name = "starneig-simple",
    .desc = "StarPU based subroutine (simplified interface)",
    .formats = (hook_data_format_t[]) {
        HOOK_DATA_FORMAT_PENCIL_LOCAL,
#ifdef STARNEIG_ENABLE_BLACS
        HOOK_DATA_FORMAT_PENCIL_BLACS,
#endif
        0 },
    .print_usage = &starpu_simple_print_usage,
    .print_args = &starpu_simple_print_args,
    .check_args = &starpu_simple_check_args,
    .prepare = &starpu_simple_prepare,
    .finalize = &starpu_simple_finalize,
    .run = &starpu_simple_run
};

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

#ifdef MAGMA_FOUND

static hook_solver_state_t magma_dgehrd_prepare(
    int argc, char * const *argv, struct hook_data_env *env)
{
    if (magma_init() != MAGMA_SUCCESS)
        return NULL;
    return (hook_solver_state_t) env->data;
}

static int magma_dgehrd_finalize(
    hook_solver_state_t state, struct hook_data_env *env)
{
    magma_finalize();
    return 0;
}

static int magma_dgehrd_run(hook_solver_state_t state)
{
    pencil_t data = (pencil_t) state;

    int n = LOCAL_MATRIX_N(data->mat_a);
    double *A = LOCAL_MATRIX_PTR(data->mat_a);
    double *Q = LOCAL_MATRIX_PTR(data->mat_q);
    int ldA = LOCAL_MATRIX_LD(data->mat_a);
    int ldQ = LOCAL_MATRIX_LD(data->mat_q);

    int info;

    double *tau = NULL;
    double *work = NULL;
    double *dT = NULL;

    threads_set_mode(THREADS_MODE_LAPACK);

    // request optimal work space size
    double _work;
    magma_dgehrd(n, 1, n, A, ldA, tau, &_work, -1, dT, &info);

    if (info != 0)
        goto finalize;

    tau = malloc(LOCAL_MATRIX_N(data->mat_a)*sizeof(double));

    int lwork = _work;
    work = malloc(lwork*sizeof(double));

    int nb = magma_get_dgehrd_nb(LOCAL_MATRIX_N(data->mat_a));
    cudaMalloc((void**)&dT, nb*n*sizeof(double));

    // reduce
    magma_dgehrd(n, 1, n, A, ldA, tau, work, lwork, dT, &info);

    if (info != 0)
        goto finalize;

    free(work);
    work = NULL;

    // copy A -> Q
    for (int i = 0; i < n; i++)
        memcpy(Q+i*ldQ, A+i*ldA, n*sizeof(double));

    // form Q
    magma_dorghr(n, 1, n, Q, ldQ, tau, dT, nb, &info);

    if (info != 0)
        goto finalize;

    // zero entries below the first sub-diagonal
    for (int i = 0; i < n-1; i++)
        memset(A+i*ldA+i+2, 0, (n-i-2)*sizeof(double));

finalize:

    threads_set_mode(THREADS_MODE_DEFAULT);

    cudaFree(dT);
    free(work);
    free(tau);
    return info;
}

const struct hook_solver hessenberg_magma_solver = {
    .name = "magma",
    .desc = "MAGMA's dgehrd subroutine",
    .formats = (hook_data_format_t[]) { HOOK_DATA_FORMAT_PENCIL_LOCAL, 0 },
    .prepare = &magma_dgehrd_prepare,
    .finalize = &magma_dgehrd_finalize,
    .run = &magma_dgehrd_run
};

#endif // MAGMA_FOUND


//===--- Sudoku/precompiled_basic.h                                     ---===//
//
// Lists the standard system includes consumed by this library,
// or project specific include files that are used frequently, but
// are changed infrequently.
//
// - To be used as part of a precompiled header configuration for a project
//   consuming this library.
// - Where frequent changes to the Sudoku library code are expected.
// - Not required when the library itself is added to a precompiled header.
//
//===----------------------------------------------------------------------===//
#pragma once

// STL containers
#include <array>
#include <bitset>
#include <vector>
// STL
#include <algorithm>
#include <iterator>
#include <limits>
#include <memory>
#include <stdexcept>
#include <utility>

// Libraries
#include <gsl/gsl>


/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef type_f33c5e03_70f2_4879_9504_332323832a40_h
#define type_f33c5e03_70f2_4879_9504_332323832a40_h

#include <ariel/config.h>
#include <gslib/type.h>
#include <gslib/string.h>
#include <gslib/math.h>
#include <gslib/std.h>

__ariel_begin__

struct ariel_export color
{
    union
    {
        struct { byte red, green, blue, alpha; };
        uint _data;
    };

public:
    color() { data() = 0; }
    color(int r, int g, int b) { set_color(r, g, b); }
    color(int r, int g, int b, int a) { set_color(r, g, b, a); }
    uint& data() { return _data; }
    uint data() const { return _data; }
    void set_color(int r, int g, int b) { red = r, green = g, blue = b, alpha = 255; }
    void set_color(int r, int g, int b, int a) { red = r, green = g, blue = b, alpha = a; }
    bool operator !=(const color& cr) const { return blue != cr.blue || green != cr.green || red != cr.red || alpha != cr.alpha; }
    bool operator ==(const color& cr) const { return blue == cr.blue && green == cr.green && red == cr.red && alpha == cr.alpha; }
    color& lerp(const color& c1, const color& c2, float s)
    {
        assert(s >= 0.f && s <= 1.f);
        float t = 1.f - s;
        red = (int)(s * c1.red + t * c2.red);
        green = (int)(s * c1.green + t * c2.green);
        blue = (int)(s * c1.blue + t * c2.blue);
        alpha = (int)(s * c1.alpha + t * c2.alpha);
        return *this;
    }
};

struct ariel_export font
{
    enum
    {
        declare_mask(ftm_italic,    0),
        declare_mask(ftm_underline, 1),
        declare_mask(ftm_strikeout, 2),
    };

public:
    string      name;
    int         size;
    int         escape;
    int         orient;
    int         weight; /* 0-9 */
    uint        mask;

private:
    friend class fsys_win32;
    friend class fsys_dwrite;
    mutable uint sysfont;

public:
    font()
    {
        size = 0;
        escape = 0;
        orient = 0;
        weight = 3;
        mask = 0;
        sysfont = 0;
    }
    font(const font& that)
    {
        name    = that.name;
        size    = that.size;
        escape  = that.escape;
        orient  = that.orient;
        weight  = that.weight;
        mask    = that.mask;
        sysfont = that.sysfont;
    }
    font(const gchar* n, int sz)
    {
        name.assign(n);
        size = sz;
        escape = 0;
        orient = 0;
        weight = 3;
        mask = 0;
        sysfont = 0;
    }
    font& operator = (const font& that)
    {
        name    = that.name;
        size  = that.size;
        escape  = that.escape;
        orient  = that.orient;
        weight  = that.weight;
        mask    = that.mask;
        return *this;
    }
    bool operator == (const font& that) const
    {
        if(name != that.name)
            return false;
        if(size != that.size)
            return false;
        if(escape != that.escape)
            return false;
        if(orient != that.orient)
            return false;
        if(weight != that.weight)
            return false;
        if(mask != that.mask)
            return false;
        return true;
    }
    bool operator != (const font& that) const
    {
        if(name != that.name)
            return true;
        if(size != that.size)
            return true;
        if(escape != that.escape)
            return true;
        if(orient != that.orient)
            return true;
        if(weight != that.weight)
            return true;
        if(mask != that.mask)
            return true;
        return false;
    }
    size_t hash_value() const
    {
        hasher h;
        h.add_bytes((const byte*)name.c_str(), name.length() * sizeof(gchar));
        h.add_bytes((const byte*)&size, sizeof(size));
        h.add_bytes((const byte*)&escape, sizeof(escape));
        h.add_bytes((const byte*)&orient, sizeof(orient));
        h.add_bytes((const byte*)&weight, sizeof(weight));
        return h.add_bytes((const byte*)&mask, sizeof(mask));
    }
};

struct ariel_export viewport
{
    float           left;
    float           top;
    float           width;
    float           height;
    float           min_depth;
    float           max_depth;
};

struct ariel_export axis_aligned_bound_box
{
    float           left = FLT_MAX;
    float           right = -FLT_MAX;
    float           top = FLT_MAX;
    float           bottom = -FLT_MAX;
    float           front = FLT_MAX;
    float           back = -FLT_MAX;

public:
    void reset()
    {
        left = top = front = FLT_MAX;
        right = bottom = back = -FLT_MAX;
    }
    float width() const { return right - left; }
    float height() const { return bottom - top; }
    float depth() const { return back - front; }
};

struct ariel_export origin_bound_sphere
{
    float           radius = 0.f;
};

struct ariel_export bound_sphere
{
    vec3            origin;
    float           radius = 0.f;
};

enum res_type
{
    res_mesh,
};

class __gs_novtable ariel_export res_node abstract
{
public:
    virtual ~res_node() {}
    virtual res_type get_type() const = 0;
    virtual const string& get_name() const { return _name; }
    virtual bool has_name() const { return !_name.empty(); }

protected:
    string          _name;

public:
    void set_name(const string& name) { _name = name; }
};

__ariel_end__

namespace std {

template<>
class hash<gs::ariel::font>
#if defined(_MSC_VER) && (_MSC_VER < 1914)
    : public unary_function<gs::ariel::font, size_t>
#endif
{
public:
    size_t operator()(const gs::ariel::font& ft) const { return ft.hash_value(); }
};

};

#endif


#pragma once

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

size_t nn_utils_sizeof_gsl_matrix(void);
size_t nn_utils_sizeof_gsl_vector(void);


#ifndef RAYS_H
#define RAYS_H

#include <complex.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <malloc.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>

#include "tsvread.h"

// A ray.
typedef struct {
  double p0[3];           // position
  double e0[3];           // direction
  complex double pol[3];  // polarization
  double OPL;             // optical path length
  double lambda;          // wavelength
  double intensity;       // intensity of this paricular ray
  double g;               // apodisation factor
} ray_t;

ray_t *readrays(char *filename, unsigned int *nrays);
void movefocus(ray_t *ray, unsigned int nrays);

#endif  // RAYS_H