/*
 * graph.cpp
 *
 * Purpose: Graph
 * Created by: Keisuke Okumura <okumura.k@coord.c.titech.ac.jp>
 */

#include "graph.h"
#include <random>
#include <unordered_set>
#include "util.h"
#include <boost/heap/fibonacci_heap.hpp>

Graph::Graph() {
  std::random_device seed_gen;
  MT = new std::mt19937(seed_gen());
  init();
}

Graph::Graph(std::mt19937* _MT) : MT(_MT) {
  init();
}

void Graph::init() {
  directed = false;
  regFlg = true;
}

Graph::~Graph() {
  for (auto v : nodes) delete v;
  nodes.clear();
  for (auto p : knownPaths) delete p.second;
  knownPaths.clear();
}

Node* Graph::getNode(int id) {
  auto itr = std::find_if(nodes.begin(), nodes.end(),
                          [id](Node* v){ return v->getId() == id; });
  // error check
  if (itr == nodes.end()) {
    std::cout << "error@Graph::getNode, "
              << "node index is over, " << id << "\n";
    std::exit(1);
  }

  return *itr;
}

Node* Graph::getNode(int x, int y) {
  auto itr = std::find_if(nodes.begin(), nodes.end(),
                          [x, y](Node* v)
                          { return v->getPos().x == x
                              && v->getPos().y == y; });
  if (itr == nodes.end()) return nullptr;
  return *itr;
}

bool Graph::existNode(int id) {
  auto itr = std::find_if(nodes.begin(), nodes.end(),
                          [id](Node* v){ return v->getId() == id; });
  return itr != nodes.end();
}

int Graph::getNodeIndex(Node* v) {
  return v->getIndex();
}

Nodes Graph::neighbor(Node* v) {
  return v->getNeighbor();
}

Nodes Graph::neighbor(int i) {
  return getNode(i)->getNeighbor();
}

Nodes Graph::getPath(Node* s, Node* g, Nodes &prohibitedNodes) {
  return {};
}

// regFlg : whether register
Nodes Graph::getPath(Node* _s, Node* _g,
                     Nodes &prohibitedNodes, int (*dist) (Node*, Node*))
{
  bool prohibited = !prohibitedNodes.empty();
  Nodes path, C;
  std::string key;

  // ==== fast implementation ====
  if (regFlg && !prohibited) {
    key = getKey(_s, _g);
    auto itrK = knownPaths.find(key);
    if (itrK != knownPaths.end()) {  // known
      path = itrK->second->path;
      return path;
    }
  }
  // =============================

  int f;
  bool invalid = true;

  // prepare node open hashtable
  boost::heap::fibonacci_heap<Fib_AN> OPEN;
  std::unordered_map<int, boost::heap::fibonacci_heap<Fib_AN>::handle_type> SEARCHED;
  std::unordered_set<int> CLOSE;
  AN* n = new AN { _s, 0, dist(_s, _g), nullptr };
  auto handle = OPEN.push(Fib_AN(n));
  SEARCHED.emplace(n->v->getId(), handle);

  while (!OPEN.empty()) {
    // argmin
    n = OPEN.top().node;

    // check goal condition
    if (n->v == _g) {
      invalid = false;
      break;
    }

    // ==== fast implementation ====
    key = getKey(n->v, _g);
    auto itrK = knownPaths.find(key);
    if (itrK != knownPaths.end()) {  // known
      Nodes kPath = itrK->second->path;
      bool valid = true;
      if (prohibited) {
        for (auto v : kPath) {
          if (inArray(v, prohibitedNodes)) {
            valid = false;
            break;
          }
        }
      }
      if (valid) {
        for (int i = 1; i < kPath.size(); ++i) {
          n = new AN { kPath[i], 0, 0, n };
        }
        invalid = false;
        break;
      }
    }
    // =============================

    // update list
    OPEN.pop();
    CLOSE.emplace(n->v->getId());

    // search neighbor
    C = neighbor(n->v);

    for (auto m : C) {
      if (prohibited && inArray(m, prohibitedNodes)) continue;
      if (CLOSE.find(m->getId()) != CLOSE.end()) continue;
      f = n->g + 1 + dist(m, _g);

      // ==== fast implementation ====
      if (regFlg) {
        key = getKey(m, _g);
        auto itrK = knownPaths.find(key);
        if (itrK != knownPaths.end()) {
          f = n->g + 1 + itrK->second->path.size() - 1;
        }
      }
      // =============================

      auto itrS = SEARCHED.find(m->getId());
      if (itrS == SEARCHED.end()) {  // new node
        AN* l = new AN { m, n->g + 1, f, n };
        auto handle = OPEN.push(Fib_AN(l));
        SEARCHED.emplace(l->v->getId(), handle);
      } else {
        auto handle = itrS->second;
        AN* l = (*handle).node;
        if (l->f > f) {
          l->g = n->g + 1;
          l->f = f;
          l->p = n;
          OPEN.increase(handle);
        }
      }
    }
  }

  if (invalid) return path;

  // back tracking
  while (n != nullptr) {
    path.push_back(n->v);
    n = n->p;
  }
  std::reverse(path.begin(), path.end());

  // register path
  if (regFlg && !prohibited) registerPath(path);

  return path;
}

std::string Graph::getKey(Node* s, Node* g) {
  int sIndex = getNodeIndex(s);
  int gIndex = getNodeIndex(g);
  std::string key = "";
  key += std::to_string(sIndex);
  key += "-";
  key += std::to_string(gIndex);
  return key;
}

void Graph::registerPath(const Nodes &path) {
  if (path.empty()) return;

  Nodes tmp = path;
  std::string key;

  Node *v1, *v2;
  do {
    v1 = tmp[0];
    v2 = tmp[tmp.size() - 1];
    key = getKey(v1, v2);
    KnownPath* knownPath = new KnownPath { v1, v2, tmp };
    knownPaths.emplace(key, knownPath);
    tmp.erase(tmp.begin());
  } while (tmp.size() > 2);
}

Paths Graph::getRandomStartGoal(int num) {
  if (num > starts.size() || num > goals.size()) {
    std::cout << "error@Graph::getStartGoal, over node size" << "\n";
    std::exit(1);
  }

  Paths points;
  Nodes ss(starts.size());
  Nodes gs(goals.size());
  bool flg;

  std::copy(starts.begin(), starts.end(), ss.begin());
  std::copy(goals.begin(),  goals.end(), gs.begin());

  while (true) {
    points.clear();
    std::shuffle(ss.begin(), ss.end(), *MT);
    std::shuffle(gs.begin(), gs.end(), *MT);

    flg = true;
    for (int i = 0; i < num; ++i) {
      if (ss[i] != gs[i]) {
        points.push_back({ ss[i], gs[i] });
      } else {
        flg = false;
        break;
      }
    }

    if (flg) break;
  }

  return points;
}


/*
Copyright (c) 2013, Regents of the University of Alaska

All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
    * Neither the name of the Geographic Information Network of Alaska nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

This code was developed by Dan Stahlke for the Geographic Information Network of Alaska.
*/

#include <cassert>

#include <boost/lexical_cast.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include "common.h"
#include "ndv.h"

using namespace dangdal;

struct Binning {
	Binning() :
		nbins(0),
		offset(0),
		scale(0)
	{ }

	int to_bin(double v) const {
		if(std::isinf(v) == -1) return 0;
		if(std::isinf(v) ==  1) return nbins-1;
		double bin_dbl = round((v-offset)/scale);
		if(std::isnan(bin_dbl)) fatal_error("nan in to_bin");
		if(bin_dbl < 0) return 0;
		if(bin_dbl > nbins-1) return nbins-1;
		int bin_int = int(bin_dbl);
		assert(bin_int >= 0 && bin_int < nbins);
		return bin_int;
	}

	double from_bin(int i) const {
		return double(i) * scale + offset;
	}

	int nbins;
	double offset;
	double scale;
};

struct Histogram {
	Histogram() :
		min(0), max(0), mean(0), stddev(0),
		data_count(0), ndv_count(0)
	{ }

	Binning binning;
	double min, max, mean, stddev;
	size_t data_count;
	size_t ndv_count;
	std::vector<size_t> counts;
};

std::vector<std::pair<double, double> > compute_minmax(
	const std::vector<GDALRasterBandH> &src_bands, const NdvDef &ndv_def,
	size_t w, size_t h
);
std::vector<Histogram> compute_histogram(
	const std::vector<GDALRasterBandH> &src_bands, const NdvDef &ndv_def,
	size_t w, size_t h, const std::vector<Binning> &binnings
);
void get_scale_from_percentile(
	const Histogram &histogram, int output_range,
	double from_percentile, double to_percentile,
	double *scale_out, double *offset_out
);
std::vector<uint8_t> invert_histogram_to_gaussian(const Histogram &histogram_in, double variance,
	int output_range);
void copyGeoCode(GDALDatasetH dst_ds, GDALDatasetH src_ds);

void usage(const std::string &cmdname) {
	printf("Usage: %s <options> src.tif dst.tif\n\n", cmdname.c_str());
	NdvDef::printUsage();
	printf(
"  -outndv <output_nodata_val>        Output no-data value\n"
"\n"
"Operation:\n"
"  -linear-stretch <target_avg> <target_stddev>      Linear stretch to a target range\n"
"  -percentile-range <from: 0.0-1.0> <to: 0.0-1.0>   Linear stretch using a percentile range of input\n"
"  -histeq <target_stddev>                           Histogram normalize to a target bell curve\n"
"  -dump-histogram                                   Just print the histogram to console\n"
"\n"
"Input can be any integer or floating type (but not complex).  Output is 8-bit.\n"
);
	exit(1);
}

