#ifndef GSL_INTERFACE_H

#ifdef HAVE_GSL
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

namespace GSL{
	static std::string errMsg;
	static int gsl_errno;
	void error_handler(const char* reason, const char* file, int line, int gsl_errno){
		GSL::gsl_errno=gsl_errno;
		errMsg=file+std::string(":")+std::to_string(line)+": "+reason;
	}

	void reset_gsl_error(){ gsl_errno=0; }

	///\class
	///\brief Container for GSL workspace to be use with the integrators.
	class IntegrateWorkspace {
	private:

	public:
		gsl_integration_workspace* ws;
		IntegrateWorkspace(size_t limit) {
			ws=gsl_integration_workspace_alloc(limit);
		}
		~IntegrateWorkspace() {
			gsl_integration_workspace_free(ws);
		}
	};

	///\brief One dimensional integral using GSL.
	/// @param ws GSL integration workspace.
	/// @param f Function to integrate.
	/// @param a Lower integration limit.
	/// @param b Upper integration limit.
	/// @param acc Accuracy parameter.
	/// @param max_iter Maximum number of iterations to perform the integral.
	template<typename FunctionType>
	double integrate(IntegrateWorkspace& ws, FunctionType&& f, double a, double b, double acc=1e-7, unsigned int max_iter=10000){
		//precondition copied from gsl-1.16/integration/qag.c:122
		//assuming epsabs=0 and epsrel=acc
		if ((acc < 50 * GSL_DBL_EPSILON || acc < 0.5e-28))
			throw std::runtime_error("Specified tolerance too small");

		using FPtr=decltype(&f);
		double (*wrapper)(double,void*)=[](double x, void* params)->double{
			auto& f=*static_cast<FPtr>(params);
			return(f(x));
		};

		double result, error;
		gsl_function F;
		F.function = wrapper;
		F.params = &f;

		gsl_integration_qag(&F, a, b, 0, acc, max_iter, GSL_INTEG_GAUSS15, ws.ws, &result, &error);

		return(result);
	}

	///\brief One dimensional integral using GSL.
	/// @param f Function to integrate.
	/// @param a Lower integration limit.
	/// @param b Upper integration limit.
	/// @param acc Accuracy parameter.
	/// @param max_iter Maximum number of iterations to perform the integral.
	template<typename FunctionType>
	double integrate(FunctionType&& f, double a, double b, double acc=1e-7, unsigned int max_iter=10000, size_t memory_alloc=10000){
		IntegrateWorkspace ws(memory_alloc);
		return integrate(ws, f, a, b, acc, max_iter);
	}
} //namespace GSL
#endif //HAVE_GSL

#endif //GSL_INTERFACE_H


#ifndef METHODS_CWA_SMD_OPT_H
#define METHODS_CWA_SMD_OPT_H

#include <gsl/gsl_multimin.h>
#include "quartz_internal/util/gsl_converter.h"

namespace method {
namespace cwa_smd_opt {
namespace details {

inline
gsl_multimin_fdfminimizer_type * minimizer_map(const std::string type) {
  if(type == "steepest_descent") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_steepest_descent);
  } else if(type == "conjugate_pr") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_conjugate_pr);
  } else if(type == "conjugate_fr") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_conjugate_fr);
  } else if(type == "bfgs") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_vector_bfgs);
  } else if(type == "bfgs2") {
    return const_cast<gsl_multimin_fdfminimizer_type *>(gsl_multimin_fdfminimizer_vector_bfgs2);
  } else {
    throw Error("minimizer " + type + " is not implemented");
  }

}

struct cwa_smd_opt_param {
  arma::mat original_points;
  arma::vec expectations_ref;
  std::vector<math::Polynomial<double>> original_operators;
  arma::vec weights;
  arma::vec scaling;
  long long grade;
};

inline
double penalty_function(
    const arma::mat & points,
    const arma::vec & expectations_ref,
    const std::vector<math::Polynomial<double>> & original_operators,
    const arma::vec & weights,
    const arma::vec & scaling,
    const long long grade) {

  double result = 0;

  for (arma::uword i = 0; i < original_operators.size(); i++) {
    const long long the_grade = original_operators[i].grade();
    if (the_grade < grade && the_grade > 0) {
      const double result_from_cwa =
          cwa_smd::details::expectation(original_operators[i], points, weights,
                                        scaling);

      result += std::pow(result_from_cwa - expectations_ref(i), 2);
    }
  }

  return result;

}

inline
arma::mat penalty_function_derivative(
    const arma::mat & points,
    const arma::vec & expectations_ref,
    const std::vector<math::Polynomial<double>> & original_operators,
    const arma::vec & weights,
    const arma::vec & scaling,
    const long long grade
) {

  arma::mat result(arma::size(points), arma::fill::zeros);

  for (arma::uword i = 0; i < original_operators.size(); i++) {
    const long long the_grade = original_operators[i].grade();
    if (the_grade < grade && the_grade > 0) {
      const double result_from_cwa =
          cwa_smd::details::expectation(original_operators[i], points, weights,
                                        scaling);

      for (arma::uword j = 0; j < points.n_cols; j++) {
        const arma::vec point = arma::diagmat(1.0 / scaling) * points.col(j);
        for (arma::uword k = 0; k < points.n_rows / 2; k++) {
          const math::Polynomial<double> x_derivative =
              original_operators[i].derivative(
                  k) / scaling(k);
          const math::Polynomial<double> p_derivative =
              original_operators[i].derivative(
                  k + points.n_rows / 2) / scaling(k + points.n_rows / 2);

          result(k, j) -=
              2.0 * (result_from_cwa - expectations_ref(i)) * weights(j) *
              x_derivative.at(point) / arma::sum(weights);
          result(k + points.n_rows / 2, j) -=
              2.0 * (result_from_cwa - expectations_ref(i)) * weights(j) *
              p_derivative.at(point) / arma::sum(weights);
        }
      }
    }
  }

  return -result;

}

inline
double penalty_function_gsl_wrapper(const gsl_vector * flattened_points,
                                    void * param) {

  const arma::vec arma_flattened_points = gsl::convert_vec(flattened_points);
  const auto converted_param = *(cwa_smd_opt_param *) param;
  const arma::mat points = arma::reshape(arma_flattened_points, arma::size(
      converted_param.original_points));

  return penalty_function(points,
                          converted_param.expectations_ref,
                          converted_param.original_operators,
                          converted_param.weights,
                          converted_param.scaling,
                          converted_param.grade);
}

inline
void penalty_function_derivative_gsl_wrapper(
    const gsl_vector * flattened_points,
    void * param,
    gsl_vector * g) {
  const arma::vec arma_flattened_points = gsl::convert_vec(flattened_points);
  const auto converted_param = *(cwa_smd_opt_param *) param;
  const arma::mat points = arma::reshape(arma_flattened_points, arma::size(
      converted_param.original_points));

  const arma::vec result =
      arma::vectorise(
          penalty_function_derivative(points,
                                      converted_param.expectations_ref,
                                      converted_param.original_operators,
                                      converted_param.weights,
                                      converted_param.scaling,
                                      converted_param.grade));

  const auto result_pointer = gsl::convert_vec(result);

  gsl_vector_memcpy(g, result_pointer);

  gsl_vector_free(result_pointer);
}

inline
void penalty_function_fdf_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param,
    double * f,
    gsl_vector * g) {
  penalty_function_derivative_gsl_wrapper(a_derivatives, param, g);

  *f = penalty_function_gsl_wrapper(a_derivatives, param);
}

inline
std::tuple<arma::mat, double, double, int> cwa_optimize(const cwa_smd_opt_param input,
                                                const double initial_step_size,
                                                const double tolerance,
                                                const double gradient_tolerance,
                                                const size_t total_steps,
                                                const std::string type) {

  /* allocate memory for minimization process */
  const auto minimizer_type = minimizer_map(type);

  const arma::uword n = input.original_points.n_elem;

  const auto penalty_function_value = penalty_function(input.original_points,
                                                       input.expectations_ref,
                                                       input.original_operators,
                                                       input.weights,
                                                       input.scaling,
                                                       input.grade);

  const double gradient_module = arma::norm(
      arma::vectorise(penalty_function_derivative(input.original_points,
                                                  input.expectations_ref,
                                                  input.original_operators,
                                                  input.weights,
                                                  input.scaling,
                                                  input.grade)));

  if (penalty_function_value < tolerance &&
      gradient_module < gradient_tolerance) {
    return {input.original_points, penalty_function_value, gradient_module, 0};
  }

  auto minimizer_environment = gsl_multimin_fdfminimizer_alloc(minimizer_type,
                                                               n);

  /* assigning function to minimizer object */
  gsl_multimin_function_fdf minimizer_object;
  minimizer_object.f = &penalty_function_gsl_wrapper;
  minimizer_object.df = &penalty_function_derivative_gsl_wrapper;
  minimizer_object.fdf = &penalty_function_fdf_gsl_wrapper;
  minimizer_object.n = n;
  minimizer_object.params = (void *) &input;

  /* starting point */
  const arma::vec flattened = arma::vectorise(input.original_points);
  gsl_vector * points = gsl::convert_vec(flattened);

  /* set environment */
  gsl_multimin_fdfminimizer_set(minimizer_environment,
                                &minimizer_object, points,
                                initial_step_size, tolerance);

  size_t iter = 0;
  int status = GSL_CONTINUE;
  do {
    iter++;

    status = gsl_multimin_fdfminimizer_iterate(minimizer_environment);

    if (status) {
      throw Error(gsl_strerror(status));
    }

    status = gsl_multimin_test_gradient(minimizer_environment->gradient,
                                        gradient_tolerance);

    if (status == GSL_SUCCESS) {
      const arma::vec result = gsl::convert_vec(minimizer_environment->x);

      const double f = minimizer_environment->f;
      const double df = arma::norm(gsl::convert_vec(minimizer_environment->gradient));
      gsl_multimin_fdfminimizer_free(minimizer_environment);
      gsl_vector_free(points);

      return {arma::reshape(result, arma::size(input.original_points)), f, df, iter};
    }
  } while (status == GSL_CONTINUE && iter < total_steps);

  throw Error("fail to converge towards the solution");
}

} // namespace details

struct State {
public:
  arma::mat points;
  arma::vec weights;
  arma::vec masses;
  arma::uword grade;
  arma::uvec expectation_table;
  arma::vec expectations;
  arma::uvec positional_indices;
  arma::uvec momentum_indices;
  arma::vec scaling;

  // Establish an easy way to construct your State
  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range,
        const arma::vec & scaling,
        const arma::vec & masses,
        const arma::uword grade) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(masses),
      grade(grade),
      expectation_table(math::space::grids_to_table(
          grade * arma::ones<arma::uvec>(points.n_rows))),
      scaling(scaling) {
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }

    const arma::uword dimension = grid.n_elem;
    const arma::uword length = std::pow(grade, dimension);

    this->expectations = arma::vec(length);
    this->positional_indices = arma::uvec(dimension / 2);
    this->momentum_indices = arma::uvec(dimension / 2);

    const arma::vec ranges = range.col(1) - range.col(0);

    // exponents check in
#pragma omp parallel for
    for (arma::uword i = 0; i < dimension / 2; i++) {
      arma::uvec X = arma::zeros<arma::uvec>(dimension);
      arma::uvec P = arma::zeros<arma::uvec>(dimension);
      X(i) = 1;
      P(i + dimension / 2) = 1;
      this->positional_indices(i) =
          math::space::indices_to_index(X, this->expectation_table);
      this->momentum_indices(i) =
          math::space::indices_to_index(P, this->expectation_table);
    }

    // expectations check in
#pragma omp parallel for
    for (arma::uword i = 0; i < length; i++) {
      const lvec indices =
          arma::conv_to<lvec>::from(
              math::space::index_to_indices(i, this->expectation_table));

      this->expectations(i) =
          cwa_smd::details::expectation(math::polynomial::Term(1.0, indices),
                                        this->points, this->weights,
                                        this->scaling);
    }
  }

  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range,
        const arma::uword grade) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(arma::ones<arma::vec>(grid.n_rows / 2)),
      grade(grade),
      expectation_table(math::space::grids_to_table(
          grade * arma::ones<arma::uvec>(points.n_rows))) {
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }

    const auto dimension = grid.n_elem;
    const auto length = std::pow(grade, dimension);

    this->expectations = arma::vec(length);
    this->positional_indices = arma::uvec(dimension / 2);
    this->momentum_indices = arma::uvec(dimension / 2);

    const arma::vec ranges = range.col(1) - range.col(0);
    this->scaling = ranges;
