#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <numeric>
#include <string>

#include <boost/program_options.hpp>

#include "datastructs/matrix.h"
#include "datastructs/equations.h"

#include "polynomials/commutative_polynomial.h"
#include "polynomials/non_commutative_polynomial.h"

#include "semirings/commutativeRExp.h"
#include "semirings/float-semiring.h"
#include "semirings/prec-rat-semiring.h"
#include "semirings/tropical-semiring.h"
#include "semirings/pseudo_linear_set.h"
#include "semirings/semilinear_set.h"
#include "semirings/bool-semiring.h"
#include "semirings/why-set.h"
#include "semirings/viterbi-semiring.h"
#include "semirings/maxmin-semiring.h"

#ifdef USE_LIBFA
#include "semirings/lossy-finite-automaton.h"
#include "polynomials/lossy_non_commutative_polynomial.h"
#endif

#ifdef USE_GENEPI
#include "semirings/semilinSetNdd.h"
#endif

#include "parser.h"

#include "solvers/newton_generic.h"
#include "solvers/kleene_seminaive.h"
#include "solvers/solver_utils.h"

#include "utils/string_util.h"

template <typename SR, template <typename> class Poly>
ValuationMap<SR> call_solver(const std::string solver_name,  const GenericEquations<Poly, SR> &equations,
   const bool scc, const bool iteration_flag, const std::size_t iterations, const bool graphviz_output){
  if(0 == solver_name.compare("newtonSymb")) {
    std::cout << "Solver: Newton Symbolic" << std::endl;
    return apply_solver<Newton, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("newtonConc")) {
    std::cout << "Solver: Newton Concrete"<< std::endl;
    return apply_solver<NewtonCL, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("newtonCLDU")) {
    std::cout << "Solver: Newton Concrete (LDU)"<< std::endl;
    return apply_solver<NewtonCLDU, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("newtonSLDU")) {
      std::cout << "Solver: Newton Symbolic (LDU)"<< std::endl;
      return apply_solver<NewtonSLDU, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("kleene")) {
    std::cout << "Solver: Kleene solver"<< std::endl;
    return apply_solver<KleeneComm, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else {
    // default-case
    std::cout << "Solver: Newton Concrete (LDU)"<< std::endl;
    return apply_solver<NewtonCLDU, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
}

int main(int argc, char* argv[]) {

  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()
    ( "scc", "apply newton method iteratively to strongly connected components of the equation graph" )
    ( "help,h", "print this help message" )
    ( "iterations,i", po::value<int>(), "specify the number of newton iterations. Default is number of equations + 1." )
    //( "verbose", "enable verbose output" )
    //( "debug", "enable debug output" )
    ( "test", "just for testing purposes ... explicit test defined in main()" )
    ( "file,f", po::value<std::string>(), "input file" )
    ( "float", "float semiring" )
    ( "rat", "semiring of extended rationals (arbitrary precision)" )
    ( "bool", "boolean semiring" )
    ( "why", "why semiring" )
    ( "maxmin", "MaxMin semiring" )
    ( "viterbi", "Viterbi semiring" )
    ( "tropical", "tropical semiring over the integers" )
    ( "rexp", "commutative regular expression semiring" )
    ( "slset", "explicit semilinear sets semiring, no simplification " )
#ifdef USE_GENEPI
    ( "slsetndd", po::value<std::string>(), "ndd semilinear sets semiring, give plugin name (lash-msdf, mona)" )
    ( "n", po::value<int>(), "number of variables, used for slsetndd" )
#endif
    ( "mlset", "abstraction over semilinear sets" )
    ( "vec-simpl", "vector simplification only (only semilinear and multilinear sets)" )
    ( "lin-simpl", "linear set simplification (only semilinear sets)" )
    ( "free", "free semiring" )
    ( "lossy", "lossy semiring" )
    ( "prefix", po::value<int>(), "prefix semiring with given length")
    ( "graphviz", "create the file graph.dot with the equation graph (NOTE: currently only with option --scc) " )
    ( "solver,s", po::value<std::string>(), "solver type (currently: \"newtonSymb\", \"newtonConc\", \"newtonCLDU\", \"newtonSLDU\", \"newtonNumeric\" (only for numeric semirings), or \"kleene\")" )
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("test")) {
    Newton<SemilinSetExp> newton;
    std::vector<VarId> variables;
    variables.push_back(Var::GetVarId("x"));
    std::cout << "- newton (cnt-SR):" << std::endl;

    std::vector<CommutativePolynomial<SemilinSetExp> > polynomials;
    CommutativePolynomial<SemilinSetExp> f1 = CommutativePolynomial<SemilinSetExp>({
        { SemilinSetExp(Var::GetVarId("a")), CommutativeMonomial{ {Var::GetVarId("x"),Var::GetVarId("x")} } },
        { SemilinSetExp(Var::GetVarId("c")), CommutativeMonomial{} } });

    polynomials.push_back(f1);

    Matrix<SemilinSetExp> result = newton.solve_fixpoint(polynomials, variables, 2);
    std::cout << result << std::endl;

    /*              auto s1 = CommutativeRExp(Var::GetVarId("a"));
                    auto s2 = CommutativeRExp(Var::GetVarId("b"));
                    auto m1 = Matrix<CommutativeRExp>(1,1,{s1});
                    auto m2 = Matrix<CommutativeRExp>(1,1,{s2});
                    */

    // this actually led to a strange bug with the ublas-matrix implementation!!
    /*              auto s1 = SemilinSetExp(Var::GetVarId("a"));
                    auto s2 = SemilinSetExp(Var::GetVarId("b"));
                    auto m1 = Matrix<SemilinSetExp>(1,1,{s1});
                    auto m2 = Matrix<SemilinSetExp>(1,1,{s2});

                    auto m3 = m1*m2;
                    std::cout << m3;
                    */
    return 0;
  }

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 0;
  }

#ifdef USE_GENEPI
  SemilinSetNdd::genepi_init();
#endif

  int iterations=0;
  if (vm.count("iterations"))
          iterations = vm["iterations"].as<int>();

  // check if we can do something useful
  if (!vm.count("float") &&
      !vm.count("rat") &&
      !vm.count("rexp") &&
      !vm.count("slset") &&
#ifdef USE_GENEPI
      !vm.count("slsetndd") &&
#endif
      !vm.count("free") &&
      !vm.count("bool") &&
      !vm.count("why") &&
      !vm.count("tropical") &&
      !vm.count("mlset") &&
      !vm.count("prefix") &&
      !vm.count("lossy") &&
      !vm.count("maxmin") &&
      !vm.count("viterbi"))
    {
    std::cout << "Please supply a supported semiring :)" << std::endl;
    return 0;
  }

  std::vector<std::string> input;
  std::string line;
  if (vm.count("file")) {
    // we are reading the input from the given file
    std::ifstream file;
    file.open(vm["file"].as<std::string>(), std::ifstream::in);
    if (file.fail()) {
      std::cerr << "Could not open input file: " << vm["file"].as<std::string>() << std::endl;
    }
    while (std::getline(file, line)) {
      input.push_back(line);
    }
  } else {
    // we are reading from stdin
    while (std::getline(std::cin, line)) {
      input.push_back(line);
    }
  }

  // join the input into one string
  std::string input_all =
    std::accumulate(input.begin(), input.end(), std::string(""));

  Parser p;
  std::string solver_name;

  if(vm.count("solver")) {
    solver_name = vm["solver"].as<std::string>();
  }
  else {
    solver_name = "newtonConc"; //seems to be the fastest
  }

  const auto iter_flag = vm.count("iterations");
  const auto graph_flag = vm.count("graphviz");
  const auto scc_flag = vm.count("scc");

  if (vm.count("slset")) {
    auto equations_raw = p.free_parser(input_all);
    auto equations = MakeCommEquationsAndMap(equations_raw, [](const FreeSemiring &c) -> SemilinSetExp {
      auto srconv = SRConverter<SemilinSetExp>();
      return c.Eval(srconv);
    });

    if (equations.empty()) return EXIT_FAILURE;
    //PrintEquations(equations);
    if (!vm.count("vec-simpl") && !vm.count("lin-simpl")) {
      DMSG("A");
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;

    } else if (vm.count("vec-simpl") && !vm.count("lin-simpl")) {
      DMSG("B");
      auto equations2 = MapEquations(equations, [](const SemilinearSet<> &s) {
        return SemilinearSetV{s};
      });
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else {
      DMSG("C");
      auto equations2 = MapEquations(equations, [](const SemilinearSet<> &s) {
        return SemilinearSetL{s};
      });
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }
#ifdef USE_GENEPI
  } else if (vm.count("slsetndd")) {
      SemilinSetNdd::solver_init(vm["n"].as<int>());
      auto equations = p.slsetndd_parser(input_all);
      if (equations.empty()) return EXIT_FAILURE;
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
      SemilinSetNdd::solver_dealloc();
#endif
  } else if (vm.count("mlset")) {

    auto equations_raw = p.free_parser(input_all);
    auto equations = MakeCommEquationsAndMap(equations_raw, [](const FreeSemiring &c) -> SemilinSetExp {
      auto srconv = SRConverter<SemilinSetExp>();
      return c.Eval(srconv);
    });

    if (equations.empty()) return EXIT_FAILURE;
    if (vm.count("vec-simpl")) {
      auto m_equations = SemilinearToPseudoLinearEquations<
        DummyDivider, SparseVecSimplifier>(equations);
      //PrintEquations(m_equations);
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else {
      auto m_equations = SemilinearToPseudoLinearEquations<
        DummyDivider, DummyVecSimplifier>(equations);
      //PrintEquations(m_equations);
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }

  } else if (vm.count("rexp")) {

    // parse the input into a list of (Var → Polynomial[SR])
    auto equations = p.rexp_parser(input_all);
    if (equations.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);

    // apply solver to the equations
    std::cout << result_string(
        call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
        ) << std::endl;

  } else if (vm.count("free")) {

    // parse the input into a list of (Var → Polynomial[SR])
	auto equations = p.free_parser(input_all);
  //auto equations2 = MakeCommEquations(equations);
  //if (equations2.empty()) return EXIT_FAILURE;

//    std::cout << result_string(
//        call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
//        ) << std::endl;
#ifdef USE_LIBFA
  } else if (vm.count("lossy")) {

    auto equations = p.free_parser(input_all);
    if (equations.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);
    auto equations2 = MapEquations(equations, [](const FreeSemiring &c) -> LossyFiniteAutomaton {
      auto srconv = SRConverter<LossyFiniteAutomaton>();
      return c.Eval(srconv);
    });

    VarId S_1 = equations2[0].first;

    NCEquationsBase<LossyFiniteAutomaton> eq2 = NCEquationsBase<LossyFiniteAutomaton>(equations2.begin(), equations2.end());

    auto approximation = NonCommutativePolynomial<LossyFiniteAutomaton>::downwardClosureCourcelle(eq2, S_1);
    std::cout << "dwc:\t" << approximation.string() << std::endl;
    std::cout << "size NFA for DWC:\t" << approximation.size() << std::endl;
    std::cout << "size minimal DFA for DWC:\t" << approximation.minimize().size() << std::endl;
#endif
  } else if (vm.count("prefix")) {

    // parse the input into a list of (Var → Polynomial[SR])
    auto equations = p.prefix_parser(input_all, vm["prefix"].as<int>());
    if (equations.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);

    // apply the newton method to the equations
    //auto result = apply_solver<Kleene, PrefixSemiring>(equations,
    //                                           vm.count("scc"),
    //                                           vm.count("iterations"),
    //                                           iterations,
    //                                           vm.count("graphviz"));
    //std::cout << result_string(result) << std::endl;

    //std::cout << result_string(
    //    call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
    //    ) << std::endl;

  } else if (vm.count("float")) {
    auto equations = p.free_parser(input_all);
    //PrintEquations(equations);
    auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> FloatSemiring {
      auto srconv = SRConverter<FloatSemiring>();
      return c.Eval(srconv);
    });

    if (equations2.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);
    //PrintEquations(equations2);

    if(0 == solver_name.compare("newtonNumeric")) {
      std::cout << "Solver: Newton Numeric (Float)"<< std::endl;
      std::cout << result_string(
          apply_solver<NewtonNumeric>(equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else  {
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }

  } else if (vm.count("bool")) {
    auto equations = p.free_parser(input_all);
    auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> BoolSemiring {
      auto srconv = SRConverter<BoolSemiring>();
      return c.Eval(srconv);
    });

    if (equations2.empty()) return EXIT_FAILURE;

    PrintEquations(equations);
    PrintEquations(equations2);
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;

  } else if (vm.count("rat")) {
    auto equations = p.free_parser(input_all);
    auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> PrecRatSemiring {
      auto srconv = SRConverter<PrecRatSemiring>();
      return c.Eval(srconv);
    });

    if (equations2.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);
    //PrintEquations(equations2);

    if(0 == solver_name.compare("newtonNumeric")) {
      std::cout << "Solver: Newton Numeric (Rat)"<< std::endl;
      std::cout << result_string(
          apply_solver<NewtonNumeric>(equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else  {
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }
  }
  else if (vm.count("why")) {
      auto equations = p.free_parser(input_all);
      auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> WhySemiring {
        auto srconv = SRConverter<WhySemiring>();
        return c.Eval(srconv);
      });

      if (equations2.empty()) return EXIT_FAILURE;

        std::cout << result_string(
            call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
            ) << std::endl;

    }
    else if (vm.count("tropical")) {
      auto equations = p.free_parser(input_all);
      auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> TropicalSemiring {
        auto srconv = SRConverter<TropicalSemiring>();
        return c.Eval(srconv);
      });

      if (equations2.empty()) return EXIT_FAILURE;

        std::cout << result_string(
            call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
            ) << std::endl;

    }
    else if (vm.count("viterbi")) {
          auto equations = p.free_parser(input_all);
          auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> ViterbiSemiring {
            auto srconv = SRConverter<ViterbiSemiring>();
            return c.Eval(srconv);
          });

          if (equations2.empty()) return EXIT_FAILURE;

            std::cout << result_string(
                call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
                ) << std::endl;
    }
    else if (vm.count("maxmin")) {
          auto equations = p.free_parser(input_all);
          auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> MaxMinSemiring {
            auto srconv = SRConverter<MaxMinSemiring>();
            return c.Eval(srconv);
          });

          if (equations2.empty()) return EXIT_FAILURE;

            std::cout << result_string(
                call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
                ) << std::endl;

    }

#ifdef USE_GENEPI
  SemilinSetNdd::genepi_dealloc();
#endif

  return EXIT_SUCCESS;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_DOT_INCLUDE
#define MTL_DOT_INCLUDE

#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/meta_math/loop1.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/utility/omp_size_type.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>

namespace mtl {

    namespace vector {

	namespace detail {

	    // Result type of dot product
	    template <typename Vector1, typename Vector2>
	    struct dot_result
	    {
		typedef typename Multiplicable<typename Collection<Vector1>::value_type,
					       typename Collection<Vector2>::value_type>::result_type type;
	    };

	    // Whether or not conjugating first argument
	    struct without_conj
	    {
		template <typename Value>
		Value operator()(const Value& v) { return v; }
	    };

	    struct with_conj
	    {
		template <typename Value>
		typename mtl::sfunctor::conj<Value>::result_type
		operator() (const Value& v)
		{
		    using mtl::conj;
		    return conj(v);
		}
	    };
	}

	namespace sfunctor {

	    template <unsigned long Index0, unsigned long Max0>
	    struct dot_aux
		: public meta_math::loop1<Index0, Max0>
	    {
		typedef meta_math::loop1<Index0, Max0>                                    base;
		typedef dot_aux<base::next_index0, Max0>                                  next_t;

		template <typename Value, typename Vector1, typename Vector2, typename Size, typename ConjOpt>
		static inline void
		apply(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
		      Value& tmp05, Value& tmp06, Value& tmp07,
		      const Vector1& v1, const Vector2& v2, Size i, ConjOpt conj_opt)
		{
		    // vampir_trace<9901> tracer;
		    tmp00+= conj_opt(v1[ i + base::index0 ]) * v2[ i + base::index0 ];
		    next_t::apply(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp00,
				  v1, v2, i, conj_opt);
		}
	    };

	    template <unsigned long Max0>
	    struct dot_aux<Max0, Max0>
	    {
		typedef meta_math::loop1<Max0, Max0>                                      base;

		template <typename Value, typename Vector1, typename Vector2, typename Size, typename ConjOpt>
		static inline void
		apply(Value& tmp00, Value&, Value&, Value&, Value&, Value&, Value&, Value&,
		      const Vector1& v1, const Vector2& v2, Size i, ConjOpt conj_opt)
		{
		    tmp00+= conj_opt(v1[ i + base::index0 ]) * v2[ i + base::index0 ];
		}
	    };

	    template <unsigned long Unroll>
	    struct dot
	    {
		template <typename Vector1, typename Vector2, typename ConjOpt>
		typename detail::dot_result<Vector1, Vector2>::type
		static inline apply(const Vector1& v1, const Vector2& v2, ConjOpt conj_opt)
		{
		    MTL_STATIC_ASSERT((Unroll >= 1), "Unroll size must be at least 1.");
		    // MTL_STATIC_ASSERT((Unroll <= 8), "Maximal unrolling is 8."); // Might be relaxed in future versions

		    vampir_trace<2003> tracer;
		    MTL_THROW_IF(mtl::size(v1) != mtl::size(v2), incompatible_size());
		    typedef typename detail::dot_result<Vector1, Vector2>::type  value_type;

#                 ifdef MTL_WITH_OPENMP
		    value_type dummy, z= math::zero(dummy), result= z;
		    typedef typename mtl::traits::omp_size_type<typename Collection<Vector1>::size_type>::type size_type;
		    size_type  i_max= mtl::size(v1), i_block= Unroll * (i_max / Unroll);

                    #pragma omp parallel
		    {

			vampir_trace<8001> tracer;
			value_type tmp00= z, tmp01= z, tmp02= z, tmp03= z, tmp04= z, tmp05= z, tmp06= z, tmp07= z;

			#pragma omp for
			for (size_type i= 0; i < i_block; i+= Unroll)
			    dot_aux<1, Unroll>::apply(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, v1, v2, i, conj_opt);

			#pragma omp critical
			    result+= ((tmp00 + tmp01) + (tmp02 + tmp03)) + ((tmp04 + tmp05) + (tmp06 + tmp07));
		    }
		    for (size_type i= i_block; i < i_max; i++)
			result+= conj_opt(v1[i]) * v2[i];

		    return result;
#                 else
		    typedef typename Collection<Vector1>::size_type              size_type;

		    value_type dummy, z= math::zero(dummy), tmp00= z, tmp01= z, tmp02= z, tmp03= z, tmp04= z,
			       tmp05= z, tmp06= z, tmp07= z;
		    size_type  i_max= mtl::size(v1), i_block= Unroll * (i_max / Unroll);

		    for (size_type i= 0; i < i_block; i+= Unroll)
			dot_aux<1, Unroll>::apply(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, v1, v2, i, conj_opt);

		    for (size_type i= i_block; i < i_max; i++)
			tmp00+= conj_opt(v1[i]) * v2[i];
		    return ((tmp00 + tmp01) + (tmp02 + tmp03)) + ((tmp04 + tmp05) + (tmp06 + tmp07));
#                 endif
		}

	    };
	}

	template <typename Vector1, typename Vector2, typename ConjOpt>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot_simple(const Vector1& v1, const Vector2& v2, ConjOpt conj_opt)
	{
	    vampir_trace<2040> tracer;
	    typedef typename Collection<Vector1>::size_type              size_type;
	    typedef typename detail::dot_result<Vector1, Vector2>::type  value_type;

	    value_type dummy, s= math::zero(dummy);
	    for (size_type i= 0, i_max= mtl::size(v1); i < i_max; ++i)
		s+= conj_opt(v1[i]) * v2[i];
	    return s;
	}

	template <unsigned long Unroll, typename Vector1, typename Vector2, typename ConjOpt>
	struct dot_class
	{
	    typedef typename detail::dot_result<Vector1, Vector2>::type result_type;
	    dot_class(const Vector1& v1, const Vector2& v2) : v1(v1), v2(v2) {}

	    operator result_type() const { return sfunctor::dot<Unroll>::apply(v1, v2, ConjOpt()); }

	    const Vector1& v1;
	    const Vector2& v2;
	};

	template <typename Vector1, typename Vector2, typename ConjOpt>
	struct dot_class<1, Vector1, Vector2, ConjOpt>
	{
	    typedef typename detail::dot_result<Vector1, Vector2>::type result_type;
	    dot_class(const Vector1& v1, const Vector2& v2) : v1(v1), v2(v2) {}

	    operator result_type() const { return dot_simple(v1, v2, ConjOpt()); }

	    const Vector1& v1;
	    const Vector2& v2;
	};

	/// Lazy dot product
	/** It is automatically evaluated when (implicitly) converted to result_type which doesn't work in template expressions.
	    Can be used for source-to-source transformations. **/
	template <typename Vector1, typename Vector2>
	dot_class<4, Vector1, Vector2, detail::with_conj>
	inline lazy_dot(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<4, Vector1, Vector2, detail::with_conj>(v1, v2);
	}

	template <unsigned long Unroll, typename Vector1, typename Vector2>
	dot_class<Unroll, Vector1, Vector2, detail::with_conj>
	inline lazy_dot(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<Unroll, Vector1, Vector2, detail::with_conj>(v1, v2);
	}

	template <typename Vector1, typename Vector2>
	dot_class<4, Vector1, Vector2, detail::without_conj>
	inline lazy_dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<4, Vector1, Vector2, detail::without_conj>(v1, v2);
	}

	template <unsigned long Unroll, typename Vector1, typename Vector2>
	dot_class<Unroll, Vector1, Vector2, detail::without_conj>
	inline lazy_dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<Unroll, Vector1, Vector2, detail::without_conj>(v1, v2);
	}

	/// Dot product defined as hermitian(v) * w
	/** Unrolled four times by default **/
	template <typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot(const Vector1& v1, const Vector2& v2)
	{
	    // return dot_simple(v1, v2, detail::with_conj());
	    return sfunctor::dot<4>::apply(v1, v2, detail::with_conj());
	}

	/// Dot product with user-specified unrolling defined as hermitian(v) * w
	template <unsigned long Unroll, typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot(const Vector1& v1, const Vector2& v2)
	{
	    return sfunctor::dot<Unroll>::apply(v1, v2, detail::with_conj());
	}
	/// Dot product without conjugate defined as trans(v) * w
	/** Unrolled four times by default **/
	template <typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return sfunctor::dot<4>::apply(v1, v2, detail::without_conj());
	}

	/// Dot product without conjugate with user-specified unrolling defined as trans(v) * w
	template <unsigned long Unroll, typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return sfunctor::dot<Unroll>::apply(v1, v2, detail::without_conj());
	}

    } // namespace vector

    using vector::dot;
    using vector::dot_real;
    using vector::lazy_dot;
    using vector::lazy_dot_real;

} // namespace mtl