int main(int argc, char *argv[]) {
	const std::string cmdname = argv[0];
	if(argc == 1) usage(cmdname);
	std::vector<std::string> arg_list = argv_to_list(argc, argv);

	std::string src_fn;
	std::string dst_fn;
	std::string output_format;

	int mode_histeq = 0;
	int mode_stddev = 0;
	double dst_avg = -1;
	double dst_stddev = -1;
	int mode_percentile = 0;
	int mode_dump_histogram = 0;
	double from_percentile = -1;
	double to_percentile = -1;
	int out_ndv = 0, set_out_ndv = 0;

	NdvDef ndv_def = NdvDef(arg_list);

	size_t argp = 1;
	while(argp < arg_list.size()) {
		const std::string &arg = arg_list[argp++];
		// FIXME - check for duplicate values
		if(arg[0] == '-') {
			try {
				if(arg == "-of") {
					if(argp == arg_list.size()) usage(cmdname);
					output_format = arg_list[argp++];
				} else if(arg == "-linear-stretch") {
					if(argp == arg_list.size()) usage(cmdname);
					dst_avg = boost::lexical_cast<double>(arg_list[argp++]);

					if(argp == arg_list.size()) usage(cmdname);
					dst_stddev = boost::lexical_cast<double>(arg_list[argp++]);

					mode_stddev = 1;
				} else if(arg == "-percentile-range") {
					if(argp == arg_list.size()) usage(cmdname);
					from_percentile = boost::lexical_cast<double>(arg_list[argp++]);

					if(argp == arg_list.size()) usage(cmdname);
					to_percentile = boost::lexical_cast<double>(arg_list[argp++]);

					mode_percentile = 1;
				} else if(arg == "-histeq") {
					if(argp == arg_list.size()) usage(cmdname);
					dst_stddev = boost::lexical_cast<double>(arg_list[argp++]);

					mode_histeq = 1;
				} else if(arg == "-dump-histogram") {
					mode_dump_histogram = 1;
				} else if(arg == "-outndv") {
					if(argp == arg_list.size()) usage(cmdname);
					int64_t ndv_long = boost::lexical_cast<int64_t>(arg_list[argp++]);
					if(ndv_long < 0 || ndv_long > 255) fatal_error("ndv must be in the range 0..255");
					out_ndv = boost::numeric_cast<uint8_t>(ndv_long);
					set_out_ndv++;
				} else {
					usage(cmdname);
				}
			} catch(boost::bad_lexical_cast &e) {
				fatal_error("cannot parse number given on command line");
			} catch(boost::bad_numeric_cast &e) {
				fatal_error("number given on command line out of range: %s", e.what());
			}
		} else {
			if(src_fn.empty()) {
				src_fn = arg;
			} else if(dst_fn.empty()) {
				dst_fn = arg;
			} else {
				usage(cmdname);
			}
		}
	}

	if(src_fn.empty()) usage(cmdname);
	if(dst_fn.empty() != (mode_dump_histogram > 0)) usage(cmdname);
	if(mode_percentile + mode_stddev + mode_histeq + mode_dump_histogram > 1) usage(cmdname);
	if(mode_stddev && (dst_avg < 0 || dst_stddev < 0)) usage(cmdname);
	if(mode_percentile && !(
		0 <= from_percentile &&
		from_percentile < to_percentile &&
		to_percentile <= 1)) usage(cmdname);

	if(output_format.empty()) output_format = "GTiff";

	GDALAllRegister();

	//////// open source ////////

	GDALDatasetH src_ds = GDALOpen(src_fn.c_str(), GA_ReadOnly);
	if(!src_ds) fatal_error("open failed");

	size_t w = GDALGetRasterXSize(src_ds);
	size_t h = GDALGetRasterYSize(src_ds);
	if(!w || !h) fatal_error("missing width/height");
	size_t src_band_count = GDALGetRasterCount(src_ds);
	printf("Input size is %zd, %zd, %zd\n", w, h, src_band_count);

	std::vector<size_t> bandlist;
	for(size_t i=0; i<src_band_count; i++) {
		bandlist.push_back(i+1);
	}
	size_t dst_band_count = bandlist.size();

	if(ndv_def.empty()) {
		ndv_def = NdvDef(src_ds, bandlist);
	}

	bool use_ndv = !ndv_def.empty();
	if(use_ndv && !set_out_ndv) {
		if(ndv_def.slabs.size() == 1) {
			const NdvSlab &slab = ndv_def.slabs[0];
			assert(slab.range_by_band.size());
			const NdvInterval &range = slab.range_by_band[0];
			if(range.first == range.second) {
				double v = range.first;
				out_ndv = (uint8_t)v;
				if((double)out_ndv == v) {
					set_out_ndv++;
				} else {
					printf("Cannot use %g as an NDV value because it cannot be cast to an 8-bit number.\n", v);
				}
			}
		}
		if(!set_out_ndv) {
			out_ndv = 0;
			set_out_ndv++;
		}
		printf("Output NDV is defaulting to %d.\n", out_ndv);
	}

	//////// open output ////////

	printf("Output size is %zd x %zd x %zd\n", w, h, dst_band_count);

	GDALDriverH dst_driver = GDALGetDriverByName(output_format.c_str());
	if(!dst_driver) fatal_error("unrecognized output format (%s)", output_format.c_str());

	//////// open bands ////////

	std::vector<GDALRasterBandH> src_bands;
	std::vector<GDALRasterBandH> dst_bands;

	for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
		src_bands.push_back(GDALGetRasterBand(src_ds, bandlist[band_idx]));
	}

	//////// find optimal binning ////////

	std::vector<Binning> binnings(dst_band_count);
	{
		// computed on demand
		std::vector<std::pair<double, double> > minmax;

		for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
			Binning &binning = binnings[band_idx];
			GDALDataType dt = GDALGetRasterDataType(src_bands[band_idx]);
			switch(dt) {
				case GDT_Byte:
					binning.nbins = 256;
					binning.offset = 0;
					binning.scale = 1;
					break;
				case GDT_UInt16:
					binning.nbins = 65536;
					binning.offset = 0;
					binning.scale = 1;
					break;
				case GDT_Int16:
					binning.nbins = 65536;
					binning.offset = -32768;
					binning.scale = 1;
					break;
				default:
					if(minmax.empty()) {
						printf("Computing min/max values...\n");
						minmax = compute_minmax(src_bands, ndv_def, w, h);
					}
					double min = minmax[band_idx].first;
					double max = minmax[band_idx].second;
					// a compromise between memory usage and datavalue resolution
					binning.nbins = 10000000;
					binning.offset = min;
					binning.scale = (max - min) / double(binning.nbins-1);
			}
		}
	}

	//////// compute lookup table ////////

	printf("\nComputing histogram...\n");
	std::vector<Histogram> histograms =
		compute_histogram(src_bands, ndv_def, w, h, binnings);

	for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
		Histogram &hg = histograms[band_idx];
		printf("band %zd: min=%g, max=%g, mean=%g, stddev=%g, valid_count=%zd, ndv_count=%zd\n",
			band_idx+1, hg.min, hg.max, hg.mean, hg.stddev, hg.data_count, hg.ndv_count);
		if(mode_dump_histogram) {
			for(int i=0; i<hg.binning.nbins; i++) {
				printf("bin %d: val=%g cnt=%zd\n",
					i, hg.binning.from_bin(i), hg.counts[i]);
			}
		}
	}
	if(mode_dump_histogram) {
		return 0;
	}

	//////// open output ////////

	GDALDatasetH dst_ds = GDALCreate(dst_driver, dst_fn.c_str(), w, h, dst_band_count, GDT_Byte, NULL);
	if(!dst_ds) fatal_error("couldn't create output");
	copyGeoCode(dst_ds, src_ds);

	for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
		dst_bands.push_back(GDALGetRasterBand(dst_ds, band_idx+1));
	}

	//////// compute tranformation parameters ////////

	const int output_range = 256;
	bool use_table; // otherwise, use linear
	std::vector<std::vector<uint8_t> > xform_table(dst_band_count);
	std::vector<double> lin_scales(dst_band_count);
	std::vector<double> lin_offsets(dst_band_count);

	if(mode_histeq) {
		use_table = true;
		for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
			xform_table[band_idx] = invert_histogram_to_gaussian(
				histograms[band_idx], dst_stddev, output_range);
		}
	} else{
		use_table = false;
		bool was_nop = false;
		if(mode_percentile) {
			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				get_scale_from_percentile(
					histograms[band_idx], output_range, from_percentile, to_percentile,
					&lin_scales[band_idx], &lin_offsets[band_idx]);
			}
		} else if(mode_stddev) {
			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				Histogram &hg = histograms[band_idx];
				lin_scales[band_idx] = hg.stddev ? dst_stddev / hg.stddev : 0;
				lin_offsets[band_idx] = hg.mean - dst_avg / lin_scales[band_idx];
			}
		} else { // no transformation
			printf("\nWarning: no transformation was specified!  I'll just cast the input to 8-bit.\n");
			was_nop = true;
			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				lin_scales[band_idx] = 1;
				lin_offsets[band_idx] = 0;
			}
		}
		if(!was_nop) {
			printf("\nLinear stretch:\n");
			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				double scale = lin_scales[band_idx];
				double offset = lin_offsets[band_idx];
				printf("band %zd: scale=%f, offset=%f, src_range=[%f, %f]\n",
					band_idx+1, scale, offset, offset, ((double)(output_range-1)/scale)+offset);
			}
		}
	}

	histograms.clear(); // free up memory

	if(use_table) {
		// avoid ndv in output for good pixels
		if(use_ndv) {
			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				for(size_t i=0; i<xform_table[band_idx].size(); i++) {
					uint8_t v = xform_table[band_idx][i];
					if(v == out_ndv) {
						if(out_ndv < output_range/2) v++;
						else v--;
					}
					xform_table[band_idx][i] = v;
				}
			}
		}
	}

	//////// do transformation ////////

	printf("\nComputing output...\n");

	int blocksize_x_int, blocksize_y_int;
	GDALGetBlockSize(src_bands[0], &blocksize_x_int, &blocksize_y_int);
	size_t blocksize_x = blocksize_x_int;
	size_t blocksize_y = blocksize_y_int;
	size_t block_len = blocksize_x*blocksize_y;

	std::vector<std::vector<double> > buf_in(dst_band_count);
	std::vector<std::vector<uint8_t> > buf_out(dst_band_count);
	for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
		buf_in[band_idx].resize(block_len);
		buf_out[band_idx].resize(block_len);
	}
	std::vector<uint8_t> ndv_mask(block_len);

	for(size_t boff_y=0; boff_y<h; boff_y+=blocksize_y) {
		size_t bsize_y = blocksize_y;
		if(bsize_y + boff_y > h) bsize_y = h - boff_y;
		for(size_t boff_x=0; boff_x<w; boff_x+=blocksize_x) {
			size_t bsize_x = blocksize_x;
			if(bsize_x + boff_x > w) bsize_x = w - boff_x;

			block_len = bsize_x*bsize_y;

			double progress =
				((double)boff_y * (double)w +
				(double)boff_x * (double)bsize_y) /
				((double)w * (double)h);
			GDALTermProgress(progress, NULL, NULL);

			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				GDALRasterIO(src_bands[band_idx], GF_Read, boff_x, boff_y, bsize_x, bsize_y,
					&buf_in[band_idx][0], bsize_x, bsize_y, GDT_Float64, 0, 0);
			}

			ndv_def.getNdvMask(buf_in, &ndv_mask[0], block_len);

			for(size_t band_idx=0; band_idx<dst_band_count; band_idx++) {
				double *p_in = &buf_in[band_idx][0];
				uint8_t *p_out = &buf_out[band_idx][0];
				uint8_t *p_ndv = &ndv_mask[0];
				if(use_table) {
					uint8_t *xform = &xform_table[band_idx][0];
					Binning binning = binnings[band_idx];

					for(size_t i=0; i<block_len; i++) {
						if(*p_ndv) {
							*p_out = out_ndv;
						} else {
							*p_out = xform[binning.to_bin(*p_in)];
							//printf("%g %d %d\n", *p_in, binning.to_bin(*p_in), *p_out);
						}
						p_in++; p_out++; p_ndv++;
					}
				} else {
					double scale = lin_scales[band_idx];
					double offset = lin_offsets[band_idx];
					for(size_t i=0; i<block_len; i++) {
						if(*p_ndv) {
							*p_out = out_ndv;
						} else {
							double out_dbl = (*p_in - offset) * scale;
							uint8_t v =
								(out_dbl < 0) ? 0 :
								(out_dbl > output_range-1) ? output_range-1 :
								uint8_t(out_dbl);
							if(v == out_ndv) {
								if(out_ndv < output_range/2) v++;
								else v--;
							}
							*p_out = v;
						}
						p_in++; p_out++; p_ndv++;
					}
				}

				GDALRasterIO(dst_bands[band_idx], GF_Write, boff_x, boff_y, bsize_x, bsize_y,
					&buf_out[band_idx][0], bsize_x, bsize_y, GDT_Byte, 0, 0);
			} // band
		} // block x
	} // block y

	GDALClose(src_ds);
	GDALClose(dst_ds);

	GDALTermProgress(1, NULL, NULL);

	return 0;
}

