//===--- Sudoku/Solver_remove_option.h                                  ---===//
//
// Helper functions, to remove options
//===----------------------------------------------------------------------===//
#pragma once

#include "Sudoku/Board.h"
#include "Sudoku/Location.h"
#include "Sudoku/Location_Utilities.h"
#include "Sudoku/Options.h"
#include "Sudoku/Size.h"
#include "Sudoku/Value.h"
#include "Sudoku/exceptions.h"
#include "Sudoku/traits.h"
#include <gsl/gsl>

#include <vector>

#include <algorithm>   // none_of
#include <type_traits> // is_base_of

#include "Solver.fwd.h" // Forward declaration - single_option

#include <cassert>

/* experiment flags */
//! DO #undef at end of file!
// activate algorithms on removing option
#define DUAL_ON_REMOVE false     // NOLINT(cppcoreguidelines-macro-usage)
#define MULTIPLE_ON_REMOVE false // NOLINT(cppcoreguidelines-macro-usage)

namespace Sudoku
{
//===----------------------------------------------------------------------===//
template<int N, typename Options = Options<elem_size<N>>>
int remove_option(Board<Options, N>&, Location<N>, Value);
template<int N, typename Options = Options<elem_size<N>>>
int remove_option(Board<Options, N>&, Location<N>, Options mask);

template<int N, typename Options = Options<elem_size<N>>, typename SectionT>
int remove_option_section(
	Board<Options, N>&, SectionT, Location<N> ignore, Value);
template<int N, typename Options = Options<elem_size<N>>, typename SectionT>
int remove_option_section(
	Board<Options, N>&,
	SectionT,
	const std::vector<Location<N>>& ignore,
	Value);
template<int N, typename Options = Options<elem_size<N>>, typename SectionT>
int remove_option_section(
	Board<Options, N>&,
	SectionT,
	const std::vector<Location<N>>& ignore,
	const Options& values);

template<int N, typename Options = Options<elem_size<N>>, typename SectionT>
int remove_option_outside_block(
	Board<Options, N>&, SectionT, Location<N> block_loc, Value);

//===----------------------------------------------------------------------===//

//	remove option from element, make answer if last option
template<int N, typename Options>
int remove_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc,
	const Value value)
{
	assert(is_valid<N>(value));

	int changes{};
	auto& item{board.at(loc)};

	if (item.test(value))
	{ // repeated answer in section (faster here)
		if (!is_option(item, value))
			throw error::invalid_Board();

		++changes;
		const auto count = item.remove_option(value).count();
		assert(count > 0); // never trigger, removed last option

#if MULTIPLE_ON_REMOVE == false
		if (count == 1)
		{
			changes += single_option(board, loc);
		}
#else
		// if (count < 4)
		{
			changes += multi_option(board, loc, count);
		}
#endif // multiple
#if DUAL_ON_REMOVE == true
		if (count == 2)
		{
			changes += dual_option(board, loc);
		}
#endif // dual
	}
	return changes;
}

// remove all options given by the mask
template<int N, typename Options>
int remove_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc,
	const Options mask)
{
	assert(is_answer_fast(mask)); // don't remove answer-bit
	assert(!mask.is_empty());     // useless

	auto& item{board.at(loc)};
	auto changes = gsl::narrow_cast<int, size_t>(item.count_all());
	if (!changes)
	{ // ignore empty elements
		return 0;
	}

	// remove options
	item -= mask;

	const auto count = gsl::narrow_cast<int, size_t>(item.count_all());
	if (count == 0)
	{ // removed last option
		throw error::invalid_Board();
	}
	changes -= count;

	if (changes && count == 1)
	{
		changes += single_option(board, loc);
	}
	return changes;
}

//	remove [value] in [ignore] from other elements in set
template<int N, typename Options, typename SectionT>
int remove_option_section(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const SectionT section,
	const Location<N> ignore,
	const Value value)
{
	{
		static_assert(Board_Section::traits::is_Section_v<SectionT>);
		static_assert(std::is_same_v<Options, typename SectionT::value_type>);
		static_assert(traits::is_input<typename SectionT::iterator>);
		assert(is_valid(ignore));
		assert(is_valid<N>(value));
		assert(is_same_section(section, ignore));
		assert(is_answer(board.at(ignore), value)); // first set as answer!
	}
	int changes{0};
	const auto end = section.cend();

	for (auto itr = section.cbegin(); itr != end; ++itr)
	{
		if (itr.location() != ignore)
		{
			changes += remove_option(board, itr.location(), value);
		}
	}
	return changes;
}

//	remove [value] from set if not part of [loc]s
template<int N, typename Options, typename SectionT>
inline int remove_option_section(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const SectionT section,
	const std::vector<Location<N>>& ignore,
	const Value value)
{
	{
		static_assert(Board_Section::traits::is_Section_v<SectionT>);
		static_assert(std::is_same_v<Options, typename SectionT::value_type>);
		static_assert(traits::is_input<typename SectionT::iterator>);
		assert(is_valid(ignore));
		assert(is_valid<N>(value));
		assert(is_same_section(section, ignore));
	}
	int changes{0};
	const auto begin = ignore.cbegin();
	const auto end   = ignore.cend();

	// TODO maybe faster to run get_same_row/col/block first?
	// and than not to check is_option(), since it is already in remove_option

	for (auto itr = section.cbegin(); itr != section.cend(); ++itr)
	{
		// TODO is the is_option check really faster?
		if (std::none_of(
				begin,
				end,
				[L1 = itr.location()](Location<N> L2) { return L1 == L2; }) &&
			is_option(*itr, value)) // <algorithm>
		{
			changes += remove_option(board, itr.location(), value);
		}
	}
	return changes;
}

// remove [value]s from [section] if not part of [loc]s
// the ignore Location vector must be sorted
template<int N, typename Options, typename SectionT>
int remove_option_section(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const SectionT section,
	const std::vector<Location<N>>& ignore,
	const Options& values)
{
	{
		static_assert(Board_Section::traits::is_Section_v<SectionT>);
		static_assert(std::is_same_v<Options, typename SectionT::value_type>);
		static_assert(traits::is_input<typename SectionT::iterator>);
		assert(is_valid(ignore));
		assert(!values.all());
		assert(!values.is_empty());
		assert(is_same_section(section, ignore));
	}
	int changes{0};
	const auto begin = ignore.cbegin();
	const auto end   = ignore.cend();

	for (auto itr = section.cbegin(); itr != section.cend(); ++itr)
	{
		if (std::none_of(
				begin,
				end,
				[L1 = itr.location()](Location<N> L2) { return L1 == L2; }) &&
			not(itr->is_answer())) // <algorithm>
		{
			changes += remove_option(board, itr.location(), values);
		}
	}
	return changes;
}

//	remove [value] from elements not in same block as [block_loc]
template<int N, typename Options, typename SectionT>
int remove_option_outside_block(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const SectionT section,
	const Location<N> block_loc,
	const Value value)
{
	{
		static_assert(Board_Section::traits::is_Section_v<SectionT>);
		static_assert(
			!Board_Section::traits::is_Block_v<SectionT>,
			"remove_option_outside_block is useless on a Block");
		static_assert(std::is_same_v<Options, typename SectionT::value_type>);
		static_assert(traits::is_input<typename SectionT::iterator>);
		assert(is_valid(block_loc));
		assert(is_valid<N>(value));
		assert(intersect_block(section, block_loc));
	}
	int changes{0};
	const auto end = section.cend();

	for (auto itr = section.cbegin(); itr != end; ++itr)
	{
		if (not is_same_block(itr.location(), block_loc))
		{
			changes += remove_option(board, itr.location(), value);
		}
	}
	return changes;
}

/*	Remove local macros */
#undef DUAL_ON_REMOVE
#undef MULTIPLE_ON_REMOVE

} // namespace Sudoku


/* movstat/gsl_movstat.h
 *
 * Copyright (C) 2018 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.
 */

#ifndef __GSL_MOVSTAT_H__
#define __GSL_MOVSTAT_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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.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 enum
{
  GSL_MOVSTAT_END_PADZERO,
  GSL_MOVSTAT_END_PADVALUE,
  GSL_MOVSTAT_END_TRUNCATE
} gsl_movstat_end_t;

/* accumulator struct
 * size          - return number of bytes needed for accumulator with maximum of n elements
 * init          - initialize accumulator state
 * insert        - insert a single sample into accumulator; if there are already n
 *                 samples in accumulator, oldest sample is overwritten
 * delete_oldest - delete oldest sample from accumulator
 * get           - return accumulated value
 */
typedef struct
{
  size_t (*size) (const size_t n);
  int (*init) (const size_t n, void * vstate);
  int (*insert) (const double x, void * vstate);
  int (*delete_oldest) (void * vstate);
  int (*get) (void * params, double * result, const void * vstate);
} gsl_movstat_accum;

typedef struct
{
  double (* function) (const size_t n, double x[], void * params);
  void * params;
} gsl_movstat_function;

#define GSL_MOVSTAT_FN_EVAL(F,n,x) (*((F)->function))((n),(x),(F)->params)

/* workspace for moving window statistics */

typedef struct
{
  size_t H;          /* number of previous samples in window */
  size_t J;          /* number of after samples in window */
  size_t K;          /* window size K = H + J + 1 */
  double *work;      /* workspace, size K */
  void *state;       /* state workspace for various accumulators */
  size_t state_size; /* bytes allocated for 'state' */
} gsl_movstat_workspace;

/* alloc.c */

GSL_FUN gsl_movstat_workspace *gsl_movstat_alloc(const size_t K);
GSL_FUN gsl_movstat_workspace *gsl_movstat_alloc2(const size_t H, const size_t J);
GSL_FUN gsl_movstat_workspace *gsl_movstat_alloc_with_size(const size_t accum_state_size, const size_t H, const size_t J);
GSL_FUN void gsl_movstat_free(gsl_movstat_workspace * w);