#endif // MTL_DOT_INCLUDE


#include <iostream>
#include <vector>
#include <fstream>
#include <utility>
#include <iterator>
#include <cstdlib>
#include <string>
#include <limits>
#include <cerrno>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/concept_check.hpp>
#include <boost/operators.hpp>
#include <boost/iterator.hpp>
#include <boost/multi_array.hpp>
#include <boost/random.hpp>
#include <sys/time.h>
#include "../src/edmonds_optimum_branching.hpp"
#include "boost/tuple/tuple.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
using boost::tuple;
using namespace std;
using namespace boost;

#ifdef DEBUG
#define DEBUG_MSG(str) do { std::cout << str << std::endl; } while( false )
#else
#define DEBUG_MSG(str) do { } while ( false )
#endif

// definitions of a complete graph that implements the EdgeListGraph
// concept of Boost's graph library.
namespace boost {
	struct complete_graph {
		complete_graph(int n_vertices) : n_vertices(n_vertices) {}
		int n_vertices;

		struct edge_iterator : public input_iterator_helper<edge_iterator, int, std::ptrdiff_t, int const *, int>
		{
			int edge_idx, n_vertices;
			edge_iterator() : edge_idx(0), n_vertices(-1) {}
			edge_iterator(int n_vertices, int edge_idx) : edge_idx(edge_idx), n_vertices(n_vertices) {}
			edge_iterator &operator++()
			{
				if (edge_idx >= n_vertices * n_vertices)
					return *this;
				++edge_idx;
				if (edge_idx / n_vertices == edge_idx % n_vertices)
					++edge_idx;
				return *this;
			}
			int operator*() const {return edge_idx;}
			bool operator==(const edge_iterator &iter) const
			{
				return edge_idx == iter.edge_idx;
			}
		};
	};

	template<>
	struct graph_traits<complete_graph> {
		typedef int                             vertex_descriptor;
		typedef int                             edge_descriptor;
		typedef directed_tag                    directed_category;
		typedef disallow_parallel_edge_tag      edge_parallel_category;
		typedef edge_list_graph_tag             traversal_category;
		typedef complete_graph::edge_iterator   edge_iterator;
		typedef unsigned                        edges_size_type;