//    this->scaling = arma::ones(arma::size(ranges));

    // exponents check in
    for (arma::uword i = 0; i < dimension / 2; i++) {
      arma::uvec X = arma::zeros<arma::uvec>(dimension);
      arma::uvec P = arma::zeros<arma::uvec>(dimension);
      X(i) = 1;
      P(i + dimension / 2) = 1;
      this->positional_indices(i) =
          math::space::indices_to_index(X, this->expectation_table);
      this->momentum_indices(i) =
          math::space::indices_to_index(P, this->expectation_table);
    }

    // expectations check in
    for (arma::uword i = 0; i < length; i++) {
      const lvec indices =
          arma::conv_to<lvec>::from(
              math::space::index_to_indices(i, this->expectation_table));

      this->expectations(i) =
          cwa_smd::details::expectation(
              math::polynomial::Term(1.0, indices),
              this->points, this->weights, this->scaling);
    }
  }

  inline
  State(const arma::mat & points,
        const arma::vec & weights,
        const arma::vec & masses,
        const arma::uvec & expectation_table,
        const arma::vec & expectations,
        const arma::uvec & positional_indices,
        const arma::uvec & momentum_indices,
        const arma::vec & scaling,
        const arma::uword grade) :
      points(points),
      weights(weights),
      masses(masses),
      grade(grade),
      expectation_table(expectation_table),
      expectations(expectations),
      positional_indices(positional_indices),
      momentum_indices(momentum_indices),
      scaling(scaling) {}

  inline
  State(const State & state) :
      points(state.points),
      weights(state.weights),
      masses(state.masses),
      grade(state.grade),
      expectation_table(state.expectation_table),
      expectations(state.expectations),
      positional_indices(state.positional_indices),
      momentum_indices(state.momentum_indices),
      scaling(state.scaling) {}

  inline
  arma::uword dim() const {
    return points.n_rows / 2;
  }

  inline
  State normalise() const {
    State state = *this;
    state.weights = state.weights / arma::sum(state.weights);

    return state;
  }

  inline
  arma::vec positional_expectation() const {

    return method::cwa::State(this->points, this->weights, this->masses)
              .positional_expectation();
  }

  inline
  arma::vec momentum_expectation() const {
    return method::cwa::State(this->points, this->weights, this->masses)
        .momentum_expectation();
  }

  State operator+(const State & B) const {
    if (!arma::approx_equal(this->weights, B.weights, "abs_diff", 1e-16) ||
        !arma::approx_equal(this->masses, B.masses, "abs_diff", 1e-16)) {
      throw Error("Different cwa states are being added");
    }

    State state = B;
    state.points += this->points;
    state.expectations += this->expectations;

    return state;
  }

  State operator*(const double B) const {

    State state = *this;
    state.expectations *= B;
    state.points *= B;

    return state;
  }

  template<typename T>
  auto expectation(const math::Polynomial<T> & polynomial) const {
    return cwa_smd::details::at_search(polynomial,
                                       this->points,
                                       this->weights,
                                       this->expectations,
                                       this->expectation_table,
                                       this->scaling,
                                       this->grade) *
           polynomial.at(this->scaling);
  }

  template<typename T>
  arma::vec expectation(const std::vector<math::Polynomial<T>>

                        & polynomials) const {
    arma::vec result(polynomials.size());

#pragma omp parallel for
    for (arma::uword i = 0; i < result.n_elem; i++) {
      result(i) = this->expectation(polynomials[i]);
    }

    return result;
  }

  State & operator=(const State &) = default;
};

struct Operator {

public:
  math::Polynomial<double> potential;
  math::Polynomial<double> H;
  std::vector<math::Polynomial<double>> original_operators;
  std::vector<math::Polynomial<double>> operators;

  Operator(const State & state,
           const math::Polynomial<double> & potential) :
      potential(potential),
      H(hamiltonian(potential, state.masses).scale(state.scaling)),
      operators() {

    std::vector<math::Polynomial<double>>
        op(std::pow(state.grade, state.dim() * 2));
    std::vector<math::Polynomial<double>>
        original_op(std::pow(state.grade, state.dim() * 2));

    op[0] = math::Polynomial<double>(state.dim() * 2, 0.0);
    original_op[0] = math::Polynomial<double>(state.dim() * 2, 1.0);

    for (arma::uword i = 1; i < op.size(); i++) {
      const auto observable =
          math::Polynomial(math::polynomial::Term<double>(1.0,
                                                          math::space::index_to_indices(
                                                              i,
                                                              state.expectation_table)));

      original_op[i] = observable;

      const arma::uword cut_off = std::min(observable.grade(), H.grade()) / 2;
      const auto moyal =
          moyal_bracket(math::Polynomial(observable), H, state.scaling,
                        cut_off);

      op[i] = moyal;
    }

    this->operators = op;
    this->original_operators = original_op;
  }

  inline
  PropagationType propagation_type() const {
    return Classic;
  }

  State operator()(const State & state) const {

    arma::mat p_submatrix = state.points.rows(state.dim(), 2 * state.dim() - 1);
    p_submatrix.each_col() /= state.masses;

    const arma::mat points_change_list =
        arma::join_cols(p_submatrix,
                        cwa::details::force(this->potential,
                                            state.points.rows(0, state.dim() -
                                                                 1)));

    arma::vec expectation_change_list =
        arma::vec(arma::size(state.expectations));

#pragma omp parallel for
    for (arma::uword i = 0; i < expectation_change_list.n_elem; i++) {
      expectation_change_list(i) =
          cwa_smd::details::at_search(this->operators[i],
                                      state.points,
                                      state.weights,
                                      state.expectations,
                                      state.expectation_table,
                                      state.scaling,
                                      state.grade);
    }

    return State(points_change_list,
                 state.weights,
                 state.masses,
                 state.expectation_table,
                 expectation_change_list,
                 state.positional_indices,
                 state.momentum_indices,
                 state.scaling,
                 state.grade);
  }

};

template<typename Potential>
OperatorWrapper<Operator, State, Potential>
cwa_opt(const double initial_step_size,
        const double tolerance,
        const double gradient_tolerance,
        const size_t total_steps,
        const std::string type = "bfgs2",
        const int print_level = 0) {
  return [initial_step_size,
      tolerance,
      gradient_tolerance,
      total_steps,
      type,
      print_level
  ](const Operator & cwa_smd_opt_operator,
    const Potential & potential) -> Propagator<State> {
    return [initial_step_size,
        tolerance,
        gradient_tolerance,
        total_steps,
        &cwa_smd_opt_operator,
        type,
        print_level
    ]
        (const State & state,
         const double dt) -> State {
      const arma::vec & ref_expectations = state.expectations;
      const arma::mat & points = state.points;
      const auto & original_operators = cwa_smd_opt_operator.original_operators;

      details::cwa_smd_opt_param input{points, ref_expectations,
                                       original_operators,
                                       state.weights, state.scaling,
                                       (long long) state.grade};

      const auto opt_result = details::cwa_optimize(input, initial_step_size,
                                tolerance, gradient_tolerance, total_steps, type);

      const arma::mat new_points = std::get<0>(opt_result);
      const double f = std::get<1>(opt_result);
      const double df = std::get<2>(opt_result);
      const int iter = std::get<3>(opt_result);

      if(print_level > 2) {
        fmt::print("f: {0:20.10f}, df: {1:20.10f}, iter: {2}", f, df, iter);
        fmt::print("\n");
      }

      State new_state = state;
      new_state.points = new_points;
      return new_state;
    };
  };
}

} // namespace cwa
}

#endif //METHODS_CWA_SMD_OPT_H


/* Copyright 2016. The Regents of the University of California.
 * Copyright 2016. Martin Uecker.
 * All rights reserved. Use of this source code is governed by
 * a BSD-style license which can be found in the LICENSE file.
 *
 * Authors:
 * 2016 Jonathan Tamir <jtamir@eecs.berkeley.edu>
 * 2016 Martin Uecker <martin.uecker@med.uni-goettingen.de>
 */

#include <assert.h>
#include <complex.h>

#include "misc/misc.h"

#ifdef USE_MACPORTS
#include <cblas_openblas.h>
#else
#include <cblas.h>
#endif

#ifdef USE_CUDA
#include "num/gpuops.h"

#include <cublas.h>
#endif

#include "blas.h"

void blas_cgemm(char transa, char transb, long M, long N,  long K, const complex float alpha, long lda, const complex float A[K][lda], long ldb, const complex float B[N][ldb], const complex float beta, long ldc, complex float C[N][ldc])
{
#ifdef USE_CUDA
#define CUCOMPLEX(x) (((union { cuComplex cu; complex float std; }){ .std = (x) }).cu)
        if (cuda_ondevice(A)) {

                cublasCgemm(transa, transb, M, N, K,  CUCOMPLEX(alpha),
                                (const cuComplex*)A, lda,
                                (const cuComplex*)B, ldb, CUCOMPLEX(beta),
                                (cuComplex*)C, ldc);
        } else
#endif
        cblas_cgemm(CblasColMajor, transa, transb, M, N, K, (void*)&alpha, (void*)A, lda, (void*)B, ldb, (void*)&beta, (void*)C, ldc);
}

void (blas_matrix_multiply)(long M, long N, long K, complex float C[N][M], const complex float A[K][M], const complex float B[N][K])
{
	blas_cgemm(CblasNoTrans, CblasNoTrans, M, N, K, 1., M, A, K, B, 0., M, C);
}

void (blas_csyrk)(char uplo, char trans, long N, long K, const complex float alpha, long lda, const complex float A[][lda], complex float beta, long ldc, complex float C[][ldc])
{
	assert('U' == uplo);
	assert(('T' == trans) || ('N' == trans));

	cblas_csyrk(CblasColMajor, CblasUpper, ('T' == trans) ? CblasTrans : CblasNoTrans, N, K, (void*)&alpha, (void*)A, lda, (void*)&beta, (void*)C, ldc);
}


/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

Copyright (c) 1999-2014 and onwards, Imperial College London
All rights reserved.
See LICENSE for details

=========================================================================*/

#ifndef _IRTKMATRIX_H

#define _IRTKMATRIX_H

#include <float.h>

#ifdef USE_VXL
#include <vnl/vnl_matrix.h>
#else
#include <gsl/gsl_matrix.h>
#endif

#include <irtkObject.h>

#include <iostream>
#include <cmath>

/**

  Matrix class.

*/

class irtkMatrix : public irtkObject
{

protected:

  /// Number of rows
  int _rows;

  /// Number of colums
  int _cols;

  /// Data
  double **_matrix;

public:

  /// Default constructor
  irtkMatrix();

  /// Constructor for given number of rows and columns
  irtkMatrix(int, int);

  /// Copy constructor
  irtkMatrix(const irtkMatrix &);

  /// Destructor
  ~irtkMatrix();

  /// Initialize matrix with number of rows and columns
  void Initialize(int, int);

  //
  // Matrix access functions
  //

  /// Returns number of rows
  int Rows() const;

  /// Returns number of columns
  int Cols() const;

  /// Puts matrix value
  void   Put(int, int, double);

  /// Gets matrix value
  double Get(int, int) const;

  //
  // Operators for matrix access
  //

  /// Puts matrix value
  double& operator()(int, int);

  /// Gets matrix value
  double  operator()(int, int) const;

  /// Access matrix get operator
  irtkMatrix  operator()(int, int, int, int);

  /// Access matrix put operator
  void    operator()(irtkMatrix &, int, int);

  //
  // Matrix operators for doubles
  //

  /// Subtraction of a double
  irtkMatrix& operator-=(const double&);

  /// Addition of a double
  irtkMatrix& operator+=(const double&);

  /// Multiplication with a double
  irtkMatrix& operator*=(const double&);

  /// Division by a double
  irtkMatrix& operator/=(const double&);

  /// Return result of subtraction of a double
  irtkMatrix  operator- (const double&);

  /// Return result of addition of a double
  irtkMatrix  operator+ (const double&);

  /// Return result of multiplication with a double
  irtkMatrix  operator* (const double&);

  /// Return result of division by a double
  irtkMatrix  operator/ (const double&);

  //
  // Matrix operators for matrices
  //

  /// Matrix copy operator
  irtkMatrix& operator =(const irtkMatrix&);

  /// Matrix subtraction operator
  irtkMatrix& operator-=(const irtkMatrix&);

  /// Matrix addition operator
  irtkMatrix& operator+=(const irtkMatrix&);

  /// Matrix multiplication operator
  irtkMatrix& operator*=(const irtkMatrix&);

  /// Return result of matrix subtraction
  irtkMatrix  operator- (const irtkMatrix&);

  /// Return result of matrix addition
  irtkMatrix  operator+ (const irtkMatrix&);

  /// Return result of matrix multiplication
  irtkMatrix  operator* (const irtkMatrix&);

  /// Matrix inversion operator
  irtkMatrix  operator! (void);

  /// Matrix transpose operator
  irtkMatrix  operator~ (void);

  /// Matrix comparison operator =
  int operator==(const irtkMatrix &);

  // Matrix exponential via Pade approximation.
  // See Golub and Van Loan, Matrix Computations, Algorithm 11.3-1.
  friend irtkMatrix expm(irtkMatrix);

  // Matrix logarithm.
  friend irtkMatrix logm(irtkMatrix);

  // Matrix square root.
  friend irtkMatrix sqrtm(irtkMatrix);

  friend irtkMatrix FrechetMean(irtkMatrix *, int, int = 10);

  friend irtkMatrix FrechetMean(irtkMatrix *, double *, int, int = 10);

#ifndef USE_STL
  /// Comparison operator != (if USE_STL is defined, negate == operator)
  int operator!=(const irtkMatrix &);
#endif

  //
  // Matrix functions
  //

  /// Calculate norm of matrix
  double Norm(void) const;

  /// Calculate trace of matrix
  double Trace(void) const;

  // The infinity norm is the maximum of the absolute value row sums.
  double InfinityNorm(void) const;

  /// Calculate determinant of matrix
  double Det() const;

  /* Calculate SVD of matrix
   * now does the same as Eigenvalues (but eigenvalues are in square root)
   * algorithm chosen according to matrix properties (symmetry, diagonizability)
   */
  void   SVD(irtkMatrix &, irtkVector &, irtkMatrix &);

  /// Identity matrix
  void   Ident();

  /// Returns true if the matrix is an identity matrix.
  bool IsIdentity() const;

  /// Invert of matrix
  void   Invert();

  /// Adjugate of matrix and return determine;
  void   Adjugate(double &);

  /// Transpose matrix
  void   Transpose();

  /* Calculate eigenvalues and eigenvectors of matrix
   * now does the same as SVD (but eigenvalues are NOT in square root)
   * algorithm chosen according to matrix properties (symmetry, diagonizability)
   */
  void   Eigenvalues(irtkMatrix &, irtkVector &, irtkMatrix &);

  /// Calculate least square fit via SVD
  void   LeastSquaresFit(const irtkVector &, irtkVector &);

  //
  // Matrix in- and output
  //

  /// Interface to output stream
  friend std::ostream& operator<< (std::ostream&, const irtkMatrix&);