std::vector<std::pair<double, double> > compute_minmax(
	const std::vector<GDALRasterBandH> &src_bands, const NdvDef &ndv_def,
	size_t w, size_t h
) {
	size_t band_count = src_bands.size();
	std::vector<std::pair<double, double> > minmax(band_count);

	int blocksize_x_int, blocksize_y_int;
	GDALGetBlockSize(src_bands[0], &blocksize_x_int, &blocksize_y_int);
	size_t blocksize_x = blocksize_x_int;
	size_t blocksize_y = blocksize_y_int;
	size_t block_len = blocksize_x*blocksize_y;

	std::vector<std::vector<double> > buf_in(band_count);
	for(size_t band_idx=0; band_idx<band_count; band_idx++) {
		buf_in[band_idx].resize(block_len);
	}
	std::vector<uint8_t> ndv_mask(block_len);

	std::vector<bool> got_data(band_count);

	for(size_t boff_y=0; boff_y<h; boff_y+=blocksize_y) {
		size_t bsize_y = blocksize_y;
		if(bsize_y + boff_y > h) bsize_y = h - boff_y;
		for(size_t boff_x=0; boff_x<w; boff_x+=blocksize_x) {
			size_t bsize_x = blocksize_x;
			if(bsize_x + boff_x > w) bsize_x = w - boff_x;

			block_len = bsize_x*bsize_y;

			double progress =
				((double)boff_y * (double)w +
				(double)boff_x * (double)bsize_y) /
				((double)w * (double)h);
			GDALTermProgress(progress, NULL, NULL);

			for(size_t band_idx=0; band_idx<band_count; band_idx++) {
				GDALRasterIO(src_bands[band_idx], GF_Read, boff_x, boff_y, bsize_x, bsize_y,
					&buf_in[band_idx][0], bsize_x, bsize_y, GDT_Float64, 0, 0);
			}

			ndv_def.getNdvMask(buf_in, &ndv_mask[0], block_len);

			for(size_t band_idx=0; band_idx<band_count; band_idx++) {
				for(size_t i=0; i<block_len; i++) {
					if(ndv_mask[i]) continue;
					double v = buf_in[band_idx][i];
					if(std::isnan(v) || std::isinf(v)) continue;

					assert(!minmax.empty());
					double &min = minmax[band_idx].first;
					double &max = minmax[band_idx].second;
					if(!got_data[band_idx]) {
						min = v;
						max = v;
						got_data[band_idx] = true;
					}
					if(v < min) min = v;
					if(v > max) max = v;
				}
			}
		}
	}
	GDALTermProgress(1, NULL, NULL);

	return minmax;
}

std::vector<Histogram> compute_histogram(
	const std::vector<GDALRasterBandH> &src_bands, const NdvDef &ndv_def,
	size_t w, size_t h, const std::vector<Binning> &binnings
) {
	size_t band_count = src_bands.size();
	std::vector<Histogram> histograms(band_count);

	for(size_t band_idx=0; band_idx<band_count; band_idx++) {
		histograms[band_idx].binning = binnings[band_idx];
		histograms[band_idx].counts.assign(binnings[band_idx].nbins, 0);
	}

	int blocksize_x_int, blocksize_y_int;
	GDALGetBlockSize(src_bands[0], &blocksize_x_int, &blocksize_y_int);
	size_t blocksize_x = blocksize_x_int;
	size_t blocksize_y = blocksize_y_int;
	size_t block_len = blocksize_x*blocksize_y;

	std::vector<std::vector<double> > buf_in(band_count);
	for(size_t band_idx=0; band_idx<band_count; band_idx++) {
		buf_in[band_idx].resize(block_len);
	}
	std::vector<uint8_t> ndv_mask(block_len);

	std::vector<bool> first_valid_pixel(band_count, true);

	for(size_t boff_y=0; boff_y<h; boff_y+=blocksize_y) {
		size_t bsize_y = blocksize_y;
		if(bsize_y + boff_y > h) bsize_y = h - boff_y;
		for(size_t boff_x=0; boff_x<w; boff_x+=blocksize_x) {
			size_t bsize_x = blocksize_x;
			if(bsize_x + boff_x > w) bsize_x = w - boff_x;

			block_len = bsize_x*bsize_y;

			double progress =
				((double)boff_y * (double)w +
				(double)boff_x * (double)bsize_y) /
				((double)w * (double)h);
			GDALTermProgress(progress, NULL, NULL);

			for(size_t band_idx=0; band_idx<band_count; band_idx++) {
				GDALRasterIO(src_bands[band_idx], GF_Read, boff_x, boff_y, bsize_x, bsize_y,
					&buf_in[band_idx][0], bsize_x, bsize_y, GDT_Float64, 0, 0);
			}

			ndv_def.getNdvMask(buf_in, &ndv_mask[0], block_len);

			for(size_t band_idx=0; band_idx<band_count; band_idx++) {
				Histogram &hg = histograms[band_idx];
				double *p = &buf_in[band_idx][0];

				for(size_t i=0; i<block_len; i++) {
					if(ndv_mask[i]) {
						hg.ndv_count++;
					} else {
						double v = p[i];
						hg.counts[hg.binning.to_bin(v)]++;
						if(first_valid_pixel[band_idx]) {
							hg.min = hg.max = v;
							first_valid_pixel[band_idx] = false;
						}
						if(v < hg.min) hg.min = v;
						if(v > hg.max) hg.max = v;
					}
				}
			}
		}
	}
	GDALTermProgress(1, NULL, NULL);

	for(size_t band_idx=0; band_idx<band_count; band_idx++) {
		Histogram &hg = histograms[band_idx];
		double accum = 0;
		for(int i=0; i<hg.binning.nbins; i++) {
			size_t cnt = hg.counts[i];
			double v = hg.binning.from_bin(i);
			hg.data_count += cnt;
			accum += v * cnt;
		}
		hg.mean = accum / hg.data_count;

		double var_accum = 0;
		for(int i=0; i<hg.binning.nbins; i++) {
			size_t cnt = hg.counts[i];
			double v = hg.binning.from_bin(i);
			var_accum += (v-hg.mean) * (v-hg.mean) * cnt;
		}
		hg.stddev = sqrt(var_accum / hg.data_count);
	}

	return histograms;
}

void get_scale_from_percentile(
	const Histogram &histogram, int output_range,
	double from_percentile, double to_percentile,
	double *scale_out, double *offset_out
) {
	size_t start_count = (size_t)(histogram.data_count * from_percentile);
	size_t end_count = (size_t)(histogram.data_count * to_percentile);

	size_t cnt = 0;
	int from_idx = -1;
	int to_idx = -1;
	for(int i=0; i<histogram.binning.nbins; i++) {
		if(cnt <= start_count) from_idx = i;
		cnt += histogram.counts[i];
		if(cnt <= end_count) to_idx = i;
		else break;
	}
	if(from_idx<0 || to_idx<0) fatal_error("impossible: could not find window");
	if(from_idx == to_idx) { from_idx=0; to_idx=histogram.binning.nbins-1; }

	double from_val = histogram.binning.from_bin(from_idx);
	double to_val = histogram.binning.from_bin(to_idx);

	*scale_out = (double)(output_range-1) / (double)(to_val-from_val);
	*offset_out = from_val;
}

std::vector<double> gen_gaussian(double variance, int bin_count) {
	std::vector<double> arr(bin_count);
	double total = 0;
	for(int i=0; i<bin_count; i++) {
		if(variance == 0.0) {
			// if variance==0.0, just use a uniform distribution
			arr[i] = 1;
		} else {
			// Gaussian distribution.  Large variance gives
			// a more level curve, small variance gives
			// a curved peaked at center.
			double x = (double)(i-bin_count/2) / variance;
			arr[i] = exp(-x*x);
		}
		total += arr[i];
	}

	for(int i=0; i<bin_count; i++) arr[i] /= total;

	return arr;
}

std::vector<uint8_t> invert_histogram(
	const Histogram &src_h_in,
	const std::vector<double> &dst_h,
	size_t output_range
) {
	std::vector<size_t> src_h(src_h_in.counts);
	size_t pixel_count = 0;
	for(size_t i=0; i<src_h.size(); i++) pixel_count += src_h[i];

	std::vector<uint8_t> out_h(src_h.size());
	double src_total = 0;
	double dst_total = 0;
	uint8_t j = 0;
	for(size_t i=0; i<src_h.size(); i++) {
		out_h[i] = j;
		src_total += src_h[i];
		while(j<output_range-1 && dst_total < src_total) {
			dst_total += dst_h[j++] * (double)pixel_count;
		}
	}

	return out_h;
}

std::vector<uint8_t> invert_histogram_to_gaussian(
	const Histogram &histogram_in, double variance,
	int output_range
) {
	std::vector<double> gaussian = gen_gaussian(variance, output_range);
	return invert_histogram(histogram_in, gaussian, output_range);
}