		static vertex_descriptor null_vertex() {return -1;}
	};

	pair<complete_graph::edge_iterator, complete_graph::edge_iterator>
	edges(const complete_graph &g)
	{
		return make_pair(complete_graph::edge_iterator(g.n_vertices, 1),
						 complete_graph::edge_iterator(g.n_vertices, g.n_vertices*g.n_vertices));
	}

	unsigned
	num_edges(const complete_graph &g)
	{
		return (g.n_vertices - 1) * (g.n_vertices - 1);
	}

	int
	source(int edge, const complete_graph &g)
	{
		return edge / g.n_vertices;
	}

	int
	target(int edge, const complete_graph &g)
	{
		return edge % g.n_vertices;
	}
}

typedef graph_traits<complete_graph>::edge_descriptor Edge;
typedef graph_traits<complete_graph>::vertex_descriptor Vertex;

int dirMST(string fin,string fout,string mode)
{
	int n_vertices;
	vector< tuple<int,int,float> > edge_list;
	tuple<int,int,float> g_edge;

	string line;
	int v1,v2,ctr = 0;
	float weight;
	ifstream inputf (fin.c_str());
	float* nodeWeights = NULL;
	if(inputf.is_open())
	{
		getline(inputf,line);
		sscanf(line.c_str(),"%d",&n_vertices);
		nodeWeights = new float[n_vertices];
		int ctr = 0;
		while(getline(inputf,line))
		{
			if(ctr<n_vertices)
			{
				sscanf(line.c_str(),"%f",&nodeWeights[ctr]);
				DEBUG_MSG("v|"<<ctr+1<<"|:"<<nodeWeights[ctr]);
				ctr++;
			}
			else
			{
				sscanf (line.c_str(),"%d,%d,%f",&v1,&v2,&weight);
				//Assumes vertices start from 0...N-1
				edge_list.push_back(make_tuple(v1,v2,weight));
			}
		}
		inputf.close();
	}
	else
	{
		cerr<<"Input file not found. Cannot be opened\n";
		exit(1);
	}

	//Read in the edgelist and set the weights to their values, all other weights will
	//be -Inf
	//2 dimensional array of integer weights -Converting to float, make sure ok

	//Build a complete graph on n vertices (hack)
	//TODO: Fix to build it based on the graph structure
	complete_graph g(n_vertices);
	multi_array<float, 2> weights(extents[n_vertices][n_vertices]);
	vector<Vertex> parent(n_vertices);
	//Vertex roots[] = {0, 1};
	Vertex* roots = new Vertex[n_vertices];
	for(int vi = 0;vi<n_vertices;vi++)
		roots[vi]=vi;

	vector<Edge> branching;
	//Initialize all weights to 0
	for(int i=0;i<n_vertices;i++)
	{
		for(int j=0;j<n_vertices;j++)
		{
			//If using Minimum Spanning Tree
			if(mode.compare("min")==0)
				weights[i][j] = numeric_limits<int>::max();
			else//Maximum spanning tree
				weights[i][j] = numeric_limits<int>::min();
		}
	}

	//Write out the indices of the edge set that form the directed spanning tree
	numeric::ublas::mapped_matrix<int> index_map (n_vertices,n_vertices);
	DEBUG_MSG("---Graph Read---\nN: "<<n_vertices);
	BOOST_FOREACH(g_edge,edge_list)
	{
		v1 = get<0>(g_edge);
		v2 = get<1>(g_edge);
		weight=get<2>(g_edge);
		DEBUG_MSG(v1<<" | "<<v2<<" | "<<weight);
		weights[v1][v2] = weight;
		index_map(v1,v2) = ctr++;
	}
	bool isMax = true;
	if(mode.compare("min")==0)
	{
		isMax = false;
		//TOptimum isMaximum set to false -> MinST
		//TOptimum isMaximum set to true -> MaxST
	}
	vector< vector< Edge > > rooted_branching(n_vertices);

	double optVal = numeric_limits<int>::max();
	if(isMax)
		optVal = numeric_limits<int>::min();
	int optRoot= -1;
	double branchingWeight;
	for(int vi=0;vi<n_vertices;vi++)
	{
		//Compute rooted branching
		if(isMax)
		{
			edmonds_optimum_branching<true, true, true>
			(g, identity_property_map(), weights.origin(),
			 roots+vi, roots + vi+1, back_inserter(rooted_branching[vi]));
		}
		else
		{
		edmonds_optimum_branching<false, true, true>
			(g, identity_property_map(), weights.origin(),
			 roots+vi, roots + vi+1, back_inserter(rooted_branching[vi]));
		}
		//Track the weight of the branching
		branchingWeight = nodeWeights[vi];
		BOOST_FOREACH(Edge e,rooted_branching[vi])
		{
			branchingWeight += weights[source(e, g)][target(e, g)];
		}
		//Track the max/min branching weight
		if(isMax && branchingWeight>optVal)
		{
			optVal = branchingWeight;
			optRoot = vi;
		}
		if(!isMax && branchingWeight<optVal)
		{
			optVal = branchingWeight;
			optRoot = vi;
		}
	}

	//Write result to file
	DEBUG_MSG("--Optimal ("<<mode<<") Branching--");
	ofstream outf(fout.c_str());
	if(outf==NULL)
	{
		cerr<<"Output file not created."<<endl;
		exit(1);
	}
	BOOST_FOREACH(Edge e ,rooted_branching[optRoot])
	{
		v1 =source(e, g);
		v2 =target(e, g);
		ctr = index_map(v1,v2);

		DEBUG_MSG(v1<<"->"<<v2<<" idx:"<<ctr);
		outf<<ctr<<endl;
	}
	delete roots;
	delete nodeWeights;
	return EXIT_SUCCESS;

}
string PNAME = "mstwrapper";
int main(int argc, char *argv[])
{
	if (argc != 4)
	{
		cerr << "Usage: " << PNAME
			 << " <input file name> <output file name> <min|max>\n";
		exit(1);
	}
	string mode = string(argv[3]);
	if(mode.compare("min")==0 || mode.compare("max")==0)//strcmp(argv[3],"min")==0 || strcmp(argv[3],"max"){
	{
		return dirMST(argv[1],argv[2],mode);
	}
	else
	{
		cerr<<"Final argument not <min> or <max>"<<endl;
		exit(1);
	}

}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

constexpr double deg_to_rad(double deg) {
	return PI * deg / 180.0;
}

constexpr double rad_to_deg(double rad) {
	return rad * 180 / PI;
}

// 二次元ベクトルクラス
class Vec2d {
	double _x;
	double _y;

	public:
	// 原点の位置ベクトル
	static Vec2d origin() { return Vec2d(0.0, 0.0); }

	// ゼロベクトル
	static Vec2d zero() { return Vec2d::origin(); }

	Vec2d(double a, double b) : _x(a), _y(b) {}

	double x() const { return this->_x; }
	double y() const { return this->_y; }

	// 外積
	double det(const Vec2d& rhs) const {
		return this->x() * rhs.y() - this->y() * rhs.x();
	}

	// 内積
	double dot(const Vec2d& rhs) const {
		return this->x() * rhs.x() + this->y() * rhs.y();
	}

	// 長さ
	double length() const { return this->distance(Vec2d::origin()); }

	// 2つの位置ベクトル間のユークリッド距離
	double distance(const Vec2d& rhs) const {
		return std::sqrt(square(this->x() - rhs.x()) +
						 square(this->y() - rhs.y()));
	}

	// 2つの位置ベクトル間のマンハッタン距離
	double manhattan_distance(const Vec2d& rhs) const {
		return std::abs(this->x() - rhs.x()) + std::abs(this->y() - rhs.y());
	}

	// ベクトルとx軸のなす角
	double argument() const { return std::atan2(this->y(), this->x()); }

	// 反時計回りに回転したベクトル
	Vec2d rotate(double rad) const {
		double xx = this->x();
		double yy = this->y();
		return Vec2d(xx * cos(rad) - yy * sin(rad),
					 xx * sin(rad) + yy * cos(rad));
	}

	// 単位ベクトル
	Vec2d unit() const {
		double len = this->length();
		return Vec2d(this->x() / len, this->y() / len);
	}

	// 法線ベクトル
	Vec2d normal() const {
		double len = this->length();
		return Vec2d(this->y() / len, -this->x() / len);
	}

	// 平行判定
	bool is_parallel(const Vec2d& rhs) const { return false; }

	// ベクトル和
	Vec2d operator+() const { return *this; }
	Vec2d operator+(const Vec2d& rhs) const {
		return Vec2d(this->x() + rhs.x(), this->y() + rhs.y());
	}

	// ベクトル差
	Vec2d operator-() const { return Vec2d(-this->x(), -this->y()); }
	Vec2d operator-(const Vec2d& rhs) const {
		return Vec2d(this->x() - rhs.x(), this->y() - rhs.y());
	}

	// スカラー積
	Vec2d operator*(const double rhs) const {
		return Vec2d(this->x() * rhs, this->y() * rhs);
	}
	Vec2d operator/(const double rhs) const {
		return Vec2d(this->x() / rhs, this->y() / rhs);
	}

	bool operator<(const Vec2d& rhs) const {
		return sign(this->x()) ? this->y() < rhs.y() : this->x() < rhs.x();
	}

	private:
};

int main() {
	int n, x0, y0, xharf, yharf;
	cin >> n;
	cin >> x0 >> y0;
	cin >> xharf >> yharf;

	Vec2d p0 = {x0, y0};
	Vec2d ph = {xharf, yharf};

	Vec2d center = (p0 + ph) / 2;
	Vec2d c_to_p0 = p0 - center;
	Vec2d c_to_pr = c_to_p0.rotate(PI * 2 / n);
	Vec2d result = center + c_to_pr;

	cout << PRECISION(15) << result.x() << " " << result.y() << endl;
	return 0;
}


#include "MapsMesh.h"

#include <igl/gaussian_curvature.h>
#include <poly2tri.h>
#include <tbb/tbb.h>

#include <Eigen/Dense>
#include <algorithm>
#include <limits>
#include <progressbar/progressbar.hpp>