/* apply.c */
GSL_FUN int gsl_movstat_apply_accum(const gsl_movstat_end_t endtype, const gsl_vector * x,
                            const gsl_movstat_accum * accum, void * accum_params,
                            gsl_vector * y, gsl_vector * z,
                            gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_apply(const gsl_movstat_end_t endtype, const gsl_movstat_function * F,
                      const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);

/* fill.c */
GSL_FUN size_t gsl_movstat_fill(const gsl_movstat_end_t endtype, const gsl_vector * x, const size_t idx,
                        const size_t H, const size_t J, double * window);

GSL_FUN int gsl_movstat_mean(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_variance(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_sd(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_median(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_min(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_max(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_minmax(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y_min, gsl_vector * y_max, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_mad0(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * xmedian,
                     gsl_vector * xmad, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_mad(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * xmedian,
                    gsl_vector * xmad, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_qqr(const gsl_movstat_end_t endtype, const gsl_vector * x, const double q,
                    gsl_vector * xqqr, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_Sn(const gsl_movstat_end_t endtype, const gsl_vector * x,
                   gsl_vector * xscale, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_Qn(const gsl_movstat_end_t endtype, const gsl_vector * x,
                   gsl_vector * xscale, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_sum(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);

/* accumulator variables */

GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_mad;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_max;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_mean;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_median;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_min;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_minmax;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_sd;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_Sn;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_sum;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_Qn;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_qqr;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_userfunc;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_variance;

__END_DECLS

#endif /* __GSL_MOVSTAT_H__ */


#ifndef __TEST_MAIN_H__
#define __TEST_MAIN_H__

#define PY_SSIZE_T_CLEAN
#include <Python.h>
#include <stdlib.h>
#include <math.h>
#include <gsl_vector.h>
#include "common.h"
#include "svm_main.h"
#include "kernels.h"

// returns predictions and true values
PyObject* __test_t(PyObject* training, PyObject* testing, PyObject* alphas, double b, int rbf);

#endif  /* __TEST_MAIN_H__ */


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

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

#ifdef SUBFIND

#include "fof.h"
#include "subfind.h"

#ifdef PERIODIC
#define NEAREST(x) (((x)>boxhalf)?((x)-boxsize):(((x)<-boxhalf)?((x)+boxsize):(x)))
#else
#define NEAREST(x) (x)
#endif

/*! Structure for communication during the density computation. Holds data that is sent to other processors.
 */
static struct SOdens_in
{
  MyDouble Pos[3];
  double R200;
  int NodeList[NODELISTLENGTH];
}
 *SOdensIn, *SOdensGet;

static struct SOdens_out
{
  double Mass;
#ifdef SO_VEL_DISPERSIONS
  double Vx200, Vy200, Vz200, Disp200;
#endif
}
 *SOdensResult, *SOdensOut;

static double *R200, *M200;

#ifdef SO_VEL_DISPERSIONS
static double *Vx200, *Vy200, *Vz200, *Disp200;
#endif

void subfind_overdensity(void)
{
  long long ntot;
  int i, j, ndone, ndone_flag, npleft, dummy, rep, iter;
  MyFloat *Left, *Right;
  char *Todo;
  int ngrp, sendTask, recvTask, place, nexport, nimport;
  double t0, t1, rguess, overdensity, Deltas[3], rhoback, z, omegaz, x, DeltaMean200, DeltaCrit200,
    DeltaTopHat;

  /* allocate buffers to arrange communication */

  Ngblist = (int *) mymalloc(NumPart * sizeof(int));
  All.BunchSize =
    (int) ((All.BufferSize * 1024 * 1024) / (sizeof(struct data_index) + sizeof(struct data_nodelist) +
					     sizeof(struct SOdens_in) + sizeof(struct SOdens_out) +
					     sizemax(sizeof(struct SOdens_in), sizeof(struct SOdens_out))));
  DataIndexTable = (struct data_index *) mymalloc(All.BunchSize * sizeof(struct data_index));
  DataNodeList = (struct data_nodelist *) mymalloc(All.BunchSize * sizeof(struct data_nodelist));

  Left = (MyFloat *) mymalloc(sizeof(MyFloat) * Ngroups);
  Right = (MyFloat *) mymalloc(sizeof(MyFloat) * Ngroups);
  R200 = (double *) mymalloc(sizeof(double) * Ngroups);
  M200 = (double *) mymalloc(sizeof(double) * Ngroups);
#ifdef SO_VEL_DISPERSIONS
  Vx200 = (double *) mymalloc(sizeof(double) * Ngroups);
  Vy200 = (double *) mymalloc(sizeof(double) * Ngroups);
  Vz200 = (double *) mymalloc(sizeof(double) * Ngroups);
  Disp200 = (double *) mymalloc(sizeof(double) * Ngroups);
#endif

  Todo = mymalloc(sizeof(char) * Ngroups);

  z = 1 / All.Time - 1;

  rhoback = 3 * All.Omega0 * All.Hubble * All.Hubble / (8 * M_PI * All.G);

  omegaz =
    All.Omega0 * pow(1 + z,
		     3) / (All.Omega0 * pow(1 + z, 3) + (1 - All.Omega0 - All.OmegaLambda) * pow(1 + z,
												 2) +
			   All.OmegaLambda);

  DeltaMean200 = 200.0;
  DeltaCrit200 = 200.0 / omegaz;

  x = omegaz - 1;
  DeltaTopHat = 18 * M_PI * M_PI + 82 * x - 39 * x * x;
  DeltaTopHat /= omegaz;

  Deltas[0] = DeltaMean200;	/* standard fixed overdensity with respect to background */
  Deltas[1] = DeltaTopHat;	/* tophat overdensity with respect to background */
  Deltas[2] = DeltaCrit200;	/* overdensity of 200 relative to critical, expressed relative to background density */

  for(rep = 0; rep < 3; rep++)	/* repeat for all three overdensity values */
    {
      for(i = 0; i < Ngroups; i++)
	{
	  if(Group[i].Nsubs > 0)
	    {
	      rguess = pow(All.G * Group[i].Mass / (100 * All.Hubble * All.Hubble), 1.0 / 3);

	      Right[i] = 3 * rguess;
	      Left[i] = 0;

	      Todo[i] = 1;
	    }
	  else
	    {
	      Todo[i] = 0;
	    }
	}

      iter = 0;

      /* we will repeat the whole thing for those groups where we didn't converge to a SO radius yet */
      do
	{
	  t0 = second();

	  i = 0;		/* begin with this index */

	  do
	    {
	      for(j = 0; j < NTask; j++)
		{
		  Send_count[j] = 0;
		  Exportflag[j] = -1;
		}

	      /* do local particles and prepare export list */

	      for(nexport = 0; i < Ngroups; i++)
		{
		  if(Todo[i])
		    {
		      R200[i] = 0.5 * (Left[i] + Right[i]);
		      if(subfind_overdensity_evaluate(i, 0, &nexport, Send_count) < 0)
			break;
		    }
		}

	      qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);

	      MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

	      for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
		{
		  Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
		  nimport += Recv_count[j];

		  if(j > 0)
		    {
		      Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
		      Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
		    }
		}

	      SOdensGet = (struct SOdens_in *) mymalloc(nimport * sizeof(struct SOdens_in));
	      SOdensIn = (struct SOdens_in *) mymalloc(nexport * sizeof(struct SOdens_in));

	      /* prepare particle data for export */
	      for(j = 0; j < nexport; j++)
		{
		  place = DataIndexTable[j].Index;

		  SOdensIn[j].Pos[0] = Group[place].Pos[0];
		  SOdensIn[j].Pos[1] = Group[place].Pos[1];
		  SOdensIn[j].Pos[2] = Group[place].Pos[2];
		  SOdensIn[j].R200 = R200[place];

		  memcpy(SOdensIn[j].NodeList,
			 DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));
		}

	      /* exchange data */
	      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
		{
		  sendTask = ThisTask;
		  recvTask = ThisTask ^ ngrp;

		  if(recvTask < NTask)
		    {
		      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
			{
			  /* get the data */
			  MPI_Sendrecv(&SOdensIn[Send_offset[recvTask]],
				       Send_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				       recvTask, TAG_DENS_A,
				       &SOdensGet[Recv_offset[recvTask]],
				       Recv_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				       recvTask, TAG_DENS_A, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
			}
		    }
		}

	      myfree(SOdensIn);
	      SOdensResult = (struct SOdens_out *) mymalloc(nimport * sizeof(struct SOdens_out));
	      SOdensOut = (struct SOdens_out *) mymalloc(nexport * sizeof(struct SOdens_out));

	      /* now do the locations that were sent to us */
	      for(j = 0; j < nimport; j++)
		subfind_overdensity_evaluate(j, 1, &dummy, &dummy);

	      if(i >= Ngroups)
		ndone_flag = 1;
	      else
		ndone_flag = 0;

	      MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

	      /* get the result */
	      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
		{
		  sendTask = ThisTask;
		  recvTask = ThisTask ^ ngrp;
		  if(recvTask < NTask)
		    {
		      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
			{
			  /* send the results */
			  MPI_Sendrecv(&SOdensResult[Recv_offset[recvTask]],
				       Recv_count[recvTask] * sizeof(struct SOdens_out),
				       MPI_BYTE, recvTask, TAG_DENS_B,
				       &SOdensOut[Send_offset[recvTask]],
				       Send_count[recvTask] * sizeof(struct SOdens_out),
				       MPI_BYTE, recvTask, TAG_DENS_B, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
			}
		    }
		}

	      /* add the result to the local particles */
	      for(j = 0; j < nexport; j++)
		{
		  place = DataIndexTable[j].Index;

		  M200[place] += SOdensOut[j].Mass;
		}

	      myfree(SOdensOut);
	      myfree(SOdensResult);
	      myfree(SOdensGet);
	    }
	  while(ndone < NTask);

	  /* do final operations on results */
	  for(i = 0, npleft = 0; i < Ngroups; i++)
	    {
	      if(Todo[i])
		{
		  overdensity = M200[i] / (4.0 * M_PI / 3.0 * R200[i] * R200[i] * R200[i]) / rhoback;

		  if((Right[i] - Left[i]) > 1.0e-4 * Left[i])
		    {
		      /* need to redo this group */
		      npleft++;

		      if(overdensity > Deltas[rep])
			Left[i] = R200[i];
		      else
			Right[i] = R200[i];

		      if(iter >= MAXITER - 10)
			{
			  printf
			    ("gr=%d task=%d  R200=%g Left=%g Right=%g Menclosed=%g Right-Left=%g\n   pos=(%g|%g|%g)\n",
			     i, ThisTask, R200[i], Left[i], Right[i],
			     M200[i], Right[i] - Left[i], Group[i].Pos[0], Group[i].Pos[1], Group[i].Pos[2]);
			  fflush(stdout);
			}
		    }
		  else
		    Todo[i] = 0;
		}
	    }

	  sumup_large_ints(1, &npleft, &ntot);

	  t1 = second();

	  if(ntot > 0)
	    {
	      iter++;

	      if(iter > 0 && ThisTask == 0)
		{
		  printf("SO iteration %d: need to repeat for %d%09d particles. (took %g sec)\n", iter,
			 (int) (ntot / 1000000000), (int) (ntot % 1000000000), timediff(t0, t1));
		  fflush(stdout);
		}

	      if(iter > MAXITER)
		{
		  printf("failed to converge in neighbour iteration in density()\n");
		  fflush(stdout);
		  endrun(1155);
		}
	    }
	}
      while(ntot > 0);

#ifdef SO_VEL_DISPERSIONS
      i = 0;			/* begin with this index */
      do
	{
	  for(j = 0; j < NTask; j++)
	    {
	      Send_count[j] = 0;
	      Exportflag[j] = -1;
	    }

	  /* do local particles and prepare export list */

	  for(nexport = 0; i < Ngroups; i++)
	    {
	      if(subfind_overdensity_evaluate_dispersion(i, 0, &nexport, Send_count) < 0)
		break;
	    }

	  qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);

	  MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

	  for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
	    {
	      Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
	      nimport += Recv_count[j];

	      if(j > 0)
		{
		  Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
		  Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
		}
	    }

	  SOdensGet = (struct SOdens_in *) mymalloc(nimport * sizeof(struct SOdens_in));
	  SOdensIn = (struct SOdens_in *) mymalloc(nexport * sizeof(struct SOdens_in));

	  /* prepare particle data for export */
	  for(j = 0; j < nexport; j++)
	    {
	      place = DataIndexTable[j].Index;

	      SOdensIn[j].Pos[0] = Group[place].Pos[0];
	      SOdensIn[j].Pos[1] = Group[place].Pos[1];
	      SOdensIn[j].Pos[2] = Group[place].Pos[2];
	      SOdensIn[j].R200 = R200[place];

	      memcpy(SOdensIn[j].NodeList,
		     DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));
	    }

	  /* exchange data */
	  for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	    {
	      sendTask = ThisTask;
	      recvTask = ThisTask ^ ngrp;

	      if(recvTask < NTask)
		{
		  if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		    {
		      /* get the data */
		      MPI_Sendrecv(&SOdensIn[Send_offset[recvTask]],
				   Send_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				   recvTask, TAG_DENS_A,
				   &SOdensGet[Recv_offset[recvTask]],
				   Recv_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				   recvTask, TAG_DENS_A, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		    }
		}
	    }

	  myfree(SOdensIn);
	  SOdensResult = (struct SOdens_out *) mymalloc(nimport * sizeof(struct SOdens_out));
	  SOdensOut = (struct SOdens_out *) mymalloc(nexport * sizeof(struct SOdens_out));

	  /* now do the locations that were sent to us */
	  for(j = 0; j < nimport; j++)
	    subfind_overdensity_evaluate_dispersion(j, 1, &dummy, &dummy);

	  if(i >= Ngroups)
	    ndone_flag = 1;
	  else
	    ndone_flag = 0;

	  MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

	  /* get the result */
	  for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	    {
	      sendTask = ThisTask;
	      recvTask = ThisTask ^ ngrp;
	      if(recvTask < NTask)
		{
		  if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		    {
		      /* send the results */
		      MPI_Sendrecv(&SOdensResult[Recv_offset[recvTask]],
				   Recv_count[recvTask] * sizeof(struct SOdens_out),
				   MPI_BYTE, recvTask, TAG_DENS_B,
				   &SOdensOut[Send_offset[recvTask]],
				   Send_count[recvTask] * sizeof(struct SOdens_out),
				   MPI_BYTE, recvTask, TAG_DENS_B, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		    }
		}
	    }

	  /* add the result to the local particles */
	  for(j = 0; j < nexport; j++)
	    {
	      place = DataIndexTable[j].Index;

	      Vx200[place] += SOdensOut[j].Vx200;
	      Vy200[place] += SOdensOut[j].Vy200;
	      Vz200[place] += SOdensOut[j].Vz200;
	      Disp200[place] += SOdensOut[j].Disp200;
	    }

	  myfree(SOdensOut);
	  myfree(SOdensResult);
	  myfree(SOdensGet);
	}
      while(ndone < NTask);
#endif

      for(i = 0; i < Ngroups; i++)
	{
	  if(Group[i].Nsubs > 0)
	    {
	      overdensity = M200[i] / (4.0 * M_PI / 3.0 * R200[i] * R200[i] * R200[i]) / rhoback;

	      if((overdensity - Deltas[rep]) > 0.1 * Deltas[rep])
		{
		  R200[i] = M200[i] = 0;
		}
	      else if(M200[i] < 5 * Group[i].Mass / Group[i].Len)
		{
		  R200[i] = M200[i] = 0;
		}
	    }
	  else
	    R200[i] = M200[i] = 0;

	  switch (rep)
	    {
	    case 0:
	      Group[i].M_Mean200 = M200[i];
	      Group[i].R_Mean200 = R200[i];
	      break;
	    case 1:
	      Group[i].M_TopHat200 = M200[i];
	      Group[i].R_TopHat200 = R200[i];
	      break;
	    case 2:
	      Group[i].M_Crit200 = M200[i];
	      Group[i].R_Crit200 = R200[i];
	      break;
	    }

#ifdef SO_VEL_DISPERSIONS
	  if(R200[i] > 0 && M200[i] > 0)
	    {
	      Vx200[i] /= M200[i];
	      Vy200[i] /= M200[i];
	      Vz200[i] /= M200[i];
	      Disp200[i] /= M200[i];
	      Disp200[i] -= (Vx200[i] * Vx200[i] + Vy200[i] * Vy200[i] + Vz200[i] * Vz200[i]);
	      Disp200[i] = sqrt(Disp200[i] / 3);	/* convert to 1D velocity dispersion */
	    }
	  else
	    {
	      Disp200[i] = 0;
	    }

	  switch (rep)
	    {
	    case 0:
	      Group[i].VelDisp_Mean200 = Disp200[i];
	      break;
	    case 1:
	      Group[i].VelDisp_TopHat200 = Disp200[i];
	      break;
	    case 2:
	      Group[i].VelDisp_Crit200 = Disp200[i];
	      break;
	    }
#endif
	}
    }

  myfree(Todo);
#ifdef SO_VEL_DISPERSIONS
  myfree(Disp200);
  myfree(Vz200);
  myfree(Vy200);
  myfree(Vx200);
#endif
  myfree(M200);
  myfree(R200);
  myfree(Right);
  myfree(Left);

  myfree(DataNodeList);
  myfree(DataIndexTable);
  myfree(Ngblist);
}

/*! This function represents the core of the SPH density computation. The
 *  target particle may either be local, or reside in the communication
 *  buffer.
 */
int subfind_overdensity_evaluate(int target, int mode, int *nexport, int *nsend_local)
{
  int startnode, listindex = 0;
  double h, mass, massret;
  MyDouble *pos;

  if(mode == 0)
    {
      pos = Group[target].Pos;
      h = R200[target];
    }
  else
    {
      pos = SOdensGet[target].Pos;
      h = SOdensGet[target].R200;
    }

  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = SOdensGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  mass = 0;

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  massret = subfind_ovderdens_treefind(pos, h, target, &startnode, mode, nexport, nsend_local);

	  if(massret < 0)
	    return -1;

	  mass += massret;
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = SOdensGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  if(mode == 0)
    M200[target] = mass;
  else
    SOdensResult[target].Mass = mass;

  return 0;
}

double subfind_ovderdens_treefind(MyDouble searchcenter[3], MyFloat hsml, int target, int *startnode,
				  int mode, int *nexport, int *nsend_local)
{
  int no, p, task, nexport_save;
  struct NODE *current;
  double mass;
  MyDouble dx, dy, dz, dist, r2;

#define FACT2 0.86602540
#ifdef PERIODIC
  MyDouble xtmp;
#endif
  nexport_save = *nexport;

  mass = 0;
  no = *startnode;

  while(no >= 0)
    {
      if(no < All.MaxPart)	/* single particle */
	{
	  p = no;
	  no = Nextnode[no];

	  dist = hsml;
	  dx = NGB_PERIODIC_LONG_X(P[p].Pos[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(P[p].Pos[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(P[p].Pos[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  if(dx * dx + dy * dy + dz * dz > dist * dist)
	    continue;

	  mass += P[p].Mass;
	}
      else
	{
	  if(no >= All.MaxPart + MaxNodes)	/* pseudo particle */
	    {
	      if(mode == 1)
		endrun(12312);

	      if(mode == 0)
		{
		  if(Exportflag[task = DomainTask[no - (All.MaxPart + MaxNodes)]] != target)
		    {
		      Exportflag[task] = target;
		      Exportnodecount[task] = NODELISTLENGTH;
		    }

		  if(Exportnodecount[task] == NODELISTLENGTH)
		    {
		      if(*nexport >= All.BunchSize)
			{
			  *nexport = nexport_save;
			  if(nexport_save == 0)
			    endrun(13005);	/* in this case, the buffer is too small to process even a single particle */
			  for(task = 0; task < NTask; task++)
			    nsend_local[task] = 0;
			  for(no = 0; no < nexport_save; no++)
			    nsend_local[DataIndexTable[no].Task]++;
			  return -1;
			}
		      Exportnodecount[task] = 0;
		      Exportindex[task] = *nexport;
		      DataIndexTable[*nexport].Task = task;
		      DataIndexTable[*nexport].Index = target;
		      DataIndexTable[*nexport].IndexGet = *nexport;
		      *nexport = *nexport + 1;
		      nsend_local[task]++;
		    }

		  DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]++] =
		    DomainNodeIndex[no - (All.MaxPart + MaxNodes)];

		  if(Exportnodecount[task] < NODELISTLENGTH)
		    DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]] = -1;
		}

	      no = Nextnode[no - MaxNodes];
	      continue;
	    }

	  current = &Nodes[no];

	  if(mode == 1)
	    {
	      if(current->u.d.bitflags & (1 << BITFLAG_TOPLEVEL))	/* we reached a top-level node again, which means that we are done with the branch */
		{
		  *startnode = -1;
		  return mass;
		}
	    }

	  no = current->u.d.sibling;	/* in case the node can be discarded */
	  dist = hsml + 0.5 * current->len;
	  dx = NGB_PERIODIC_LONG_X(current->center[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(current->center[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(current->center[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  /* now test against the minimal sphere enclosing everything */
	  dist += FACT1 * current->len;
	  if((r2 = (dx * dx + dy * dy + dz * dz)) > dist * dist)
	    continue;

	  if((current->u.d.bitflags & ((1 << BITFLAG_TOPLEVEL) + (1 << BITFLAG_DEPENDS_ON_LOCAL_MASS))) == 0)	/* only use fully local nodes */
	    {
	      /* test whether the node is contained within the sphere */
	      dist = hsml - FACT2 * current->len;
	      if(dist > 0)
		if(r2 < dist * dist)
		  {
		    mass += current->u.d.mass;
		    continue;
		  }
	    }

	  no = current->u.d.nextnode;	/* ok, we need to open the node */
	}
    }

  *startnode = -1;
  return mass;
}

#ifdef SO_VEL_DISPERSIONS

int subfind_overdensity_evaluate_dispersion(int target, int mode, int *nexport, int *nsend_local)
{
  int ngb, n, j, startnode, listindex = 0;
  double h, vx, vy, vz, v2, vvx, vvy, vvz;
  MyDouble *pos;
  double boxsize, boxhalf, vel_to_peculiar, H_of_a;

  boxsize = All.BoxSize;
  boxhalf = 0.5 * All.BoxSize;

  H_of_a = hubble_function(All.Time);
  vel_to_peculiar = 1.0 / All.Time;

  if(mode == 0)
    {
      pos = Group[target].Pos;
      h = R200[target];
    }
  else
    {
      pos = SOdensGet[target].Pos;
      h = SOdensGet[target].R200;
    }

  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = SOdensGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  vx = vy = vz = v2 = 0;

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  ngb = subfind_ovderdens_treefind_dispersion(pos, h, target, &startnode, mode, nexport, nsend_local);

	  if(ngb < 0)
	    return -1;

	  for(n = 0; n < ngb; n++)
	    {
	      j = Ngblist[n];

	      vvx = vel_to_peculiar * P[j].Vel[0] + H_of_a * All.Time * NEAREST(P[j].Pos[0] - pos[0]);
	      vvy = vel_to_peculiar * P[j].Vel[1] + H_of_a * All.Time * NEAREST(P[j].Pos[1] - pos[1]);
	      vvz = vel_to_peculiar * P[j].Vel[2] + H_of_a * All.Time * NEAREST(P[j].Pos[2] - pos[2]);

	      vx += P[j].Mass * vvx;
	      vy += P[j].Mass * vvy;
	      vz += P[j].Mass * vvz;

	      v2 += P[j].Mass * (vvx * vvx + vvy * vvy + vvz * vvz);
	    }
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = SOdensGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  if(mode == 0)
    {
      Disp200[target] = v2;
      Vx200[target] = vx;
      Vy200[target] = vy;
      Vz200[target] = vz;
    }
  else
    {
      SOdensResult[target].Disp200 = v2;
      SOdensResult[target].Vx200 = vx;
      SOdensResult[target].Vy200 = vy;
      SOdensResult[target].Vz200 = vz;
    }

  return 0;
}

int subfind_ovderdens_treefind_dispersion(MyDouble searchcenter[3], MyFloat hsml, int target, int *startnode,
					  int mode, int *nexport, int *nsend_local)
{
  int no, p, task, nexport_save, numngb;
  struct NODE *current;
  MyDouble dx, dy, dz, dist, r2;

#define FACT2 0.86602540
#ifdef PERIODIC
  MyDouble xtmp;
#endif
  nexport_save = *nexport;

  numngb = 0;
  no = *startnode;

  while(no >= 0)
    {
      if(no < All.MaxPart)	/* single particle */
	{
	  p = no;
	  no = Nextnode[no];

	  dist = hsml;
	  dx = NGB_PERIODIC_LONG_X(P[p].Pos[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(P[p].Pos[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(P[p].Pos[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  if(dx * dx + dy * dy + dz * dz > dist * dist)
	    continue;

	  Ngblist[numngb++] = p;
	}
      else
	{
	  if(no >= All.MaxPart + MaxNodes)	/* pseudo particle */
	    {
	      if(mode == 1)
		endrun(12312);

	      if(mode == 0)
		{
		  if(Exportflag[task = DomainTask[no - (All.MaxPart + MaxNodes)]] != target)
		    {
		      Exportflag[task] = target;
		      Exportnodecount[task] = NODELISTLENGTH;
		    }

		  if(Exportnodecount[task] == NODELISTLENGTH)
		    {
		      if(*nexport >= All.BunchSize)
			{
			  *nexport = nexport_save;
			  if(nexport_save == 0)
			    endrun(13005);	/* in this case, the buffer is too small to process even a single particle */
			  for(task = 0; task < NTask; task++)
			    nsend_local[task] = 0;
			  for(no = 0; no < nexport_save; no++)
			    nsend_local[DataIndexTable[no].Task]++;
			  return -1;
			}
		      Exportnodecount[task] = 0;
		      Exportindex[task] = *nexport;
		      DataIndexTable[*nexport].Task = task;
		      DataIndexTable[*nexport].Index = target;
		      DataIndexTable[*nexport].IndexGet = *nexport;
		      *nexport = *nexport + 1;
		      nsend_local[task]++;
		    }

		  DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]++] =
		    DomainNodeIndex[no - (All.MaxPart + MaxNodes)];

		  if(Exportnodecount[task] < NODELISTLENGTH)
		    DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]] = -1;
		}

	      no = Nextnode[no - MaxNodes];
	      continue;
	    }

	  current = &Nodes[no];

	  if(mode == 1)
	    {
	      if(current->u.d.bitflags & (1 << BITFLAG_TOPLEVEL))	/* we reached a top-level node again, which means that we are done with the branch */
		{
		  *startnode = -1;
		  return numngb;
		}
	    }

	  no = current->u.d.sibling;	/* in case the node can be discarded */
	  dist = hsml + 0.5 * current->len;
	  dx = NGB_PERIODIC_LONG_X(current->center[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(current->center[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(current->center[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  /* now test against the minimal sphere enclosing everything */
	  dist += FACT1 * current->len;
	  if((r2 = (dx * dx + dy * dy + dz * dz)) > dist * dist)
	    continue;

	  no = current->u.d.nextnode;	/* ok, we need to open the node */
	}
    }

  *startnode = -1;
  return numngb;
}

#endif

#endif


#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"
#include "error_cblas_l2.h"

void
cblas_zgbmv (const enum CBLAS_ORDER order, const enum CBLAS_TRANSPOSE TransA,
             const int M, const int N, const int KL, const int KU,
             const void *alpha, const void *A, const int lda, const void *X,
             const int incX, const void *beta, void *Y, const int incY)
{
#define BASE double
#include "source_gbmv_c.h"
#undef BASE
}


/*
  LODO library: laser-stabilized odometry
  Copyright (C) 2004 Andrew Howard  ahoward@usc.edu

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

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

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

/** @file lodo.h

@brief The lodo library provides laser-stabilizied odometric pose
estimates: the inputs are raw odometry and laser scans, the output is
a corrected pose estimate.  The drift in this corrected estimate is
much lower than that seen with odometry alone.  See, for example, the
figure below: this plot shows the trajectory of a robot that has
travelled 125m and executed 19 complete rotations before returning to
the starting location.  The final cumulative orientation error is less
than 5 degrees (versus 110 degrees for pure odometry).

@image html lodo_comparison.gif "Odometry versus laser-stabilized odometry"

@par How it works

The lodo library uses an incremental SLAM algorithm to correct drift
in the robot's orientation.  The algorithm has three key data structures:

- The current laser scan (a list of range values).
- The local map (a ring buffer containing recent laser scans).
- The polar error function (a grid generated from the local map).

The algorithm applied to each new laser scan is as follows:

-# Generate the polar error function:
  - For each scan in the local map:
    - For each point on the polygonal boundary of the map scan:
      - Project point into the local coordinate frame of the new scan.
      - Convert to polar coordinates and mark the corresponding cell
        in the polar error function as "occupied".
  - For each cell in the polar error function, compute the distance
    to the nearest occupied cell with the same radial value.
-# Find orientation correction:
  - For each possible correction:
    - Apply correction to each point in the new scan and convert into
      polar coordinates.
    - Look up error value in the polar error function.
    - Add to the todal error for this correction value.
  - Select correction value with the lowest total error.
-# Build map:
  - If the new scan has a signficant number of "outlier" points
    (large error values), add this scan to the current map.  The map
    is a ring buffer, so adding a new scan will necessarily lead to
    an old one being discarded.

The intuition behind this algorithm is best explained in diagrams (below).

Figure (a) shows a map scan in global carestian coordinates; the map
scan is projected into the coordinate frame of the new scan and
converted to polar coordinates (b); using dynamic programming, we
compute the error function (c).  This polar error function can be used
to test possible orientation corrections very efficiently: changes in
the orientation of the new scan correspond the translations of the
polar error function.

 @image html lodo_map.gif "(a) Original map scan (cartesian coordinates)."
 @image html lodo_map_polar.gif "(b) Map scan in local polar coordinates."

 @image html lodo_map_pef.gif "(c) Calculated polar error function."

@par lodo_caveats

The current version of the library takes about 15ms to process each
scan on a 2.8 GHz P4, so expect it to use lots of cycles on your
robot.  There is still lots of optimization to do, however, so expect
future releases to clock in around the 5ms mark.

@todo Accuracy, reliability and performance

*/

#ifndef LODO_H
#define LODO_H

#include <gsl/gsl_min.h>
#include "slap.h"

#ifdef __cplusplus
extern "C"
{
#endif

/// Limits
#define LODO_MAX_RANGES 1024

/// @brief Data pertaining to an individual scan
typedef struct
{
  /// Odometric pose (x, y, theta)
  pose2_t opose;

  /// Corrected pose (x, y, theta)
  pose2_t cpose;

  /// Range values
  double ranges[LODO_MAX_RANGES];

} lodo_scan_t;

/// @brief Working data for a map point
typedef struct
{
  /// Local cartesian position
  vector3_t local;

  /// Local polar position
  vector3_t polar;

} lodo_map_point_t;

/// @brief Working data for scan point
typedef struct
{
  /// Range bin in PEF
  int ni;

} lodo_scan_point_t;

/// @brief Laser odometry module data
typedef struct
{
  /// Number of points in each scan
  int num_ranges;

  /// Maximum accepted range
  double range_max;

  /// Start angle for scans
  double range_start;

  /// Angular resolution of scans
  double range_step;

  /// Laser pose relative to robot
  pose2_t laser_pose;

  /// Maximum error value on a single point (m)
  double max_error;

  /// Number of bins in polar error function
  int pef_num_ranges, pef_num_bearings;

  /// Range bin starting value and resolution in polar error function.
  double pef_range_start, pef_range_step;

  /// Bearing bin starting value and resolution in polar error
  /// function.  Note that the pef_bearing_step measures a distance,
  /// not an angle.
  double pef_bearing_start, pef_bearing_step;

  /// Polar error function (map)
  int pef_size;
  int *pef;

  /// Search interval for fitting (fitting will check interval
  /// [-fit_interval, +fit_interval] radians).
  double fit_interval;

  /// Error threshold for good fits (error must be less than this
  /// value for a good fit).
  double fit_err_thresh;

  /// Error value for outlier points (points with error values larger
  /// than this are labeled as outliers).
  double fit_outlier_dist;

  /// Outlier fraction threshold (used to decide when scans should be
  /// added to the map).
  double fit_outlier_frac;

  /// Cumulative odometric distance and rotation
  double odom_dist, odom_turn;

  /// Odometer interval for adding scans.
  double map_dist_interval, map_turn_interval;

  /// Odometer value of last scan in map
  double map_last_dist, map_last_turn;

  /// Map (ring buffer of scans)
  int map_scan_count, max_map_scans;
  lodo_scan_t *map_scans;

  /// Current scan
  int scan_count;
  lodo_scan_t scan;

  /// Working space for current scan (useful pre-computed values).
  lodo_scan_point_t scan_points[LODO_MAX_RANGES];

  /// Working space for projected map points
  int num_map_points, max_map_points;
  lodo_map_point_t *map_points;

  /// Minimizer
  gsl_min_fminimizer *mini;

  /// Fit correction
  double fit_correct;

  /// True if last scan fitted
  int fit_valid;

  /// True if the last scan should be added to the map
  int fit_add;

} lodo_t;

/// @brief Allocate object
/// @param num_ranges Number of range readings in each scan (should be 181).
/// @param range_max Maximum useable range value (e.g., 8.00 or 16.00).
/// @param range_res Resolution for comparing range values (i.e., range bin width).
/// @param range_start Starting angle for range readings (should be -M_PI / 2).
/// @param range_step Angular step size for each successive range reading
///                   (should be M_PI / 180).
/// @returns Object handle.
lodo_t *lodo_alloc(int num_ranges, double range_max, double range_res,
                   double range_start, double range_step);

/// @brief Free object
void lodo_free(lodo_t *self);

/// @brief Add a scan, compute correction, update map
/// @param self Object handle.
/// @param odom_pose Raw odometric pose of the robot.
/// @param num_ranges Number of range readings.
/// @param ranges Array of laser range readings.
/// @returns Returns the corrected robot pose.
pose2_t lodo_add_scan(lodo_t *self, pose2_t odom_pose, int num_ranges, double *ranges);

/// @brief Project map points into scan polar coordinates
/// @internal
void lodo_project_map(lodo_t *self);

/// @brief Project map free space into the scan polar frame
/// @internal
void lodo_project_map_free(lodo_t *self, lodo_scan_t *scan_m,
                           matrix33_t Pd, matrix33_t Pm);

/// @brief Compute correction for a scan
/// @internal
double lodo_correct(lodo_t *self);

/// @brief Test a scan offset
/// @internal
double lodo_test_offset(lodo_t *self, double offset, double *outliers);

/// @brief Print projected map points
void lodo_print_map(lodo_t *self);

/// @brief Print a polar error functions for scan and map
void lodo_print_pef(lodo_t *self);

/// @brief Print error histograms
void lodo_print_err(lodo_t *self);

/// @brief Draw a scan
void lodo_draw_scan(lodo_t *self, lodo_scan_t *scan);

/// @brief Draw the current map (hits)
void lodo_draw_map(lodo_t *self);

#ifdef __cplusplus
}
#endif

#endif


/*
 *  vbHmmTs.c
 *  Model-specific core functions for VB-HMM-TS.
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2015
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.1.0
 *  Last modified on 2015.09.17
 */

#include "vbHmmTs.h"
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <string.h>
#include "rand.h"

#ifdef _OPENMP
#include "omp.h"
#endif

//// Uncomment one/both of the following defenitions to activate constraint on I or/and K.
//#define  INTENSITY_CAP
//#define  TRANSITION_RATE_CAP
#ifdef INTENSITY_CAP
#define  maxIntensityRatio  10.0
#endif
#ifdef TRANSITION_RATE_CAP
#define  minPhotonNumPerState  10.0
#endif

#define  MAX(a,b)  ((a)>(b)?(a):(b))
#define  MIN(a,b)  ((a)<(b)?(a):(b))

//static int isGlobalAnalysis = 0;

void setFunctions_ts(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_ts;
    funcs.freeModelParameters   = freeModelParameters_ts;
    funcs.newModelStats         = newModelStats_ts;
    funcs.freeModelStats        = freeModelStats_ts;
    funcs.initializeVbHmm       = initializeVbHmm_ts;
    funcs.pTilde_z1             = pTilde_z1_ts;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_ts;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_ts;
    funcs.calcStatsVars         = calcStatsVars_ts;
    funcs.maximization          = maximization_ts;
    funcs.varLowerBound         = varLowerBound_ts;
    funcs.reorderParameters     = reorderParameters_ts;
    funcs.outputResults         = outputResults_ts;
    setFunctions( funcs );
}

//void setGFunctions_ts(){
//    gCommonFunctions funcs;
//    funcs.newModelParameters    = newModelParameters_ts;
//    funcs.freeModelParameters   = freeModelParameters_ts;
//    funcs.newModelStats         = newModelStats_ts;
//    funcs.freeModelStats        = freeModelStats_ts;
//    funcs.newModelStatsG        = newModelStatsG_ts;
//    funcs.freeModelStatsG       = freeModelStatsG_ts;
//    funcs.initializeVbHmmG      = initializeVbHmmG_ts;
//    funcs.pTilde_z1             = pTilde_z1_ts;
//    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_ts;
//    funcs.pTilde_xn_zn          = pTilde_xn_zn_ts;
//    funcs.calcStatsVarsG        = calcStatsVarsG_ts;
//    funcs.maximizationG         = maximizationG_ts;
//    funcs.varLowerBoundG        = varLowerBoundG_ts;
//    funcs.reorderParametersG    = reorderParametersG_ts;
//    funcs.outputResultsG        = outputResultsG_ts;
//    setGFunctions( funcs );
//    isGlobalAnalysis = 1;
//}

void outputResults_ts( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    outputTsResults( xn, gv, iv, logFP );
}

//void outputResultsG_ts( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//    outputTsResultsG( xns, gv, ivs, logFP );
//}

void *newModelParameters_ts( xn, sNo )
xnDataSet *xn;
int sNo;
{
    int i;
    tsParameters *p = (void*)malloc( sizeof(tsParameters) );

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    // hyperparameter for p( k(i,j) ) (i != j)
    p->uKMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uKMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUKArr = (double*)malloc( sNo * sizeof(double) );
    // hyperparameter for p( I(k) )
    p->aIArr = (double*)malloc( sNo * sizeof(double) );
    p->bIArr = (double*)malloc( sNo * sizeof(double) );

    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgI = (double *)malloc( sNo * sizeof(double) );
    p->avgLnI = (double *)malloc( sNo * sizeof(double) );
    p->avgK = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnK = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgK[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnK[i] = (double *)malloc( sNo * sizeof(double) );
    }
    p->avgLnKI = (double *)malloc( sNo * sizeof(double) );

    return p;
}

void freeModelParameters_ts( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    tsParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uKMat[i] );
    }
    free( gp->uKMat );
    free( gp->sumUKArr );
    free( gp->aIArr );
    free( gp->bIArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    free( gp->avgI );
    free( gp->avgLnI );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgK[i] );
        free( gp->avgLnK[i] );
    }
    free( gp->avgK );
    free( gp->avgLnK );
    free( gp->avgLnKI );

    free( *p );
    *p = NULL;
}

void *newModelStats_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        tsStats *s = (tsStats*)malloc( sizeof(tsStats) );

        int i;
        s->Ni = (double *)malloc( sNo * sizeof(double) );
        s->Ti = (double *)malloc( sNo * sizeof(double) );
        s->Nii = (double *)malloc( sNo * sizeof(double) );
        s->Nij = (double *)malloc( sNo * sizeof(double) );
        s->Mij = (double **)malloc( sNo * sizeof(double*) );
        for( i = 0 ; i < sNo ; i++ )
        {   s->Mij[i] = (double *)malloc( sNo * sizeof(double) );   }

        return s;

//    } else {
//
//        return NULL;
//
//    }
}

void freeModelStats_ts( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        tsStats *gs = *s;
        int i;
        free( gs->Ni );
        free( gs->Ti );
        free( gs->Nii );
        free( gs->Nij );
        for( i = 0 ; i < sNo ; i++ )
        {   free( gs->Mij[i] );   }
        free( gs->Mij );

        free( gs );
        *s = NULL;
//    }
}

//void *newModelStatsG_ts( xns, gv, ivs)
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    tsGlobalStats *gs = (tsGlobalStats*)malloc( sizeof(tsGlobalStats) );
//
//    return gs;
//}

//void freeModelStatsG_ts( gs, xns, gv, ivs )
//void **gs;
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//
//    free( *gs );
//    *gs = NULL;
//}

void initializeVbHmm_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsData *d = xn->data;
    int sNo = gv->sNo;
    tsParameters *p = gv->params;

    int i, j;

    // hyperparameter for p( pi(k) )
    p->sumUPi = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        p->uPiArr[i] = 1.0;
        p->sumUPi += p->uPiArr[i];
    }

    // hyperparameter for p( k(i,j) ) (i != j)
    for( i = 0 ; i < sNo ; i++ ){
        p->sumUKArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            p->uKMat[i][j] = 1.0;
            if( j != i ){
                p->sumUKArr[i] += p->uKMat[i][j];
            }
        }
    }

    double meanI = (double)xn->N / d->T;

    // hyperparameter for p( I(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->aIArr[i] = 1.0;
        p->bIArr[i] = 1.0 / meanI;
    }

    initialize_indVars_ts( xn, gv, iv );

    calcStatsVars_ts( xn, gv, iv );
    maximization_ts( xn, gv, iv );
}

//void initializeVbHmmG_ts( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void initialize_indVars_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;

    int i;
    size_t n;
    double sumPar;
    for( n = 0 ; n < dLen ; n++ ){
        sumPar = 0.0;
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] = enoise(1.0) + 1.0;
            sumPar += gmMat[n][i];
        }
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] /= sumPar;
        }
    }
}

xnDataSet *newXnDataSet_ts( filename )
const char *filename;
{
    xnDataSet *xn = (xnDataSet*)malloc( sizeof(xnDataSet) );
    xn->name = (char*)malloc( strlen(filename) + 2 );
    strncpy( xn->name, filename, strlen(filename)+1 );
    xn->data = (tsData*)malloc( sizeof(tsData) );
    tsData *d = (tsData*)xn->data;
    d->T = 0.0;
    d->dt = NULL;
    d->time = NULL;
    return xn;
}

void freeXnDataSet_ts( xn )
xnDataSet **xn;
{
    tsData *d = (tsData*)(*xn)->data;
    free( d->dt );
    free( d->time );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

double pTilde_z1_ts( i, params )
int i;
void *params;
{
    tsParameters *p = (tsParameters*)params;
    return exp( p->avgLnPi[i] );
}

double pTilde_zn_zn1_ts( i, j, params )
int i, j;
void *params;
{
    tsParameters *p = (tsParameters*)params;
    if( i == j ){
        return exp( p->avgLnI[i] - p->avgLnKI[i] );
    } else {
        return exp( p->avgLnK[i][i] + p->avgLnK[i][j] - p->avgLnKI[i] );
    }
}

double pTilde_xn_zn_ts( xnWv, n, i, params )
xnDataSet *xnWv;
size_t n;
int i;
void *params;
{
    tsParameters *p = (tsParameters*)params;
    tsData *d = (tsData*)xnWv->data;
    return exp( p->avgLnI[i] - p->avgI[i] * d->dt[n] );
}

void calcStatsVars_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsData *d = (tsData*)xn->data;
    tsStats *s = (tsStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, *Ti = s->Ti;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i] = 1e-10;
        Ti[i] = 1e-10;
        Nii[i] = 0.0;
        Nij[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            Mij[i][j] = 1e-10;
        }
    }
    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){
            Ni[i]  += gmMat[n][i];
            Ti[i]  += gmMat[n][i] * d->dt[n];
            Nii[i] += xiMat[n][i][i];
            for( j = 0 ; j < sNo ; j++ ){
                if( j != i ){
                    Mij[i][j] += xiMat[n][i][j];
                    Nij[i]    += xiMat[n][i][j];
                }
            }
        }
    }
    for( i = 0 ; i < sNo ; i++ ){
        Nii[i] = MAX( Nii[i], 1.0 );
        Nij[i] = MAX( Nij[i], 1.0 );
    }
}

//void calcStatsVarsG_ts( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void maximization_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsParameters *p = (tsParameters*)gv->params;
    tsStats *s = (tsStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgK = p->avgK, **avgLnK = p->avgLnK;
    double *avgLnKI = p->avgLnKI, *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uKMat = p->uKMat, *sumUKArr = p->sumUKArr;
    double *Ni = s->Ni, *Ti = s->Ti;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        avgPi[i] = ( uPiArr[i] + gmMat[0][i] ) / ( sumUPi + 1.0 );
        avgLnPi[i] = gsl_sf_psi( uPiArr[i] + gmMat[0][i] ) - gsl_sf_psi( sumUPi + 1.0 );

        avgK[i][i] = (Ni[i] + aIArr[i]) * Nij[i] / (Nii[i] - 1.0) / (Ti[i] + bIArr[i]);
        avgLnK[i][i]  = gsl_sf_psi(Ni[i] + aIArr[i]) + gsl_sf_psi(Nij[i]);
        avgLnK[i][i] += -gsl_sf_psi(Nii[i]) - log(Ti[i] + bIArr[i]);

        avgI[i] = (Ni[i] + aIArr[i]) / (Ti[i] + bIArr[i]);
        avgLnI[i] = gsl_sf_psi(Ni[i] + aIArr[i]) - log(Ti[i] + bIArr[i]);

        avgLnKI[i]  = gsl_sf_psi(Ni[i] + aIArr[i]) + gsl_sf_psi(Nii[i] + Nij[i]);
        avgLnKI[i] += -gsl_sf_psi(Nii[i]) - log(Ti[i] + bIArr[i]);

#ifdef INTENSITY_CAP
        double meanI = (double)xnWv->N / xnWv->T;
        avgI[i]  = MIN( avgI[i], maxIntensityRatio * meanI );
        avgLnI[i]  = MIN( avgLnI[i], log(maxIntensityRatio * meanI) );
#endif
#ifdef TRANSITION_RATE_CAP
        avgK[i][i] = MIN( avgK[i][i], avgI[i]/minPhotonNumPerState );
        avgLnK[i][i]  = MIN( avgLnK[i][i], avgLnI[i] - log(minPhotonNumPerState) );
        avgLnKI[i]  = MIN( avgLnKI[i], avgLnI[i] + log(1.0 + 1.0/minPhotonNumPerState) );
#endif

        for( j = 0 ; j < sNo ; j++ ){
            if( i != j ){
                avgK[i][j] = ( uKMat[i][j] + Mij[i][j] ) / ( sumUKArr[i] + Nij[i] );
                avgLnK[i][j] = gsl_sf_psi( uKMat[i][j] + Mij[i][j] ) - gsl_sf_psi( sumUKArr[i] + Nij[i] );
            }
        }
    }
}

//void maximizationG_ts( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

double varLowerBound_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsParameters *p = (tsParameters*)gv->params;
    tsStats *s = (tsStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *avgLnPi = p->avgLnPi, **avgLnK = p->avgLnK;
    double *avgLnKI = p->avgLnKI, *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uKMat = p->uKMat, *sumUKArr = p->sumUKArr;
    double *Ni = s->Ni, *Ti = s->Ti;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    size_t n;
    int i, j;

    double lnpPi;
    lnpPi = gsl_sf_lngamma(sumUPi);
    double Ck = 1.0, lnpKii = (double)sNo * log(Ck);
    double lnpKij = 0.0;
    double lnpI = 0.0;
    double lnqPi;
    lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqKiiI = 0.0;
    double lnqKij = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpKii -= avgLnK[i][i];

        lnpI += aIArr[i] * log(bIArr[i]) - gsl_sf_lngamma(aIArr[i]);
        lnpI += (aIArr[i]-1.0)*avgLnI[i] - bIArr[i]*avgI[i];

        lnqPi += (uPiArr[i]+gmMat[0][i]-1) * (gsl_sf_psi(uPiArr[i]+gmMat[0][i]) - gsl_sf_psi(sumUPi+1));
        lnqPi -= gsl_sf_lngamma(uPiArr[i] + gmMat[0][i]);

        lnqKiiI += gsl_sf_lngamma(Nii[i] + Nij[i]) + (Ni[i] + aIArr[i]) * log(Ti[i] + bIArr[i]);
        lnqKiiI += -gsl_sf_lngamma(Ni[i] + aIArr[i]) -gsl_sf_lngamma(Nii[i]);
        lnqKiiI += -gsl_sf_lngamma(Nij[i]) + (Nij[i]-1.0)*avgLnK[i][i];
        lnqKiiI += (Ni[i] + Nii[i] + aIArr[i] - 1.0)*avgLnI[i];
        lnqKiiI += -(Nii[i] + Nij[i])*avgLnKI[i] - (Ti[i] + bIArr[i])*avgI[i];

        lnpKij += gsl_sf_lngamma(sumUKArr[i]);
        lnqKij += gsl_sf_lngamma(sumUKArr[i] + Nij[i]);
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                lnpKij += (uKMat[i][j]-1)*avgLnK[i][j] - gsl_sf_lngamma(uKMat[i][j]);
                lnqKij += (uKMat[i][j]+Mij[i][j]-1)*(gsl_sf_psi(uKMat[i][j]+Mij[i][j])-gsl_sf_psi(sumUKArr[i]+Nij[i]));
                lnqKij -= gsl_sf_lngamma( uKMat[i][j] + Mij[i][j] );
            }
        }
    }

    double lnpX = 0.0;
    for( n = 0 ; n < dLen ; n++ ){
        lnpX += log( cn[n] );
    }

    double val;
    val  = lnpPi + lnpKii + lnpKij + lnpI;
    val -= lnqPi + lnqKiiI + lnqKij;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

//double varLowerBoundG_ts( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void reorderParameters_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsParameters *p = (tsParameters*)gv->params;
    tsStats *s = (tsStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgK = p->avgK;
    double **avgLnK = p->avgLnK, *avgLnKI = p->avgLnKI;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *Ni = s->Ni, *Ti = s->Ti;
    size_t n;
    int i, j;

    int *index = (int*)malloc( sNo * sizeof(int) );
    double *store = (double*)malloc( sNo * sizeof(double) );
    double **s2D = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s2D[i] = (double*)malloc( sNo * sizeof(double) );   }

    // index indicates order of avgI values (0=biggest avgI -- sNo=smallest avgI).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgI[i] < avgI[j] ){
                    index[i]--;
                } else if( avgI[i] == avgI[j] ){
                    if( i < j )
                    {   index[i]--;   }
                }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnI[i] = store[i];   }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgK[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgK[i][j] = s2D[i][j];   }
    }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgLnK[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgLnK[i][j] = s2D[i][j];   }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnKI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnKI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ni[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ni[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ti[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ti[i] = store[i];   }

    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = gmMat[n][i];   }
        for( i = 0 ; i < sNo ; i++ ){   gmMat[n][i] = store[i];   }
    }

    for( n = 0 ; n < dLen ; n++ ){
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = xiMat[n][i][j];   }
        }
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   xiMat[n][i][j] = s2D[i][j];   }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   free( s2D[i] );   }
    free( s2D );
    free( store );
    free( index );
}

//void reorderParametersG_ts( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void outputTsResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    tsParameters *p = (tsParameters*)gv->params;
    int sNo = gv->sNo;
    int i, j;

    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   intensities: ( %g", p->avgI[0]);
    for( i = 1 ; i < sNo ; i++ )
    {   fprintf(logFP, ", %g", p->avgI[i]);   }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   k_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
        fprintf(logFP, " ( %g", p->avgK[i][0]);
        for( j = 1 ; j < sNo ; j++ )
        {   fprintf(logFP, ", %g", p->avgK[i][j]);   }
        fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

    char fn[256];
    FILE *fp;
    size_t n;

    sprintf( fn, "%s.param%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "I, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", K%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g, %g", p->avgI[i], p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgK[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

    sprintf( fn, "%s.Lq%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < gv->iteration ; n++ ){
            fprintf( fp, "%24.20e\n", gv->LqArr[n] );
        }
        fclose(fp);
    }

    sprintf( fn, "%s.maxS%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < xn->N ; n++ ){
            fprintf( fp, "%d\n", iv->stateTraj[n] );
        }
        fclose(fp);
    }

}

//void outputTsResultsG( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//}

//


/* interpolation/bsearch.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_interp.h>

#ifndef HIDE_INLINE_STATIC
size_t
gsl_interp_bsearch (
  const double x_array[], double x,
  size_t index_lo,
  size_t index_hi
  )
{
  size_t ilo = index_lo;
  size_t ihi = index_hi;
  while (ihi > ilo + 1)
    {
      size_t i = (ihi + ilo) / 2;
      if (x_array[i] > x)
        ihi = i;
      else
        ilo = i;
    }

  return ilo;
}
#endif


/* Provides the Lua Linear module. See LICENSE for license terms. */

#ifndef LUALINEAR_INCLUDED
#define LUALINEAR_INCLUDED

#include <assert.h>
#include <stdlib.h>
#include <lua.h>
#include <lauxlib.h>
#include <cblas.h>

/**
 * Meta tables.
 */
#define LUALINEAR_VECTOR_METATABLE "linear.vector"
#define LUALINEAR_MATRIX_METATABLE "linear.matrix"

/**
 * OpenMP.
 */
#define LUALINEAR_OMP_MINSIZE 2048

/**
 * Vector.
 */
struct vector {
        int size;
        int inc;
        float *values;
        int ref;
};

/**
 * Pushes a new vector onto the stack.
 */
inline struct vector *lualinear_newvector (lua_State *L, int size) {
	struct vector *vector;

	assert(size >= 1);
	vector = (struct vector *)lua_newuserdata(L, sizeof(struct vector));
	vector->size = size;
	vector->inc = 1;
	vector->values = NULL;
	vector->ref = LUA_NOREF;
	luaL_getmetatable(L, LUALINEAR_VECTOR_METATABLE);
	lua_setmetatable(L, -2);
	vector->values = (float *)calloc(size, sizeof(float));
	if (vector->values == NULL) {
		luaL_error(L, "cannot allocate values");
	}
        return vector;
}

/**
 * Pushes an existing vector onto the stack.
 */
inline struct vector *lualinear_wrapvector (lua_State *L, int size,
		float *values) {
	struct vector *vector;

	assert(size >= 1);
	vector = (struct vector *)lua_newuserdata(L, sizeof(struct vector));
	vector->size = size;
	vector->inc = 1;
	vector->values = values;
	vector->ref = LUA_REFNIL;
	luaL_getmetatable(L, LUALINEAR_VECTOR_METATABLE);
	lua_setmetatable(L, -2);
	return vector;
}

/**
 * Matrix.
 */
struct matrix {
        int rows;
        int cols;
        int ld;
        CBLAS_ORDER order;
        float *values;
        int ref;
};

/**
 * Pushes a new matrix onto the stack.
 */
inline struct matrix *lualinear_newmatrix (lua_State *L, int rows, int cols,
                CBLAS_ORDER order) {
	struct matrix *matrix;

	assert(rows >= 1 && cols >= 1);
	matrix = (struct matrix *)lua_newuserdata(L, sizeof(struct matrix));
	matrix->rows = rows;
	matrix->cols = cols;
	matrix->ld = order == CblasRowMajor ? cols : rows;
	matrix->order = order;
	matrix->values = NULL;
	matrix->ref = LUA_NOREF;
	luaL_getmetatable(L, LUALINEAR_MATRIX_METATABLE);
	lua_setmetatable(L, -2);
	matrix->values = (float *)calloc((size_t)rows * cols, sizeof(float));
	if (matrix->values == NULL) {
		luaL_error(L, "cannot allocate values");
	}
	return matrix;
}

/**
 * Pushes an existing matrix onto the stack-
 */
inline struct matrix *lualinear_wrapmatrix (lua_State *L, int rows, int cols,
		CBLAS_ORDER order, float *values) {
	struct matrix *matrix;

	assert(rows >= 1 && cols >= 1);
	matrix = (struct matrix *)lua_newuserdata(L, sizeof(struct matrix));
	matrix->rows = rows;
	matrix->cols = cols;
	matrix->ld = order == CblasRowMajor ? cols : rows;
	matrix->order = order;
	matrix->values = values;
	matrix->ref = LUA_REFNIL;
	luaL_getmetatable(L, LUALINEAR_MATRIX_METATABLE);
	lua_setmetatable(L, -2);
	return matrix;
}

/**
 * Opens the Linear module in a Lua state.
 */
int luaopen_linear (lua_State *L);

#endif /* LUALINEAR_INCLUDED */


#pragma once

#include <gsl/gsl>

#include <string>
#include <string_view>
#include <memory>

#include "aas/aas_math.h"

namespace aas {

	/**
	 * Animations can either be advanced by time (automatic), distance moved, or object rotation.
	 */
	enum class SkelAnimDriver : uint8_t {
		Time = 0,
		Distance,
		Rotation
	};

	struct SkelAnimStream {
		uint16_t frames;
		int16_t variationId; // This might actually be a uint8_t
		float frameRate;
		float dps;
		// Offset to the start of the key-frame stream in relation to SkaAnimation start
		uint32_t dataOffset;
	};

	struct SkelAnimStreamData;

	struct SkelAnimEvent {
		int16_t frame;
		char type[48];
		char action[128];
	};

	struct SkelAnim {
		char name[64];
		SkelAnimDriver driveType;
		uint8_t loopable;
		uint16_t eventCount; // seems to be uint8_t in reality...
		uint32_t eventOffset;
		uint16_t streamCount;
		uint16_t unk;
		SkelAnimStream streams[10];

		const gsl::span<SkelAnimEvent> GetEventData() const {
			if (eventCount == 0) {
				return {};
			} else {
				auto data = (SkelAnimEvent*)(((char*)this) + eventOffset);
				return gsl::span(data, eventCount);
			}
		}

		const SkelAnimStreamData* GetStreamKeyframes(int streamIdx = 0) const {
			return (SkelAnimStreamData*)(((char*)this) + streams[streamIdx].dataOffset);
		}
	};

	struct SkelBoneState {
		DX::XMFLOAT4 scale;
		DX::XMFLOAT4 rotation;
		DX::XMFLOAT4 translation;

		static void Lerp(gsl::span<SkelBoneState> out,
			const gsl::span<SkelBoneState> from,
			const gsl::span<SkelBoneState> to,
			int count,
			float fraction);
	};

	struct SkelBone {
		int16_t flags;
		int16_t parentId;
		char name[40];
		int field_2C;
		float field_30;
		SkelBoneState initialState;
	};

	class Skeleton {
	public:
		Skeleton(std::string filename, std::vector<uint8_t> data);

		gsl::span<SkelBone> GetBones() const {
			return bones_;
		}

		gsl::span<SkelAnim> GetAnimations() const {
			return animations_;
		}

		/**
		 * Searches for a bone in this skeleton by name (case-insensitive).
		 * Returns nullptr if no bone is found.
		 */
		const SkelBone* FindBoneByName(std::string_view name) const;

		/**
		* Searches for the index of a bone in this skeleton by name (case-insensitive).
		* Returns -1 if no bone is found.
		*/
		int FindBoneIdxByName(std::string_view name) const;

		/**
		 * Searches for an animation in this skeleton by it's name (case insensitive)
		 * and returns the index of the animation in the skeleton.
		 * If no animation is found, returns -1.
		 */
		int FindAnimIdxByName(std::string_view name) const;

		const std::string &GetFilename() const {
			return filename_;
		}

	private:
		std::string filename_;
		std::vector<uint8_t> data_;

		// These are views into the data buffer
		gsl::span<SkelBone> bones_;
		gsl::span<SkelAnim> animations_;
	};

	std::unique_ptr<Skeleton> LoadSkeletonFile(std::string_view filename);

}


// @(#)root/matrix:$Name:  $:$Id: TMatrixTSparse.h,v 1.5 2006/05/17 06:22:06 brun Exp $
// Authors: Fons Rademakers, Eddy Offermann   Feb 2004

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

//#ifndef ROOT_TMatrixTSparse
//#define ROOT_TMatrixTSparse

//#ifndef ROOT_TMatrixTBase
#include "TMatrixTBase.cpp" //KEVIN .... program wont' link correctedly when using templates unless i include cxx file instead of .h
#include "Rtypes.h"   //KEVIN ADDED THIS
//#endif
//#ifndef ROOT_TMatrixTUtils
//#include "TMatrixTUtils.h"
//#endif

//#ifdef CBLAS
//#include <vecLib/vBLAS.h>
//#include <cblas.h>
//#endif

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TMatrixTSparse                                                       //
//                                                                      //
// Template class of a general sparse matrix in the Harwell-Boeing      //
// format                                                               //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

//template<class Element> class TMatrixT;

template<class Element> class TMatrixTSparse : public TMatrixTBase<Element> {

protected:

   Int_t   *fRowIndex;  //[fNrowIndex] row index
   Int_t   *fColIndex;  //[fNelems]    column index
   Element *fElements;  //[fNelems]

   void Allocate(Int_t nrows,Int_t ncols,Int_t row_lwb = 0,Int_t col_lwb = 0,
                 Int_t init = 0,Int_t nr_nonzeros = 0);

  // Elementary constructors
   //void AMultB (const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0) {
   //             const TMatrixTSparse<Element> bt(TMatrixTSparse::kTransposed,b); AMultBt(a,bt,constr); }
   //void AMultB (const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0) {
   //             const TMatrixTSparse<Element> bsp = b;
   //             const TMatrixTSparse<Element> bt(TMatrixTSparse::kTransposed,bsp); AMultBt(a,bt,constr); }
   //void AMultB (const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0) {
   //             const TMatrixTSparse<Element> bt(TMatrixTSparse::kTransposed,b); AMultBt(a,bt,constr); }

   //void AMultBt(const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0);
   //void AMultBt(const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0);

   //void APlusB (const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0);
   //void APlusB (const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0);
   //void APlusB (const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0) { APlusB(b,a,constr); }

   //void AMinusB(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0);
   //void AMinusB(const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0);
   //void AMinusB(const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0);

public:

   enum EMatrixCreatorsOp1 { kZero,kUnit,kTransposed,kAtA }; //IN ROSA'S CODE THESE TWO LINES ARE IN TMATRIXDBASE.H
   enum EMatrixCreatorsOp2 { kMult,kMultTranspose,kPlus,kMinus };

   void AMultBt(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=1); //CHANGED CONSTR=0 TO CONSTR = 1 FROM ROSA'S CODE
   //IN ADDITION THIS WAS ORIGINALLY PRIVATE... MOVED IT TO PUBLIC FOR TESTTMATRIXDSPARSE.CPP

   TMatrixTSparse() { fElements = 0; fRowIndex = 0; fColIndex = 0; }
   TMatrixTSparse(Int_t nrows,Int_t ncols);
   //TMatrixTSparse(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb);
   TMatrixTSparse(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros,
                  Int_t *row, Int_t *col,Element *data);
   //TMatrixTSparse(const TMatrixTSparse<Element> &another);
   //TMatrixTSparse(const TMatrixT<Element>       &another);

   //TMatrixTSparse(EMatrixCreatorsOp1 op,const TMatrixTSparse<Element> &prototype);
   //TMatrixTSparse(const TMatrixTSparse<Element> &a,EMatrixCreatorsOp2 op,const TMatrixTSparse<Element> &b);

   //virtual ~TMatrixTSparse() { Clear(); }

   virtual const Element *GetMatrixArray  () const;
   virtual       Element *GetMatrixArray  ();
   virtual const Int_t    *GetRowIndexArray() const;
   virtual       Int_t    *GetRowIndexArray();
   virtual const Int_t    *GetColIndexArray() const;
   virtual       Int_t    *GetColIndexArray();

   //virtual TMatrixTBase<Element>   &SetRowIndexArray(Int_t *data) { memmove(fRowIndex,data,(this->fNrows+1)*sizeof(Int_t)); return *this; }
   //virtual TMatrixTBase<Element>   &SetColIndexArray(Int_t *data) { memmove(fColIndex,data,this->fNelems*sizeof(Int_t)); return *this; }

   //virtual void                     GetMatrix2Array (Element *data,Option_t *option="") const;
   //virtual TMatrixTBase<Element>   &SetMatrixArray  (const Element *data,Option_t * /*option*/="")
   //                                                 { memcpy(fElements,data,this->fNelems*sizeof(Element)); return *this; }

   //KEVIN: SetMatrixArray used to be virtual..... gives me vtable errors.... don't know what it is though... googled it
   TMatrixTBase<Element>   &SetMatrixArray  (Int_t nr_nonzeros,Int_t *irow,Int_t *icol,Element *data);
           TMatrixTSparse<Element> &SetSparseIndex  (Int_t nelem_new);
	   //        TMatrixTSparse<Element> &SetSparseIndex  (const TMatrixTBase<Element> &another);
           //TMatrixTSparse<Element> &SetSparseIndexAB(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b);
	   //virtual TMatrixTBase<Element>   &InsertRow       (Int_t row,Int_t col,const Element *v,Int_t n=-1);
	   //virtual void                     ExtractRow      (Int_t row,Int_t col,      Element *v,Int_t n=-1) const;

	   //virtual TMatrixTBase<Element>   &ResizeTo(Int_t nrows,Int_t ncols,Int_t nr_nonzeros=-1);
	   //  virtual TMatrixTBase<Element>   &ResizeTo(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros=-1);
	   //inline  TMatrixTBase<Element>   &ResizeTo(const TMatrixTSparse<Element> &m) {return ResizeTo(m.GetRowLwb(),m.GetRowUpb(),m.GetColLwb(),
	   //                                                                                m.GetColUpb(),m.GetNoElements()); }

//virtual void Clear(Option_t * /*option*/ ="") { if (this->fIsOwner) {
//                                                   if (fElements) delete [] fElements; fElements = 0;
//                                                     if (fRowIndex) delete [] fRowIndex; fRowIndex = 0;
//                                                     if (fColIndex) delete [] fColIndex; fColIndex = 0;
//                                                   }
//                                                   this->fNelems    = 0;
//                                                   this->fNrowIndex = 0;
//                                                 }

//           TMatrixTSparse<Element> &Use   (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros,
//                                           Int_t *pRowIndex,Int_t *pColIndex,Element *pData);
//           TMatrixTSparse<Element> &Use   (Int_t nrows,Int_t ncols,Int_t nr_nonzeros,
//                                           Int_t *pRowIndex,Int_t *pColIndex,Element *pData);
//           TMatrixTSparse<Element> &Use   (TMatrixTSparse<Element> &a);
//   virtual TMatrixTBase<Element>   &GetSub(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,
//                                            TMatrixTBase<Element> &target,Option_t *option="S") const;
//           TMatrixTSparse<Element>  GetSub(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Option_t *option="S") const;
//   virtual TMatrixTBase<Element>   &SetSub(Int_t row_lwb,Int_t col_lwb,const TMatrixTBase<Element> &source);

//   virtual Bool_t IsSymmetric() const { return (*this == TMatrixTSparse<Element>(kTransposed,*this)); }
	   TMatrixTSparse<Element> &Transpose (const TMatrixTSparse<Element> &source);
	   inline TMatrixTSparse<Element> &T () { return this->Transpose(*this); }

//   inline void Mult(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b) { AMultB(a,b,0); }

//   virtual TMatrixTBase<Element> &Zero       ();
//   virtual TMatrixTBase<Element> &UnitMatrix ();

//   virtual Element RowNorm () const;
//   virtual Element ColNorm () const;
      virtual Int_t   NonZeros() const { return this->fNelems; }

//   virtual TMatrixTBase<Element> &NormByDiag(const TVectorT<Element> &/*v*/,Option_t * /*option*/)
//                                              { MayNotUse("NormByDiag"); return *this; }

   // Either access a_ij as a(i,j)
//   inline       Element                          operator()(Int_t rown,Int_t coln) const;
//                Element                         &operator()(Int_t rown,Int_t coln);

   // or as a[i][j]
//   inline const TMatrixTSparseRow_const<Element> operator[](Int_t rown) const { return TMatrixTSparseRow_const<Element>(*this,rown); }
//   inline       TMatrixTSparseRow      <Element> operator[](Int_t rown)       { return TMatrixTSparseRow      <Element>(*this,rown); }

//   TMatrixTSparse<Element> &operator=(const TMatrixT<Element>       &source);
//   TMatrixTSparse<Element> &operator=(const TMatrixTSparse<Element> &source);

//   TMatrixTSparse<Element> &operator= (Element val);
//   TMatrixTSparse<Element> &operator-=(Element val);
//   TMatrixTSparse<Element> &operator+=(Element val);
//   TMatrixTSparse<Element> &operator*=(Element val);

//   TMatrixTSparse<Element> &operator+=(const TMatrixTSparse<Element> &source) { TMatrixTSparse<Element> tmp(*this);
//                                                                                if (this == &source) APlusB (tmp,tmp,1);
//                                                                                else                 APlusB (tmp,source,1); return *this; }
//   TMatrixTSparse<Element> &operator+=(const TMatrixT<Element>       &source) { TMatrixTSparse<Element> tmp(*this);
//                                                                                APlusB(tmp,source,1); return *this; }
//   TMatrixTSparse<Element> &operator-=(const TMatrixTSparse<Element> &source) { TMatrixTSparse<Element> tmp(*this);
//                                                                                if (this == &source) AMinusB (tmp,tmp,1);
//                                                                                else                 AMinusB(tmp,source,1); return *this; }
//   TMatrixTSparse<Element> &operator-=(const TMatrixT<Element>       &source) { TMatrixTSparse<Element> tmp(*this);
//                                                                                AMinusB(tmp,source,1); return *this; }
//   TMatrixTSparse<Element> &operator*=(const TMatrixTSparse<Element> &source) { TMatrixTSparse<Element> tmp(*this);
//                                                                                if (this == &source) AMultB (tmp,tmp,1);
//                                                                                else                 AMultB (tmp,source,1); return *this; }
//   TMatrixTSparse<Element> &operator*=(const TMatrixT<Element>       &source) { TMatrixTSparse<Element> tmp(*this);
//                                                                                AMultB(tmp,source,1); return *this; }

//   virtual TMatrixTBase  <Element> &Randomize  (Element alpha,Element beta,Double_t &seed);
//   virtual TMatrixTSparse<Element> &RandomizePD(Element alpha,Element beta,Double_t &seed);

//   ClassDef(TMatrixTSparse,3) // Template of Sparse Matrix class
};

template <class Element> inline const Element *TMatrixTSparse<Element>::GetMatrixArray  () const { return fElements; }
template <class Element> inline       Element *TMatrixTSparse<Element>::GetMatrixArray  ()       { return fElements; }
template <class Element> inline const Int_t   *TMatrixTSparse<Element>::GetRowIndexArray() const { return fRowIndex; }
template <class Element> inline       Int_t   *TMatrixTSparse<Element>::GetRowIndexArray()       { return fRowIndex; }
template <class Element> inline const Int_t   *TMatrixTSparse<Element>::GetColIndexArray() const { return fColIndex; }
template <class Element> inline       Int_t   *TMatrixTSparse<Element>::GetColIndexArray()       { return fColIndex; }

//template <class Element>
//inline TMatrixTSparse<Element> &TMatrixTSparse<Element>::Use   (Int_t nrows,Int_t ncols,Int_t nr_nonzeros,
//                                                                Int_t *pRowIndex,Int_t *pColIndex,Element *pData)
//                                                                  { return Use(0,nrows-1,0,ncols-1,nr_nonzeros,pRowIndex,pColIndex,pData); }
//template <class Element>
//inline TMatrixTSparse<Element> &TMatrixTSparse<Element>::Use   (TMatrixTSparse<Element> &a)
//                                                                  { R__ASSERT(a.IsValid());
//                                                                     return Use(a.GetRowLwb(),a.GetRowUpb(),a.GetColLwb(),a.GetColUpb(),
//                                                                                a.GetNoElements(),a.GetRowIndexArray(),
//                                                                                a.GetColIndexArray(),a.GetMatrixArray()); }
//template <class Element>
//inline TMatrixTSparse<Element>  TMatrixTSparse<Element>::GetSub(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,
//                                                                Option_t *option) const
//                                                                  {
//                                                                    TMatrixTSparse<Element> tmp;
//                                                                    this->GetSub(row_lwb,row_upb,col_lwb,col_upb,tmp,option);
//                                                                    return tmp;
//                                                                  }

//template <class Element>
//inline Element TMatrixTSparse<Element>::operator()(Int_t rown,Int_t coln) const
//{
//   R__ASSERT(this->IsValid());
//   if (this->fNrowIndex > 0 && this->fRowIndex[this->fNrowIndex-1] == 0) {
//      Error("operator=()(Int_t,Int_t) const","row/col indices are not set");
//      printf("fNrowIndex = %d fRowIndex[fNrowIndex-1] = %d\n",this->fNrowIndex,this->fRowIndex[this->fNrowIndex-1]);
//      return 0.0;
//   }
//   const Int_t arown = rown-this->fRowLwb;
//   const Int_t acoln = coln-this->fColLwb;
//   R__ASSERT(arown < this->fNrows && arown >= 0);
//   R__ASSERT(acoln < this->fNcols && acoln >= 0);
//   const Int_t sIndex = fRowIndex[arown];
//   const Int_t eIndex = fRowIndex[arown+1];
//   const Int_t index = TMath::BinarySearch(eIndex-sIndex,fColIndex+sIndex,acoln)+sIndex;
//   if (index < sIndex || fColIndex[index] != acoln) return 0.0;
//   else                                             return fElements[index];
//}

//template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixTSparse<Element> &source1,const TMatrixTSparse<Element> &source2);
//template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixTSparse<Element> &source1,const TMatrixT<Element>       &source2);
//template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixT<Element>       &source1,const TMatrixTSparse<Element> &source2);
//template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixTSparse<Element> &source ,      Element                  val    );
//template <class Element> TMatrixTSparse<Element>  operator+ (      Element                  val    ,const TMatrixTSparse<Element> &source );
//template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixTSparse<Element> &source1,const TMatrixTSparse<Element> &source2);
//template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixTSparse<Element> &source1,const TMatrixT<Element>       &source2);
//template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixT<Element>       &source1,const TMatrixTSparse<Element> &source2);
//template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixTSparse<Element> &source ,      Element                  val    );
//template <class Element> TMatrixTSparse<Element>  operator- (      Element                  val    ,const TMatrixTSparse<Element> &source );
//template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixTSparse<Element> &source1,const TMatrixTSparse<Element> &source2);
//template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixTSparse<Element> &source1,const TMatrixT<Element>       &source2);
//template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixT<Element>       &source1,const TMatrixTSparse<Element> &source2);
///template <class Element> TMatrixTSparse<Element>  operator* (      Element                  val    ,const TMatrixTSparse<Element> &source );
//template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixTSparse<Element> &source,       Element                  val    );

//template <class Element> TMatrixTSparse<Element> &Add        (TMatrixTSparse<Element> &target,      Element                   scalar,
//                                                              const TMatrixTSparse<Element> &source);
//template <class Element> TMatrixTSparse<Element> &ElementMult(TMatrixTSparse<Element> &target,const TMatrixTSparse<Element>  &source);
//template <class Element> TMatrixTSparse<Element> &ElementDiv (TMatrixTSparse<Element> &target,const TMatrixTSparse<Element>  &source);

//template <class Element> Bool_t AreCompatible(const TMatrixTSparse<Element> &m1,const TMatrixTSparse<Element> &m2,Int_t verbose=0);

//#endif