  /// Interface to input stream
  friend std::istream& operator>> (std::istream&, irtkMatrix&);

  /// Print matrix
  void Print();

  /// Read matrix from file
  void Read (char *);

  /// Write matrix to file
  void Write(char *);

  /// Import matrix from text file (requires no. of expected rows and cols)
  void Import (char *, int, int);

#ifdef USE_VXL

  /// Conversion to VNL matrix
  template <class T> void Matrix2Vnl(vnl_matrix<T> *m) const;

  /// Conversion from VNL matrix
  template <class T> void Vnl2Matrix(vnl_matrix<T> *m);

#else

  /// Conversion to GSL matrix (memory must be allocated)
  void Matrix2GSL(gsl_matrix *) const;

  /// Conversion from GSL matrix
  void GSL2Matrix(gsl_matrix *);

  //
  // Matrix operators for vectors
  //

  /// Return result of multiplication of matrix and vector
  irtkVector  operator* (const irtkVector&);

#endif
};

//
// Access operators
//

inline int irtkMatrix::Rows() const
{
  return _rows;
}

inline int irtkMatrix::Cols() const
{
  return _cols;
}

inline void irtkMatrix::Put(int rows, int cols, double matrix)
{
#ifdef NO_BOUNDS
  _matrix[cols][rows] = matrix;
#else
  if ((rows >= 0) && (rows < _rows) && (cols >= 0) && (cols < _cols)) {
    _matrix[cols][rows] = matrix;
  } else {
    cout << "irtkMatrix::Put: parameter out of range\n";
  }
#endif
}

inline double irtkMatrix::Get(int rows, int cols) const
{
#ifdef NO_BOUNDS
  return _matrix[cols][rows];
#else
  if ((rows >= 0) && (rows < _rows) && (cols >= 0) && (cols < _cols)) {
    return _matrix[cols][rows];
  } else {
    cout << "irtkMatrix::Get: parameter out of range\n";
    return 0;
  }
#endif
}

inline double &irtkMatrix::operator()(int rows, int cols)
{
#ifdef NO_BOUNDS
  return _matrix[cols][rows];
#else
  if ((rows >= 0) && (rows < _rows) && (cols >= 0) && (cols < _cols)) {
    return _matrix[cols][rows];
  } else {
    cout << "irtkMatrix::operator(): parameter out of range\n";
    return _matrix[0][0];
  }
#endif
}

inline double irtkMatrix::operator()(int rows, int cols) const
{
#ifdef NO_BOUNDS
  return _matrix[cols][rows];
#else
  if ((rows >= 0) && (rows < _rows) && (cols >= 0) && (cols < _cols)) {
    return _matrix[cols][rows];
  } else {
    cout << "irtkMatrix::operator(): parameter out of range\n";
    return 0;
  }
#endif
}

//
// Matrix operators for doubles
//

inline irtkMatrix& irtkMatrix::operator-=(const double &x)
{
  int i, j;

  for (j = 0; j < _cols; j++) {
    for (i = 0; i < _rows; i++) {
      _matrix[j][i] -= x;
    }
  }
  return *this;
}

inline irtkMatrix& irtkMatrix::operator+=(const double &x)
{
  int i, j;

  for (j = 0; j < _cols; j++) {
    for (i = 0; i < _rows; i++) {
      _matrix[j][i] += x;
    }
  }
  return *this;
}

inline irtkMatrix& irtkMatrix::operator*=(const double &x)
{
  int i, j;

  for (j = 0; j < _cols; j++) {
    for (i = 0; i < _rows; i++) {
      _matrix[j][i] *= x;
    }
  }
  return *this;
}

inline irtkMatrix& irtkMatrix::operator/=(const double &x)
{
  int i, j;

  for (j = 0; j < _cols; j++) {
    for (i = 0; i < _rows; i++) {
      _matrix[j][i] /= x;
    }
  }
  return *this;
}

inline irtkMatrix irtkMatrix::operator-(const double &x)
{
  int i, j;
  irtkMatrix m;

  m = *this;
  for (j = 0; j < _cols; j++) {
    for (i = 0; i < m._rows; i++) {
      m._matrix[j][i] = _matrix[j][i] - x;
    }
  }
  return m;
}

inline irtkMatrix irtkMatrix::operator+(const double &x)
{
  int i, j;
  irtkMatrix m;

  m = *this;
  for (j = 0; j < _cols; j++) {
    for (i = 0; i < m._rows; i++) {
      m._matrix[j][i] = _matrix[j][i] + x;
    }
  }
  return m;
}

inline irtkMatrix irtkMatrix::operator*(const double &x)
{
  int i, j;
  irtkMatrix m;

  m = *this;
  for (j = 0; j < _cols; j++) {
    for (i = 0; i < m._rows; i++) {
      m._matrix[j][i] = _matrix[j][i] * x;
    }
  }
  return m;
}

inline irtkMatrix irtkMatrix::operator/(const double &x)
{
  int i, j;
  irtkMatrix m;

  m = *this;
  for (j = 0; j < _cols; j++) {
    for (i = 0; i < m._rows; i++) {
      m._matrix[j][i] = _matrix[j][i] / x;
    }
  }
  return m;
}

//
// Matrix operators for matrices
//

inline int irtkMatrix::operator==(const irtkMatrix& m)
{
  int i, j;

  if ((m._rows != _rows) || (m._cols != _cols)) return 0;
  for (j = 0; j < _cols; j++) {
    for (i = 0; i < m._rows; i++) {
      if (m._matrix[j][i] != _matrix[j][i]) return 0;
    }
  }
  return 1;
}

#ifndef USE_STL
inline int irtkMatrix::operator!=(const irtkMatrix& m)
{
  return !(*this == m);
}
#endif

inline double irtkMatrix::Trace(void) const
{
    int i, j;
    double trace = 0;

    if(_rows == _cols){

        // The trace of a matrix
        for (j = 0; j < _cols; j++) {
            for (i = 0; i < _rows; i++) {
                trace += _matrix[j][i];
            }
        }
        return trace;
    }else{
        std::cerr << "irtkMatrix::Trace() matrix number of col != row" << std::endl;
        return 0;
    }
}

inline double irtkMatrix::Norm(void) const
{
  int i, j;
  double norm = 0;

  // The norm of a matrix M is defined as trace(M * M~)
  for (j = 0; j < _cols; j++) {
    for (i = 0; i < _rows; i++) {
      norm += _matrix[j][i]*_matrix[j][i];
    }
  }
  return std::sqrt(norm);
}

inline double irtkMatrix::InfinityNorm(void) const
{
  int i, j;
  double normInf = -1.0 * DBL_MAX;
  double sum;

  for (i = 0; i < _rows; ++i) {
    sum = 0;
    for (j = 0; j < _cols; ++j) {
      sum += abs(_matrix[j][i]);
    }
    if (sum > normInf)
      normInf = sum;
  }
  return normInf;
}

#endif


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

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

#ifndef HMM_IO_H_
#define HMM_IO_H_

#include <map>
#include <istream>
#include <Eigen/Dense>
#include <gsl_assert.h>
#include <range/v3/all.hpp>
#include <iostream>

#include "maikel/hmm/hidden_markov_model.h"
#include "maikel/function_profiler.h"

namespace maikel { namespace hmm {

  struct getline_error: public std::runtime_error {
      getline_error(std::string s): std::runtime_error(s) {}
  };

  struct read_ascii_matrix_error: public std::runtime_error {
      read_ascii_matrix_error(std::string s): std::runtime_error(s) {}
  };

  struct read_sequence_error: public std::runtime_error {
      read_sequence_error(std::string s): std::runtime_error(s) {}
  };

  inline std::istream& getline(std::istream& in, std::istringstream& linestream)
  {
    Expects(in);
    std::string line;
    if (!std::getline(in, line))
      throw getline_error("Could not read the line from given stream.");
    linestream.str(line);
    linestream.clear();
    return in;
  }

  template <class size_type>
    std::pair<size_type, size_type> getdims(std::istream& in)
    {
      Expects(in);
      std::pair<size_type, size_type> dim;
      std::istringstream line(ranges::front(ranges::getlines(in)));
      if (!(line >> dim.first >> dim.second))
        throw read_ascii_matrix_error("Could not read dimensions.");
      return dim;
    }

  template <class float_type>
    typename std::enable_if<
        std::is_floating_point<float_type>::value,
    Eigen::Matrix<float_type, Eigen::Dynamic, Eigen::Dynamic>>::type
    read_ascii_matrix(std::istream& in, std::size_t rows, std::size_t cols)
    {
      Expects(in);
      typename Eigen::Matrix<float_type, Eigen::Dynamic, Eigen::Dynamic> matrix(rows, cols);
      std::istringstream line;
      for (std::size_t i = 0; i < rows; ++i) {
        getline(in, line);
        for (std::size_t j = 0; j < cols; ++j)
          if (!(line >> matrix(i,j)))
            throw read_ascii_matrix_error("Could not read entries in line: " + line.str() + ".");
      }
      Ensures(gsl::narrow<std::size_t>(matrix.rows()) == rows &&
              gsl::narrow<std::size_t>(matrix.cols()) == cols);
      return matrix;
    }

  template <class Derived>
    Eigen::DenseBase<Derived>& normalize_rows(Eigen::DenseBase<Derived>& matrix)
    {
      using Index = typename Eigen::DenseBase<Derived>::Index;
      for (Index i = 0; i < matrix.rows(); ++i)
        matrix.row(i) /= matrix.row(i).sum();
      return matrix;
    }

  template <class float_type>
    typename std::enable_if<
        std::is_floating_point<float_type>::value,
    hidden_markov_model<float_type>>::type
    read_hidden_markov_model(std::istream& in)
    {
      Expects(in);
      using matrix     = typename hidden_markov_model<float_type>::matrix;
      using row_vector = typename hidden_markov_model<float_type>::row_vector;
      using index      = typename hidden_markov_model<float_type>::size_type;
      index states;
      index symbols;

      std::tie(states, symbols) = getdims<index>(in);
      matrix A  = read_ascii_matrix<float_type>(in, states, states);
      matrix B  = read_ascii_matrix<float_type>(in, states, symbols);
      row_vector pi = read_ascii_matrix<float_type>(in, 1, states);
      normalize_rows(A);
      normalize_rows(B);
      normalize_rows(pi);

      Ensures(A.rows() == states && A.cols() == states);
      Ensures(B.rows() == states && B.cols() == symbols);
      Ensures(pi.rows() == 1 && pi.cols() == states);
      return hidden_markov_model<float_type>(A, B, pi);
    }

  template <class float_type>
    typename std::enable_if<
        std::is_floating_point<float_type>::value,
    void>::type
    print_model_parameters(std::ostream& out, hidden_markov_model<float_type> const& model)
    {
      out << "N= " << model.states() << "\n";
      out << "M= " << model.symbols() << "\n";
      out << "A:\n" << model.A << "\n";
      out << "B:\n" << model.B << "\n";
      out << "pi:\n" << model.pi << "\n";
      out << std::flush;
    }

  template <class size_type>
    size_type read_sequence_length(std::istream& in)
    {
      std::string line;
      std::getline(in, line);
      std::istringstream linestream(line);
      size_type length;
      linestream >> length;
      return length;
    }

  template <class Integral>
    std::map<std::string, Integral>
    read_symbol_map(std::istream& in)
    {
      std::map<std::string, Integral> symbol_to_index;
      Integral count{};
      std::string buffer;
      std::getline(in, buffer);
      std::istringstream lstream(buffer);
      while (lstream >> buffer)
        if (symbol_to_index.insert(make_pair(buffer, count)).second)
          ++count;
      return symbol_to_index;
    }

  template <class Integral>
    std::vector<Integral>
    read_sequence(std::istream& in)
    {
      std::vector<Integral> sequence;
      std::map<std::string, Integral> symbol_to_index = read_symbol_map<Integral>(in);
      sequence.reserve(read_sequence_length<std::size_t>(in));
      auto symbol_map = [&symbol_to_index] (std::string const& symbol) {
          auto found = symbol_to_index.find(symbol);
          if (found == symbol_to_index.end())
            throw read_sequence_error("Unkown Symbols in Input.");
          return found->second;
      };
      auto sequence_input = ranges::istream_range<std::string>(in);
      ranges::copy(sequence_input | ranges::view::transform(symbol_map), ranges::back_inserter(sequence));
      return sequence;
    }

  template <class Integral, class Symbol>
    std::vector<Integral>
    read_sequence(std::istream& in, std::map<Symbol,Integral>& symbol_to_index)
    {
      MAIKEL_PROFILER;
      std::vector<Integral> sequence;
      sequence.reserve(read_sequence_length<std::size_t>(in));
      auto symbol_map = [&symbol_to_index] (Symbol const& symbol) {
          auto found = symbol_to_index.find(symbol);
          if (found == symbol_to_index.end())
            throw read_sequence_error("Unkown Symbols in Input.");
          return found->second;
      };
      auto sequence_input = ranges::istream_range<Symbol>(in);
      ranges::copy(sequence_input | ranges::view::transform(symbol_map), ranges::back_inserter(sequence));
      return sequence;
    }
} // namespace hmm
} // namespace maikel

#endif /* HMM_IO_H_ */