namespace Maps {

template <class Mesh>
static void OpenMesh2IGL(const Mesh *mesh, Eigen::MatrixX3d &V, Eigen::MatrixX3i &F) {

    assert(mesh->is_trimesh());

    auto verticesCount = std::distance(mesh->vertices_sbegin(), mesh->vertices_end());
    auto facesCount = std::distance(mesh->faces_sbegin(), mesh->faces_end());
    V.resize(verticesCount, 3);
    F.resize(facesCount, 3);

    std::map<typename Mesh::VertexHandle, int> newVertexHandleMap;
    int j = 0;
    for (auto i = mesh->vertices_sbegin(); i != mesh->vertices_end(); i++, j++) {
        auto p = mesh->point(*i);
        V(j, 0) = p[0];
        V(j, 1) = p[1];
        V(j, 2) = p[2];
        newVertexHandleMap[*i] = j;
    }

    j = 0;
    for (auto i = mesh->faces_sbegin(); i != mesh->faces_end(); i++, j++) {
        auto fh = *i;
        int vi = 0;
        for (typename Mesh::ConstFaceVertexCCWIter fvi = mesh->cfv_ccwbegin(fh);
             vi < 3 && fvi != mesh->cfv_ccwend(fh); ++fvi) {
            F(j, vi) = newVertexHandleMap[*fvi];
            vi++;
        }
    }
}

int MapMesh::CDTTrangle(const Coordinate2DPair &coordinates,
                        std::vector<std::array<VertexHandle, 3>> &faces,
                        BaryCoor &barycentricCoordinates) {

    faces.clear();

    std::vector<p2t::Point *> points(coordinates.size());
    for (int i = 0; i < coordinates.size(); i++) {
        double x = coordinates[i].second[0];
        double y = coordinates[i].second[1];

        points[i] = new p2t::Point(x, y);
    }

    p2t::CDT cdt(points);

    try {
        cdt.Triangulate();
    } catch (const std::runtime_error &exception) {
        std::cout << exception.what() << std::endl;
        throw exception;
    }

    auto triangles = cdt.GetTriangles();
    if (!p2t::IsDelaunay(triangles)) {
        return -1;
    }

    int trangleIdx = -1;
    for (int j = 0; j < triangles.size(); j++) {
        auto &triangle = triangles[j];
        std::array<Point2D, 3> triangle2D;
        triangle2D[0] = Point2D(triangle->GetPoint(0)->x, triangle->GetPoint(0)->y);
        triangle2D[1] = Point2D(triangle->GetPoint(1)->x, triangle->GetPoint(1)->y);
        triangle2D[2] = Point2D(triangle->GetPoint(2)->x, triangle->GetPoint(2)->y);

        std::array<VertexHandle, 3> face;
        for (int i = 0; i < 3; i++) {
            auto position = std::find(points.begin(), points.end(), triangle->GetPoint(i));
            if (position == points.end()) return -1;
            face[i] = coordinates[position - points.begin()].first;
        }
        faces.push_back(face);

        /**
         * 计算原点的重心坐标
         */

        if (!IsInTriangle(Point2D(0, 0), triangle2D)) continue;

        auto [alpha, beta, gamma] = CalculateBaryCoor(Point2D(0, 0), triangle2D);
        barycentricCoordinates[0] = std::make_pair(face[0], alpha);
        barycentricCoordinates[1] = std::make_pair(face[1], beta);
        barycentricCoordinates[2] = std::make_pair(face[2], gamma);
        trangleIdx = j;
    }

    for (auto &point : points) {
        delete point;
    }

    return trangleIdx;
}

int MapMesh::MVTTrangle(const Coordinate2DPair &coordinates, int startIdx,
                        std::vector<std::array<VertexHandle, 3>> &faces,
                        BaryCoor &barycentricCoordinates) {
    faces.clear();
    int trangleIdx = -1;
    for (size_t i = 0, ii = 1; i < coordinates.size(); i++, ii++, ii %= coordinates.size()) {
        if (i == startIdx || ii == startIdx) continue;

        std::array<VertexHandle, 3> face(
            {coordinates[i].first, coordinates[ii].first, coordinates[startIdx].first});

        faces.push_back(face);

        std::array<Point2D, 3> triangle;
        triangle[0] = coordinates[startIdx].second;
        triangle[1] = coordinates[i].second;
        triangle[2] = coordinates[ii].second;

        /**
         * 计算原点的重心坐标
         */

        if (!IsInTriangle(Point2D(0, 0), triangle)) continue;

        auto [alpha, beta, gamma] = CalculateBaryCoor(Point2D(0, 0), triangle);
        barycentricCoordinates[0] = std::make_pair(coordinates[startIdx].first, alpha);
        barycentricCoordinates[1] = std::make_pair(coordinates[i].first, beta);
        barycentricCoordinates[2] = std::make_pair(coordinates[ii].first, gamma);

        trangleIdx = static_cast<int>(faces.size() - 1);
    }
    return trangleIdx;
}

std::optional<MapMesh::Point2D> MapMesh::ReCalculate2DCoordinates(
    std::map<VertexHandle, Point2D> &originCoor, VertexHandle deleteVertex) const {
    if (!IsVertexDeleted(deleteVertex)) return std::nullopt;
    auto baryCoor = data(deleteVertex).baryCoor.value();

    return originCoor[baryCoor[0].first] * baryCoor[0].second +
           originCoor[baryCoor[1].first] * baryCoor[1].second +
           originCoor[baryCoor[2].first] * baryCoor[2].second;
}

void MapMesh::ReTrangleAndAddFace(const VertexHandle &deleteVertex) {
    // 记录1领域面中所有包含的点, 并更新这些点的参数
    // 假设1领域面f上记录了一个点v, v中记录了v在f上的重心坐标(a, b, c), 面f在经过2维映射变成了f'
    // 更新过程: 1. 根据f'和(a, b, c)计算v的二维映射点v'
    //           2. 重新计算包含v'的三角形p‘, 并根据p’重新计算坐标(a', b', c')
    //           3. 删除f, 添加面p, 更新v的重心坐标, 建立p与v的关联

    std::vector<FaceHandle> ringFaces(vf_begin(deleteVertex), vf_end(deleteVertex));
    Coordinate2DPair coordinates;
    Calculate2D(deleteVertex, coordinates);

    // 重新计算1领域面上所有包含的点的二维坐标
    std::map<VertexHandle, Point2D> currentPoint2DMap(coordinates.begin(), coordinates.end());
    std::map<VertexHandle, Point2D> deletePoint2DMap;
    for (auto &face : ringFaces) {
        for (auto &vertex : data(face).vertrices) {
            auto newCoor = ReCalculate2DCoordinates(currentPoint2DMap, vertex);
            if (!newCoor.has_value()) break;
            deletePoint2DMap[vertex] = newCoor.value();
        }
    }

    delete_vertex(deleteVertex, false);

    BaryCoor barycentricCoordinates;
    std::vector<std::array<VertexHandle, 3>> newFaces;

    int trangleIdx = CDTTrangle(coordinates, newFaces, barycentricCoordinates);
    std::vector<FaceHandle> facesHandle;
    if (trangleIdx != -1 && TryToAddFaces(newFaces, facesHandle)) {
        data(facesHandle[trangleIdx]).vertrices.push_back(deleteVertex);
        data(deleteVertex).baryCoor = barycentricCoordinates;

        // 判断被删除的点在那个三角形内部, 并计算重心坐标

        for (auto &[vertex, point2D] : deletePoint2DMap) {
            auto newParma = UpdateParam(newFaces, currentPoint2DMap, point2D);
            if (!newParma.has_value()) continue;
            auto [idx, alpha, beta, gamma] = newParma.value();
            data(facesHandle[idx]).vertrices.push_back(vertex);

            BaryCoor baryCoor;
            baryCoor[0].first = newFaces[idx][0], baryCoor[0].second = alpha;
            baryCoor[1].first = newFaces[idx][1], baryCoor[1].second = beta;
            baryCoor[2].first = newFaces[idx][2], baryCoor[2].second = gamma;
            data(vertex).baryCoor = baryCoor;
        }

        return;
    }
    for (int i = 0; i < coordinates.size(); i++) {
        newFaces.clear();
        trangleIdx = MVTTrangle(coordinates, i, newFaces, barycentricCoordinates);
        if (TryToAddFaces(newFaces, facesHandle)) {

            data(facesHandle[trangleIdx]).vertrices.push_back(deleteVertex);
            data(deleteVertex).baryCoor = barycentricCoordinates;

            for (auto &[vertex, point2D] : deletePoint2DMap) {
                auto newParma = UpdateParam(newFaces, currentPoint2DMap, point2D);
                if (!newParma.has_value()) continue;
                auto [idx, alpha, beta, gamma] = newParma.value();
                data(facesHandle[idx]).vertrices.push_back(vertex);

                BaryCoor baryCoor;
                baryCoor[0].first = newFaces[idx][0], baryCoor[0].second = alpha;
                baryCoor[1].first = newFaces[idx][1], baryCoor[1].second = beta;
                baryCoor[2].first = newFaces[idx][2], baryCoor[2].second = gamma;
                data(vertex).baryCoor = baryCoor;
            }
            return;
        }
    }
    throw std::runtime_error("can't add face");
}

void MapMesh::Initialize() {
    originFaces = std::vector<FaceHandle>(faces_sbegin(), faces_end());
    for (const auto &face : originFaces) {
        originFaceVertices[face] = std::vector<VertexHandle>(fv_begin(face), fv_end(face));
    }
}

void MapMesh::CalculateCurvature() {
    Eigen::MatrixX3d V;
    Eigen::MatrixX3i F;
    Eigen::VectorXd K;

    OpenMesh2IGL(this, V, F);
    igl::gaussian_curvature(V, F, K);

    int j = 0;
    maxCurvature = (std::numeric_limits<double>::min)();
    for (auto i = vertices_sbegin(); i != vertices_end(); i++, j++) {
        data(*i).curvature = K[j];
        if (K[j] > maxCurvature) {
            maxCurvature = K[j];
        }
    }
}

void MapMesh::CalculateWeight(double lambda) {
    while (!curvatureQueue.empty()) {
        curvatureQueue.pop();
    }

    for (auto i = vertices_sbegin(); i != vertices_end(); i++) {
        data(*i).weight = lambda * data(*i).curvature / maxCurvature +
                          (1 - lambda) * data(*i).ringArea / maxRingArea;
        curvatureQueue.push(*i);
    }
}

void MapMesh::CalculateAreas() {
    maxRingArea = (std::numeric_limits<double>::min)();
    for (auto faceIter = faces_sbegin(); faceIter != faces_end(); faceIter++) {
        double area = CalculateArea(*faceIter);
        data(*faceIter).area = area;
    }

    for (auto vertexIter = vertices_sbegin(); vertexIter != vertices_end(); vertexIter++) {
        double ringArea = 0;
        for (auto faceIter = vf_begin(*vertexIter); faceIter != vf_end(*vertexIter); faceIter++) {
            ringArea += data(*faceIter).area;
        }
        if (ringArea > maxRingArea) {
            maxRingArea = ringArea;
        }
        data(*vertexIter).ringArea = ringArea;
    }
}

void MapMesh::MapFaceFromOriginMesh(const FaceHandle &face, std::array<Point, 3> &mapFace) const {
    if (originFaceVertices.find(face) == originFaceVertices.end()) return;

    const std::vector<VertexHandle> &vertices = originFaceVertices.at(face);
    assert(vertices.size() == 3);
    for (int i = 0; i < 3; i++) {
        auto vertexHandle = vertices[i];
        if (!IsVertexDeleted(vertexHandle)) {
            mapFace[i] = point(vertexHandle);
            continue;
        }

        auto baryCoor = data(vertexHandle).baryCoor.value();
        mapFace[i] = point(baryCoor[0].first) * baryCoor[0].second +
                     point(baryCoor[1].first) * baryCoor[1].second +
                     point(baryCoor[2].first) * baryCoor[2].second;
    }
}

void MapMesh::FaceSubDivision() {
    std::vector<FaceHandle> originFaceHandle(faces_sbegin(), faces_end());

    for (const auto &faceHandle : originFaceHandle) {
        std::vector<VertexHandle> vertices(fv_begin(faceHandle), fv_end(faceHandle));
        assert(vertices.size() == 3);

        for (int i = 0, ii = 1; i < 3; i++, ii++, ii %= 3) {
            auto newPoint = (point(vertices[i]) + point(vertices[ii])) / 2.0;
            auto newPointHandle = add_vertex(newPoint);
            data(newPointHandle).isNew = true;
            vertices.push_back(newPointHandle);
        }
        delete_face(faceHandle);
        add_face({vertices[0], vertices[3], vertices[5]});
        add_face({vertices[3], vertices[1], vertices[4]});
        add_face({vertices[5], vertices[4], vertices[2]});
        add_face({vertices[3], vertices[4], vertices[5]});
    }
}

void MapMesh::DownSampling() {
    request_face_status();
    request_vertex_status();
    request_edge_status();

    for (auto vertexIter = vertices_sbegin(); vertexIter != vertices_end(); vertexIter++) {
        data(*vertexIter).canBeDeleted = true;
    }

    CalculateCurvature();
    CalculateAreas();
    CalculateWeight(0.5);
    while (!curvatureQueue.empty()) {
        auto vertexHandle = curvatureQueue.top();
        curvatureQueue.pop();
        if (!data(vertexHandle).canBeDeleted) continue;

        for (auto ringVertex = vv_begin(vertexHandle); ringVertex != vv_end(vertexHandle);
             ringVertex++) {
            data(*ringVertex).canBeDeleted = false;
        }

        ReTrangleAndAddFace(vertexHandle);
    }
    SaveBaseLevelMesh();
}

void MapMesh::Remesh() {

    int N = static_cast<int>(std::distance(vertices_sbegin(), vertices_end()));
    progressbar bar(N);

    std::mutex _mutex;

    auto IsMapedInASingleFace = [&](const FaceHandle &faceHandle) -> bool {
        std::set<VertexHandle> verticesHandle;
        for (const auto &vertexHandle : originFaceVertices[faceHandle]) {
            if (data(vertexHandle).baryCoor.has_value()) {
                auto baryCoor = data(vertexHandle).baryCoor.value();
                verticesHandle.insert(baryCoor[0].first);
                verticesHandle.insert(baryCoor[1].first);
                verticesHandle.insert(baryCoor[2].first);
            } else {
                verticesHandle.insert(vertexHandle);
            }
        }

        return verticesHandle.size() == 3;
    };

    auto findMaxCommonVertex = [&](const std::vector<FaceHandle> &facesHandle) {
        std::multiset<VertexHandle> verticesHandle;
        for (const auto &faceHandle : facesHandle) {
            for (const auto &vertexHandle : baseLevelMesh->fv_range(faceHandle)) {
                verticesHandle.insert(vertexHandle);
            }
        }

        VertexHandle maxCommonVertex;
        int maxCount = -1;
        for (const auto &vertex : verticesHandle) {
            int count = static_cast<int>(verticesHandle.count(vertex));
            if (count > maxCount) {
                maxCount = static_cast<int>(verticesHandle.count(vertex));
                maxCommonVertex = vertex;
            }
        }

        return maxCommonVertex;
    };

    auto isRingFace = [&](const VertexHandle &vertexHandle, const FaceHandle &faceHandle) {
        for (const auto &face : baseLevelMesh->vf_range(vertexHandle)) {
            if (face == faceHandle) return true;
        }
        return false;
    };

    auto isRingPoint =
        [&](const VertexHandle &vertexHandle, const std::vector<VertexHandle> &ringVertices) {
        for (const auto &ringVertex : ringVertices) {
            bool found = false;
            for (const auto &realRingVertex : baseLevelMesh->vv_range(vertexHandle)) {
                if (realRingVertex == ringVertex) {
                    found = true;
                }
            }
            if (!found) return false;
        }
        return true;
    };

    oneapi::tbb::parallel_for_each(vertices_sbegin(), vertices_end(),
                                   [&](const VertexHandle &vertex) {
        if (!data(vertex).isNew) {
            _mutex.lock();
            bar.update();
            _mutex.unlock();
            /* continue; */
            return;
        }

        // 在基面上找到公共顶点，并展开到2维平面计算重心坐标
        auto vertexFaceOption = baseLevelMesh->FindFace(point(vertex));
        if (!vertexFaceOption.has_value()) {
            _mutex.lock();
            bar.update();
            _mutex.unlock();
            /* continue; */
            return;
        }
        auto vertexFace = vertexFaceOption.value();

        for (const auto &face : originFaces) {
            std::array<Point, 3> mapFace;
            const std::vector<VertexHandle> &fv = originFaceVertices[face];

            if (IsMapedInASingleFace(face)) {  // 原面能够完全映射到基面上
                MapFaceFromOriginMesh(face, mapFace);
                if (IsInTriangle(point(vertex), mapFace)) {
                    auto [alpha, beta, gamma] = CalculateBaryCoor(point(vertex), mapFace);
                    Point newPoint =
                        alpha * point(fv[0]) + beta * point(fv[1]) + gamma * point(fv[2]);
                    point(vertex) = newPoint;
                    break;
                }
            } else {

                std::vector<FaceHandle> baseFaces;
                std::vector<VertexHandle> fixVertex;
                for (const auto &vertexHandle : fv) {
                    if (!IsVertexDeleted(vertexHandle)) {
                        fixVertex.push_back(vertexHandle);
                        continue;
                    }

                    auto baryCoor = data(vertexHandle).baryCoor.value();
                    auto face = baseLevelMesh->FindFace(
                        {baryCoor[0].first, baryCoor[1].first, baryCoor[2].first});

                    if (face.has_value()) {
                        baseFaces.push_back(face.value());
                    }
                }

                VertexHandle commonVertex;
                if (baseFaces.size() == 3) {
                    if (std::find(baseFaces.begin(), baseFaces.end(), vertexFace) ==
                        baseFaces.end()) {
                        continue;
                    }
                    commonVertex = findMaxCommonVertex(baseFaces);
                } else {
                    bool foundCommonVertex = false;
                    for (const auto &baseFace : baseFaces) {
                        for (const auto &baseVert : baseLevelMesh->fv_range(baseFace)) {
                            if (isRingFace(baseVert, vertexFace) &&
                                isRingPoint(baseVert, fixVertex)) {
                                commonVertex = baseVert;
                                foundCommonVertex = true;
                                break;
                            }
                        }
                        if (foundCommonVertex) break;
                    }
                    if (!foundCommonVertex) continue;
                }

                Coordinate2DPair coorPair;
                baseLevelMesh->Calculate2D(commonVertex, coorPair);

                Coordinate2DMap coorMap(coorPair.begin(), coorPair.end());
                Point2D vertex2D(0, 0);
                auto vertex2DBaryCoor = baseLevelMesh->CalculateBaryCoor(point(vertex), vertexFace);
                for (const auto &vertex2DBaryCoorItem : vertex2DBaryCoor) {
                    if (coorMap.find(vertex2DBaryCoorItem.first) == coorMap.end()) {
                        continue;
                    }
                    vertex2D +=
                        coorMap.at(vertex2DBaryCoorItem.first) * vertex2DBaryCoorItem.second;
                }

                std::array<std::pair<VertexHandle, Point2D>, 3> face2D;
                bool canMapTo2D = true;
                for (int i = 0; i < 3; i++) {
                    face2D[i].first = fv[i];
                    face2D[i].second = Point2D(0, 0);
                    if (!IsVertexDeleted(fv[i])) {
                        face2D[i].second = coorMap[fv[i]];
                        continue;
                    }

                    auto baryCoor = data(fv[i]).baryCoor.value();
                    for (const auto &baryCoorItem : baryCoor) {
                        if (baryCoorItem.first == commonVertex) continue;
                        if (coorMap.find(baryCoorItem.first) == coorMap.end()) {
                            canMapTo2D = false;
                            break;
                            /* throw std::runtime_error("can't found item"); */
                        }
                        face2D[i].second += coorMap.at(baryCoorItem.first) * baryCoorItem.second;
                    }
                }
                if (!canMapTo2D) continue;

                std::array<Point2D, 3> triangle{face2D[0].second, face2D[1].second,
                                                face2D[2].second};

                if (!IsInTriangle(vertex2D, triangle)) continue;

                auto [alpha, beta, gamma] = CalculateBaryCoor(vertex2D, triangle);

                point(vertex) = alpha * point(face2D[0].first) + beta * point(face2D[1].first) +
                                gamma * point(face2D[2].first);
                break;
            }
        }
        _mutex.lock();
        bar.update();
        _mutex.unlock();
    });
}

std::optional<std::tuple<int, double, double, double>> MapMesh::UpdateParam(
    const std::vector<std::array<VertexHandle, 3>> &faces,
    const std::map<VertexHandle, Point2D> &point2DMap, const Point2D &point) {

    for (int i = 0; i < faces.size(); i++) {
        const auto &face = faces[i];
        std::array<Point2D, 3> triangle;
        triangle[0] = point2DMap.at(face[0]);
        triangle[1] = point2DMap.at(face[1]);
        triangle[2] = point2DMap.at(face[2]);
        if (!IsInTriangle(point, triangle)) continue;

        auto [alpha, beta, gamma] = CalculateBaryCoor(point, triangle);
        return std::make_tuple(i, alpha, beta, gamma);
    }

    return std::nullopt;
}

void MapMesh::Calculate2D(VertexHandle vertex, Coordinate2DPair &coordinates) const {
    coordinates.clear();
    auto vertex3D = point(vertex);

    double K_i = 0;
    for (const auto &face : vf_range(vertex)) {
        K_i += CalculateAngle(vertex, face);
    }
    double a = 2 * M_PI / K_i;

    K_i = 0;

    VertexHandle lastVertex;
    int i = 0;
    for (const auto &ringPoint : vv_range(vertex)) {
        if (i != 0) {
            auto Vec1 = point(lastVertex) - point(vertex);
            auto Vec2 = point(ringPoint) - point(vertex);

            K_i += std::acos(Vec1.normalized().dot(Vec2.normalized()));
        }
        auto point3D = point(ringPoint);
        double r_k = (point3D - vertex3D).norm();

        Point2D point2D{std::pow(r_k, a) * std::cos(K_i * a), std::pow(r_k, a) * std::sin(K_i * a)};

        coordinates.emplace_back(ringPoint, point2D);
        lastVertex = ringPoint;

        i++;
    }
}

}  // namespace Maps