void copyGeoCode(GDALDatasetH dst_ds, GDALDatasetH src_ds) {
	double affine[6];
	if(GDALGetGeoTransform(src_ds, affine) == CE_None) {
		GDALSetGeoTransform(dst_ds, affine);
	}
	GDALSetProjection(dst_ds, GDALGetProjectionRef(src_ds));
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_helpers.hpp>

#ifndef ABB_IRB1600_145_FK_FAST_HPP
#define ABB_IRB1600_145_FK_FAST_HPP

namespace ABB_IRB1600_145_FK_FAST
{
    const size_t ABB_IRB1600_145_NUM_ACTIVE_JOINTS = 6;
    const size_t ABB_IRB1600_145_NUM_LINKS = 8;

    const std::string ABB_IRB1600_145_ACTIVE_JOINT_1_NAME = "joint_1";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_2_NAME = "joint_2";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_3_NAME = "joint_3";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_4_NAME = "joint_4";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_5_NAME = "joint_5";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_6_NAME = "joint_6";

    const std::string ABB_IRB1600_145_LINK_1_NAME = "link_0";
    const std::string ABB_IRB1600_145_LINK_2_NAME = "link_1";
    const std::string ABB_IRB1600_145_LINK_3_NAME = "link_2";
    const std::string ABB_IRB1600_145_LINK_4_NAME = "link_3";
    const std::string ABB_IRB1600_145_LINK_5_NAME = "link_4";
    const std::string ABB_IRB1600_145_LINK_6_NAME = "link_5";
    const std::string ABB_IRB1600_145_LINK_7_NAME = "link_6";
    const std::string ABB_IRB1600_145_LINK_8_NAME = "link_7";

    typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> VectorIsometry3d;

    inline Eigen::Isometry3d Get_base_joint1_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1245);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.15, -0.1395, 0.362);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.0, 0.028, 0.7);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.314, 0.107, 0.0);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.286, 0.0, 0.0);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_Fixed_link_6_joint_tool_LinkJointTransform(void)
    {
        Eigen::Translation3d pre_joint_translation(0.065, 0.0, 0.0);
        Eigen::Quaterniond pre_joint_rotation(0.7071067811865476, 0.0, 0.7071067811865476, 0.0);
        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        return pre_joint_transform;
    }

    inline VectorIsometry3d GetLinkTransforms(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        assert(configuration.size() == ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
        VectorIsometry3d link_transforms(ABB_IRB1600_145_NUM_LINKS);
        link_transforms[0] = base_transform;
        link_transforms[1] = link_transforms[0] * Get_base_joint1_LinkJointTransform(configuration[0]);
        link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
        link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
        link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
        link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
        link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
        link_transforms[7] = link_transforms[6] * Get_Fixed_link_6_joint_tool_LinkJointTransform();
        return link_transforms;
    }

    inline VectorIsometry3d GetLinkTransforms(std::map<std::string, double> configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        std::vector<double> configuration_vector(ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
        configuration_vector[0] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_1_NAME];
        configuration_vector[1] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_2_NAME];
        configuration_vector[2] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_3_NAME];
        configuration_vector[3] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_4_NAME];
        configuration_vector[4] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_5_NAME];
        configuration_vector[5] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_6_NAME];
        return GetLinkTransforms(configuration_vector, base_transform);
    }

    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
        EigenHelpers::MapStringIsometry3d link_transforms_map;
        link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
        link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
        link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
        link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
        link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
        link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
        link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
        link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
        return link_transforms_map;
    }

    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
        EigenHelpers::MapStringIsometry3d link_transforms_map;
        link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
        link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
        link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
        link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
        link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
        link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
        link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
        link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
        return link_transforms_map;
    }
}

#endif // ABB_IRB1600_145_FK_FAST_HPP


// OS / System
#include <iostream>
#include <vector>

// Boost
#include <boost/any.hpp>

// Lapack
// #include <Accelerate/Accelerate.h>

extern "C" void dgetrf_(int* dim1, int* dim2, double* a, int* lda, int* ipiv, int* info);
extern "C" void dgetrs_(char *TRANS, int *N, int *NRHS, double *A, int *LDA, int *IPIV, double *B, int *LDB, int *INFO );

// RapidJSON
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"

using namespace std;
using namespace rapidjson;

int main(int argc, char *argv[]){
    cout << "Hello there, World!" << endl;
    std::cout << "Hello Main2!" << std::endl;

    // Boost
    boost::any foo = 42;
    foo.clear();

    // Lapack
    char trans = 'N';
    int dim = 2;
    int nrhs = 1;
    int LDA = dim;
    int LDB = dim;
    int info;

    vector<double> a, b;

    a.push_back(1);
    a.push_back(1);
    a.push_back(1);
    a.push_back(-1);

    b.push_back(2);
    b.push_back(0);

    int ipiv[3];

    dgetrf_(&dim, &dim, &*a.begin(), &LDA, ipiv, &info);
    dgetrs_(&trans, &dim, &nrhs, & *a.begin(), &LDA, ipiv, & *b.begin(), &LDB, &info);

    std::cout << "solution is:";
    std::cout << "[" << b[0] << ", " << b[1] << ", " << "]" << std::endl;
    std::cout << "Info = " << info << std::endl;

    // RapidJson
     // 1. Parse a JSON string into DOM.
    const char* json = "{\"project\":\"rapidjson\",\"stars\":10}";
    Document d;
    d.Parse(json);

    // 2. Modify it by DOM.
    Value& s = d["stars"];
    s.SetInt(s.GetInt() + 1);

    // 3. Stringify the DOM
    StringBuffer buffer;
    Writer<StringBuffer> writer(buffer);
    d.Accept(writer);
    // Output {"project":"rapidjson","stars":11}
    std::cout << buffer.GetString() << std::endl;

    // Static Analysis
    // this should throw a compile time warning as well
    // as get caught by the cppcheck static analysis ( with make analysis )
    char tmp[10];
    tmp[11] = 's';
    return 0;
}


/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <ThermalOperator.hh>
#include <instantiation.hh>

#include <deal.II/base/index_set.h>
#include <deal.II/base/types.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/mapping_q1.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/hp/fe_values.h>
#include <deal.II/matrix_free/fe_evaluation.h>