/*! \file profile_coord_map.h
    \brief Grid initialization, finding flux coordinates, and getting profile data

    Called by all the top-level functions.
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <Python.h>
#include <gsl/gsl_vector.h>	// needed for 2-d root-finding
#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_linalg.h>
#include "eq_quantity.h"

#ifndef REAL_TYPEDEF
#define REAL_TYPEDEF
#ifndef single			// compiler option determines variable size
typedef double REAL;
#else
typedef float REAL;
#endif
#endif

#ifndef DEBUG			// default debug level: no output to stderr
#define DEBUG 0
#endif

#ifndef DEVELOPMENT		// default development level: use the best of what has been tested to work
#define DEVELOPMENT 0
#endif

#ifndef VERBOSE
#define VERBOSE 0
#endif

#define MAX_N_TRIES 4		// number of times to try to get the root-finder unstuck

// used by desiredrz: constants to choose the desired grid in R,Z space
// used by desiredxyz: constants for desired 3d grid in x,y,z
extern double Xmin,Xmax,Ymin,Ymax,Zmin,Zmax;

// used by getAllProfiles
#define NORM_TO_KEV 4.7894e4      // conversion constant from normalized units to keV (called Tem_00 in fortran files)  Normalization in GTS: Length 100cm, Time Deuterium Cyclotron frequency in 1T B field -- OMEGA_D^-1,
// T normalized st. vth = sqrt(T).  Temperature has the energy dimention.

#define INVCM3_TO_INVM3 1e6	// convert cm^{-3} to m^{-3}

// used by decayNToutsideLCFS and getBoundaryPoints
#define ABOUNDARY 0.9
#define AINSIDE 0.88
#define AZERO 1.2
//#define DECAY_SCALING 0.1*(ABOUNDARY-AINSIDE) // the 0.1 makes it drop off faster

#define MINIMUM(x,y) ((x) < (y) ? (x) : (y))

extern char* NTFileName;

// ---------- local function declarations
int cylToLargeAR(REAL R[],REAL Z[], REAL r[], REAL theta[],int n,REAL R0,REAL Z0);
int largeARtoCyl(REAL r[],REAL theta[], REAL R[], REAL Z[],int n,REAL R0,REAL Z0);
int desiredrz(REAL R[],REAL Z[],REAL R1d[],REAL Z1d[],int nr,int nz,REAL R0,REAL Z0);
int desiredxyz(int nx,int ny,int nz,REAL x1d[],REAL y1d[],REAL z1d[],REAL x[],REAL y[],REAL z[],REAL R0,REAL Z0);
int mesh2dGrid(REAL x2d[],REAL y2d[],REAL x1d[],REAL y1d[],int nx,int ny);
int mesh3dGrid(REAL x3d[],REAL y3d[],REAL z3d[],REAL x1d[],REAL y1d[],REAL z1d[],int nx,int ny,int nz);

int guessatheta(REAL a[],REAL theta[], REAL R[], REAL Z[],int n,REAL R0,REAL Z0);
REAL error_amt(REAL R,REAL Z,REAL a,REAL theta);
void print_state(size_t iter, gsl_multiroot_fsolver * s);
int findcorrectatheta(REAL a[],REAL theta[],REAL Ract[],REAL Zact[],REAL Bm[],REAL Rwant[], REAL Zwant[],int n,int flag[],int location[],int mismatch[]);
int coord_f(const gsl_vector *x,void *params,gsl_vector *f);
int getBoundaryPoints(double** R_bdy,double** Z_bdy,int n_bdy);
void free_BoundaryPoints(double* R_bdy,double* Z_bdy);

int get_mag_axis(REAL coords[]);
int decayNToutsideLCFS(int npts,REAL a[],REAL ne[],REAL Te[],REAL Ti[],int* flag);

int getFluxCoords(int npts,REAL a[],REAL theta[],REAL Bm[],REAL Ract[],REAL Zact[],REAL Rinitial[],REAL Zinitial[],REAL Rwant[],REAL Zwant[],REAL mag_axis_coords[],int flag[],int location[], int mismatch[]);

int getAllProfiles(int npts,REAL Bpol[],REAL BR[],REAL BZ[],REAL Ti[],REAL Te[],REAL P[],REAL ne[],REAL qprofile[],REAL a[],REAL theta[],REAL Rwant[],REAL Zwant[],REAL R_bdy[],REAL Z_bdy[],int InOutFlag[],int location[]);

int getProfile(REAL a,REAL* ne,REAL* Ti,REAL* Te,REAL* a_p,REAL* n_p,REAL* Ti_p,REAL* Te_p,int n_prof );

int adiabaticElectronResponse(int npts,int ntimesteps,REAL ne_tilde[],REAL ne0[],REAL phi[],REAL Te[],int flag[]);

//modified by Lei Shi adding function to find the outside points

//declaration
void inner_check(double R[],double Z[],double R_bdy[],double Z_bdy[],int flag[],int n,int n_bdy,double tol);
//R,Z contains grid point coordinates, R_bdy Z_bdy contains boundary points, flag show inside(with value 1) and outside(with value 0), n the number of grid points, n_bdy the number of boundary points.

//if (R[i],Z[i]) is inside or on the boundary of the contour specified by R_bdy and Z_bdy, flag[i] is set to 1, otherwise 0;

//definition

int horipos_check(double Rc,double Zc, double R1,double Z1,double R2,double Z2,double tol){
  double R=R1+ (Zc-Z1)/(Z2-Z1)*(R2-R1);

  if(fabs(R-Rc)<=tol)
    return 2;
  else if(R<Rc)
    return 1;
  else
    return -1;
}

void inner_check(double R[],double Z[],double R_bdy[],double Z_bdy[],int flag[],int n,int n_bdy,double tol){

  int i;

  for(i=0;i<n;i++){
    double Rc=R[i];
    double Zc=Z[i];

    flag[i]=0;

    int vertpos=0;
    int horipos1=0;
    int horipos2=0;
    int j;
    for(j=0;j<n_bdy;j++){
      if(Z_bdy[j]==Zc){
	if (horipos1==0)
	  {
	    if(fabs(R_bdy[j]-Rc) < tol)
	      horipos1=2;
	    else if(R_bdy[j]<Rc)
	      horipos1=1;
	    else
	      horipos1=-1;
	    continue;
	  }
	else{
	  {
	    if(fabs(R_bdy[j]-Rc) < tol)
	      horipos2=2;
	    else if(R_bdy[j]<Rc)
	      horipos2=1;
	    else
	      horipos2=-1;
	    flag[i]= (horipos1 != horipos2)?1:0;
	    break;
	  }
	}
      }
      else if(Z_bdy[j]<Zc){
	if(vertpos==0){
	  vertpos = -1;
	  continue;
	}
	else if(vertpos==-1)
	  continue;
	else{
	  if(horipos1 == 0){
	    vertpos = -1;
	    horipos1 = horipos_check(Rc,Zc,R_bdy[j],Z_bdy[j],R_bdy[j-1],Z_bdy[j-1],tol);
	    continue;
	  }
	  else{
	    horipos2=horipos_check(Rc,Zc,R_bdy[j],Z_bdy[j],R_bdy[j-1],Z_bdy[j-1],tol);
	    flag[i]= (horipos1 != horipos2)?1:0;
	    break;
	  }

	}

      }
      else{
	if(vertpos==0){
	  vertpos = 1;
	  continue;
	}
	else if(vertpos == 1)
	  continue;
	else{
	  if(horipos1 == 0){
	    vertpos = 1;
	    horipos1 = horipos_check(Rc,Zc,R_bdy[j],Z_bdy[j],R_bdy[j-1],Z_bdy[j-1],tol);
	    continue;
	  }
	  else{
	    horipos2=horipos_check(Rc,Zc,R_bdy[j],Z_bdy[j],R_bdy[j-1],Z_bdy[j-1],tol);
	    flag[i]= (horipos1 != horipos2)?1:0;
	    break;
	  }
	}
      }
    }
  }
}

//end modification

// ---------- data structures
struct coordparams{ 		// structure for 2D root finding
  REAL Rwant;
  REAL Zwant;
};

// ---------- function definitions

//! Convert cylindrical coords (R,Z) to a large aspect ratio approximation of (r,theta)
/*! For n points, where (R0,Z0) is the position of the magnetic axis */
int cylToLargeAR(REAL R[],REAL Z[], REAL r[], REAL theta[],int n,REAL R0,REAL Z0){
  REAL x,z;
  int i;

  for(i=0;i<n;i++){		// here I use: R-R0 = a*cos(theta)
    x = R[i]-R0;		//    and Z = a*sin(theta)
    z = Z[i]-Z0;
    theta[i]=-atan2(z,x);	// poloidal angle (correct quadrant)
    //    r[i] = sqrt(x*x+z*z);	// minor rad.
    r[i] = hypot(x,z);
  }

  return 0;
}

//! Convert a large aspect ratio (r,theta) to cylindrical coords (R,Z)
/*! For n points, where (R0,Z0) is the position of the magnetic axis */
int largeARtoCyl(REAL r[],REAL theta[], REAL R[], REAL Z[],int n,REAL R0,REAL Z0){
  int i;
  for(i=0;i<n;i++){		// here I use: R-R0 = a*cos(theta)
    R[i] = R0 + r[i]*cos(theta[i]);
    Z[i] = Z0 - r[i]*sin(theta[i]);
  }

  return 0;
}

//! Generate a regular, 2D array meshes in (R,Z), and also 1D arrays for each dimension
/*! 2D arrays vary fastest along R (columns) and slowly along Z (rows).
  Also generates 1D arrays R1d, Z1d.
  nr: points in R, nz: # points in z; total # points = nr*nz
  R0,Z0 are the coordinates for the magnetic axis, which is not currently used.
*/
int desiredrz(REAL X[],REAL Y[],REAL X1d[],REAL Y1d[],int nx,int ny,REAL X0,REAL Y0){
  int i;
  REAL dx = (Xmax-Xmin)/(nx-1);
  REAL dy = (Ymax-Ymin)/(ny-1);
  for(i=0;i<ny;i++) Y1d[i] = (REAL)Ymin + i*dy;
  for(i=0;i<nx;i++) X1d[i] = (REAL)Xmin + i*dx;
  mesh2dGrid(X,Y,X1d,Y1d,nx,ny);
  // printf("desiredrz: dr:%g,dz:%g\n",dr,dz);
  // for(i=0;i<nz;i++){
//     Z1d[i] = (REAL)ZMIN + i*dz;
//     for(j=0;j<nr;j++){
//       R[i*nr+j] = (REAL)RMIN + j*dr;
//       Z[i*nr+j] = (REAL)ZMIN + i*dz;
//     }
//   }
  return 0;
}

//! Generate a regular, 3D array meshes in (x,y,z), and also 1D arrays for each dimension
/*! 3D arrays vary fastest along x (columns), then along y (rows), slowest along z (pages).
  x is locally radially outward, y is vertically upward, z is locally toroidal to make a
  right-hand coordinate system.
  nx: points in x direction, ny: points in y direction, nz: points in z direction;
  total # points = nx*ny*nz.
  R0,Z0 are the coordinates for the magnetic axis, which is not currently used.
*/
int desiredxyz(int nx,int ny,int nz,REAL x1d[],REAL y1d[],REAL z1d[],REAL x[],REAL y[],REAL z[],REAL X0,REAL Y0){
  int i,j,k;
  REAL dx = (Xmax-Xmin)/(nx-1);
  REAL dy = (Ymax-Ymin)/(ny-1);
  REAL dz = (Zmax-Zmin)/(nz-1);
#if DEBUG > 1
  fprintf(stderr,"desiredrz: dx:%g,dy:%g,dz:%g\n",dx,dy,dz);
#endif
  for(i=0;i<nz;i++) z1d[i] = (REAL)Zmin + i*dz;
  for(i=0;i<ny;i++) y1d[i] = (REAL)Ymin + i*dy;
  for(i=0;i<nx;i++) x1d[i] = (REAL)Xmin + i*dx;
  mesh3dGrid(x,y,z,x1d,y1d,z1d,nx,ny,nz);
  // for(i=0;i<nz;i++){
//     z1d[i] = (REAL)zMIN + i*dz;
//     for(j=0;j<ny;j++){
//       for(k=0;k<nx;k++){
// 	x[i*ny*nx+j*nx+k] = (REAL)xMIN + k*dx;
// 	y[i*ny*nx+j*nx+k] = (REAL)yMIN + j*dy;
// 	z[i*ny*nx+j*nx+k] = (REAL)zMIN + i*dz;
//       }
//     }
//   }
  return 0;
}

//! Generate 2d mesh grids from 1d grids in each dimension
/*! Takes 1d arrays of coordinates x1d,y1d and generates 2d arrays
    x2d,y2d of nx columns and ny rows, corresponding to the x,y
    coordinate of each point.
*/
int mesh2dGrid(REAL x2d[],REAL y2d[],REAL x1d[],REAL y1d[],int nx,int ny){
  int i,j;
  for(i=0;i<ny;i++){
    for(j=0;j<nx;j++){
      x2d[i*nx+j] = x1d[j];
      y2d[i*nx+j] = y1d[i];
    }
  }

  return 0;
}

//! Generate 3d mesh grids from 1d grids in each dimension
/*! Takes 1d arrays of coordinates x1d,y1d,z1d and generates 3d arrays
    x3d,y3d,z3d of nx columns, ny rows, and nz pages corresponding to the x,y,z
    coordinates of each point.
*/
int mesh3dGrid(REAL x3d[],REAL y3d[],REAL z3d[],REAL x1d[],REAL y1d[],REAL z1d[],int nx,int ny,int nz){
  int i,j,k;
 for(i=0;i<nz;i++){
     for(j=0;j<ny;j++){
      for(k=0;k<nx;k++){
	x3d[i*ny*nx+j*nx+k] = x1d[k];
	y3d[i*ny*nx+j*nx+k] = y1d[j];
	z3d[i*ny*nx+j*nx+k] = z1d[i];
      }
    }
  }

    return 0;
}