// *=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*
// ** Copyright UCAR, CSU  (c) 1990 - 2018
// ** University Corporation for Atmospheric Research (UCAR)
// ** National Center for Atmospheric Research (NCAR)
// ** Colorado State University
// ** BSD licence applies
// ** DISCLAIMER: THIS SOFTWARE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS
// ** OR IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
// ** WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
// *=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*

#include <iostream>
#include <string>
#include "Fractl.hh"
#include "Params.hh"
#include "Filters.hh"
#include "Interps.hh"

// Eigen linear algebra library
// http://eigen.tuxfamily.org/index.php?title=Main_Page
#include <Eigen/Dense>

double sumTimea = 0;
int cntTimeb = 0;
double sumTimeb = 0;
double sumTimec = 0;
double sumTimee = 0;
double sumTimef = 0;

bool Fractl::calcWinds()
{
  // One approach is to iterate:
  //   calculate V and U winds
  //   calculate W winds
  //   recalculate V and U winds using the new W winds
  //   recalculate W with the new V, U.
  //   etc.
  //
  // However, tests show that there is negligible gain
  // in accuracy after the first iteration.
  // So the loop limit is hardcoded at 1.

  for (int imain = 0; imain < 1; imain++) {
    // bool showStat = true;
    // Based on current w, calc u, v.
    if (gridType == Params::GRID_MISH)
      calcAllVUOnMish(
		numNbrMax,       // max num nearest nbrs
		pointVec,        // all observations
		radarKdTree,     // nearest nbr tree for pointVec
		cellMat);
    else
      calcAllVU(
		numNbrMax,       // max num nearest nbrs
		pointVec,        // all observations
		radarKdTree,     // nearest nbr tree for pointVec
		cellMat);

    printRunTime("calcAllVU", &timea);

    // Run low-pass filtering if requested

    Filter *filter = FilterFactory::createFilter(uvFilter);		// TOD indices
    if (filter != NULL) {
      filter->filter_U(cellMat, nradx, nrady, nradz, 1, NULL);
      filter->filter_V(cellMat, nradx, nrady, nradz, 1, NULL);
      delete filter;
    }

    // Run interpolation for missing data if requested
    // TODO: add old RadarWind interpolation and code Interp::interpolate

    Interp *interp = InterpFactory::createInterp(uvInterp);
    if (interp != NULL) {
      interp->interpolate_U(cellMat, nradx, nrady, nradz, std::numeric_limits<double>::quiet_NaN());
      interp->interpolate_V(cellMat, nradx, nrady, nradz, std::numeric_limits<double>::quiet_NaN());
      delete interp;
    }

    // NO LONGER USED:
    // Interpolate missing values from neighbors.
    //
    // The problem is that some cells don't have enough valid
    // neighbors to calculate their winds, so their values are
    // left as NaN.
    //
    // The idea was to fill the NaN cells
    // by interpolating the wind values from nearby cells.
    //
    // But what happens is that generally if a cell doesn't have
    // enough good neighbors for a valid wind calculation,
    // any neighbor cells we might use for interpolation are
    // pretty flakey.
    //xxx del interpolation?
    // Interpolate NaN values in cellMat.uu, cellMat.vv.
    //interpMissing( bugs, nradz, nrady, nradx, cellMat, tmpmat);
    //printRunTime("interpMissing for V,U", &timea);

    // Based on u, v, calc w

    if (gridType == Params::GRID_MISH)
      calcAllWOnMish(cellMat);
    else
      calcAllW(cellMat);

    printRunTime("calcAllW", &timea);

    // Run low-pass filtering on W if requested

    filter = FilterFactory::createFilter(wFilter);
    if (filter != NULL) {
      filter->filter_W(cellMat, nradx, nrady, nradz, 1, NULL);  // TODO indices
      delete filter;
    }

    // Calc deltas using verification data, if any.
    // Write outTxt file.
    checkVerif(imain, cellMat);
    printRunTime("checkVerif", &timea);
  } // for imain
  return true;
}

//======================================================================

// Calculate V and U winds at all cells in the z,y,x grid.