namespace adamantine
{

template <int dim, int fe_degree, typename MemorySpaceType>
ThermalOperator<dim, fe_degree, MemorySpaceType>::ThermalOperator(
    MPI_Comm const &communicator, BoundaryType boundary_type,
    std::shared_ptr<MaterialProperty<dim>> material_properties,
    std::vector<std::shared_ptr<HeatSource<dim>>> heat_sources)
    : _communicator(communicator), _boundary_type(boundary_type),
      _material_properties(material_properties), _heat_sources(heat_sources),
      _inverse_mass_matrix(
          new dealii::LA::distributed::Vector<double, MemorySpaceType>())
{
  _matrix_free_data.tasks_parallel_scheme =
      dealii::MatrixFree<dim, double>::AdditionalData::partition_color;
  _matrix_free_data.mapping_update_flags =
      dealii::update_values | dealii::update_gradients |
      dealii::update_JxW_values | dealii::update_quadrature_points;
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::reinit(
    dealii::DoFHandler<dim> const &dof_handler,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::hp::QCollection<1> const &q_collection)
{
  _matrix_free.reinit(dealii::StaticMappingQ1<dim>::mapping, dof_handler,
                      affine_constraints, q_collection, _matrix_free_data);
  _affine_constraints = &affine_constraints;
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::
    compute_inverse_mass_matrix(
        dealii::DoFHandler<dim> const &dof_handler,
        dealii::AffineConstraints<double> const &affine_constraints,
        dealii::hp::FECollection<dim> const &fe_collection)
{
  // Compute the inverse of the mass matrix
  dealii::hp::QCollection<dim> mass_matrix_q_collection;
  mass_matrix_q_collection.push_back(dealii::QGaussLobatto<dim>(fe_degree + 1));
  mass_matrix_q_collection.push_back(dealii::QGaussLobatto<dim>(2));
  auto locally_owned_dofs = dof_handler.locally_owned_dofs();
  dealii::IndexSet locally_relevant_dofs;
  dealii::DoFTools::extract_locally_relevant_dofs(dof_handler,
                                                  locally_relevant_dofs);
  _inverse_mass_matrix->reinit(locally_owned_dofs, locally_relevant_dofs,
                               _communicator);
  dealii::hp::FEValues<dim> hp_fe_values(
      fe_collection, mass_matrix_q_collection,
      dealii::update_quadrature_points | dealii::update_values |
          dealii::update_JxW_values);
  unsigned int const dofs_per_cell = fe_collection.max_dofs_per_cell();
  unsigned int const n_q_points =
      mass_matrix_q_collection.max_n_quadrature_points();
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  dealii::Vector<double> cell_mass(dofs_per_cell);
  for (auto const &cell : dealii::filter_iterators(
           dof_handler.active_cell_iterators(),
           dealii::IteratorFilters::LocallyOwnedCell(),
           dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
  {
    cell_mass = 0.;
    hp_fe_values.reinit(cell);
    dealii::FEValues<dim> const &fe_values =
        hp_fe_values.get_present_fe_values();
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
    {
      for (unsigned int q = 0; q < n_q_points; ++q)
      {
        for (unsigned int j = 0; j < dofs_per_cell; ++j)
        {

          cell_mass[i] += fe_values.shape_value(j, q) *
                          fe_values.shape_value(i, q) * fe_values.JxW(q);
        }
      }
    }
    cell->get_dof_indices(local_dof_indices);
    affine_constraints.distribute_local_to_global(cell_mass, local_dof_indices,
                                                  *_inverse_mass_matrix);
  }
  _inverse_mass_matrix->compress(dealii::VectorOperation::add);

  unsigned int const local_size = _inverse_mass_matrix->locally_owned_size();
  for (unsigned int k = 0; k < local_size; ++k)
  {
    if (_inverse_mass_matrix->local_element(k) > 1e-15)
      _inverse_mass_matrix->local_element(k) =
          1. / _inverse_mass_matrix->local_element(k);
    else
      _inverse_mass_matrix->local_element(k) = 0.;
  }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::clear()
{
  _matrix_free.clear();
  _inverse_mass_matrix->reinit(0);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::vmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  dst = 0.;
  vmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::Tvmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  dst = 0.;
  Tvmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::vmult_add(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  // Execute the matrix-free matrix-vector multiplication
  _matrix_free.cell_loop(&ThermalOperator::cell_local_apply, this, dst, src);
  // We compute the boundary conditions by hand. We would like to use
  // dealii::MatrixFree::loop() but there are two problems: 1. the function does
  // not work with FE_Nothing 2. even if it did we could only use it for the
  // domain boundary, we would still need to deal with the interface with
  // FE_Nothing ourselves.
  if (!(_boundary_type & BoundaryType::adiabatic))
  {
    unsigned int const dofs_per_cell = _matrix_free.get_dofs_per_cell();
    dealii::Vector<double> cell_src(dofs_per_cell);
    dealii::Vector<double> cell_dst(dofs_per_cell);
    auto &dof_handler = _matrix_free.get_dof_handler();
    std::vector<dealii::types::global_dof_index> local_dof_indices(
        dofs_per_cell);
    dealii::QGauss<dim - 1> face_quadrature(fe_degree + 1);
    dealii::FEFaceValues<dim> fe_face_values(
        dof_handler.get_fe(), face_quadrature,
        dealii::update_values | dealii::update_quadrature_points |
            dealii::update_JxW_values);
    unsigned int const n_face_q_points = face_quadrature.size();
    // Loop over the locally owned cells with an active FE index of zero
    for (auto const &cell : dealii::filter_iterators(
             dof_handler.active_cell_iterators(),
             dealii::IteratorFilters::LocallyOwnedCell(),
             dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
    {
      cell_dst = 0.;
      cell->get_dof_indices(local_dof_indices);
      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        cell_src[i] = src[local_dof_indices[i]];
      bool is_on_domain_boundary = false;
      for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell;
           ++f)
      {
        // We need to add the boundary conditions on the faces on the boundary
        // but also on the faces at the interface with FE_Nothing
        auto const &face = cell->face(f);
        if ((face->at_boundary()) ||
            ((!face->at_boundary()) &&
             (cell->neighbor(f)->active_fe_index() != 0)))
        {
          double conv_temperature_infty = 0.;
          double conv_heat_transfer_coef = 0.;
          double rad_temperature_infty = 0.;
          double rad_heat_transfer_coef = 0.;
          if (_boundary_type & BoundaryType::convective)
          {
            conv_temperature_infty = _material_properties->get_cell_value(
                cell, Property::convection_temperature_infty);
            conv_heat_transfer_coef = _material_properties->get_cell_value(
                cell, StateProperty::convection_heat_transfer_coef);
          }
          if (_boundary_type & BoundaryType::radiative)
          {
            rad_temperature_infty = _material_properties->get_cell_value(
                cell, Property::radiation_temperature_infty);
            rad_heat_transfer_coef = _material_properties->get_cell_value(
                cell, StateProperty::radiation_heat_transfer_coef);
          }

          fe_face_values.reinit(cell, face);
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
          {
            for (unsigned int i = 0; i < dofs_per_cell; ++i)
            {
              for (unsigned int q = 0; q < n_face_q_points; ++q)
              {
                // FIXME Need to be aware that we are using face quadrature
                // points not volumetric quadrature point. Right now we accept
                // this slight error.
                double const inv_rho_cp = get_inv_rho_cp(cell, q);

                cell_dst[i] -= inv_rho_cp *
                               (conv_heat_transfer_coef *
                                    (cell_src[j] - conv_temperature_infty) +
                                rad_heat_transfer_coef *
                                    (cell_src[j] - rad_temperature_infty)) *
                               fe_face_values.shape_value(i, q) *
                               fe_face_values.shape_value(j, q) *
                               fe_face_values.JxW(q);
              }
            }
          }
          is_on_domain_boundary = true;
        }
      }

      if (is_on_domain_boundary)
      {
        _affine_constraints->distribute_local_to_global(cell_dst,
                                                        local_dof_indices, dst);
      }
    }
  }

  // Because cell_loop resolves the constraints, the constrained dofs are not
  // called they stay at zero. Thus, we need to force the value on the
  // constrained dofs by hand. The variable scaling is used so that we get the
  // right order of magnitude.
  // TODO: for now the value of scaling is set to 1
  double const scaling = 1.;
  std::vector<unsigned int> const &constrained_dofs =
      _matrix_free.get_constrained_dofs();
  for (auto &dof : constrained_dofs)
    dst.local_element(dof) += scaling * src.local_element(dof);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::Tvmult_add(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  // The system of equation is symmetric so we can use vmult_add
  vmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::update_state_ratios(
    unsigned int cell, unsigned int q,
    dealii::VectorizedArray<double> temperature,
    std::array<dealii::VectorizedArray<double>,
               static_cast<unsigned int>(MaterialState::SIZE)> &state_ratios)
    const
{
  unsigned int constexpr liquid =
      static_cast<unsigned int>(MaterialState::liquid);
  unsigned int constexpr powder =
      static_cast<unsigned int>(MaterialState::powder);
  unsigned int constexpr solid =
      static_cast<unsigned int>(MaterialState::solid);

  // Loop over the vectorized arrays
  for (unsigned int n = 0; n < temperature.size(); ++n)
  {
    // Get the material id at this point
    dealii::types::material_id material_id = _material_id(cell, q)[n];

    // Get the material thermodynamic properties
    double const solidus =
        _material_properties->get(material_id, Property::solidus);
    double const liquidus =
        _material_properties->get(material_id, Property::liquidus);

    // Update the state ratios
    state_ratios[powder] = _powder_ratio(cell, q);

    if (temperature[n] < solidus)
      state_ratios[liquid][n] = 0.;
    else if (temperature[n] > liquidus)
      state_ratios[liquid][n] = 1.;
    else
    {
      state_ratios[liquid][n] =
          (temperature[n] - solidus) / (liquidus - solidus);
    }
    // Because the powder can only become liquid, the solid can only
    // become liquid, and the liquid can only become solid, the ratio of
    // powder can only decrease.
    state_ratios[powder][n] =
        std::min(1. - state_ratios[liquid][n], state_ratios[powder][n]);
    // Use max to make sure that we don't create matter because of
    // round-off.
    state_ratios[solid][n] =
        std::max(1. - state_ratios[liquid][n] - state_ratios[powder][n], 0.);
  }

  _liquid_ratio(cell, q) = state_ratios[liquid];
  _powder_ratio(cell, q) = state_ratios[powder];
}

template <int dim, int fe_degree, typename MemorySpaceType>
dealii::VectorizedArray<double>
ThermalOperator<dim, fe_degree, MemorySpaceType>::get_inv_rho_cp(
    unsigned int cell, unsigned int q,
    std::array<dealii::VectorizedArray<double>,
               static_cast<unsigned int>(MaterialState::SIZE)>
        state_ratios,
    dealii::VectorizedArray<double> temperature) const
{
  // Here we need the specific heat (including the latent heat contribution)
  // and the density

  auto material_id = _material_id(cell, q);
  // First, get the state-independent material properties
  dealii::VectorizedArray<double> solidus, liquidus, latent_heat;
  for (unsigned int n = 0; n < solidus.size(); ++n)
  {
    solidus[n] = _material_properties->get(material_id[n], Property::solidus);
    liquidus[n] = _material_properties->get(material_id[n], Property::liquidus);
    latent_heat[n] =
        _material_properties->get(material_id[n], Property::latent_heat);
  }

  // Now compute the state-dependent properties
  dealii::VectorizedArray<double> density =
      _material_properties->compute_material_property(
          StateProperty::density, material_id.data(), state_ratios.data(),
          temperature);

  dealii::VectorizedArray<double> specific_heat =
      _material_properties->compute_material_property(
          StateProperty::specific_heat, material_id.data(), state_ratios.data(),
          temperature);

  // Add in the latent heat contribution
  unsigned int constexpr liquid =
      static_cast<unsigned int>(MaterialState::liquid);

  for (unsigned int n = 0; n < specific_heat.size(); ++n)
  {
    if (state_ratios[liquid][n] > 0.0 && (state_ratios[liquid][n] < 1.0))
    {
      specific_heat[n] += latent_heat[n] / (liquidus[n] - solidus[n]);
    }
  }

  _inv_rho_cp(cell, q) = 1.0 / (density * specific_heat);

  return _inv_rho_cp(cell, q);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::cell_local_apply(
    dealii::MatrixFree<dim, double> const &data,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src,
    std::pair<unsigned int, unsigned int> const &cell_range) const
{
  // Get the subrange of cells associated with the fe index 0
  std::pair<unsigned int, unsigned int> cell_subrange =
      data.create_cell_subrange_hp_by_index(cell_range, 0);

  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(data);
  dealii::Tensor<1, dim> unit_tensor;
  for (unsigned int i = 0; i < dim; ++i)
    unit_tensor[i] = 1.;

  std::array<dealii::VectorizedArray<double>,
             static_cast<unsigned int>(MaterialState::SIZE)>
      state_ratios = {{dealii::make_vectorized_array(-1.0),
                       dealii::make_vectorized_array(-1.0),
                       dealii::make_vectorized_array(-1.0)}};

  // Loop over the "cells". Note that we don't really work on a cell but on a
  // set of quadrature point.
  for (unsigned int cell = cell_subrange.first; cell < cell_subrange.second;
       ++cell)
  {
    // Reinit fe_eval on the current cell
    fe_eval.reinit(cell);
    // Store in a local vector the local values of src
    fe_eval.read_dof_values(src);
    // Evaluate the function and its gradient on the reference cell
    fe_eval.evaluate(dealii::EvaluationFlags::values |
                     dealii::EvaluationFlags::gradients);
    // Apply the Jacobian of the transformation, multiply by the variable
    // coefficients and the quadrature points
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
    {
      auto temperature = fe_eval.get_value(q);

      // Calculate the local material properties
      update_state_ratios(cell, q, temperature, state_ratios);
      auto inv_rho_cp = get_inv_rho_cp(cell, q, state_ratios, temperature);
      auto mat_id = _material_id(cell, q);
      auto thermal_conductivity =
          _material_properties->compute_material_property(
              StateProperty::thermal_conductivity, mat_id.data(),
              state_ratios.data(), temperature);

      fe_eval.submit_gradient(
          -inv_rho_cp * thermal_conductivity * fe_eval.get_gradient(q), q);

      // Compute source term
      dealii::Point<dim, dealii::VectorizedArray<double>> const &q_point =
          fe_eval.quadrature_point(q);

      dealii::VectorizedArray<double> quad_pt_source = 0.0;
      for (unsigned int i = 0;
           i < _matrix_free.n_active_entries_per_cell_batch(cell); ++i)
      {
        dealii::Point<dim> q_point_loc;
        for (unsigned int d = 0; d < dim; ++d)
          q_point_loc(d) = q_point(d)[i];

        for (auto &beam : _heat_sources)
          quad_pt_source[i] +=
              beam->value(q_point_loc, _time, _current_source_height);
      }
      quad_pt_source *= inv_rho_cp;

      fe_eval.submit_value(quad_pt_source, q);
    }
    // Sum over the quadrature points.
    fe_eval.integrate(dealii::EvaluationFlags::values |
                      dealii::EvaluationFlags::gradients);
    fe_eval.distribute_local_to_global(dst);
  }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree,
                     MemorySpaceType>::get_state_from_material_properties()
{
  unsigned int const n_cells = _matrix_free.n_cell_batches();
  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(
      _matrix_free);

  _liquid_ratio.reinit(n_cells, fe_eval.n_q_points);
  _powder_ratio.reinit(n_cells, fe_eval.n_q_points);
  _material_id.reinit(n_cells, fe_eval.n_q_points);
  _inv_rho_cp.reinit(n_cells, fe_eval.n_q_points);

  for (unsigned int cell = 0; cell < n_cells; ++cell)
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      for (unsigned int i = 0;
           i < _matrix_free.n_active_entries_per_cell_batch(cell); ++i)
      {
        typename dealii::DoFHandler<dim>::cell_iterator cell_it =
            _matrix_free.get_cell_iterator(cell, i);
        // Cast to Triangulation<dim>::cell_iterator to access the material_id
        typename dealii::Triangulation<dim>::active_cell_iterator cell_tria(
            cell_it);

        _liquid_ratio(cell, q)[i] = _material_properties->get_state_ratio(
            cell_tria, MaterialState::liquid);
        _powder_ratio(cell, q)[i] = _material_properties->get_state_ratio(
            cell_tria, MaterialState::powder);
        _material_id(cell, q)[i] = cell_tria->material_id();
      }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree,
                     MemorySpaceType>::set_state_to_material_properties()
{
  _material_properties->set_state(_liquid_ratio, _powder_ratio,
                                  _cell_it_to_mf_cell_map,
                                  _matrix_free.get_dof_handler());
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::
    evaluate_material_properties(
        dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> const
            &temperature)
{
  if (!(_boundary_type & BoundaryType::adiabatic))
    _material_properties->update_boundary_material_properties(
        _matrix_free.get_dof_handler(), temperature);

  // Store the volumetric material properties
  unsigned int const n_cells = _matrix_free.n_cell_batches();
  for (unsigned int cell = 0; cell < n_cells; ++cell)
    for (unsigned int i = 0;
         i < _matrix_free.n_active_entries_per_cell_batch(cell); ++i)
    {
      typename dealii::DoFHandler<dim>::cell_iterator cell_it =
          _matrix_free.get_cell_iterator(cell, i);
      _cell_it_to_mf_cell_map[cell_it] = std::make_pair(cell, i);
    }
}
} // namespace adamantine

INSTANTIATE_DIM_FEDEGREE_HOST(TUPLE(ThermalOperator))


#include "astar.h"
#include <queue>
#include <deque>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <math.h>
#include <stdio.h>
#include "forward_kinematics.h"
#define BASIC 0
#define NOALPHA 1
#define WRENCH 2
#define WRENCH_NOALPHA 3
#define WRENCH_RATIO 4
#define ENERGY_LINEAR 5
#define ENERGY_ANGULAR 6
#define ENERGY_KINETIC 7
#define TORQUE_RATE 8
#define TORQUE_KINETIC 9
Astar::Astar(){
	dist_threshold = 9;
	int n_ticks = 100;
	numticks = n_ticks;
	n_ticks = numticks;
    time_step = .01;
    angle_threshold = .1;
	space = new State**[n_ticks];
	obj_mass = 1;
	for(int i = 0; i < n_ticks; i++){
		space[i] = new State*[n_ticks];
		for(int j = 0; j < n_ticks; j++){
			space[i][j] = new State[n_ticks];
			for(int k = 0; k < n_ticks; k++){
				space[i][j][k].visited = false;
				space[i][j][k].heuristic = -1;
				space[i][j][k].value = -1;
				space[i][j][k].id[0] = i;
				space[i][j][k].id[1] = j;
				space[i][j][k].id[2] = k;
			}
		}
	}

}

std::vector<PState*> Astar::run(int* start, double* target,double targett0){
	// std::cout << "Running A*\n";
	// std::cout << "Initializing target\n";
    this->target = target;
    this -> t0 = radian_to_ticks(targett0,numticks);
    // std::cout << "Initialize Start\n";
	compute_fk(start[0], start[1], start[2]);
	State* starts = &space[start[0]][start[1]][start[2]];
	starts->heuristic
		= heuristic(&space[start[0]][start[1]][start[2]]);
	starts->prev[0] = -1;
	starts->prev[1] = -1;
	starts->prev[2] = -1;

	starts->value = starts->heuristic;
	// std::cout << "Initialzing Priority Queue\n";
	PState* pstart = new PState;
	pstart->value = starts->value;
	pstart->state[0] = start[0];
	pstart->state[1] = start[1];
	pstart->state[2] = start[2];

	frontier.push(pstart);

	State* current = starts;
	PState* tracker = new PState;
	// std::cout << "Searching For Target\n";

	while(!frontier.empty() && will_continue(current)){
		/* Get the next priority */
		PState* pcur = frontier.top();
		frontier.pop();

		tracker->value = pcur->value;
		tracker->state[0] = pcur->state[0];
		tracker->state[1] = pcur->state[1];
		tracker->state[2] = pcur->state[2];
		current =
			&space[pcur->state[0]][pcur->state[1]][pcur->state[2]];

		if(current->visited == false){
			current->visited = true;
			expand_frontier(pcur->state[0],pcur->state[1],pcur->state[2]);
		}
		delete(pcur);
	}

	// std::cout << "Search Completed\n";
	std::vector<PState*> path;
	State* back_tracker = current;;
	path.push_back(tracker);
	// std::cout <<"Compiling Path\n";
	while(back_tracker != starts){
		int i0 = back_tracker->prev[0];
		int i1 = back_tracker->prev[1];
		int i2 = back_tracker->prev[2];
		PState* next = new PState;
		next->value = back_tracker->value;
		next->state[0] = i0;
		next->state[1] = i1;
		next->state[2] = i2;
		back_tracker = &space[i0][i1][i2];
		path.push_back(next);
	}

	return path;

}

void Astar::expand_frontier(int is, int js, int ks){
	for(int i = -1; i <= 1; i++){
		for(int j = -1; j <= 1; j++){
			for(int k = -1; k <= 1; k++){
				if(i == 0 && j == 0 && k == 0){
					continue;
				}

				if(inbounds(is+i, js+j, ks+k)){
					/* Check if visited */

					if(space[is+i][js+j][ks+k].visited){
						continue;
					}
					/* Has the heuristic been calculated */

					if(space[is+i][js+j][ks+k].heuristic < 0){
						compute_fk(is+i, js+j, ks+k);
						space[is+i][js+j][ks+k].heuristic
							= heuristic(&space[is+i][js+j][ks+k]);
					}
					double curcost =  (space[is][js][ks].value -
						space[is][js][ks].heuristic) +
						cost(&space[is][js][ks],
						&space[is+i][js+j][ks+k]);

				    /* Check if new value is less than min value */
					if(space[is+i][js+j][ks+k].value < 0){
						space[is+i][js+j][ks+k].value =
							space[is+i][js+j][ks+k].heuristic+curcost;
						space[is+i][js+j][ks+k].prev[0] = is;
		                space[is+i][js+j][ks+k].prev[1] = js;
		                space[is+i][js+j][ks+k].prev[2] = ks;

						PState* p = new PState;
						p->state[0] = is+i;
						p->state[1] = js+j;
						p->state[2] = ks+k;
						p->value = space[is+i][js+j][ks+k].value;
						frontier.push(p);
					} else {
						if(curcost + space[is+i][js+j][ks+k].heuristic
							< space[is+i][js+j][ks+k].value){
							space[is+i][js+j][ks+k].value =
								curcost+space[is+i][js+j][ks+k].heuristic;
							space[is+i][js+j][ks+k].prev[0] = is;
		                	space[is+i][js+j][ks+k].prev[1] = js;
		                	space[is+i][js+j][ks+k].prev[2] = ks;
							PState* p = new PState;
							p->state[0] = is+i;
							p->state[1] = js+j;
							p->state[2] = ks+k;
							p->value = space[is+i][js+j][ks+k].value;
							frontier.push(p);
						}
					}
				}
			}
		}
	}
}

double Astar::cost(State* s1, State* s2){
#if COSTFUNCTION == NOALPHA
	return pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2);
#elif COSTFUNCTION == WRENCH
	double dist = pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2)
		+pow(s1->alpha - s2->alpha,2);
	double t1 = tick_to_radians(s2->id[0],numticks);
	double t2 = tick_to_radians(s2->id[1],numticks);
	double t3 = tick_to_radians(s2->id[2],numticks);
	double c1 = 150*cos(t1)*obj_mass;
	double c12 = 150*cos(t1+t2)*obj_mass;
	double s123 = 116.525*sin(t1+t2+t3)*obj_mass;

	double mag_torque = pow(c1+c12+s123,2)
		+pow(c12+s123, 2)+pow(s123, 2);
	return dist+mag_torque;
#elif COSTFUNCTION == WRENCH_NOALPHA
	double dist = pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2);
	double t1 = tick_to_radians(s2->id[0],numticks);
	double t2 = tick_to_radians(s2->id[1],numticks);
	double t3 = tick_to_radians(s2->id[2],numticks);
	double c1 = 150*cos(t1)*obj_mass;
	double c12 = 150*cos(t1+t2)*obj_mass;
	double s123 = 116.525*sin(t1+t2+t3)*obj_mass;

	double mag_torque = pow(c1+c12+s123,2)
		+pow(c12+s123, 2)+pow(s123, 2);
	return dist+mag_torque;
#elif COSTFUNCTION == WRENCH_RATIO
	double dist = pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2)
		+pow(s1->alpha - s2->alpha,2);
	double s2t1 = tick_to_radians(s2->id[0],numticks);
	double s2t2 = tick_to_radians(s2->id[1],numticks);
	double s2t3 = tick_to_radians(s2->id[2],numticks);
	double s2c1 = 150*cos(s2t1)*obj_mass;
	double s2c12 = 150*cos(s2t1+s2t2)*obj_mass;
	double s2s123 = 116.525*sin(s2t1+s2t2+s2t3)*obj_mass;

	double s1t1 = tick_to_radians(s1->id[0],numticks);
	double s1t2 = tick_to_radians(s1->id[1],numticks);
	double s1t3 = tick_to_radians(s1->id[2],numticks);
	double s1c1 = 150*cos(s1t1)*obj_mass;
	double s1c12 = 150*cos(s1t1+s1t2)*obj_mass;
	double s1s123 = 116.525*sin(s1t1+s1t2+s1t3)*obj_mass;

	double mag_torque2 = pow(s2c1+s2c12+s2s123,2)
		+pow(s2c12+s2s123, 2)+pow(s2s123, 2)+.0001;

	double mag_torque1 = pow(s1c1+s1c12+s1s123,2)
		+pow(s1c12+s1s123, 2)+pow(s1s123, 2)+.0001;
	return dist + (mag_torque2/mag_torque1);
#elif COSTFUNCTION == ENERGY_LINEAR
	double dist = pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2)
		+pow(s1->alpha - s2->alpha,2);
	double s2t1 = tick_to_radians(s2->id[0],numticks);
	double s2t2 = tick_to_radians(s2->id[1],numticks);
	double s2t3 = tick_to_radians(s2->id[2],numticks);
	double s1t1 = tick_to_radians(s1->id[0],numticks);
	double s1t2 = tick_to_radians(s1->id[1],numticks);
	double s1t3 = tick_to_radians(s1->id[2],numticks);

	Eigen::Matrix<double,3,5> J = jacobian(0,s1->id[0],s1->id[1],s1->id[2],0,numticks);
	Eigen::Matrix<double,5,1> thetadot;
	thetadot << 0, ((s2t1-s1t1)/time_step), ((s2t2-s1t2)/time_step), ((s2t3-s1t3)/time_step),  0;
	Eigen::Vector3d vel = J*thetadot;
        return dist+ obj_mass*vel.squaredNorm();
#elif COSTFUNCTION == ENERGY_ANGULAR
    double dist = pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2);

	double alphadist = dist + pow(s1->alpha - s2->alpha,2);
	double s2t1 = tick_to_radians(s2->id[0],numticks);
	double s2t2 = tick_to_radians(s2->id[1],numticks);
	double s2t3 = tick_to_radians(s2->id[2],numticks);
	double s1t1 = tick_to_radians(s1->id[0],numticks);
	double s1t2 = tick_to_radians(s1->id[1],numticks);
	double s1t3 = tick_to_radians(s1->id[2],numticks);

	double angular_vel = ((s2t1-s1t1)+(s2t2-s1t2)+(s2t3-s1t3))/time_step;
	return alphadist + obj_mass*dist*pow(angular_vel,2);

#elif COSTFUNCTION == ENERGY_KINETIC
	double dist = pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2);

	double alphadist = dist + pow(s1->alpha - s2->alpha,2);
	double s2t1 = tick_to_radians(s2->id[0],numticks);
	double s2t2 = tick_to_radians(s2->id[1],numticks);
	double s2t3 = tick_to_radians(s2->id[2],numticks);
	double s1t1 = tick_to_radians(s1->id[0],numticks);
	double s1t2 = tick_to_radians(s1->id[1],numticks);
	double s1t3 = tick_to_radians(s1->id[2],numticks);

	double angular_vel = ((s2t1-s1t1)+(s2t2-s1t2)+(s2t3-s1t3))/time_step;
	Eigen::Matrix<double,3,5> J = jacobian(0,s1->id[0],s1->id[1],s1->id[2],0,numticks);
	Eigen::Matrix<double,5,1> thetadot;
	thetadot << 0, ((s2t1-s1t1)/time_step), ((s2t2-s1t2)/time_step), ((s2t3-s1t3)/time_step),  0;
	Eigen::Vector3d vel = J*thetadot;
    return alphadist+ obj_mass*vel.squaredNorm()+ obj_mass*dist*pow(angular_vel,2);
#elif COSTFUNCTION == TORQUE_RATE

	double s2t1 = tick_to_radians(s2->id[0],numticks);
	double s2t2 = tick_to_radians(s2->id[1],numticks);
	double s2t3 = tick_to_radians(s2->id[2],numticks);
	double s2c1 = 150*cos(s2t1)*obj_mass;
	double s2c12 = 150*cos(s2t1+s2t2)*obj_mass;
	double s2s123 = 116.525*sin(s2t1+s2t2+s2t3)*obj_mass;

	double s1t1 = tick_to_radians(s1->id[0],numticks);
	double s1t2 = tick_to_radians(s1->id[1],numticks);
	double s1t3 = tick_to_radians(s1->id[2],numticks);
	double s1c1 = 150*cos(s1t1)*obj_mass;
	double s1c12 = 150*cos(s1t1+s1t2)*obj_mass;
	double s1s123 = 116.525*sin(s1t1+s1t2+s1t3)*obj_mass;

	double mag_torque2 = pow(s2c1+s2c12+s2s123,2)
		+pow(s2c12+s2s123, 2)+pow(s2s123, 2)+.0001;

	double mag_torque1 = pow(s1c1+s1c12+s1s123,2)
		+pow(s1c12+s1s123, 2)+pow(s1s123, 2)+.0001;
	return abs(mag_torque2-mag_torque1)/time_step;
#elif COSTFUNCTION == TORQUE_KINETIC
	//Torque Rate
	double s2t1 = tick_to_radians(s2->id[0],numticks);
	double s2t2 = tick_to_radians(s2->id[1],numticks);
	double s2t3 = tick_to_radians(s2->id[2],numticks);
	double s2c1 = 150*cos(s2t1)*obj_mass;
	double s2c12 = 150*cos(s2t1+s2t2)*obj_mass;
	double s2s123 = 116.525*sin(s2t1+s2t2+s2t3)*obj_mass;

	double s1t1 = tick_to_radians(s1->id[0],numticks);
	double s1t2 = tick_to_radians(s1->id[1],numticks);
	double s1t3 = tick_to_radians(s1->id[2],numticks);
	double s1c1 = 150*cos(s1t1)*obj_mass;
	double s1c12 = 150*cos(s1t1+s1t2)*obj_mass;
	double s1s123 = 116.525*sin(s1t1+s1t2+s1t3)*obj_mass;

	double mag_torque2 = pow(s2c1+s2c12+s2s123,2)
		+pow(s2c12+s2s123, 2)+pow(s2s123, 2)+.0001;

	double mag_torque1 = pow(s1c1+s1c12+s1s123,2)
		+pow(s1c12+s1s123, 2)+pow(s1s123, 2)+.0001;

	//Kinetic

	Eigen::Matrix<double,3,5> J = jacobian(0,s1->id[0],s1->id[1],s1->id[2],0,numticks);
	Eigen::Matrix<double,5,1> thetadot;
	thetadot << 0, ((s2t1-s1t1)/time_step), ((s2t2-s1t2)/time_step), ((s2t3-s1t3)/time_step),  0;
	Eigen::Vector3d vel = J*thetadot;
	return abs(mag_torque2-mag_torque1)/time_step+obj_mass*vel.squaredNorm();

#else
	return pow(s1->x - s2->x,2)+pow(s1->z - s2->z,2)
		+pow(s1->alpha - s2->alpha,2);

#endif
}

double Astar::heuristic(State* s){
#if COSTFUNCTION == NOALPHA || COSTFUNCTION == WRENCH_NOALPHA
	return pow(s->x - target[0],2)+pow(s->z - target[1],2);
#elif COSTFUNCTION == TORQUE_RATE || COSTFUNCTION == TORQUE_KINETIC
	return 0;
#else
	return pow(s->x - target[0],2)+pow(s->z - target[1],2)
		+pow(s->alpha - target[2],2);
#endif
}

void Astar::compute_fk(int i, int j, int k){
	double x;
	double z;
	double alpha;
	fk(&x, &z, &alpha,t0, i, j, k,numticks);
	space[i][j][k].x  = x;
	space[i][j][k].z = z;
	space[i][j][k].alpha = alpha;
}

bool Astar::inbounds(int i, int j, int k){
	if(i<numticks && j < numticks && k < numticks){
		if(i >= 0 && j >= 0 && k >= 0)
			return true;
	}
	return false;
}

bool Astar::will_continue(State* current){
#if COSTFUNCTION == NOALPHA || COSTFUNCTION == WRENCH_NOALPHA
	double dist = pow(current->x-target[0], 2) + pow(current->z-target[1], 2);
	return dist > dist_threshold;
#elif COSTFUNCTION == TORQUE_RATE || COSTFUNCTION == TORQUE_KINETIC
	double dist = pow(current->x-target[0], 2) + pow(current->z-target[1], 2);
	return dist > dist_threshold;
#else
	double dist = pow(current->x-target[0], 2) + pow(current->z-target[1], 2);
	double angle = current->alpha;
	return !(dist < dist_threshold && (fabs(angle-target[2]) < angle_threshold
		|| (fabs(angle+2*M_PI-target[2]) < angle_threshold)
		|| (fabs(angle-2*M_PI-target[2]) < angle_threshold )));
#endif

}


// Copyright 2013-2020 Lawrence Livermore National Security, LLC and other
// HavoqGT Project Developers. See the top-level LICENSE file for details.
//
// SPDX-License-Identifier: MIT

#ifndef HAVOQGT_MPI_GEN_PREFERENTIAL_ATTACHMENT_EDGE_LIST_HPP_INCLUDED
#define HAVOQGT_MPI_GEN_PREFERENTIAL_ATTACHMENT_EDGE_LIST_HPP_INCLUDED

#include <havoqgt/mpi.hpp>
#include <havoqgt/detail/hash.hpp>
#include <havoqgt/detail/preferential_attachment.hpp>
#include <boost/random.hpp>
#include <algorithm>

namespace havoqgt {

template <typename EdgeType>
void gen_preferential_attachment_edge_list(std::vector<EdgeType>& local_edges,
                                           uint64_t in_base_seed,
                                           uint64_t in_node_scale,
                                           uint64_t in_edge_scale,
                                           double in_beta,
                                           double in_prob_rewire,
                                           MPI_Comm in_comm)
{
  namespace ad = havoqgt::detail;
  int mpi_rank, mpi_size;
  CHK_MPI( MPI_Comm_rank(in_comm, &mpi_rank) );
  CHK_MPI( MPI_Comm_size(in_comm, &mpi_size) );

  //
  // Calc global number of nodes and edges
  uint64_t global_num_nodes = uint64_t(1) << in_node_scale;
  uint64_t global_num_edges = uint64_t(1) << in_edge_scale;
  uint64_t k = global_num_edges / global_num_nodes;
  uint64_t edges_per_rank = global_num_edges / mpi_size;

  ad::preferential_attachment_helper<uint64_t> pa(k, global_num_edges, in_beta,
                                             in_base_seed*mpi_rank + mpi_rank);

  //
  // Generate inital pa edges.  Includes unresolved links
  double init_time_start = MPI_Wtime();
  local_edges.resize(edges_per_rank);
  for(uint64_t i=0; i<edges_per_rank; ++i) {
    uint64_t edge_index = uint64_t(mpi_rank) + i*uint64_t(mpi_size);
    local_edges[i] = pa.gen_edge(edge_index);
  }
  double init_time_end = MPI_Wtime();
  if(mpi_rank == 0) {
    std::cout << "Initial rng for edges took " << init_time_end - init_time_start
              << " seconds. " << std::endl;
  }

  //
  // We loop until all edges are resolved
  uint64_t iteration_count(1), count_missing(0), count_total_mising(0);
  do {
    //
    // Count number still missing
    for(uint64_t i=0; i<edges_per_rank; ++i) {
      if(pa.is_pointer(local_edges[i].second)) {
        ++count_total_mising;
      }
    }

    count_missing = 0;
    double time_start = MPI_Wtime();
    //
    // Generate vectors to exchange
    std::vector<std::vector<uint64_t> > tmp_send_to_rank(mpi_size);
    std::vector<std::vector<uint64_t> > tmp_mark_send_to_rank(mpi_size);
    uint64_t count_to_send(0);
    for(uint64_t i=0; i<local_edges.size(); ++i) {
      if(pa.is_pointer(local_edges[i].second)) {
        int owner = pa.value_of_pointer(local_edges[i].second) % mpi_size;
        tmp_mark_send_to_rank[owner].push_back(i);
        tmp_send_to_rank[owner].push_back(pa.value_of_pointer(local_edges[i].second));
        ++count_to_send;
      }
    }
    //
    // Combine send/recv vectors
    std::vector<uint64_t> to_send(count_to_send); to_send.reserve(1);
    std::vector<uint64_t> mark_to_send(count_to_send);
    std::vector<int>      sendcnts(mpi_size,0); sendcnts.reserve(1);
    to_send.clear(); mark_to_send.clear();
    for(int i = 0; i < mpi_size; ++i) {
      for(size_t j = 0; j < tmp_send_to_rank[i].size(); ++j) {
        to_send.push_back(tmp_send_to_rank[i][j]);
        mark_to_send.push_back(tmp_mark_send_to_rank[i][j]);
      }
      sendcnts[i] = tmp_send_to_rank[i].size();
    }
    //
    // Release memory from temp vectors
    {
      std::vector<std::vector<uint64_t> > empty;
      tmp_send_to_rank.swap(empty);
    }
    {
      std::vector<std::vector<uint64_t> > empty;
      tmp_mark_send_to_rank.swap(empty);
    }
    //
    // Exchange vectors
    std::vector<uint64_t> to_recv; // not sized
    std::vector<int>      recvcnts; // not sized
    mpi_all_to_all(to_send, sendcnts, to_recv, recvcnts, MPI_COMM_WORLD);
    //
    // Look up pointers, pointer jump!
    for(size_t i=0; i<to_recv.size(); ++i) {
      uint64_t local_index = to_recv[i]/mpi_size;
      to_recv[i] = local_edges[local_index].second;
    }
    //
    // Return exchange vectors
    mpi_all_to_all(to_recv, recvcnts, to_send, sendcnts, MPI_COMM_WORLD);
    //
    // Return new pointers to marked location
    for(size_t i=0; i<to_send.size(); ++i) {
        local_edges[ mark_to_send[i] ].second = to_send[i];;
    }
    //
    // Count number still missing
    for(uint64_t i=0; i<edges_per_rank; ++i) {
      if(pa.is_pointer(local_edges[i].second)) {
        ++count_missing;
      }
    }
    count_missing = mpi_all_reduce(count_missing, std::plus<uint64_t>(), MPI_COMM_WORLD);

    //
    // Iteration ouput
    double time_end = MPI_Wtime();
    if(mpi_rank == 0) {
      std::cout << "Iteration " << iteration_count << " took "
                << time_end - time_start << " seconds.   "
                << count_missing << " still missing. " << std::endl;
    }
    ++iteration_count;
  } while(count_missing > 0);
  //
  // Output total nuumber of global exchanges
  count_total_mising = mpi_all_reduce(count_total_mising, std::plus<uint64_t>(), MPI_COMM_WORLD);
  if(mpi_rank == 0) {
    std::cout << "Total missing (how much data globally exchanged) = " << count_total_mising << std::endl;
  }

  //
  // Randomly rewire
  if(in_prob_rewire > double(0)) {
    boost::mt19937 rng(in_base_seed + uint64_t(mpi_rank) * 3ULL);
    boost::random::uniform_int_distribution<> rand_node(0,global_num_nodes-1);
    boost::random::uniform_01<> rand_real;
    for(size_t i=0; i<local_edges.size(); ++i) {
      if(rand_real(rng) < in_prob_rewire) {
        EdgeType rand_edge(rand_node(rng), rand_node(rng));
        local_edges[i] = rand_edge;
      }
    }
  }

  //
  // Scramble edges
  for(size_t i=0; i<local_edges.size(); ++i) {
    //TODO:  This needs a mod because we never exactly make the correct
    //number of vertices.   We need a better solution!
    local_edges[i].first  %= global_num_nodes;
    local_edges[i].second %= global_num_nodes;
    local_edges[i].first  = ad::hash_nbits(local_edges[i].first,  in_node_scale);
    local_edges[i].second = ad::hash_nbits(local_edges[i].second, in_node_scale);
  }

  // //
  // // Symmetrizing b/c PA is always undirected.
  // uint64_t old_size = local_edges.size();
  // local_edges.reserve(old_size * 2);
  // for(uint64_t i=0; i<old_size; ++i) {
  //   EdgeType edge;
  //   edge.first = local_edges[i].second;
  //   edge.second = local_edges[i].first;
  //   local_edges.push_back( edge );
  // }

  // //
  // // Shuffle edges because we need to mix the hubs in for future partitioning
  // std::random_shuffle(local_edges.begin(), local_edges.end());
}

} //end namespace havoqgt

#endif //end HAVOQGT_MPI_GEN_PREFERENTIAL_ATTACHMENT_EDGE_LIST_HPP_INCLUDED


#include "file_io.hpp"
#include <boost/filesystem.hpp>
#include <cmath>

size_t get_cnt_lines(const std::string &name_file)
{
    throw (std::runtime_error("not yet implemented"));
    return 0;
}

size_t read_int(const std::string &name_file)
{
    std::ifstream is(name_file, std::ifstream::in);
    size_t res;
    is >> res;
    return res;
}

size_t get_filesize(const std::string & name_file)
{
    std::ifstream in(name_file, std::ifstream::ate | std::ifstream::binary);
    if (!in.is_open())
    {
        std::string message="get_filesize() can not open file ";
        throw std::runtime_error (message+name_file);
    }
    return in.tellg();
}

double get_PMI(Index cnt,Index id1,Index id2,Vocabulary const & vocab)
{
    return log2((static_cast<double>(cnt)*vocab.cnt_words_processed)/(vocab.freq_per_id[id1]*vocab.freq_per_id[id2]));
}

void dump_crs(std::string path_out,std::vector<Accumulator> const & counters,Vocabulary const & vocab, bool binary)
{
    std::ofstream file;
    auto mode=binary? std::ios::out | std::ios::binary : std::ios::out;
    std::string str_path = (boost::filesystem::path(path_out) / boost::filesystem::path(binary?"bigrams.data.bin":"bigrams.data")).string();
    file.open (str_path,mode);
    if(!file) throw  std::runtime_error("can not open output file " + str_path + " , check the path");
    for (size_t first=0;first<counters.size();first++)
        for (const auto& second : counters[first])
        {
            float v=get_PMI(second.second,first,second.first,vocab);
            if (binary)
                file.write( reinterpret_cast<const char*>(&v),sizeof(v));
            else
                file<<v<<"\n";
        }
    file.close();

    str_path = (boost::filesystem::path(path_out) / boost::filesystem::path(binary?"bigrams.col_ind.bin":"bigrams.col_ind")).string();
    file.open (str_path,mode);
    for (size_t first=0;first<counters.size();first++)
        for (const auto& second : counters[first])
        {
            size_t v=second.first;
            if (binary)
               file.write( reinterpret_cast<const char*>(&v),sizeof(v));
            else
                file<<v<<"\n";
        }
    file.close();

    str_path = (boost::filesystem::path(path_out) / boost::filesystem::path(binary?"bigrams.row_ptr.bin":"bigrams.row_ptr")).string();
    file.open (str_path,mode);
    Index row_ptr=0;
    Index id_last=0;
    for (size_t first=0;first<counters.size();first++)
    {
        if (first==0)
            {
                if (binary)
                    file.write( reinterpret_cast<const char*>(&row_ptr),sizeof(row_ptr));
                else
                    file<<row_ptr<<"\n";
            }
        else
            for (size_t k=id_last;k<first;k++)
                if (binary)
                    file.write( reinterpret_cast<const char*>(&row_ptr),sizeof(row_ptr));
                else
                    file<<row_ptr<<"\n";
            id_last=first;
            row_ptr+=counters[first].size();
        }
        for (size_t k=id_last;k<vocab.cnt_words;k++)
            if (binary)
                file.write( reinterpret_cast<const char*>(&row_ptr),sizeof(row_ptr));
            else
                file<<row_ptr<<"\n";
       file.close();
   }

void write_cooccurrence_text(std::string name_file,std::vector<Accumulator> const & counters,Vocabulary const & vocab)
{
std::ofstream file;
file.open (name_file);
if(!file) throw  std::runtime_error("can not open output file "+name_file+" , check the path");
for (size_t first=0;first<counters.size();first++)
{
    for (const auto& second : counters[first])
    {
      //if (t.second>0)
       // file<<first.first<<"\t"<<second.first<<"\t"<<second.second<<"\n";
        double v=log2((static_cast<double>(second.second)*vocab.cnt_words_processed)/(vocab.freq_per_id[first]*vocab.freq_per_id[second.first]));
        //file<<first<<"\t"<<second.first<<"\t"<<v<<"\n";
        file<<vocab.lst_id2word[first]<<"\t"<<vocab.lst_id2word[second.first]<<"\t"<<second.second<<"\t"<<v<<"\n";
    }
}

file.close();
}