//! Given cylindrical coords (R,Z) calculates the initial guess for the flux coords (a,theta)
// first guess as to what the correct a,theta values are for the desired R,Z points
// this is the "large aspect ratio, circ. xsection where I take a = minor radius and
// theta is simply poloidal angle
// then I modify the angle by doing an interpolation
// n is total # of points in 1d array
int guessatheta(REAL a[],REAL theta[], REAL R[], REAL Z[],int n,REAL R0,REAL Z0){
  int i,k;
  int n1dgrid = (int)sqrt(n);
  int ngrid = n1dgrid*n1dgrid;//+10;

#if DEVELOPMENT >= 0

  REAL rwant[n],polwant[n];
  k = cylToLargeAR(R,Z,rwant,polwant,n,R0,Z0);// the minor radius and polodial angles of the R,Z we want

  // --- current method: 2D interpolation for 2 variables:
  //       (rgrid,polgrid) -> a
  //       (rgrid,polgrid) -> theta
  REAL agrid[ngrid],thetagrid[ngrid],Rgrid[ngrid],Zgrid[ngrid];
  REAL rgrid[ngrid],polgrid[ngrid],Bgrid[ngrid];

  // set up a grid to interpolate on
  // use to get an even grid in a,theta
  double da = 1.5/(n1dgrid-1);
  double dtheta = 2.0*M_PI/n1dgrid;
  for(i=0;i<n1dgrid;i++){
    for(k=0;k<n1dgrid;k++){
      agrid[i*n1dgrid+k] = 0.5*da + i*da;
      thetagrid[i*n1dgrid+k] = -M_PI+k*dtheta;
    }
  }

  k = esigetrzb_(Rgrid,Zgrid,Bgrid,agrid,thetagrid,&ngrid); // get Rgrid and Zgrid
  k = cylToLargeAR(Rgrid,Zgrid,rgrid,polgrid,ngrid,R0,Z0);	// get the grid minor radius and poloidal angle

  // interpolate using (rgrid,polgrid) -> (agrid,thetagrid)
  // to get (rwant,thetawant) -> (a,theta)
  k = interp2d(n,ngrid,rwant,polwant,a,theta,rgrid,polgrid,agrid,thetagrid);

#if DEBUG > 0
  fprintf(stderr,"point  agrid   thetagrid  Rgrid  Zgrid  rgrid  polgrid\n");
  for(i=0;i<ngrid;i++)
    fprintf(stderr,"%5d, %g, %g, %g, %g, %g, %g\n",i,agrid[i],thetagrid[i],
	    Rgrid[i],Zgrid[i],rgrid[i],polgrid[i]);
  fprintf(stderr,"point  a   theta  R  Z  rwant  polwant\n");
  for(i=0;i<n;i++){
    fprintf(stderr,"%5d, %g, %g, %g, %g, %g, %g\n",i,a[i],theta[i],
	    R[i],Z[i],rwant[i],polwant[i]);

  }

#endif

    for(i=0;i<n;i++) if(a[i]>2.5) a[i]=2.5;	// limit the maximum radial flux coord

#endif

  // // --- previous method: 1D interpolation of both variables independently
  //   //        polgrid -> (agrid=1,thetagrid)
  //   //        rgrid   -> (agrid,thetagrid=pi/6)

  //   REAL agrid[n1dgrid],thetagrid[n1dgrid],Rgrid[n1dgrid],Zgrid[n1dgrid];
  //   REAL rgrid[n1dgrid],polgrid[n1dgrid],Bgrid[n1dgrid];

  //   // first do the poloidal interpolation
  //   double dtheta = 2*M_PI/n1dgrid;
  //     for(i=0;i<n1dgrid;i++){
  //       agrid[i] = 1;
  //     thetagrid[i] = -M_PI+i*dtheta;
  //     }
  //   k = esigetrzb_(Rgrid,Zgrid,Bgrid,agrid,thetagrid,&n);
  //   k = cylToLargeAR(Rgrid,Zgrid,rgrid,polgrid,n,R0,Z0);	// get the grid minor radius and poloidal angle
  //   k = interpgsl(polwant,theta,polgrid,thetagrid,n,n1dgrid);  // interpolate in poloidal angle:

  //   // then do the radial interpolation
  //   double da = 1.5/(n1dgrid-1);
  //   for(i=0;i<n1dgrid;i++){
  //       agrid[i] = i*da;
  //     thetagrid[i] = M_PI/6;
  //     }
  //   k = esigetrzb_(Rgrid,Zgrid,Bgrid,agrid,thetagrid,&n1dgrid);
  //   k = cylToLargeAR(Rgrid,Zgrid,rgrid,polgrid,n1dgrid,R0,Z0);	// get minor rad. and pol. angle
  //   k = interpgsl(rwant,a,rgrid,agrid,n,n1dgrid); // interpolate radially

  //   // // modification that I discarded
  //   //   for(i=0;i<n;i++){
  //   //     a[i] += 1.5*a[i]*exp(-400.0*theta[i]*theta[i]/(M_PI*M_PI));//a[i]*1.5*fabs(exp(-fabs(theta[i])/M_PI)-exp(-1.0/4.0));//0.5*a[i]*(1-2*fabs(theta[i])/M_PI)*(1-2*fabs(theta[i])/M_PI);
  //   //   }
  //   // --- end method: 1D interpolation of both variables independently

  // --- solution in use previously
#if DEVELOPMENT < 0
  REAL dtheta=2*M_PI/(n-1);// divide flux-coord domain
  REAL x,alarge[n],q[n],Rmap[n],Zmap[n],Bmap[n],qpol[n];//,stheta[n-1];
  // guess a large-aspect ratio, circular cross section
  cylToLargeAR(R,Z,a,theta,n,R0,Z0);
  // for(i=0;i<n;i++){		// here I use: R-R0 = a*cos(theta)
  //     x = R[i]-R0;		//    and Z = a*sin(theta)
  //     theta[i]=-atan2(Z[i],x);	// poloidal angle (correct quadrant)
  //     a[i] = sqrt(x*x+Z[i]*Z[i]);	// minor rad.
  //   }

  // set up interpolation
  // modify large aspect ratio, circ. xsection by interpolating between angles
  // for large minor radius, map theta to actual poloidal angle:
  for(i=0;i<n;i++){//generate an array of evenly-spaced flux coords
    q[i] = -M_PI + dtheta*i;	// to interpolate what qpol (R,Z), q (a,theta)
    alarge[i] = 1;		// is mapped to
  }
  k = esigetrzb_(Rmap,Zmap,Bmap,alarge,q,&n);
  // check to see the poloidal angle these evenly-spaced flux coords mapped to
  for(i=0;i<n;i++){		// here I use: R-R0 = a*cos(theta), Z = a*sin(theta)
    x = Rmap[i]-R0;
    qpol[i]=-atan2(Zmap[i],x);	// poloidal angle (correct quadrant)
    //  rmax[i]=sqrt(x*x+Z[i]*Z[i]);
    //printf("%g,",qpol[i]);
  }
  // now interpolate theta
  REAL thetatmp[n];
  k = interpgsl(theta,thetatmp,qpol,q,n,n);
  for(i=0;i<n;i++) theta[i]=thetatmp[i];      //uncomment to skip interpolation

  // for(i=0;i<n;i++){
  //   a[i] += 1.5*a[i]*exp(-400.0*theta[i]*theta[i]/(M_PI*M_PI));//a[i]*1.5*fabs(exp(-fabs(theta[i])/M_PI)-exp(-1.0/4.0));//0.5*a[i]*(1-2*fabs(theta[i])/M_PI)*(1-2*fabs(theta[i])/M_PI);
  // }  //not worth it; only removed 12 total iterations
  // --- end solution in use previously
#endif

  return 0;

}

//! the 2D function solved by the root finder
int coord_f(const gsl_vector *x,void *params,gsl_vector *f){
  REAL Rwant = ((struct coordparams *) params)->Rwant; // desired R,Z
  REAL Zwant = ((struct coordparams *) params)->Zwant;

  REAL a[1] = {gsl_vector_get (x, 0)}; // current a,theta
  REAL theta[1] = {gsl_vector_get (x, 1)};
  int n=1;
  int k;
  REAL Ract[1],Zact[1],Bm[1]; // actual R,Z at this value of a,theta

  if(!finite(a[0])) a[0] = ABOUNDARY; // in portalr4 this was necessary to eliminate faults
  if(!finite(theta[0])) theta[0] = 0.0;

  k = esigetrzb_(Ract,Zact,Bm,a,theta,&n); // solve for the actual R,Z values

  gsl_vector_set (f, 0, Rwant-Ract[0]); // the error
  gsl_vector_set (f, 1, Zwant-Zact[0]);
  return GSL_SUCCESS;
}

//! returns the distance between desired (R,Z) position and the position mapped to by flux coords (a,theta)
REAL error_amt(REAL R,REAL Z,REAL a,REAL theta){
  REAL Ract,Zact,Bact;
  int single=1;
  esigetrzb_(&Ract,&Zact,&Bact,&a,&theta,&single);

  //  return sqrt((R-Ract)*(R-Ract)+(Z-Zact)*(Z-Zact));
  return hypot(R-Ract,Z-Zact);
}

//! Print current status of the root-finder
void print_state(size_t iter, gsl_multiroot_fsolver * s){
  fprintf (stderr,"iter = %3u x = % .3f % .3f "
	   "f(x) = % .3e % .3e\n",
	   iter,
	   gsl_vector_get (s->x, 0),
	   gsl_vector_get (s->x, 1),
	   gsl_vector_get (s->f, 0),
	   gsl_vector_get (s->f, 1));
}

//! performs 2D root finding to get the (a,theta) for an array of given (R,Z) values
/*! Uses the gsl mulidimensional hybrids root solver ``gsl_multiroot_fsolver_hybrids''.  */
// Modified by Lei Shi
// use function esirz2agq to find (a,theta) for a given (R,Z) inside LCFS, and treat outside points seperately

//write CDF function show the flags

inline void ERR_CI(int e,const char* s)
{
  fprintf(stderr,"error:%s in %s\n",nc_strerror(e),s);
  exit(e);
}

void writeCDF(char* fname,int ntotal, double R[],double Z[],int n_bdy,double R_bdy[],double Z_bdy[],int flag[]){
  int R_dim_id,Z_dim_id,R_bdy_dim_id,Z_bdy_dim_id,flag_dim_id;
  int nr_id,nz_id,ntotal_id,n_bdy_id;
  int R_id,Z_id,R_bdy_id,Z_bdy_id,flag_id;
  int f_id;
  int ecode;
  if(ecode=nc_create(fname,NC_CLOBBER,&f_id))
    ERR_CI(ecode,"create file");
  if(ecode=nc_def_dim(f_id,"r_dim",ntotal,&R_dim_id))
    ERR_CI(ecode,"def r dim");
  if(ecode=nc_def_dim(f_id,"z_dim",ntotal,&Z_dim_id))
    ERR_CI(ecode,"def z dim");
  if(ecode=nc_def_dim(f_id,"r_bdy_dim",n_bdy,&R_bdy_dim_id))
    ERR_CI(ecode,"def r_bdy dim");
  if(ecode=nc_def_dim(f_id,"z__bdy_dim",n_bdy,&Z_bdy_dim_id))
    ERR_CI(ecode,"def z_bdy dim");
  if(ecode=nc_def_dim(f_id,"flag_dim",ntotal,&flag_dim_id))
    ERR_CI(ecode,"def flag dim");
  if(ecode=nc_def_var(f_id,"nr",NC_INT,0,NULL,&nr_id))
    ERR_CI(ecode,"def nr var");
  if(ecode=nc_def_var(f_id,"nz",NC_INT,0,NULL,&nz_id))
    ERR_CI(ecode,"def nz var");
  if(ecode=nc_def_var(f_id,"ntotal",NC_INT,0,NULL,&ntotal_id))
    ERR_CI(ecode,"def ntotal var");
  if(ecode=nc_def_var(f_id,"n_bdy",NC_INT,0,NULL,&n_bdy_id))
    ERR_CI(ecode,"def n_bdy var");
  if(ecode=nc_def_var(f_id,"r",NC_DOUBLE,1,&R_dim_id,&R_id))
    ERR_CI(ecode,"def r var");
  if(ecode=nc_def_var(f_id,"z",NC_DOUBLE,1,&Z_dim_id,&Z_id))
    ERR_CI(ecode,"def z var");
  if(ecode=nc_def_var(f_id,"r_bdy",NC_DOUBLE,1,&R_bdy_dim_id,&R_bdy_id))
    ERR_CI(ecode,"def r_bdy var");
  if(ecode=nc_def_var(f_id,"z_bdy",NC_DOUBLE,1,&Z_bdy_dim_id,&Z_bdy_id))
    ERR_CI(ecode,"def z_bdy var");
  if(ecode=nc_def_var(f_id,"flag",NC_INT,1,&flag_dim_id,&flag_id))
    ERR_CI(ecode,"def flag var");
  if(ecode=nc_enddef(f_id))
    ERR_CI(ecode,"enddef");

  if(ecode=nc_put_var_int(f_id,nr_id,&ntotal))
    ERR_CI(ecode,"put nr var");
  if(ecode=nc_put_var_int(f_id,nz_id,&ntotal))
    ERR_CI(ecode,"put nz var");
  if(ecode=nc_put_var_int(f_id,ntotal_id,&ntotal))
    ERR_CI(ecode,"put ntotal var");
  if(ecode=nc_put_var_int(f_id,n_bdy_id,&n_bdy))
    ERR_CI(ecode,"put n_bdy var");
  if(ecode=nc_put_var_double(f_id,R_id,R))
    ERR_CI(ecode,"put R var");
  if(ecode=nc_put_var_double(f_id,Z_id,Z))
    ERR_CI(ecode,"put Z var");
  if(ecode=nc_put_var_double(f_id,R_bdy_id,R_bdy))
    ERR_CI(ecode,"put R_bdy var");
  if(ecode=nc_put_var_double(f_id,Z_bdy_id,Z_bdy))
    ERR_CI(ecode,"put Z_bdy var");
  if(ecode=nc_put_var_int(f_id,flag_id,flag))
    ERR_CI(ecode,"put flag var");

  if(ecode=nc_close(f_id))
    ERR_CI(ecode,"close file");

  FILE* txtfile;
  txtfile=fopen("./bdy_out.txt","w");
  fprintf(txtfile,"%d\n",n_bdy);
  int i;
  for(i=0;i<n_bdy;i++)
  {
    fprintf(txtfile,"%lf\t%lf\t",R_bdy[i],Z_bdy[i]);
  }
  fclose(txtfile);

}