void Fractl::calcAllVU(
		       long numNbrMax,              // max num nearest nbrs
		       vector<Point *> *pointVec,   // all observations
		       KD_tree * radarKdTree,       // nearest nbr tree for pointVec
		       Cell ***& cellMat)           // we set Cell.uu, vv
{

  KD_real * centerLoc = new KD_real[ndim];
  long okCount = 0;

  for (long iz = 0; iz < nradz; iz++) {
    for (long iy = 0; iy < nrady; iy++) {
      for (long ix = 0; ix < nradx; ix++) {

        centerLoc[0] = zgridmin + iz * zgridinc;
        centerLoc[1] = ygridmin + iy * ygridinc;
        centerLoc[2] = xgridmin + ix * xgridinc;

        if (bugs >= Params::DEBUG_NORM) {
          cout << setprecision(5);
          cout << endl << "calcAllVU: iz: " << iz
	       << "  iy: " << iy
	       << "  ix: " << ix
	       << "  z: " << centerLoc[0]
	       << "  y: " << centerLoc[1]
	       << "  x: " << centerLoc[2]
	       << endl;
        }

        Cell * pcell = & cellMat[iz][iy][ix];
        calcCellVU(
		   centerLoc,             // query point
		   numNbrMax,             // max num nearest nbrs
		   pointVec,              // all observations
		   radarKdTree,           // nearest nbr tree for pointVec
		   pcell);                 // Cell.vv, uu are set.

        bool ok = false;
        if (isOkDouble( pcell->vv)
	    && isOkDouble( pcell->uu))
	  {
	    ok = true;
	    okCount++;
	  }

        if (bugs >= Params::DEBUG_VERBOSE) {
          cout << setprecision(7);
          cout << "calcAllVU: ok:" << ok
	       << "  iz: " << iz
	       << "  iy: " << iy
	       << "  ix: " << ix
	       << "  loc:"
	       << "  " << centerLoc[0]
	       << "  " << centerLoc[1]
	       << "  " << centerLoc[2]
	       << "  W: " << cellMat[iz][iy][ix].ww
	       << "  V: " << cellMat[iz][iy][ix].vv
	       << "  U: " << cellMat[iz][iy][ix].uu << endl;
        }

      } // for ix
    } // for iy
  } // for iz

  cout << "---------------- okCount: " << okCount << endl;

  cout << "sumTimea: " << sumTimea << endl;
  cout << "cntTimeb: " << cntTimeb << "  sumTimeb: " << sumTimeb << endl;
  cout << "sumTimec: " << sumTimec << endl;
  cout << "sumTimee: " << sumTimee << endl;
  cout << "sumTimef: " << sumTimef << endl;

  delete[] centerLoc;

} // end calcAllVU

//======================================================================

// Try to calc V and U on the Samurai Mish grid

void Fractl::calcAllVUOnMish(
			     long numNbrMax,              // max num nearest nbrs
			     vector<Point *> *pointVec,   // all observations
			     KD_tree * radarKdTree,       // nearest nbr tree for pointVec
			     Cell ***& cellMat)           // we set Cell.uu, vv
{

  KD_real * centerLoc = new KD_real[ndim];
  long okCount = 0;

  // for (long iz = 0; iz < nradz; iz++)
  //   for (long iy = 0; iy < nrady; iy++)
  //     for (long ix = 0; ix < nradx; ix++)

  //       centerLoc[0] = zgridmin + iz * zgridinc;
  //       centerLoc[1] = ygridmin + iy * ygridinc;
  //       centerLoc[2] = xgridmin + ix * xgridinc;

  // This is the way Samurai iterates on the Mish.
  // *Pos is the position in meter of the observation
  // i*   is the index of the position in node table

  for (int ii = -1; ii < (nradx); ii++) {
    for (int imu = -1; imu <= 1; imu += 2) {
      double iPos = xgridmin + xgridinc * (ii + (0.5 * sqrt(1. / 3.) * imu + 0.5));

      for (int ji = -1; ji < (nrady); ji++) {
	for (int jmu = -1; jmu <= 1; jmu += 2) {
	  double jPos = ygridmin + ygridinc * (ji + (0.5 * sqrt(1. / 3.) * jmu + 0.5));

	  for (int ki = -1; ki < (nradz); ki++) {
	    for (int kmu = -1; kmu <= 1; kmu += 2) {
	      double kPos = zgridmin + zgridinc * (ki + (0.5 * sqrt(1. / 3.) * kmu + 0.5));

	      int ix = (ii + 1) * 2 + (imu + 1) / 2;
	      int iy = (ji + 1) * 2 + (jmu + 1) / 2;
	      int iz = (ki + 1) * 2 + (kmu + 1) / 2;

	      // centerLoc[0] = zgridmin + iz * zgridinc;
	      // centerLoc[1] = ygridmin + iy * ygridinc;
	      // centerLoc[2] = xgridmin + ix * xgridinc;

	      centerLoc[0] = kPos;
	      centerLoc[1] = jPos;
	      centerLoc[2] = iPos;

	      if (bugs >= Params::DEBUG_NORM) {
		std::cout << setprecision(5);
		std::cout << endl << "calcAllVU: iz: " << iz
			  << "  iy: " << iy
			  << "  ix: " << ix
			  << "  z: " << centerLoc[0]
			  << "  y: " << centerLoc[1]
			  << "  x: " << centerLoc[2]
			  << endl;
	      }

	      Cell * pcell = & cellMat[iz][iy][ix];
	      calcCellVU(centerLoc,             // query point
			 numNbrMax,             // max num nearest nbrs
			 pointVec,              // all observations
			 radarKdTree,           // nearest nbr tree for pointVec
			 pcell);                 // Cell.vv, uu are set.

	      bool ok = false;
	      if (isOkDouble( pcell->vv) && isOkDouble( pcell->uu)) {
		ok = true;
		okCount++;
	      }

	      if (bugs >= Params::DEBUG_VERBOSE) {
		cout << setprecision(7);
		cout << "calcAllVU: ok:" << ok
		     << "  iz: " << iz
		     << "  iy: " << iy
		     << "  ix: " << ix
		     << "  loc:"
		     << "  " << centerLoc[0]
		     << "  " << centerLoc[1]
		     << "  " << centerLoc[2]
		     << "  W: " << cellMat[iz][iy][ix].ww
		     << "  V: " << cellMat[iz][iy][ix].vv
		     << "  U: " << cellMat[iz][iy][ix].uu << endl;
	      }
	    }
	  } // ki
	}
      } // ji
    }
  }  // ii

  cout << "---------------- okCount: " << okCount << endl;

  cout << "sumTimea: " << sumTimea << endl;
  cout << "cntTimeb: " << cntTimeb << "  sumTimeb: " << sumTimeb << endl;
  cout << "sumTimec: " << sumTimec << endl;
  cout << "sumTimee: " << sumTimee << endl;
  cout << "sumTimef: " << sumTimef << endl;

  delete[] centerLoc;

} // end calcAllVUOnMish

//======================================================================

// Calculate V and U winds for a single location.
//
// At a given location centerLoc, find the nearest nbrs
// in pointVec, and use the radial velocities to calculate
// the winds V, U, (not W).
//
// Let vx, vy, vz be estimates of the wind velocity,
// and vr be the radial velocity.
//
// For each radar m, we use the linear model:
//    vr_est[m] = cos(theta) cos(elev) vx
//      + sin(theta) cos(elev) vy
//      + sin(elev) vz
//    vr_est[m] = conx vx + cony * vy + conz * vz
//
// Let E[m] = error = vr_true[m] - vr_est[m]
//
// We want to minimize Q = sum_m E[m]^2
//
//
// Let
//   sumxx = sum(conx * conx)
//   sumxr = sum(conx * vr)
//   etc.
//
// Normal equations, derived from
// 0 = d(Q)/d(vx), 0 = d(Q)/d(vy), 0 = d(Q)/d(vz),
// give:
// vx sumxx + vy sumxy + vz sumxz = sumxr
// vx sumxy + vy sumyy + vz sumyz = sumyr
// vx sumxz + vy sumyz + vz sumzz = sumzr
//
// Or for 2 dim,
// vx sumxx + vy sumxy = sumxr - vz sumxz
// vx sumxy + vy sumyy = sumyr - vz sumyz
//
// Using Cramer's rule on the 2-dim case:
// Let demom = sumxx sumyy - sumxy^2
// vx_hat = ( (sumxr - vz sumxz) sumyy - (sumyr - vz sumyz) sumxy ) / denom
// vy_hat = ( (sumyr - vz sumyz) sumxx - (sumxr - vz sumxz) sumxy ) / denom