int findcorrectatheta(REAL a[],REAL theta[],REAL Ract[], REAL Zact[],REAL Bm[], REAL Rwant[], REAL Zwant[],int n,int flag[],int location[], int mismatch[]){
  // find correct a theta coordinates, for inside LCFS points, a theta are obtained by ESIrz2agq function, for outside points, a is assumed to be proportional to the distance from the magnetic axis, and theta is chosen as the same value as the corresponding boundary point. The corresponding boundary point is chosen to be the boundary point closest to the connecting line of magnetic axis and the desired location.
  // Note that in this function, magnetude of B, BR and BZ are also calculated. BR and BZ calculation is not trivial, look into the attached file "Calc_BR_BZ.txt" for more detailed discussion.
  // new version starts

  extern int NBOUNDARY;
  extern double* R_bdy,*Z_bdy;
  fprintf(stderr,"start findcorrecttheta.\n");
  getBoundaryPoints(&R_bdy,&Z_bdy,NBOUNDARY);
  fprintf(stderr,"after getBoundaryPoints.\n");

  double b_axis;
  int single_point=1;
  double tol=1e-9;  //used in inner_check,when given point is near the bdy within tol distance, it's considered on the bdy.
  double dR,dZ;
  double match_tol = 1e-2;

  double theta_guess;
  double theta_bdy;
  double a_guess;
  double a_bdy=ABOUNDARY;
  double zero=0;

  double Z_rel;//the relative Z respect to mag_axis
  double R_rel;//the relative R respect to mag_axis

  double mag_axis_coords[2];
  int loc;
  int sign_ini;
  int sign_cur;

  char CDFfname[20]="./InOutFlags.cdf";
  char logfile[20]="./sl_log.txt";

  FILE* errlog;

  dR = fabs(Rwant[1]-Rwant[0]);
  match_tol *= dR; // absolute tolerance is the percentage tolerance times the finest grid resolution
  fprintf(stderr,"match_tol = %lf\n", match_tol);
  errlog=fopen(logfile,"w+");
  fprintf(errlog,"match_tol = %lf\n", match_tol);
  esigetrzb_(&mag_axis_coords[0],&mag_axis_coords[1],&b_axis,&zero,&zero,&single_point);

  fprintf(stderr,"before inner_check.\n");
  inner_check(Rwant,Zwant,R_bdy,Z_bdy,flag,n,NBOUNDARY,tol);
  fprintf(stderr,"after before write CDF.\n");
  writeCDF(CDFfname,n,Rwant,Zwant,NBOUNDARY,R_bdy,Z_bdy,flag);
  int i;

  /*  for(i=0;i<NBOUNDARY;i++){
    fprintf(errlog,"%lf\t%lf\n",R_bdy[i],Z_bdy[i]);
  }
  fprintf(errlog,"Boundary ends.\n");
  */
  int ierror;
  double b_tmp;

  fprintf(stderr,"Start getting locations.\n");
  for(i=0;i<n;i++){
    if(flag[i]==1){
      //fprintf(stderr,"point %d mapping start,",i);
      int k=ESIrz2agq(&a[i],&theta[i],&Rwant[i],&Zwant[i],&ierror,single_point);
      if(k!=0){
	fprintf(errlog,"Point %d (%lf,%lf) has encountered an error: %d in rz2agq.\n",i,Rwant[i],Zwant[i],ierror);
      }
      esigetrzb_(&Ract[i],&Zact[i],&Bm[i],&a[i],&theta[i],&single_point);
      double err=hypot((Rwant[i]-Ract[i]),(Zwant[i]-Zact[i]));
      double dis=hypot((Rwant[i]-mag_axis_coords[0]),(Zwant[i]-mag_axis_coords[1]));
      // output err and dis for checking
      //fprintf(errlog,"Point %d (%lf,%lf) %lf away from axis has been mapped %lf away\n",i,Rwant[i],Zwant[i],dis,err);
      if( err > match_tol){
	fprintf(errlog,"Point %d (%lf,%lf) %lf away from axis has been mapped %lf away\n",i,Rwant[i],Zwant[i],dis,err);
	mismatch[i] = 1;
	//fprintf(stderr,"Point %d (%lf,%lf) %lf away from axis has been mapped %lf away\n",i,Rwant[i],Zwant[i],dis,err);
	if(fabs(Zwant[i])<0.1)
	{
	  a[i]=0;
	  theta[i]=0;
	}
      }
      location[i]=-1; // inside points do not need location information
    }
    else if(flag[i]==0){
      //      fprintf(stderr,"outter point.\n");
      Bm[i]=b_axis*mag_axis_coords[0]/Rwant[i]; // let B inversely proportional to R, based on B on axis
      Ract[i]=Rwant[i];
      Zact[i]=Zwant[i];
      Z_rel=Zwant[i]-mag_axis_coords[1];
      R_rel=Rwant[i]-mag_axis_coords[0];
      int quarter=0;
      if(R_rel>=0){
	if(Z_rel>=0) quarter=1;
	else quarter =4;
      }
      else{
	if(Z_rel>=0) quarter=2;
	else quarter=3;
      }

      theta_guess=atan((Z_rel/R_rel));
      if(quarter == 2 || quarter == 3)
	theta_guess += M_PI;
      if(quarter == 4)
	theta_guess += 2*M_PI;

      Z_rel=Z_bdy[0]-mag_axis_coords[1];
      R_rel=R_bdy[0]-mag_axis_coords[0];
      theta_bdy=atan((Z_rel/R_rel));

      if(R_rel>=0){
	if(Z_rel>0) quarter=1;
	else quarter =4;
      }
      else{
	if(Z_rel>0) quarter=2;
	else quarter=3;
      }
      if(quarter == 2 || quarter == 3)
	theta_bdy+=M_PI;
      if(quarter == 4)
	theta_bdy += 2*M_PI;

      sign_ini=(theta_guess>=theta_bdy)?1:-1;

      int j;
      for(j=0;j<NBOUNDARY;j++)
	{
	  Z_rel=Z_bdy[j]-mag_axis_coords[1];
	  R_rel=R_bdy[j]-mag_axis_coords[0];
	  theta_bdy=atan((Z_rel/R_rel));

	  if(R_rel>=0){
	    if(Z_rel>0) quarter=1;
	    else quarter =4;
	  }
	  else{
	    if(Z_rel>0) quarter=2;
	    else quarter=3;
	  }
	  if(quarter == 2 || quarter == 3)
	    theta_bdy+=M_PI;
	  if(quarter == 4)
	    theta_bdy += 2*M_PI;//set the theta into (0,2Pi]

	  sign_cur=theta_guess>=theta_bdy?1:-1;
	  if(sign_ini*sign_cur<0){ //if passes the theta we want and is not on the opposite side, then we find the right boundary point
	    location[i]=j;
	    break;
	  }

	}
      if(j==NBOUNDARY)
	location[i]=0;

      Z_rel=Zwant[i]-mag_axis_coords[1];
      R_rel=Rwant[i]-mag_axis_coords[0];
      //      fprintf(stderr,"start guess a\n");
      a_guess=sqrt((Z_rel*Z_rel+R_rel*R_rel)/((R_bdy[loc]-mag_axis_coords[0])*(R_bdy[loc]-mag_axis_coords[0])+(Z_bdy[loc]-mag_axis_coords[1])*(Z_bdy[loc]-mag_axis_coords[1])))*a_bdy;
      a[i]=a_guess;
      theta[i]=2*M_PI/NBOUNDARY*loc;

    }
    else{
      fprintf(stderr,"flag error: point %d at (%lf,%lf) has flag %d.\n",i,Rwant[i],Zwant[i],flag[i]);
      exit(5);
    }
  }
  fprintf(stderr,"finish getting locations.\n");
  fclose(errlog);
  return 0;

  /*old version by Erik
  // declarations for root solver
  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;
  int status;			// status of root-finder (notifies if stuck, etc)

  size_t iter = 0;
  int i;
  int ntries;			// num of times for this point that we get stuck
  const size_t ndims = 2;	// 2D coordinate
  struct coordparams p;		// parameters used by ea. instance of solver: Rwant,Zwant
  gsl_multiroot_function f = {&coord_f, ndims, &p}; // root solver setup
  gsl_vector *x = gsl_vector_alloc (ndims); // R,Z values used by root solver

  T = gsl_multiroot_fsolver_hybrids; // solver type
  s = gsl_multiroot_fsolver_alloc (T, 2);

  // loop over all points
  for(i=0;i<n;i++){
    status = GSL_CONTINUE;	// reset root finder status

    // check to see if the nearest point provides a better initial estimate
    if(i>0){
      if(error_amt(Rwant[i],Zwant[i],a[i],theta[i]) > error_amt(Rwant[i],Zwant[i],a[i-1],theta[i-1])){ // if the nearest point is a better guess than the default initial guess, use the previous point
	a[i] = a[i-1];
	theta[i] = theta[i-1];
      }
    }

    // set up this instance of the root finder
    p.Rwant = Rwant[i];		// pass the desired coordinates as parameters
    p.Zwant = Zwant[i];
    gsl_vector_set (x, 0, a[i]);
    gsl_vector_set (x, 1, theta[i]);

    // try a few times with different initial conditions if the solver gets stuck
    for(ntries=0;ntries<MAX_N_TRIES;ntries++){
      if(status == GSL_SUCCESS) break; // in this case, the root-find was successful

      // choose a new initial condition if the solver is stuck
      if(ntries>0){
	#if DEVELOPMENT <= 0
	gsl_vector_set(x,0,0.2);
	gsl_vector_set(x,1,0.25*M_PI+ 0.8*gsl_vector_get(x,1));
	#else
	//	gsl_vector_set(x,0,0.2 + fmod(0.3+gsl_vector_get(x,0),1.2));
	gsl_vector_set(x,0,0.2 + fmod(0.4+gsl_vector_get(x,0),2.0));
	//	gsl_vector_set(x,1,-M_PI + fmod(1.5*M_PI+gsl_vector_get(x,1),2.0*M_PI));
	gsl_vector_set(x,1,-M_PI + fmod(1.25*M_PI+gsl_vector_get(x,1),2.0*M_PI));
	#endif
      }
      gsl_multiroot_fsolver_set (s, &f, x); // initialize the root finder
      iter = 0;			// reset iteration counter
      do{
	iter++;
	status = gsl_multiroot_fsolver_iterate (s);
#if DEBUG > 0
	fprintf(stderr,"point:%d, ntries:%d, status:%d, ",i,ntries,status);
	print_state (iter, s);	// uncomment to give info at each iteration
#endif
	if (status){   // check if solver is stuck
	  break;
	}
	status = gsl_multiroot_test_residual (s->f, 1e-7);
      }while (status == GSL_CONTINUE && iter < 200);

    }

    a[i] = gsl_vector_get(s->x,0);
    theta[i] = gsl_vector_get(s->x,1);
  }

  gsl_multiroot_fsolver_free (s);
  gsl_vector_free (x);

  // make sure all radial flux coords are positive (a>0)
  for(i=0;i<n;i++) a[i]=copysign(a[i],1.0);

  // get the actual R,Z coordinates we've mapped
  status = esigetrzb_(Ract,Zact,Bm,a,theta,&n);

#if VERBOSE > 0
  REAL err_dist;

  //modified by Lei start
  const int NBOUNDARY=4000;
  double R_bdy[NBOUNDARY],Z_bdy[NBOUNDARY];
  getBoundaryPoints(R_bdy,Z_bdy,NBOUNDARY);

  //  for(i=0;i<NBOUNDARY;i++){
  //    fprintf(stdout,"(%lf,%lf),",R_bdy[i],Z_bdy[i]);
  //  }

  for(i=0;i<n;i++){
    err_dist = hypot((Rwant[i]-Ract[i]),(Zwant[i]-Zact[i]));

    if(err_dist > 1e-7)//if the error is unnormally large, try to find a reasonable a and theta for that point, and adjust the B value
      {
	//	fprintf(stderr,"Warning, point %d is mapped %.4g [m] away from requested position.\n",i,err_dist);

	int single_point=1;
	double zero=0;
	double b;         // B field on the axis
	double theta_guess;
	double theta_bdy;
	double a_guess;
	double a_bdy=1;

	double Z_rel;//the relative Z respect to mag_axis
	double R_rel;//the relative R respect to mag_axis

	double mag_axis_coords[2];
	int loc;
	int sign_ini;
	int sign_cur;

	esigetrzb_(&mag_axis_coords[0],&mag_axis_coords[1],&b,&zero,&zero,&single_point);

	Bm[i]=b*mag_axis_coords[0]/Rwant[i]; // let B inversely proportional to R, based on B on axis

	Z_rel=Zwant[i]-mag_axis_coords[1];
	R_rel=Rwant[i]-mag_axis_coords[0];

	theta_guess=atan((Z_rel/R_rel));
	if(theta_guess<=0)
	  theta_guess += 2*M_PI;
	theta_bdy=atan((Z_bdy[0]-mag_axis_coords[1])/(R_bdy[0]-mag_axis_coords[0]));
	if(theta_bdy<=0)
	  theta_bdy+=2*M_PI;
	sign_ini=(theta_guess>=theta_bdy)?1:-1;
	//	fprintf(stderr,"pt[%d]:\n sign_ini = %d\n",i,sign_ini);
	int j;
	for(j=0;j<NBOUNDARY;j++)
	  {
	    theta_bdy=atan((Z_bdy[j]-mag_axis_coords[1])/(R_bdy[j]-mag_axis_coords[0]));
	    if(theta_bdy<=0)
	      theta_bdy += 2*M_PI;//set the theta into [0,2Pi)
	    sign_cur=theta_guess>=theta_bdy?1:-1;
	    if(sign_ini*sign_cur<0 && Zwant[i]*Z_bdy[j]>=0){ //if passes the theta we want and is not on the opposite side, then we find the right boundary point

	      loc=j;
	      break;
	    }

	  }

	a_guess=sqrt((Z_rel*Z_rel+R_rel*R_rel)/((R_bdy[loc]-mag_axis_coords[0])*(R_bdy[loc]-mag_axis_coords[0])+(Z_bdy[loc]-mag_axis_coords[1])*(Z_bdy[loc]-mag_axis_coords[1])))*a_bdy;
	a[i]=a_guess;
	theta[i]=2*M_PI/NBOUNDARY*loc;
	//	if(theta_bdy>M_PI)
	//  fprintf(stderr,"sign_cur=%d\n loc= %d,Rwant=%lf, Zwant=%lf, theta=%lf, Rbd=%lf, Zbd=%lf, thetabd=%lf, a=%lf",sign_cur,loc,Rwant[i],Zwant[i],theta_guess,R_bdy[loc],Z_bdy[loc],theta_bdy,a[i]);
      }
  }

  //Modified by Lei End
#endif

old version ends*/
}

//! Calculates the (R,Z) coordinates of the boundary of the last-closed-flux-surface
/*! Assumes the LCFS is given by the macro ABOUNDARY (set to 1.0 by default) */
int getBoundaryPoints(double** R_bdy,double** Z_bdy,int n_bdy){
  double dtheta = 2*M_PI/(n_bdy-1);
  double a[n_bdy],theta[n_bdy],B_bdy[n_bdy];
  *R_bdy = (double*)xmalloc(sizeof(double)*n_bdy);
  *Z_bdy = (double*)xmalloc(sizeof(double)*n_bdy);
  int i,k;
  for(i=0;i<n_bdy;i++){
    theta[i] = dtheta*i;
    a[i] = ABOUNDARY;
  }

  k=esigetrzb_(*R_bdy,*Z_bdy,B_bdy,a,theta,&n_bdy);

}

void free_BoundaryPoints(double* R_bdy,double* Z_bdy){
  xfree(R_bdy);
  xfree(Z_bdy);
}

//! Find the (R,Z) coordinates of the magnetic axis
/*! Uses the esi functions.
 */
int get_mag_axis(REAL coords[]){
  REAL bm;
  REAL mag_axis = 0;
  int single_point = 1;

  // get the position of the magnetic axis

  //printf("before esigetrzb_\n");
  int k = esigetrzb_(&coords[0],&coords[1],&bm,&mag_axis,&mag_axis,&single_point);
  //printf("after esigetrzb_\n");
  return 0;
}

// modified by Lei Shi to get desired shape outside the LCFS
// The shape function is set to be

// shape1:exponential tan shape
// n(a)=n(1)*exp(-n'(1)/n(1)*tan(PI/2*(a-1)/(a0-1)));

// shape2:cubic shape
// n(a)=c3*a^3+c2*a^2+c1^a+c0

// so that n(1) and n'(1) are unchanged, and n(a0) and n'(a0) are 0;
inline double Shape1(double x, double xb,double x0,double yb,double ypb){
  return yb*exp(ypb/yb*tan(M_PI/2*(x-xb)/(x0-xb)));
}

double Shape2(double x,double xb,double x0,double yb,double ypb){
  double xb2=xb*xb;
  double xb3=xb*xb2;
  double x02=x0*x0;
  double x03=x0*x02;

  double a_data[]=
    {
      1,xb,xb2, xb3,
      1,x0,x02, x03,
      0,1, 2*xb,3*xb2,
      0,1, 2*x0,3*x02
    };

  double b_data[]={yb,0,ypb,0};
  gsl_matrix_view m= gsl_matrix_view_array(a_data,4,4);
  gsl_vector_view b= gsl_vector_view_array(b_data,4);
  gsl_vector *c = gsl_vector_alloc(4);
  int s;
  gsl_permutation *p=gsl_permutation_alloc(4);
  gsl_linalg_LU_decomp(&m.matrix,p,&s);
  gsl_linalg_LU_solve(&m.matrix,p,&b.vector,c);

  double C[4];
  int i;
  for(i=0;i<4;i++)
    C[i]=gsl_vector_get(c,i);

  return C[0]+C[1]*x+C[2]*x*x+C[3]*x*x*x;
}

inline double Decay(int shape,double a,double a_bdy,double a0,double y_bdy,double yp_bdy){
  switch(shape){
  case 1:
    return Shape1(a,a_bdy,a0,y_bdy,yp_bdy);
  case 2:
    return Shape2(a,a_bdy,a0,y_bdy,yp_bdy);
  default:
    fprintf(stderr,"Wrong decay shape number, check the notes in profile_coord_map.h before function decayNToutsideLCFS for more info.\n");
  }
}

//! Cause density and temperature to exponentially decay outside the LCFS
/*! Use the value of density, Ti, and Te at the Last-Closed-Flux-Surface
  and force these quantities to exponentially decay outside this surface.
  This is done so there is no abrupt increase in these quantities that might
  frustrate the paraxial solver in the reflectometry code.
*/

int decayNToutsideLCFS(int npts,REAL a[],REAL ne[],REAL Te[],REAL Ti[],int* flag){
  REAL abound = ABOUNDARY; // (a,theta) on the boundary that we use
  REAL ain = AINSIDE;	// (a,theta) inside the LCFS we use
  // (* to find the exponential drop off *) in our case, to find n'(1)

  REAL a0 = AZERO; // the flux coord where we set n & T to be zero

  //load the equilibrium profile
  int** n_prof=(int**)PyMem_Malloc(sizeof(int*));
  REAL **a_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  REAL **n_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  REAL **Ti_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  REAL **Te_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  int k;
  k=loadNTProfiles(NTFileName,n_prof,a_p,n_p,Ti_p,Te_p);
  if(k!=0){
    fprintf(stderr,"Loading NTProfile error %d.\n",k);
    exit(1);
  }

  // values of ne,te,ti at boundary and just inside LCFS
  REAL nebound,tibound,tebound,nein,tin,tein;
  getProfile(abound,&nebound,&tibound,&tebound,*a_p,*n_p,*Ti_p,*Te_p,**n_prof);
  getProfile(ain,&nein,&tin,&tein,*a_p,*n_p,*Ti_p,*Te_p,**n_prof);

  nebound *=INVCM3_TO_INVM3;
//  tibound *=1;
//  tebound *=1;
  nein *=INVCM3_TO_INVM3;
//  tin *=1;
//  tein *=1;

  // the 1/e distance in radial flux coord
  // the scale factor is to make it drop off faster
  //  REAL lambda_ne = -fabs(DECAY_SCALING*nebound/(nein-nebound));
  //  REAL lambda_ti = -fabs(DECAY_SCALING*tibound/(tin-tibound));
  //  REAL lambda_te = -fabs(DECAY_SCALING*tebound/(tein-tebound));

  // the direvitives are obtained based on linear estimation

  REAL ne_prime = (nebound-nein)/(abound-ain);
  REAL ti_prime = (tibound-tin)/(abound-ain);
  REAL te_prime = (tebound-tein)/(abound-ain);

  fprintf(stderr,"n'=%lf,t'=%lf",ne_prime,te_prime);

  // update the values in the ne,te,ti arrays
//  int i;
//  for(i=0;i<npts;i++){
//    if(a[i]>1.0){
//    ne[i] = nebound*exp((a[i]-abound)/lambda_ne);
//    Te[i] = tebound*exp((a[i]-abound)/lambda_te);
//    Ti[i] = tibound*exp((a[i]-abound)/lambda_ti);
//    }
  int i;
  for(i=0;i<npts;i++){
    if(flag[i] == 0 ){
      if(a[i] <= a0){
	ne[i]=Decay(2,a[i],abound,a0,nebound,ne_prime);
	Te[i]=Decay(2,a[i],abound,a0,tebound,te_prime);
	Ti[i]=Decay(2,a[i],abound,a0,tibound,ti_prime);
	//	fprintf(stderr,"inside a[%d]=%lf, ne=%lf, Te=%lf , Ti=%lf\n",i,a[i],ne[i],Te[i],Ti[i]);
      }
      else{
	ne[i]=Te[i]=Ti[i]=0;  // if the point is outside a0, set all to be 0
	//	fprintf(stderr,"outside a[%d]=%lf, ne=%lf, Te=%lf , Ti=%lf\n",i,a[i],ne[i],Te[i],Ti[i]);
      }
    }
  }
  return 0;
}

//! Runs all the functions to map from cylindrical (R,Z) coords to flux coords
/*! Given the number of points npts, the desired cylindrical coords (Rwant,Zwant)
    and the (R,Z) coordinate array of the magnetic axis position "mag_axis_coords",
    gives the initial guess (Rinitial,Zinitial), the flux coords (a,theta), and
    the actual (Ract,Zact) these flux coords map to.
 */
int getFluxCoords(int npts,REAL a[],REAL theta[],REAL Bm[],REAL Ract[],REAL Zact[],REAL Rinitial[],REAL Zinitial[],REAL Rwant[],REAL Zwant[],REAL mag_axis_coords[],int flag[],int location[], int mismatch[]){
  // field-line coords: a: flux (radial), theta: angle (poloidal), Bm: |B|
  // (Rwant,Zwant): the R,Z coordinatew we want to map to
  // (Rinitial,Zinitial): the R,Z coords of our initial guess
  // (Ract,Zact): actual R,Z coordinates we end up with
  int k;
  //  k = guessatheta(a,theta,Rwant,Zwant,npts,mag_axis_coords[0],mag_axis_coords[1]); // initial guess for the flux coords
  //  k = esigetrzb_(Rinitial,Zinitial,Bm,a,theta,&npts); // map to cylindrical coords

  fprintf(stderr, "before findcorrectatheta.\n");
  k = findcorrectatheta(a,theta,Ract,Zact,Bm,Rwant,Zwant,npts,flag,location,mismatch); // use esi provided interpolation to get flux coords

  return k;
}

//! Gets all the profile data at the specified flux coordinates
/*! Given the number of points npts, the flux coords (a,theta), gets the
    values of: Bpol, Ti, Te, P, ne, and the q profile at these points.
 */
extern char* NTFileName;

int getProfile(REAL a,REAL* ne,REAL* Ti,REAL* Te,REAL* a_p,REAL* n_p,REAL* Ti_p,REAL* Te_p,int n_prof){ //get ne,Ti,Te for a specified flux surface psi=a, linear interpolation based on the loaded ne,Ti,Te profile

  interpxy(&a,ne,a_p,n_p,1,n_prof);
  interpxy(&a,Ti,a_p,Ti_p,1,n_prof);
  interpxy(&a,Te,a_p,Te_p,1,n_prof);
  return 0;
}