void Fractl::calcCellVU(
			KD_real * centerLoc,           // query point: z, y, x
			long numNbrMax,                // max num nearest nbrs
			vector<Point *> *pointVec,     // all observations
			KD_tree * radarKdTree,         // nearest nbr tree for pointVec
			Cell * pcell)                  // we fill vv, uu.
{
  struct timeval timea;
  addDeltaTime( &timea, NULL);

  pcell->uu = numeric_limits<double>::quiet_NaN();
  pcell->vv = numeric_limits<double>::quiet_NaN();

  bool showDetail = testDetail(
			       centerLoc[0],         // z
			       centerLoc[1],         // y
			       centerLoc[2]);        // x

  if (bugs >= Params::DEBUG_EXTRA && showDetail) {
    cout << setprecision(7);
    cout << "calcCellVU.entry: showDetail:" << endl;
    cout << "    centerLoc: z: " << centerLoc[0] << endl;
    cout << "    centerLoc: y: " << centerLoc[1] << endl;
    cout << "    centerLoc: x: " << centerLoc[2] << endl;
    cout << "    ww: " << pcell->ww << endl;
    cout << "    vv: " << pcell->vv << endl;
    cout << "    uu: " << pcell->uu << endl;
    cout << "    meanNbrDbz: " << pcell->meanNbrDbz << endl;
    cout << "    meanNbrNcp: " << pcell->meanNbrNcp << endl;
    cout << "    meanNbrElevDeg: " << pcell->meanNbrElevDeg << endl;
    cout << "    meanNbrKeepDist: " << pcell->meanNbrKeepDist << endl;
    cout << "    meanNbrOmitDist: " << pcell->meanNbrOmitDist << endl;
    cout << "    condNum: " << pcell->conditionNumber << endl;
  }

  if (numNbrMax == 0) throwerr("cell numNbrMax == 0");

  int nbrIxs[numNbrMax];
  KD_real nbrDistSqs[numNbrMax];              // nbr dist^2
  for (long inbr = 0; inbr < numNbrMax; inbr++) {
    nbrIxs[inbr] = -1;
  }

  Statistic nbrDbzStat;
  Statistic nbrNcpStat;
  Statistic nbrElevDegStat;
  Statistic nbrKeepDistStat;
  Statistic nbrOmitDistStat;

  Point** nearPts = new Point *[numNbrMax];

  addDeltaTime( &timea, &sumTimea);
  struct timeval timeb;
  addDeltaTime( &timeb, NULL);

  // Find nearest nbrs
  radarKdTree->nnquery(
		       centerLoc,        // query point
		       numNbrMax,        // desired num nearest nbrs
		       KD_EUCLIDEAN,     // Metric
		       1,                // MinkP
		       nbrIxs,           // out: parallel array, indices of nearest nbrs
		       nbrDistSqs);      // out: parallel array, squares of distances of nbrs

  cntTimeb++;
  addDeltaTime( &timeb, &sumTimeb);
  struct timeval timec;
  addDeltaTime( &timec, NULL);

  if (bugs >= Params::DEBUG_VERBOSE && showDetail) {
    cout << setprecision(7);
    cout << "  calcCellVU: showDetail:  nearPts for centerLoc: z: "
	 << centerLoc[0] << "  y: " << centerLoc[1]
	 << "  x: " << centerLoc[2] << endl;
  }

  int numNbrActual = 0;

  // Keep points within base + factor * aircraft dist

  for (long inbr = 0; inbr < numNbrMax; inbr++) {
    if (nbrIxs[inbr] < 0) throwerr("nbrIxs < 0");

    Point * nearPt = pointVec->at( nbrIxs[inbr]);
    double aircraftDist = calcDistPtAircraft( nearPt);
    double localDist = calcDistLocPt( centerLoc, nearPt);
    double maxDist = maxDistBase + maxDistFactor * aircraftDist;

    // Use local distance constraint from grid point center
    double roi = sqrt(xgridinc * xgridinc + ygridinc * ygridinc + zgridinc * zgridinc);
    if (roi < maxDist) maxDist = roi;

    const char * msg;

    if (localDist < maxDist) {
      nbrDbzStat.addOb( nearPt->dbz);
      nbrNcpStat.addOb( nearPt->ncp);
      nbrElevDegStat.addOb( nearPt->elevRad * 180 / M_PI);
      nbrKeepDistStat.addOb( localDist);
      nearPts[numNbrActual++] = nearPt;
      msg = "KEEP";
    } // if localDist < maxDist

    else {
      nbrOmitDistStat.addOb( localDist);
      msg = "OMIT";
    }

    if (bugs >= Params::DEBUG_VERBOSE && showDetail) {
      cout << setprecision(5);
      cout << "    " << msg
	   << "  inbr: " << inbr
	   << "  dist: " << localDist
	   << "  pt:"
	   << "  coordz: " << nearPt->coordz
	   << "  coordy: " << nearPt->coordy
	   << "  coordx: " << nearPt->coordx
	   << "  vg: " << nearPt->vg
	   << endl;
    }

  } // for inbr

  if (bugs >= Params::DEBUG_EXTRA && showDetail) {
    cout << "  calcCellVU: showDetail: cell z: "
	 << centerLoc[0] << "  y: " << centerLoc[1]
	 << "  x: " << centerLoc[2] << "  numNbrActual: " << numNbrActual << endl;
  }

  if (numNbrActual >= 2) {    // if numNbrActual is ok

    pcell->meanNbrDbz = nbrDbzStat.dsum / nbrDbzStat.numGood;
    pcell->meanNbrNcp = nbrNcpStat.dsum / nbrNcpStat.numGood;
    pcell->meanNbrElevDeg = nbrElevDegStat.dsum / nbrElevDegStat.numGood;
    pcell->meanNbrKeepDist = nbrKeepDistStat.dsum / nbrKeepDistStat.numGood;
    pcell->meanNbrOmitDist = nbrOmitDistStat.dsum / nbrOmitDistStat.numGood;

    if (bugs >= Params::DEBUG_EXTRA && showDetail) {
      for (long inbr = 0; inbr < numNbrActual; inbr++) {
	Point * pt = nearPts[inbr];
	cout << setprecision(7);
	cout << "  calcCellVU: showDetail: nbr: inbr: " << inbr
	     << "  vg: " << pt->vg
	     << "  dbz: " << pt->dbz
	     << "  ncp: " << pt->ncp
	     << "  theta deg: " << (pt->thetaRad * 180 / M_PI)
	     << "  elev deg: " << (pt->elevRad * 180 / M_PI)
	     << setprecision(15)
	     << "  deltaTime: "
	     << (pt->rayTime - nearPts[0]->rayTime)
	     << endl;
      }
      for (long inbr = 0; inbr < numNbrActual; inbr++) {
	Point * pt = nearPts[inbr];
	cout << "  showDetail.from.aircraft.to.nbr: set arrow "
	     << (inbr + 1)
	     << " from " << pt->aircraftx << "," << pt->aircrafty
	     << " to " << pt->coordx << "," << pt->coordy
	     << endl;
      }
    } // if showDetail

    addDeltaTime( &timec, &sumTimec);
    struct timeval timee;
    addDeltaTime( &timee, NULL);

    // 3d solve
    //    vr = cos(theta) cos(elev) vx
    //      + sin(theta) cos(elev) vy
    //      + sin(elev) vz

    // 2d solve
    //    vr - sin(elev) vz = cos(theta) cos(elev) vx
    //      + sin(theta) cos(elev) vy

    // Find wwind = W wind estimate near the pt.
    double wwind = pcell->ww;
    bool isCellOk = false;

    if (useEigen) {

      Eigen::MatrixXd amat( numNbrActual, 2);
      Eigen::VectorXd bvec( numNbrActual);

      for (long inbr = 0; inbr < numNbrActual; inbr++) {
	Point * pt = nearPts[inbr];
	amat( inbr, 0) = cos( pt->thetaRad) * cos( pt->elevRad);
	amat( inbr, 1) = sin( pt->thetaRad) * cos( pt->elevRad);

	// Find wwind = W wind estimate near the pt.
	double wwind = pcell->ww;

	bvec( inbr) = pt->vg - wwind * sin( pt->elevRad);
      }
      if (bugs >= Params::DEBUG_EXTRA && showDetail) {
	cout << "\n  calcCellVU: eigen: amat:\n" << amat << endl;
	cout << "\n  calcCellVU: eigen: bvec:\n" << bvec << endl;
      }

      Eigen::JacobiSVD<Eigen::MatrixXd> svd(
					    amat, Eigen::ComputeThinU | Eigen::ComputeThinV);
      Eigen::VectorXd singVals = svd.singularValues();
      Eigen::MatrixXd thinV = svd.matrixV();
      long slen = singVals.size();
      // long vlen = thinV.rows();

      pcell->conditionNumber = numeric_limits<double>::infinity();
      if (slen > 0 && singVals[slen-1] != 0)
	pcell->conditionNumber = singVals[0] / singVals[slen-1];

      if (bugs >= Params::DEBUG_EXTRA && showDetail) {
	cout << "\n  calcCellVU: eigen: singVals:\n" << singVals << endl;
	cout << "\n  calcCellVU: num nonzeroSingularValues:\n"
	     << svd.nonzeroSingularValues() << endl;
	cout << "  calcCellVU: conditionNumber: "
	     << pcell->conditionNumber << endl;
      }

      // Using a conditionNumberCutoff > 10 causes some cells
      // to have extreme values for U and V.

      if (pcell->conditionNumber < conditionNumberCutoff)
	isCellOk = true;
      if (isCellOk) {
	Eigen::VectorXd xvec = svd.solve( bvec);
	Eigen::VectorXd errvec = amat * xvec - bvec;
	// double maxAbsErr = errvec.array().abs().maxCoeff();
	// double meanSqErr = errvec.dot( errvec) / numNbrActual;   // dot product

	pcell->vv = xvec(1);       // v
	pcell->uu = xvec(0);       // u

	double ustd = 0;
	double vstd = 0;

	for (long s = 0; s < slen; s++) {
	  // double thin0 = TODO check if the calls to thinV() has side effects
	  thinV(0, s);
	  // double thin1 =
	  thinV(1, s);
	  // double sing =
	  singVals[s];
	  ustd += pow( (thinV(s, 0) / singVals[s]) , 2.0);
	  vstd += pow( (thinV(s, 1) / singVals[s]) , 2.0);
	}
	pcell->ustd = sqrt(ustd);
	pcell->vstd = sqrt(vstd);

        if (fabs(pcell->vv) > maxV) {
          pcell->vv = numeric_limits<double>::quiet_NaN();
	  pcell->vstd = numeric_limits<double>::quiet_NaN();
	}
        if (fabs(pcell->uu) > maxU) {
          pcell->uu = numeric_limits<double>::quiet_NaN();
	  pcell->ustd = numeric_limits<double>::quiet_NaN();
	}

      } // if isCellOk
    } // if useEigen

    // Else use cramer's method
    else {
      double sxx = 0;
      double sxy = 0;
      double syy = 0;
      double sxz = 0;
      double syz = 0;

      for (long inbr = 0; inbr < numNbrActual; inbr++) {
	Point * pt = nearPts[inbr];
	double xval = cos( pt->thetaRad) * cos( pt->elevRad);
	double yval = sin( pt->thetaRad) * cos( pt->elevRad);
	double zval = pt->vg - wwind * sin( pt->elevRad);

	sxx += xval * xval;
	sxy += xval * yval;
	syy += yval * yval;
	sxz += xval * zval;
	syz += yval * zval;
      }
      double detbase = sxx*syy - sxy*sxy;
      double det1    = sxz*syy - syz*sxy;
      double det2    = sxx*syz - sxy*sxz;
      if (bugs >= Params::DEBUG_EXTRA && showDetail) {
	cout << "  calcCellVU: cramer: showDetail: "
	     << "  sxx: " << sxx
	     << "  sxy: " << sxy
	     << "  syy: " << syy
	     << "  sxz: " << sxz
	     << "  syz: " << syz << endl;
	cout << "  calcCellVU: cramer: showDetail: "
	     << "  detbase: " << detbase
	     << "  det1: " << det1
	     << "  det2: " << det2 << endl;
      }

      // Using a detCutoff <= 0.01 causes some cells
      // to have extreme values for U and V.
      // A detCutoff of 0.1 gives roughly similar results to using
      // a conditionNumber cutoff of 10.
      double detCutoff = 0.1;    // xxxxxxxxx

      if (fabs(detbase) > detCutoff) {
	pcell->uu = det1 / detbase;
	pcell->vv = det2 / detbase;
      }
    }

    addDeltaTime( &timee, &sumTimee);
    delete[] nearPts;
  } // else numNbrActual is ok

  struct timeval timef;
  addDeltaTime( &timef, NULL);

  if (bugs >= Params::DEBUG_EXTRA && showDetail) {
    cout << setprecision(7);
    cout << "calcCellVU.exit: showDetail:" << endl;
    cout << "    centerLoc: z: " << centerLoc[0] << endl;
    cout << "    centerLoc: y: " << centerLoc[1] << endl;
    cout << "    centerLoc: x: " << centerLoc[2] << endl;
    cout << "    ww: " << pcell->ww << endl;
    cout << "    vv: " << pcell->vv << endl;
    cout << "    uu: " << pcell->uu << endl;
    cout << "    meanNbrDbz: " << pcell->meanNbrDbz << endl;
    cout << "    meanNbrNcp: " << pcell->meanNbrNcp << endl;
    cout << "    meanNbrElevDeg: " << pcell->meanNbrElevDeg << endl;
    cout << "    meanNbrKeepDist: " << pcell->meanNbrKeepDist << endl;
    cout << "    meanNbrOmitDist: " << pcell->meanNbrOmitDist << endl;
    cout << "    condNum: " << pcell->conditionNumber << endl;
  }

} // end calcCellVU

// xxx all alloc: delete

//======================================================================

// Calculate the W (vertical) winds for all cells in the z,y,x grid.
//
// Consider the vertical column of cells for some
// given horizontal coordinates iy, ix.
// By the mass continuity equation, the total volume of air coming
// into the column must equal the total volume of air leaving the column.
//
// 0 = totalFlow = sum_iz (density_iz * sideFlow_iz) + endFlow
// where
//   density_iz = the density at level iz
//   sideFlow_iz = flow through the sides of the cell at level iz
//   endFlow = flow in the bottom and out the top end of the column
//           = densityBot * bottomFlow - densityTop * topFlow
//
// We assume the end flows are 0, so
//   bottendFlow = 0
//   topFlow = 0
//   endFlow = 0
//
// sideFlow_iz = uFlow_iz + vFlow_iz
// uFlow_iz = (flow in from cell ix-1) - (flow out to cell ix+1)
// uFlow_iz = 0.5 * (u[ix-1] + u[ix]) - 0.5 * (u[ix] + u[ix+1])
//          = 0.5 * (u[ix-1] - u[ix+1])
//
// sideFlow_iz = 0.5 * (u[ix-1] - u[ix+1] + v[ix-1] - v[ix+1])
//
// 0 = totalFlow = sum_iz (density_iz * 0.5 * (
//       v[iy-1] - v[iy+1]
//     + u[ix-1] - u[ix+1]))
//
// But in real life the totalFlow sum is not 0.
//
// Now we ask what modifications to the U and V values
// would make the totalFlow 0.
// We want to modify those U,V values with larger elevation angles more.
// xxx future: handle geometric uncertainty values too.
//
// At each layer, we have 4 terms to adjust: 2 U terms and 2 V terms.
// Let us subtract h[iz] from each term on level iz.
//
// Find h[iz] such that:
//   0 = sum_iz (density_iz * 0.5 * (
//         (v[iy-1]-h[iz]) - (v[iy+1]+h[iz])
//       + (u[ix-1]-h[iz]) - (u[ix+1])+h[iz]))
//
//     = totalFlow - sum_iz (density_iz * 0.5 * 4 * h[iz])
//
//   totalFlow = 2 * sum_iz (density_iz * h[iz])
//
// Let h[iz] be weighted by the elevation angle, so
//   h[iz] = H * wgt[iz]
//   wgt[iz] = cos(elevationAngle)
//
//   totalFlow = 2 * sum_iz (density[iz] * H * wgt[iz])
//   H = totalFlow / (2 * sum_iz (density[iz] * wgt[iz]))