int getAllProfiles(int npts,REAL Bpol[], REAL BR[],REAL BZ[] ,REAL Ti[],REAL Te[],REAL P[],REAL ne[],REAL qprofile[],REAL a[],REAL theta[], REAL Rwant[], REAL Zwant[],REAL R_bdy[],REAL Z_bdy[],int InOutFlag[], int location[]){
  // used by esilink2c and esiget2dfunctions
  REAL *F = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *Fa = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *gFa = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *gFaa = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *gYa = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *gYaa = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *Ta = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *Pa = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *r = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *ra = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *rq = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *z = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *za = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *zq = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *B = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *Ba = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *Bq = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *gh = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *gha = (REAL *)PyMem_Malloc(npts*sizeof(REAL));
  REAL *ghq = (REAL *)PyMem_Malloc(npts*sizeof(REAL));

  int k = esilink2c_(F,Fa,gFa,gFaa,gYa,gYaa,Ti,Ta,P,Pa,r,ra,rq,z,za,zq,B,Ba,Bq,gh,gha,ghq);
  k = esiget2dfunctions_(a,theta,&npts);

  // get q profile
  int i;
  for(i=0;i<npts;i++){// reverse of method documented in escZ_march03.c
    if(a[i]==0.0){		// this method is used in setup_v2.F90
      qprofile[i] = -gFaa[i]/gYaa[i]; // the magnetic axis is a special case
    }else qprofile[i]=-gFa[i]/gYa[i];
  }

  int** n_prof=(int**)PyMem_Malloc(sizeof(int*));
  REAL **a_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  REAL **n_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  REAL **Ti_p=(REAL**)PyMem_Malloc(sizeof(REAL*));
  REAL **Te_p=(REAL**)PyMem_Malloc(sizeof(REAL*));

  printf("NTFileName before loadNTProfiles: %s\n",NTFileName);
  k=loadNTProfiles(NTFileName,n_prof,a_p,n_p,Ti_p,Te_p);
  if(k!=0){
    fprintf(stderr,"Loading NTProfile error %d.\n",k);
    exit(1);
  }

  // get density and temperature profiles
  for(i=0;i<npts;i++){
    if(InOutFlag[i]==1){
      getProfile(a[i],&ne[i],&Ti[i],&Te[i],*a_p,*n_p,*Ti_p,*Te_p,**n_prof);
      Ti[i] *= 100;
      Te[i] *= 100;
      ne[i] *= INVCM3_TO_INVM3;
    }
    else{
      ne[i]=0;
      Ti[i]=0;
      Te[i]=0;
    }
  }

  //get BR, BZ for new FWR3D
  // the basic idea is using flux coordinates to express BR = B dot grad(R).
  // look into attached pdf file for more details about calculating these two components.

  fprintf(stderr,"before getting BR,BZ. \n");
  //get magnetic axis coordinates
  double mag_axis_coords[2];
  get_mag_axis(mag_axis_coords);

  for (i=0;i<npts;i++){
    if(InOutFlag[i]==1){

      // for inside points, all relavent quantites can be gotten from ESI equilibrium reading functions defined in eq_quantity.h

      //fprintf(stderr,"inside point [%d].\n",i);
      initialize_esi_out(&a[i],&theta[i],1);
      //fprintf(stderr,"esi_out initialized.\n");
      //fprintf(stderr,"a=%lf,theta=%lf,n=%d.\n",esi_output.aaa[0],esi_output.the[0],esi_output.n);
      load_esi_quantities();
      //fprintf(stderr,"esi_quantities loaded\n");
      double** metric = get_metric_elements();
      //fprintf(stderr,"metric_elements got.\n");
      double gaa = metric[0][0];
      double gtt = metric[1][0];
      double gat = metric[3][0];
      double* J = get_Jacobian();
      //fprintf(stderr,"Jacobian got.\n");
      double dpsi_da = -esi_output.gYaj[0];
      double dr_dth = esi_output.rqj[0];
      double dr_da = esi_output.raj[0];
      double dz_dth = esi_output.zqj[0];
      double dz_da = esi_output.zaj[0];

      // now, define some intermediate quantities:
      double mod_grad_R = sqrt(dr_da*dr_da*gaa + dr_dth*dr_dth*gtt + 2*dr_da*dr_dth*gat);
      double mod_grad_Z = sqrt(dz_da*dz_da*gaa + dz_dth*dz_dth*gtt + 2*dz_da*dz_dth*gat);

      // calculate BR, BZ at this point
      //fprintf(stderr,"before assigning BR,BZ. \n");
      BR[i] = -dpsi_da*dr_dth/(mod_grad_R*J[0]);
      BZ[i] = -dpsi_da*dz_dth/(mod_grad_Z*J[0]);

      // clean up
      //fprintf(stderr,"before cleaning up.");
      free_all(metric,J);
    }
    else{
      //if the point is outside of the LCFS, we estimate the BR, BZ using the information gotten from the corresponding points ON the LCFS. Assume Bpol is decaying proportionally to the distance away from the magnetic axis.
      //fprintf(stderr,"outside point[%d].\n",i);
      extern double* R_bdy,*Z_bdy;
      extern int NBOUNDARY;
      int loc = location[i];
      //fprintf(stderr,"location got.\n");

      double theta_loc = 2*M_PI/(NBOUNDARY-1) * loc; //Note that the convention is that the first and last boundary points corresponds to the same location(theta =0 and 2*pi), so boundary points form a closed circle.
      double a_loc = ABOUNDARY;
      double Rb = R_bdy[loc];
      double Zb = Z_bdy[loc];

      //fprintf(stderr,"Rb,Zb got.\n");

      // for the boundary point, do the same thing to obtain BR, BZ there.
      initialize_esi_out(&a_loc,&theta_loc,1);
      //fprintf(stderr,"esi_out initialized.\n");
      load_esi_quantities();
      //fprintf(stderr,"esi_quantities loaded. \n");
      double** metric = get_metric_elements();
      //fprintf(stderr,"metric elements got.\n");
      double gaa = metric[0][0];
      double gtt = metric[1][0];
      double gat = metric[3][0];
      double* J = get_Jacobian();
      //fprintf(stderr,"Jacobian got.\n");
      double dpsi_da = -esi_output.gYaj[0];
      double dr_dth = esi_output.rqj[0];
      double dr_da = esi_output.raj[0];
      double dz_dth = esi_output.zqj[0];
      double dz_da = esi_output.zaj[0];

      // now, define some intermediate quantities:
      double mod_grad_R = sqrt(dr_da*dr_da*gaa + dr_dth*dr_dth*gtt + 2*dr_da*dr_dth*gat);
      double mod_grad_Z = sqrt(dz_da*dz_da*gaa + dz_dth*dz_dth*gtt + 2*dz_da*dz_dth*gat);

      // calculate BR, BZ at this point
      double BR_bdy = -dpsi_da*dr_dth/(mod_grad_R*J[0]);
      double BZ_bdy = -dpsi_da*dz_dth/(mod_grad_Z*J[0]);
      //fprintf(stderr,"BR,BZ bdy got.\n");
      // clean up
      free_all(metric,J);

      // now calculate the BR, BZ at the outside point
      double R = Rwant[i];
      double Z = Zwant[i];

      //calculate the distances between relavent points
      double dis_want = hypot(R-mag_axis_coords[0],Z-mag_axis_coords[1]);
      double dis_bdy = hypot(Rb-mag_axis_coords[0],Zb-mag_axis_coords[1]);

      //get BR, BZ based on the ratio of distances to magnetic axis
      BR[i] = BR_bdy * dis_bdy / dis_want;
      BZ[i] = BZ_bdy * dis_bdy / dis_want;

    }
  }

  //calculate Bpol
  for(i=0;i<npts;i++){
    Bpol[i] = sqrt(BR[i]*BR[i] + BZ[i]*BZ[i]);
  }

  fprintf(stderr,"after getting BR,BZ. \n");

  // free arrays that are no longer needed:
  clearNTProfiles(n_prof,a_p,n_p,Ti_p,Te_p);
  PyMem_Free(F);
  PyMem_Free(Fa);
  PyMem_Free(gFa);
  PyMem_Free(gFaa);
  PyMem_Free(gYa);
  PyMem_Free(gYaa);
  PyMem_Free(Ta);
  PyMem_Free(Pa);
  PyMem_Free(r);
  PyMem_Free(ra);
  PyMem_Free(rq);
  PyMem_Free(z);
  PyMem_Free(za);
  PyMem_Free(zq);
  PyMem_Free(B);
  PyMem_Free(Ba);
  PyMem_Free(Bq);
  PyMem_Free(gh);
  PyMem_Free(gha);
  PyMem_Free(ghq);

  return 0;
}

//! Determine the density fluctuation assuming adiabatic electron response to potential fluctuations.
/*! Given the number of points npts, the background density (ne0), electron temperature (Te),
    and potential fluctuation (phi), computes the density fluctuation (ne_tilde) and the total
    density (ne), using: ne_tilde = ne0*phi/Te, and ne = ne0 + ne_tilde
 */
int adiabaticElectronResponse(int npts,int ntimesteps,REAL ne_tilde[],REAL ne0[],REAL phi[],REAL Te[],int flag[]){
    int i,j;
    for(i=0;i<ntimesteps;i++){
      for(j=0;j<npts;j++){
	if(flag[j]==1){
	  double* phi_this = &phi[i*npts+j];
	  *phi_this *= NORM_TO_KEV;
	  ne_tilde[i*npts+j] =  (*phi_this)/Te[j];
	}
	else
	  ne_tilde[i*npts+j] = 0;
      }
    }
    return 0;
  }


/**
 * @file blas_mangle.h
 * @brief Macros to "de-mangle" BLAS routines and includes and allow generic calls to them in the code.
 * @author Moritz Kreutzer <moritz.kreutzer@fau.de>
 */
#ifndef GHOST_BLAS_MANGLE_H
#define GHOST_BLAS_MANGLE_H

#include <strings.h>

#ifdef GHOST_HAVE_MKL
#include <mkl_cblas.h>
#elif defined(GHOST_HAVE_GSL)
#include <gsl_cblas.h>
#else
#include <cblas.h>
#endif

#define blas_order(order) order==GHOST_DENSEMAT_COLMAJOR?CblasColMajor:CblasRowMajor
#define blas_trans(trans) trans[0]=='N'?CblasNoTrans:(trans[0]=='C'?CblasConjTrans:CblasTrans)

#define sgemm(order,transa,transb,m,n,k,alpha,a,lda,b,ldb,beta,c,ldc) \
    cblas_sgemm(blas_order(order),blas_trans(transa),blas_trans(transb),*m,*n,*k,*alpha,a,*lda,b,*ldb,*beta,c,*ldc)
#define dgemm(order,transa,transb,m,n,k,alpha,a,lda,b,ldb,beta,c,ldc) \
    cblas_dgemm(blas_order(order),blas_trans(transa),blas_trans(transb),*m,*n,*k,*alpha,a,*lda,b,*ldb,*beta,c,*ldc)
#define cgemm(order,transa,transb,m,n,k,alpha,a,lda,b,ldb,beta,c,ldc) \
    cblas_cgemm(blas_order(order),blas_trans(transa),blas_trans(transb),*m,*n,*k,alpha,a,*lda,b,*ldb,beta,c,*ldc)
#define zgemm(order,transa,transb,m,n,k,alpha,a,lda,b,ldb,beta,c,ldc) \
    cblas_zgemm(blas_order(order),blas_trans(transa),blas_trans(transb),*m,*n,*k,alpha,a,*lda,b,*ldb,beta,c,*ldc)

#endif


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

#ifndef SALESMAN_EXAMPLE_PATH_MERGER_H
#define SALESMAN_EXAMPLE_PATH_MERGER_H

#include <algorithm>
#include <functional>
#include <iterator>
#include <stack>
#include <utility>
#include <vector>
#include <gsl/gsl_util>
#include <gsl/span>
#include <repeat.h>
#include "disjoint_set_data_structure.h"
#include "permutation.h"

template<class UniformRandomBitGenerator>
class path_merger {
public:
	explicit path_merger(UniformRandomBitGenerator& g) noexcept;
	permutation operator()(const permutation& lhs, const permutation& rhs);
private:
	using edge_type = std::pair<unsigned, unsigned>;
	using edge_vector = std::vector<edge_type>;
	edge_vector to_edges(const permutation& perm) const;
	permutation to_permutation(const edge_vector& edges) const;
	UniformRandomBitGenerator& rand;
};

template<class UniformRandomBitGenerator>
path_merger<UniformRandomBitGenerator>::path_merger(UniformRandomBitGenerator& g) noexcept
	: rand(g) {}

template<class UniformRandomBitGenerator>
permutation path_merger<UniformRandomBitGenerator>::operator()(const permutation& lhs, const permutation& rhs) {
	Expects(lhs.size() == rhs.size());
	Expects(lhs.size() > 0);
	const std::size_t size = lhs.size();
	edge_vector lhs_edges = to_edges(lhs);
	edge_vector rhs_edges = to_edges(rhs);
	edge_vector result_edges;
	result_edges.reserve(size);
	std::set_intersection(lhs_edges.begin(), lhs_edges.end(), rhs_edges.begin(), rhs_edges.end(), std::back_inserter(result_edges));
	disjoint_set_data_structure components(size);
	std::vector<unsigned> missing_edges(size, 2);
	for (const auto& [left, right] : result_edges) {
		components.merge(left, right);
		gsl::at(missing_edges, left)--;
		gsl::at(missing_edges, right)--;
	}
	std::shuffle(lhs_edges.begin(), lhs_edges.end(), rand);
	std::shuffle(rhs_edges.begin(), rhs_edges.end(), rand);
	gsl::span<edge_type> spans[2] {lhs_edges, rhs_edges};
	while (!std::all_of(std::begin(spans), std::end(spans), std::mem_fn(&gsl::span<edge_type>::empty))) {
		for (auto&& span : spans) {
			auto it = std::find_if(span.begin(), span.end(), [&](const edge_type& edge) {
				const auto& [lhs, rhs] = edge;
				return gsl::at(missing_edges, lhs) && gsl::at(missing_edges, rhs) && components.find(lhs) != components.find(rhs);
			});
			if (it != span.end()) {
				const auto& [left, right] = *it;
				result_edges.emplace_back(left, right);
				components.merge(left, right);
				gsl::at(missing_edges, left)--;
				gsl::at(missing_edges, right)--;
			}
			span = span.subspan(it - span.begin());
		}
	}
	std::stack<unsigned, std::vector<unsigned>> s;
	for (unsigned i = 0; i < size; i++) {
		repeat(gsl::at(missing_edges, i), [&] {
			if (!s.empty() && components.find(s.top()) != components.find(i)) {
				const unsigned& left = s.top();
				const unsigned& right = i;
				result_edges.emplace_back(left, right);
				components.merge(left, right);
				gsl::at(missing_edges, left)--;
				gsl::at(missing_edges, right)--;
				s.pop();
			} else {
				s.push(i);
			}
		});
	}
	if (!s.empty()) {
		const unsigned left = s.top();
		s.pop();
		const unsigned& right = s.top();
		result_edges.emplace_back(left, right);
	}
	Ensures(result_edges.size() == lhs.size());
	return to_permutation(result_edges);
}

template<class UniformRandomBitGenerator>
auto path_merger<UniformRandomBitGenerator>::to_edges(const permutation& perm) const -> edge_vector {
	Expects(perm.size() > 0);
	edge_vector result {std::minmax({perm.front(), perm.back()})};
	result.reserve(perm.size());
	std::transform(std::next(perm.begin()), perm.end(), perm.begin(), std::inserter(result, result.end()), [](unsigned lhs, unsigned rhs) {
		return std::minmax({lhs, rhs});
	});
	std::sort(result.begin(), result.end());
	Ensures(result.size() == perm.size());
	return result;
}

template<class UniformRandomBitGenerator>
permutation path_merger<UniformRandomBitGenerator>::to_permutation(const edge_vector& edges) const {
	Expects(edges.size() > 0);
	const std::size_t size = edges.size();
	std::vector<std::vector<unsigned>> graph(size);
	for (auto&& node : graph) {
		node.reserve(2);
	}
	for (const auto& [lhs, rhs] : edges) {
		gsl::at(graph, lhs).push_back(rhs);
		gsl::at(graph, rhs).push_back(lhs);
	}
	permutation result(size);
	unsigned prev = 0;
	unsigned cur = graph.front().front();
	std::for_each(std::next(result.begin()), result.end(), [&](unsigned& element) {
		element = cur;
		const auto& node = gsl::at(graph, cur);
		unsigned next = node.front() == prev ? node.back() : node.front();
		prev = cur;
		cur = next;
	});
	Ensures(result.size() == edges.size());
	return result;
}

#endif