void Fractl::calcAllWOnMish(
		      Cell ***& cellMat)           // We set Cell.ww
{
  long maxx = (xgridmax - xgridmin) * 2 / xgridinc + 1;
  long maxy = (ygridmax - ygridmin) * 2 / ygridinc + 1;
  long maxz = (zgridmax - zgridmin) * 2 / zgridinc + 1;

  double * density = new double[maxz + 3];	 // range is 0..maxz
  double * wgts = new double[maxz + 3];

  // double * density = new double[nradz];
  // double * wgts = new double[nradz];

  //  for (long iz = 0; iz < nradz; iz++)
  //    density[iz] = calcDensity( zgridmin + iz * zgridinc);

  // Use samurai index computation since we need both position and index

  for (int ki = -1; ki < (nradz); ki++) {
    for (int kmu = -1; kmu <= 1; kmu += 2) {
      double kPos = zgridmin + zgridinc * (ki + (0.5 * sqrt(1. / 3.) * kmu + 0.5));
      int ik = (ki + 1) * 2 + (kmu + 1) / 2;
      density[ik] = calcDensity(kPos);
    }
  }

  // Omit the edges of the region as we use ix-1, ix+1, iy-1, iy+1.

  // for (long iy = 1; iy < nrady - 1; iy++)
  //   for (long ix = 1; ix < nradx - 1; ix++)

  for (long iy = 1; iy < maxy - 1; iy++) {
    for (long ix = 1; ix < maxx - 1; ix++) {

      // for (long iz = 0; iz < nradz; iz++)
      for (long iz = 0; iz < maxz; iz++) {
        // xxx Future:
        // Find c = the nearest cell to this one
        // through which the aircraft flew.
        // Let cosElev = horizDistToC / slantDistToC
        // wgt = cosElev

        wgts[iz] = 1.0;
      }

      // Calc totalFlow = sum of everything flowing into the column,
      // not counting the top or bottom faces.

      double totalFlow = 0;
      double sumWgt = 0;

      // for (long iz = 0; iz < nradz; iz++)
      for (long iz = 0; iz < maxz; iz++) {

        if ( isOkDouble( cellMat[iz][iy][ix-1].uu)
	     && isOkDouble( cellMat[iz][iy][ix+1].uu)
	     && isOkDouble( cellMat[iz][iy-1][ix].vv)
	     && isOkDouble( cellMat[iz][iy+1][ix].vv))
	  {
	    totalFlow += density[iz] * 0.5
	      * ( cellMat[iz][iy][ix-1].uu - cellMat[iz][iy][ix+1].uu
		  +  cellMat[iz][iy-1][ix].vv - cellMat[iz][iy+1][ix].vv);
	    sumWgt += density[iz] * wgts[iz];
	  }
      } // for iz

      double hcon;
      if (fabs(sumWgt) < epsilon) hcon = 0;
      else hcon = totalFlow / (2 * sumWgt);

      // Calc W wind = totalFlow, starting at the bottom,
      // using the modified U, V winds.
      double wwind = baseW;
      // for (long iz = 0; iz < nradz; iz++)
      for (long iz = 0; iz < maxz; iz++) {
        if ( isOkDouble( cellMat[iz][iy][ix-1].uu)
	     && isOkDouble( cellMat[iz][iy][ix+1].uu)
	     && isOkDouble( cellMat[iz][iy-1][ix].vv)
	     && isOkDouble( cellMat[iz][iy+1][ix].vv))
	  {
	    wwind += density[iz] * 0.5
	      * (  cellMat[iz][iy][ix-1].uu - cellMat[iz][iy][ix+1].uu
		   + cellMat[iz][iy-1][ix].vv - cellMat[iz][iy+1][ix].vv
		   - 4 * hcon * wgts[iz]);
	    if (! isOkDouble( wwind))
	      throwerr("calcAllW: invalid w wind");
	    if (fabs(wwind) < maxW) {
	      cellMat[iz][iy][ix].ww = wwind;
	    } else cellMat[iz][iy][ix].ww = numeric_limits<double>::quiet_NaN();
	  } else cellMat[iz][iy][ix].ww = numeric_limits<double>::quiet_NaN();
      } // for iz
      if (fabs(wwind - baseW) > epsilon) {
        cout << setprecision(15);
        cout << "calcAllW: iy: " << iy << "  ix: " << ix
	     << "  baseW: " << baseW << "  wwind: " << wwind << endl;
        cout.flush();
        throwerr("wwind error");
      }
    } // for ix
  } // for iy

  //xxx maybe omit:
  // We only calculated the inner points for iy, ix.
  // But we also will need the edge values when
  // we calc the next iteration of U, V values.
  // So set the edges from the nearest interior points.
  // May be NaN.

  // for (long iz = 0; iz < nradz; iz++)
  for (long iz = 0; iz < maxz; iz++) {
    // Corners
    cellMat[iz][0][0].ww             = cellMat[iz][1][1].ww;
    cellMat[iz][0][maxx-1].ww       = cellMat[iz][1][maxx-2].ww;
    cellMat[iz][maxy-1][0].ww       = cellMat[iz][maxy-2][1].ww;
    cellMat[iz][maxy-1][maxx-1].ww = cellMat[iz][maxy-2][maxx-2].ww;

    // Side edges
    // for (long iy = 1; iy < nrady - 1; iy++)
    for (long iy = 1; iy < maxy - 1; iy++) {
      cellMat[iz][iy][0].ww       = cellMat[iz][iy][1].ww;
      cellMat[iz][iy][maxx-1].ww = cellMat[iz][iy][maxx-2].ww;
    }
    // Top and bottom edges
    // for (long ix = 1; ix < nradx - 1; ix++)
    for (long ix = 1; ix < maxx - 1; ix++) {
      cellMat[iz][0][ix].ww       = cellMat[iz][1][ix].ww;
      cellMat[iz][maxy-1][ix].ww = cellMat[iz][maxy-2][ix].ww;
    }
  } // for iz

  delete[] density;
  delete[] wgts;

} // end calcAllWOnMish

//==================================================================

void Fractl::calcAllW(
		      Cell ***& cellMat)           // We set Cell.ww
{
  double * density = new double[nradz];
  double * wgts = new double[nradz];
  for (long iz = 0; iz < nradz; iz++)
    density[iz] = calcDensity( zgridmin + iz * zgridinc);

  // Omit the edges of the region as we use ix-1, ix+1, iy-1, iy+1.
  for (long iy = 1; iy < nrady - 1; iy++) {
    for (long ix = 1; ix < nradx - 1; ix++) {

      for (long iz = 0; iz < nradz; iz++) {
        // xxx Future:
        // Find c = the nearest cell to this one
        // through which the aircraft flew.
        // Let cosElev = horizDistToC / slantDistToC
        // wgt = cosElev

        wgts[iz] = 1.0;
      }

      // Calc totalFlow = sum of everything flowing into the column,
      // not counting the top or bottom faces.

      double totalFlow = 0;
      double sumWgt = 0;

      for (long iz = 0; iz < nradz; iz++) {
        if ( isOkDouble( cellMat[iz][iy][ix-1].uu)
	     && isOkDouble( cellMat[iz][iy][ix+1].uu)
	     && isOkDouble( cellMat[iz][iy-1][ix].vv)
	     && isOkDouble( cellMat[iz][iy+1][ix].vv))
	  {
	    totalFlow += density[iz] * 0.5
	      * ( cellMat[iz][iy][ix-1].uu - cellMat[iz][iy][ix+1].uu
		  +  cellMat[iz][iy-1][ix].vv - cellMat[iz][iy+1][ix].vv);
	    sumWgt += density[iz] * wgts[iz];

	    bool showDetail = testDetail(
					 zgridmin + iz * zgridinc,      // z
					 ygridmin + iy * ygridinc,      // y
					 xgridmin + ix * xgridinc);      // x
	    // detailSpec);                   // z, y, x, delta

	    if (showDetail) {
	      cout << setprecision(7);
	      cout << "calcAllW: showDetail:" << endl
		   << "    iz: " << iz << endl
		   << "    iy: " << iy << endl
		   << "    ix: " << ix << endl
		   << "    den: " << density[iz] << endl
		   << "    wgt: " << wgts[iz] << endl
		   << "    U-: " << cellMat[iz][iy][ix-1].uu << endl
		   << "    U+: " << cellMat[iz][iy][ix+1].uu << endl
		   << "    V-: " << cellMat[iz][iy-1][ix].vv << endl
		   << "    V+: " << cellMat[iz][iy+1][ix].vv << endl;
	    }
	  }
      } // for iz

      double hcon;
      if (fabs(sumWgt) < epsilon) hcon = 0;
      else hcon = totalFlow / (2 * sumWgt);

      // Calc W wind = totalFlow, starting at the bottom,
      // using the modified U, V winds.
      double wwind = baseW;
      for (long iz = 0; iz < nradz; iz++) {
        if ( isOkDouble( cellMat[iz][iy][ix-1].uu)
	     && isOkDouble( cellMat[iz][iy][ix+1].uu)
	     && isOkDouble( cellMat[iz][iy-1][ix].vv)
	     && isOkDouble( cellMat[iz][iy+1][ix].vv))
	  {
	    wwind += density[iz] * 0.5
	      * (  cellMat[iz][iy][ix-1].uu - cellMat[iz][iy][ix+1].uu
		   + cellMat[iz][iy-1][ix].vv - cellMat[iz][iy+1][ix].vv
		   - 4 * hcon * wgts[iz]);
	    if (! isOkDouble( wwind))
	      throwerr("calcAllW: invalid w wind");
            if (fabs(wwind) < maxW) {
              cellMat[iz][iy][ix].ww = wwind;
            } else cellMat[iz][iy][ix].ww = numeric_limits<double>::quiet_NaN();

	    bool showDetail = testDetail(
					 zgridmin + iz * zgridinc,      // z
					 ygridmin + iy * ygridinc,      // y
					 xgridmin + ix * xgridinc);      // x

	    if (showDetail) {
	      cout << setprecision(7);
	      cout << "calcAllW: showDetail:" << endl
		   << "  iz: " << iz << endl
		   << "  iy: " << iy << endl
		   << "  ix: " << ix << endl
		   << "  den: " << density[iz] << endl
		   << "  U-: " << cellMat[iz][iy][ix-1].uu << endl
		   << "  U+: " << cellMat[iz][iy][ix+1].uu << endl
		   << "  V-: " << cellMat[iz][iy-1][ix].vv << endl
		   << "  V+: " << cellMat[iz][iy+1][ix].vv << endl
		   << "  hcon: " << hcon << endl
		   << "  wgt: " << wgts[iz] << endl
		   << "  con: " << (4 * hcon * wgts[iz]) << endl
		   << "  wwind: " << wwind << endl;
	    }
	  }
        else cellMat[iz][iy][ix].ww = numeric_limits<double>::quiet_NaN();
      } // for iz
      if (fabs(wwind - baseW) > epsilon) {
        cout << setprecision(15);
        cout << "calcAllW: iy: " << iy << "  ix: " << ix
	     << "  baseW: " << baseW << "  wwind: " << wwind << endl;
        cout.flush();
        throwerr("wwind error");
      }
    } // for ix
  } // for iy

  //xxx maybe omit:
  // We only calculated the inner points for iy, ix.
  // But we also will need the edge values when
  // we calc the next iteration of U, V values.
  // So set the edges from the nearest interior points.
  // May be NaN.

  for (long iz = 0; iz < nradz; iz++) {
    // Corners
    cellMat[iz][0][0].ww             = cellMat[iz][1][1].ww;
    cellMat[iz][0][nradx-1].ww       = cellMat[iz][1][nradx-2].ww;
    cellMat[iz][nrady-1][0].ww       = cellMat[iz][nrady-2][1].ww;
    cellMat[iz][nrady-1][nradx-1].ww = cellMat[iz][nrady-2][nradx-2].ww;

    // Side edges
    for (long iy = 1; iy < nrady - 1; iy++) {
      cellMat[iz][iy][0].ww       = cellMat[iz][iy][1].ww;
      cellMat[iz][iy][nradx-1].ww = cellMat[iz][iy][nradx-2].ww;
    }
    // Top and bottom edges
    for (long ix = 1; ix < nradx - 1; ix++) {
      cellMat[iz][0][ix].ww       = cellMat[iz][1][ix].ww;
      cellMat[iz][nrady-1][ix].ww = cellMat[iz][nrady-2][ix].ww;
    }
  } // for iz

  delete[] density;
  delete[] wgts;

} // end calcAllWOnMish

//==================================================================

// Calc distance from between a Point and the aircraft

double Fractl::calcDistPtAircraft( Point * pta) {
  double sumsq = 0;
  double delta;
  delta = pta->coordz - pta->aircraftz;
  sumsq += delta * delta;
  delta = pta->coordy - pta->aircrafty;
  sumsq += delta * delta;
  delta = pta->coordx - pta->aircraftx;
  sumsq += delta * delta;
  return sqrt( sumsq);
}

//==================================================================

// Calc distance from a location to Point, in km.

double Fractl::calcDistLocPt( double * loc, Point * pta) {
  double sumsq = 0;
  double delta;
  delta = loc[0] - pta->coordz;
  sumsq += delta * delta;
  delta = loc[1] - pta->coordy;
  sumsq += delta * delta;
  delta = loc[2] - pta->coordx;
  sumsq += delta * delta;
  return sqrt( sumsq);
}

//==================================================================

// Calc distance between two Points, in km.

double Fractl::calcDistPtPt( Point * pta, Point * ptb) {
  double sumsq = 0;
  double delta;
  delta = pta->coordz - ptb->coordz;
  sumsq += delta * delta;
  delta = pta->coordy - ptb->coordy;
  sumsq += delta * delta;
  delta = pta->coordx - ptb->coordx;
  sumsq += delta * delta;
  return sqrt( sumsq);
}