/*
 The MIT License (MIT)

 Copyright (c) [2016] [BTC.COM]

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
*/

#include "CommonGrin.h"

#include "cuckoo/cuckaroo.h"
#include "cuckoo/cuckarood.h"
#include "cuckoo/cuckaroom.h"
#include "cuckoo/cuckatoo.h"
#include "cuckoo/siphash.h"
#include "libblake2/blake2.h"

#include <boost/multiprecision/cpp_int.hpp>
#include <limits>
#include <Utils.h>

static const uint8_t BASE_EDGE_BITS = 24;
static const uint8_t DEFAULT_MIN_EDGE_BITS = 31;
static const uint8_t SECOND_POW_EDGE_BITS = 29;

static const uint64_t MAX_DIFFICUTY = std::numeric_limits<uint64_t>::max();

static const uint64_t BLOCK_TIME_SEC = 60;
static const uint64_t HOUR_HEIGHT = 3600 / BLOCK_TIME_SEC;
static const uint64_t DAY_HEIGHT = 24 * HOUR_HEIGHT;
static const uint64_t WEEK_HEIGHT = 7 * DAY_HEIGHT;
static const uint64_t YEAR_HEIGHT = 52 * WEEK_HEIGHT;

static const uint64_t GRIN_BASE = 1000000000;
static const uint64_t REWARD = BLOCK_TIME_SEC * GRIN_BASE;

static uint64_t
PowDifficultyGrinScaled(uint64_t hash, uint32_t secondaryScaling) {
  boost::multiprecision::uint128_t x = secondaryScaling;
  x <<= 64;
  x /= hash;
  return x > MAX_DIFFICUTY ? MAX_DIFFICUTY : static_cast<uint64_t>(x);
}

// verify that edges are ascending and form a cycle in header-generated graph
bool VerifyPowGrinPrimary(
    const std::vector<uint64_t> &edges, siphash_keys &keys, uint32_t edgeBits) {
  return verify_cuckatoo(edges, keys, edgeBits);
}

// verify that edges are ascending and form a cycle in header-generated graph
bool VerifyPowGrinSecondary(
    const std::vector<uint64_t> &edges,
    siphash_keys &keys,
    uint32_t edgeBits,
    uint16_t version) {
  switch (version) {
  case 3:
    return verify_cuckaroom(edges, keys, edgeBits);
  case 2:
    return verify_cuckarood(edges, keys, edgeBits);
  default:
    return verify_cuckaroo(edges, keys, edgeBits);
  }
}

bool VerifyPowGrin(
    const PreProofGrin &preProof,
    uint32_t edgeBits,
    const std::vector<uint64_t> &proofs) {
  if (edgeBits != SECOND_POW_EDGE_BITS && edgeBits < DEFAULT_MIN_EDGE_BITS)
    return false;

  siphash_keys siphashKeys;
  char preProofKeys[32];
  blake2b(
      preProofKeys, sizeof(preProofKeys), &preProof, sizeof(preProof), 0, 0);
  siphashKeys.setkeys(preProofKeys);
  return edgeBits == SECOND_POW_EDGE_BITS
      ? VerifyPowGrinSecondary(
            proofs, siphashKeys, edgeBits, preProof.prePow.version.value())
      : VerifyPowGrinPrimary(proofs, siphashKeys, edgeBits);
}

uint256 PowHashGrin(uint32_t edgeBits, const std::vector<uint64_t> &proofs) {
  // Compress the proofs to a bit vector
  std::vector<uint8_t> proofBits((proofs.size() * edgeBits + 7) / 8, 0);
  uint64_t edgeMask = (static_cast<uint64_t>(1) << edgeBits) - 1;
  size_t i = 0;
  for (uint64_t proof : proofs) {
    proof &= edgeMask;
    for (uint32_t j = 0; j < edgeBits; ++j) {
      if (0x1 & (proof >> j)) {
        uint32_t position = i * edgeBits + j;
        proofBits[position / 8] |= (1 << (position % 8));
      }
    }
    ++i;
  }

  // Generate the blake2b hash
  uint256 hash;
  blake2b(hash.begin(), sizeof(hash), proofBits.data(), proofBits.size(), 0, 0);
  return hash;
}

uint32_t GraphWeightGrin(uint64_t height, uint32_t edgeBits) {
  uint64_t xprEdgeBits = edgeBits;

  auto bitsOverMin =
      edgeBits <= DEFAULT_MIN_EDGE_BITS ? 0 : edgeBits - DEFAULT_MIN_EDGE_BITS;
  auto expiryHeight = (1 << bitsOverMin) * YEAR_HEIGHT;
  if (edgeBits < 32 && height >= expiryHeight) {
    auto weeks = 1 + (height - expiryHeight) / WEEK_HEIGHT;
    xprEdgeBits = xprEdgeBits > weeks ? xprEdgeBits - weeks : 0;
  }

  return ((2 << (edgeBits - BASE_EDGE_BITS)) * xprEdgeBits);
}

uint32_t
PowScalingGrin(uint64_t height, uint32_t edgeBits, uint32_t secondaryScaling) {
  return edgeBits == SECOND_POW_EDGE_BITS ? secondaryScaling
                                          : GraphWeightGrin(height, edgeBits);
}

uint64_t PowDifficultyGrin(
    uint64_t height,
    uint32_t edgeBits,
    uint32_t secondaryScaling,
    const std::vector<uint64_t> &proofs) {
  // Compress the proofs to a bit vector
  std::vector<uint8_t> proofBits((proofs.size() * edgeBits + 7) / 8, 0);
  uint64_t edgeMask = (static_cast<uint64_t>(1) << edgeBits) - 1;
  size_t i = 0;
  for (uint64_t proof : proofs) {
    proof &= edgeMask;
    for (uint32_t j = 0; j < edgeBits; ++j) {
      if (0x1 & (proof >> j)) {
        uint32_t position = i * edgeBits + j;
        proofBits[position / 8] |= (1 << (position % 8));
      }
    }
    ++i;
  }

  // Generate the blake2b hash
  boost::endian::big_uint64_buf_t hash[4];
  blake2b(hash, sizeof(hash), proofBits.data(), proofBits.size(), 0, 0);

  // Scale the difficulty
  return PowDifficultyGrinScaled(
      hash[0].value(), PowScalingGrin(height, edgeBits, secondaryScaling));
}

uint64_t GetBlockRewardGrin(uint64_t height) {
  return REWARD;
}


#include "config.h"

#include <boost/container/flat_map.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "filelib.h"
#include "stringlib.h"
#include "weights.h"
#include "sparse_vector.h"
#include "candidate_set.h"
#include "sentence_metadata.h"
#include "ns.h"
#include "ns_docscorer.h"
#include "verbose.h"
#include "hg.h"
#include "ff_register.h"
#include "decoder.h"
#include "fdict.h"
#include "sampler.h"

using namespace std;
namespace po = boost::program_options;

boost::shared_ptr<MT19937> rng;
vector<training::CandidateSet> kbests;
SparseVector<weight_t> G, u, lambdas;
double pseudo_doc_decay = 0.9;

bool InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
    ("decoder_config,c",po::value<string>(),"[REQ] Decoder configuration file")
    ("devset,d",po::value<string>(),"[REQ] Source/reference development set")
    ("weights,w",po::value<string>(),"Initial feature weights file")
    ("mt_metric,m",po::value<string>()->default_value("ibm_bleu"), "Scoring metric (ibm_bleu, nist_bleu, koehn_bleu, ter, combi)")
    ("size",po::value<unsigned>()->default_value(0), "Process rank (for multiprocess mode)")
    ("rank",po::value<unsigned>()->default_value(1), "Number of processes (for multiprocess mode)")
    ("optimizer,o",po::value<unsigned>()->default_value(1), "Optimizer (Adaptive MIRA=1)")
    ("fear,f",po::value<unsigned>()->default_value(1), "Fear selection (model-cost=1, maxcost=2, maxscore=3)")
    ("hope,h",po::value<unsigned>()->default_value(1), "Hope selection (model+cost=1, mincost=2)")
    ("eta0", po::value<double>()->default_value(0.1), "Initial step size")
    ("random_seed,S", po::value<uint32_t>(), "Random seed (if not specified, /dev/random will be used)")
    ("mt_metric_scale,s", po::value<double>()->default_value(1.0), "Scale MT loss function by this amount")
    ("pseudo_doc,e", "Use pseudo-documents for approximate scoring")
    ("k_best_size,k", po::value<unsigned>()->default_value(500), "Size of hypothesis list to search for oracles");
  po::options_description clo("Command line options");
  clo.add_options()
    ("config", po::value<string>(), "Configuration file")
    ("help,H", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help")
          || !conf->count("decoder_config")
          || !conf->count("devset")) {
    cerr << dcmdline_options << endl;
    return false;
  }
  return true;
}

struct TrainingObserver : public DecoderObserver {
  explicit TrainingObserver(const EvaluationMetric& m, const int k) : metric(m), kbest_size(k), cur_eval() {}

  const EvaluationMetric& metric;
  const int kbest_size;
  const SegmentEvaluator* cur_eval;
  SufficientStats pdoc;
  unsigned hi, vi, fi;  // hope, viterbi, fear

  void SetSegmentEvaluator(const SegmentEvaluator* eval) {
    cur_eval = eval;
  }

  virtual void NotifySourceParseFailure(const SentenceMetadata& smeta) {
    cerr << "Failed to translate sentence with ID = " << smeta.GetSentenceID() << endl;
    abort();
  }

  unsigned CostAugmentedDecode(const training::CandidateSet& cs,
                               const SparseVector<double>& w,
                               double alpha = 0) {
    unsigned best_i = 0;
    double best = -numeric_limits<double>::infinity();
    for (unsigned i = 0; i < cs.size(); ++i) {
      double s = cs[i].fmap.dot(w);
      if (alpha)
        s += alpha * metric.ComputeScore(cs[i].eval_feats + pdoc);
      if (s > best) {
        best = s;
        best_i = i;
      }
    }
    return best_i;
  }

  virtual void NotifyTranslationForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    pdoc *= pseudo_doc_decay;
    const unsigned sent_id = smeta.GetSentenceID();
    kbests[sent_id].AddUniqueKBestCandidates(*hg, kbest_size, cur_eval);
    vi = CostAugmentedDecode(kbests[sent_id], lambdas);
    hi = CostAugmentedDecode(kbests[sent_id], lambdas, 1.0);
    fi = CostAugmentedDecode(kbests[sent_id], lambdas, -1.0);
    cerr << sent_id << " ||| " << TD::GetString(kbests[sent_id][vi].ewords) << " ||| " << metric.ComputeScore(kbests[sent_id][vi].eval_feats + pdoc) << endl;
    pdoc += kbests[sent_id][vi].eval_feats;  // update pseudodoc stats
  }
};

int main(int argc, char** argv) {
  SetSilent(true);  // turn off verbose decoder output
  register_feature_functions();

  po::variables_map conf;
  if (!InitCommandLine(argc, argv, &conf)) return 1;

  if (conf.count("random_seed"))
    rng.reset(new MT19937(conf["random_seed"].as<uint32_t>()));
  else
    rng.reset(new MT19937);

  string metric_name = UppercaseString(conf["mt_metric"].as<string>());
  if (metric_name == "COMBI") {
    cerr << "WARNING: 'combi' metric is no longer supported, switching to 'COMB:TER=-0.5;IBM_BLEU=0.5'\n";
    metric_name = "COMB:TER=-0.5;IBM_BLEU=0.5";
  } else if (metric_name == "BLEU") {
    cerr << "WARNING: 'BLEU' is ambiguous, assuming 'IBM_BLEU'\n";
    metric_name = "IBM_BLEU";
  }
  EvaluationMetric* metric = EvaluationMetric::Instance(metric_name);
  DocumentScorer ds(metric, conf["devset"].as<string>());
  cerr << "Loaded " << ds.size() << " references for scoring with " << metric_name << endl;
  kbests.resize(ds.size());
  double eta = 0.001;

  ReadFile ini_rf(conf["decoder_config"].as<string>());
  Decoder decoder(ini_rf.stream());

  vector<weight_t>& dense_weights = decoder.CurrentWeightVector();
  if (conf.count("weights")) {
    Weights::InitFromFile(conf["weights"].as<string>(), &dense_weights);
    Weights::InitSparseVector(dense_weights, &lambdas);
  }

  TrainingObserver observer(*metric, conf["k_best_size"].as<unsigned>());

  unsigned num = 200;
  for (unsigned iter = 1; iter < num; ++iter) {
    lambdas.init_vector(&dense_weights);
    unsigned sent_id = rng->next() * ds.size();
    cerr << "Learning from sentence id: " << sent_id << endl;
    observer.SetSegmentEvaluator(ds[sent_id]);
    decoder.SetId(sent_id);
    decoder.Decode(ds[sent_id]->src, &observer);
    if (observer.vi != observer.hi) {  // viterbi != hope
      SparseVector<double> grad = kbests[sent_id][observer.fi].fmap;
      grad -= kbests[sent_id][observer.hi].fmap;
      cerr << "GRAD: " << grad << endl;
      const SparseVector<double>& g = grad;
#if HAVE_CXX11 && (__GNUC_MINOR__ > 4 || __GNUC__ > 4)
      for (auto& gi : g) {
#else
      for (SparseVector<double>::const_iterator it = g.begin(); it != g.end(); ++it) {
        const pair<unsigned,double>& gi = *it;
#endif
        if (gi.second) {
          u[gi.first] += gi.second;
          G[gi.first] += gi.second * gi.second;
          lambdas.set_value(gi.first, 1.0);  // this is a dummy value to trigger recomputation
        }
      }
      for (SparseVector<double>::iterator it = lambdas.begin(); it != lambdas.end(); ++it) {
        const pair<unsigned,double>& xi = *it;
        double z = fabs(u[xi.first] / iter) - 0.0;
        double s = 1;
        if (u[xi.first] > 0) s = -1;
        if (z > 0 && G[xi.first]) {
          lambdas.set_value(xi.first, eta * s * z * iter / sqrt(G[xi.first]));
        } else {
          lambdas.set_value(xi.first, 0.0);
        }
      }
    }
  }
  cerr << "Optimization complete.\n";
  Weights::WriteToFile("-", dense_weights, true);
  return 0;
}


#include <functional>
#include <utility>
#include <list>

using std::list;
using std::pair;

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>

#include "eigenIncluder.hpp"
#include "algebraTrace.hpp"
#include "streamTrace.hpp"
#include "mongoWrite.hpp"
#include "instrument.hpp"
#include "acsConfig.hpp"
#include "algebra.hpp"
#include "common.hpp"
#include "mongo.hpp"

// #pragma GCC optimize ("O0")

/** \file
* This software uses specialised kalman filter classes to perform filtering.
* Using a classes such as KFState, KFMeas, etc, prevents duplication of code, and ensures that many edge cases are taken care of
* without the need for the developer to consider them explicitly.
*
* The basic workflow for using the filter is:
* create filter object,
* initialise the state transition matrix at the beginning of each epoch
* create a list of measurements (only adding entries for required states, using KFKeys to reference the state element)
* combining measurements that were in a list into a single matrix corresponding to the new state,
* and filtering - filtering internally saves the states for RTS code, using a single sequential file that has some headers added so that it can be traversed backwards
*
* The filter has some pre/post fit checks that remove measurements that are out of expected ranges,
* and there are functions provided for setting, resetting, and getting values, noises, and covariance values.
*
* KFKeys are used to identify states. They may have a type, SatStat value, string, and number associated with them, and can be set and read from filter objects as required.
*
* KFMeasEntrys are used for an individual measurement, before being combined into KFMeas objects that contain all of the measurements for a filter iteration.
*
* Internally, the data is stored in maps and Eigen matrices/vectors, but the accessors should be used rather than the vectors themselves to ensure that states have been initialised and are in the expected order.
*/

bool KFKey::operator ==(const KFKey& b) const
{
	if (str.compare(b.str)	!= 0)		return false;
	if (Sat					!= b.Sat)	return false;
	if (type				!= b.type)	return false;
	if (num					!= b.num)	return false;
	else								return true;
}

bool KFKey::operator <(const KFKey& b) const
{
	int strCompare = str.compare(b.str);

	if (strCompare < 0)		return true;
	if (strCompare > 0)		return false;

	if (type < b.type)		return true;
	if (type > b.type)		return false;

	if (Sat < b.Sat)		return true;
	if (Sat > b.Sat)		return false;

	if (num < b.num)		return true;
	else					return false;
}

/** Clears and initialises the state transition matrix to identity at the beginning of an epoch.
* Also clears any noise that was being added for the initialisation of a new state.
*/
void KFState::initFilterEpoch()
{
	ZAdditionMap.		clear();
	initNoiseMap.		clear();

	KFKey oneKey;
	oneKey.type = KF::ONE;

	for (auto& [key1, mapp]	: stateTransitionMap)
	{
		if (key1 == oneKey)
		{
			continue;
		}

		//remove initialisation elements for subsequent epochs
		mapp.erase(oneKey);
	}

	stateTransitionMap	[oneKey][oneKey]	= {1, 0};
}

/** Finds the position in the KF state vector of particular states.
*/
int KFState::getKFIndex(
	KFKey		key)		///< [in]	Key to search for in state
{
	auto index = kfIndexMap.find(key);
	if (index == kfIndexMap.end())
	{
		return -1;
	}
	return index->second;
}

/** Returns the value and variance of a state within the kalman filter object
*/
bool KFState::getKFValue(
	KFKey		key,			///< [in]	Key to search for in state
	double&		value,			///< [out]	Output value
	double*		variance)		///< [out]	Output variance
{
	auto a = kfIndexMap.find(key);
	if (a == kfIndexMap.end())
	{
//		std::cout << std::endl << "Warning: State not found in filter: " << key << std::endl;
		return false;
	}
	int index = a->second;
	if (index >= x.size())
	{
		return false;
	}
	value = x(index);

	if (variance)
	{
		*variance = P(index,index);
	}

	return true;
}

/** Returns the standard deviation of a state within the kalman filter object
*/
bool KFState::getKFSigma(
	KFKey		key,		///< [in]	Key to search for in state
	double&		sigma)		///< [out]	Output value
{
	auto a = kfIndexMap.find(key);
	if (a == kfIndexMap.end())
	{
		return false;
	}
	int index = a->second;
	if (index >= x.size())
	{
		return false;
	}

	sigma = sqrt(P(index,index));
	return true;
}

/** Sets the value of a state within the kalman filter object
*/
bool KFState::setKFValue(
	KFKey		key,		///< [in]	Key to search for in state
	double		value)		///< [in]	Input value
{
	auto a = kfIndexMap.find(key);
	if (a == kfIndexMap.end())
	{
		return false;
	}

	int index = a->second;
	if (index >= x.size())
	{
		return false;
	}

	x(index) = value;

	return true;
}

/** Sets the process noise of a state within the kalman filter object
*/
bool KFState::setKFNoise(
	KFKey		kfKey,		///< [in]	Key to search for in state
	double		value)		///< [in]	Input value
{
	procNoiseMap[kfKey] = value;

	return true;
}

/** Adds dynamics to a filter state by inserting off-diagonal, time dependent elements to transition matrix
*/
void KFState::setKFTransRate(
	KFKey			dest,			///< [in]	Key to search for in state to change in transition
	KFKey			source,			///< [in]	Key to search for in state as source
	double			value,			///< [in]	Input value
	InitialState	initialState)	///< [in]	Initial state for rate state.
{
	addKFState(source, initialState);

	stateTransitionMap[dest][source] = {value, 1};
}

/** Adds dynamics to a filter state by inserting off-diagonal, non-time dependent elements to transition matrix
*/
void KFState::setKFTrans(
	KFKey			dest,			///< [in]	Key to search for in state to change in transition
	KFKey			source,			///< [in]	Key to search for in state as source
	double			value,			///< [in]	Input value
	InitialState	initialState)	///< [in]	Initial state for rate state.
{
	addKFState(dest, initialState);

	auto& [oldValue, rate] = stateTransitionMap[dest][source];

	if (rate != 0)
	{
		std::cout << "ERROR: BAD TRANS SUM" << std::endl;
		return;
	}

	value += oldValue;

	stateTransitionMap[dest][source] = {value, 0};
}

/** Remove a state from a kalman filter object.
*/
void KFState::removeState(
	KFKey			kfKey)				///< [in]	Key to search for in state
{
	ZTransitionMap.			erase(kfKey);
	stateTransitionMap.		erase(kfKey);
	procNoiseMap.			erase(kfKey);
	gaussMarkovTauMap.		erase(kfKey);
	gaussMarkovMuMap.		erase(kfKey);
}

/** Tries to add a state to the filter object.
*  If it does not exist, it adds it to a list of states to be added.
*  Call consolidateKFState() to apply the list to the filter object
*/
void KFState::addKFState(
	KFKey			kfKey,			///< [in]	The key to add to the state
	InitialState	initialState)	///< [in]	The initial conditions to add to the state
{
	auto iter = stateTransitionMap.find(kfKey);
	if (iter != stateTransitionMap.end())
	{
		//is an existing state, just update values
		if (initialState.Q		!= 0)		{	procNoiseMap		[kfKey]	= initialState.Q;		}
		if (initialState.mu		!= 0)		{	gaussMarkovMuMap	[kfKey]	= initialState.mu;		}
		if (initialState.tau	!= 0)		{	gaussMarkovTauMap	[kfKey] = initialState.tau;		}

		return;
	}

	//this is a new state, add to the state transition matrix to create a new state.

	KFKey ONE = {KF::ONE};
	ZAdditionMap		[kfKey]			=  1;
	ZTransitionMap		[kfKey][kfKey]	=  1;
	stateTransitionMap	[kfKey][kfKey]	= {1,				0};
	stateTransitionMap	[kfKey][ONE]	= {initialState.x,	0};
	initNoiseMap		[kfKey]			=  initialState.P;		//todo aaron, check if these can be neglected for zero entries
	procNoiseMap		[kfKey]			=  initialState.Q;
	gaussMarkovTauMap	[kfKey]			=  initialState.tau;
	gaussMarkovMuMap	[kfKey]			=  initialState.mu;

	if (initialState.P == 0)
	{
		//will be an uninitialised variable, do a least squares solution
		lsqRequired = true;
	}
}

/** Tries to add a noise element.
*  If it does not exist, it adds it to a list of states to be added.
*  Call consolidateKFState() to apply the list to the filter object
*/
void KFState::addNoiseElement(
	ObsKey			obsKey,
	double			variance)
{
	noiseElementMap[obsKey]	= variance;
}

/** Add process noise and dynamics to filter object according to time gap.
 * This will also sort states according to their kfKey as a result of the way the state transition matrix is generated.
 */
void KFState::stateTransition(
	Trace&		trace,		///< [out]	Trace file for output
	GTime		newTime)	///< [in]	Time of update for process noise and dynamics (s)
{
	KFState& kfState = *this;

	double tgap = 0;
	if	( newTime	!= GTime::noTime()
		&&time		!= GTime::noTime())
	{
		tgap = newTime.time - time.time;
	}

	if	( newTime	!= GTime::noTime())
	{
		time = newTime;
	}

//	TestStack ts(__FUNCTION__);

	int newStateCount = stateTransitionMap.size();
	if (newStateCount == 0)
	{
		std::cout << "THIS IS WEIRD" << std::endl;
		return;
	}

	//Initialise and populate a state transition and Z transition matrix
	SparseMatrix<double>	F_z		= SparseMatrix<double>	(newStateCount, x.rows());
	SparseMatrix<double>	F		= SparseMatrix<double>	(newStateCount, x.rows());
// 	MatrixXd			F		= MatrixXd::Zero(newStateCount, x.rows());
	VectorXd			Z_plus	= VectorXd::Zero(newStateCount);

	//add transitions for any states (usually close to identity)
	int row = 0;
	map<KFKey, short int> newKFIndexMap;
	for (auto& [newStateKey, newStateMap] : stateTransitionMap)
	{
		newKFIndexMap[newStateKey] = row;

		for (auto& [sourceStateKey, values] : newStateMap)
		{
			int sourceIndex	= getKFIndex(sourceStateKey);

			if	( (sourceIndex < 0)
				||(sourceIndex >= F.cols()))
			{
				continue;
			}
			auto& [value, tExp] = values;

			double tau = -1;

			auto gmIter = gaussMarkovTauMap.find(sourceStateKey);
			if (gmIter != gaussMarkovTauMap.end())
			{
				auto& [dummy, sourceTau] = *gmIter;

				tau = sourceTau;
			}

			double scalar = 1;

			if (tau < 0)
			{
				//Random Walk model (special case for First Order Gauss Markov model when tau == inf)

				for (int i = 0; i < tExp; i++)
				{
					scalar *= tgap / (i+1);
				}

// 				F(row, sourceIndex) = value * scalar;
				F.insert(row, sourceIndex) = value * scalar;

				continue;
			}

			//First Order Gauss Markov model, Ref: Carpenter and Lee (2008) - A Stable Clock Error Model Using Coupled First- and Second-Order Gauss-Markov Processes - https://ntrs.nasa.gov/api/citations/20080044877/downloads/20080044877.pdf

			double tempTerm = 1;
			scalar = exp(-tgap/tau);

			for (int i = 0; i < tExp; i++)
			{
				scalar = tau * (tempTerm - scalar);	//recursive formula derived according to Ref: Carpenter and Lee (2008)
				tempTerm *= tgap / (i+1);
			}

			double transition = value * scalar;

// 			F(row, sourceIndex) = transition;
			F.insert(row, sourceIndex) = transition;

			//Add state transitions to ONE element, to allow for tiedown to average value mu
			//derived from integrating and distributing terms for v = (v0 - mu) * exp(-t/tau) + mu;
			//tempTerm calculated above appears to be same as required for these terms too, (at least for tExp = 0,1)

			auto muIter = gaussMarkovMuMap.find(sourceStateKey);
			if (muIter != gaussMarkovMuMap.end())
			{
				auto& [dummy2, mu] = *muIter;

// 				F(row, 0) = mu * (tempTerm - transition);
				F.insert(row, 0) = mu * (tempTerm - transition);
			}
		}

		row++;
	}

	for (auto& [kfKey1, map] : ZTransitionMap)
	for (auto& [kfKey2, value] : map)
	{
		int index2	= getKFIndex(kfKey2);

		if	( (index2 < 0)
			||(index2 >= F.cols()))
		{
			continue;
		}

		auto it = newKFIndexMap.find(kfKey1);	//todo aaron, this shold be ensured true elsewhere
		if (it == newKFIndexMap.end())
		{
			continue;
		}

		auto& [dummy, row] = *it;

// 		F_z(row, index2) = value;
		F_z.insert(row, index2) = value;
	}

	for (auto& [kfKey, value] : ZAdditionMap)
	{
		int row = newKFIndexMap[kfKey];

		Z_plus(row) = value;
	}

	//scale and add process noise
	MatrixXd Q0 = MatrixXd::Zero(newStateCount, newStateCount);
	tgap = fabs(tgap);

	//add noise as 'process noise' as the method of initialising a state's variance
	for (auto& [kfKey, value]	: initNoiseMap)
	{
		auto iter = newKFIndexMap.find(kfKey);
		if (iter == newKFIndexMap.end())
		{
			std::cout << kfKey << " broke" << std::endl;
			continue;
		}
		int index	= iter->second;

		if	( (index < 0)
			||(index >= Q0.rows()))
		{
			continue;
		}

		Q0(index, index) = value;
	}

	//add time dependent process noise
	if (tgap)
	for (auto& [dest,	map]	: stateTransitionMap)
	for (auto& [source,	vals]	: map)
	{
		auto& [val, tExp] = vals;

		auto initIter = initNoiseMap.find(dest);
		if (initIter != initNoiseMap.end())
		{
			//this was initialised this epoch
			double init = initIter->second;

			if (init == 0)
			{
				//this was initialised with no noise, do lsq, dont add process noise
				continue;
			}
		}

		auto destIter = newKFIndexMap.find(dest);
		if (destIter == newKFIndexMap.end())
		{
			std::cout << dest << " broke" << std::endl;
			continue;
		}

		int destIndex	= destIter->second;

		if	( (destIndex < 0)
			||(destIndex >= Q0.rows()))
		{
			continue;
		}

		auto sourceIter = newKFIndexMap.find(source);
		if (sourceIter == newKFIndexMap.end())
		{
			std::cout << dest << " broKe" << std::endl;
			continue;
		}

		int sourceIndex	= sourceIter->second;

		if	( (sourceIndex < 0)
			||(sourceIndex >= Q0.rows()))
		{
			continue;
		}

		auto iter2 = procNoiseMap.find(source);
		if (iter2 == procNoiseMap.end())
		{
// 			std::cout << dest << " brOke" << std::endl;
			continue;
		}

		auto [dummy, sourceProcessNoise] = *iter2;

		auto gmIter = gaussMarkovTauMap.find(source);
		if (gmIter != gaussMarkovTauMap.end())
		{
			auto& [dummy, tau] = *gmIter;

			if (tau < 0)
			{
				//Random Walk model (special case for First Order Gauss Markov model when tau == inf)

				if		(tExp == 0)	{	Q0(destIndex,	destIndex) += sourceProcessNoise / 1	* tgap;}
				else if	(tExp == 1)	{	Q0(destIndex,	destIndex) += sourceProcessNoise / 3	* tgap * tgap * tgap;
		// 								Q0(sourceIndex, destIndex) += sourceProcessNoise / 2	* tgap * tgap;
		// 								Q0(destIndex, sourceIndex) += sourceProcessNoise / 2	* tgap * tgap;
									}
				else if (tExp == 2)	{	Q0(destIndex,	destIndex) += sourceProcessNoise / 20	* tgap * tgap * tgap * tgap * tgap;}
			}
			else
			{
				//First Order Gauss Markov model, Ref: Carpenter and Lee (2008) - A Stable Clock Error Model Using Coupled First- and Second-Order Gauss-Markov Processes - https://ntrs.nasa.gov/api/citations/20080044877/downloads/20080044877.pdf

				if		(tExp == 0)	{	Q0(destIndex,	destIndex) += sourceProcessNoise / 2	* tau * (1 - exp(-2*tgap/tau));		}
				else if	(tExp == 1)	{	Q0(destIndex,	destIndex) += sourceProcessNoise / 2	* tau * tau * (	+ 2 * tgap 								//one tau from front tau3 distributed to prevent divide by zero
																												- 4 * tau * (1 - exp(-1*tgap/tau))
																												+ 1 * tau * (1 - exp(-2*tgap/tau)));	//correct formula re-derived according to Ref: Carpenter and Lee (2008)
		// 								Q0(sourceIndex, destIndex) += sourceProcessNoise / 2	* tau * tau * (1-exp(-tgap/tau)) * (1-exp(-tgap/tau));
		// 								Q0(destIndex, sourceIndex) += sourceProcessNoise / 2	* tau * tau * (1-exp(-tgap/tau)) * (1-exp(-tgap/tau));
									}
				else if (tExp == 2)	{	std::cout << "FOGM model is not applied to acceleration term at the moment" << std::endl;	}	//todo Eugene: add process noise for acceleration term
			}
		}
		else
		{
			std::cout << "Tau value not found in filter: " << source << std::endl;
			continue;
		}
	}

	//output the state transition matrix to a trace file (used by RTS smoother)
	if (rts_filename.empty() == false)
	{
		TransitionMatrixObject transitionMatrixObject;
		transitionMatrixObject.rows = F.rows();
		transitionMatrixObject.cols = F.cols();

		for (int k = 0; k < F.outerSize(); ++k)
		for (Eigen::SparseMatrix<double>::InnerIterator it(F, k); it; ++it)
		{
			double transition = it.value();

			if (transition == 0)
			{
				continue;
			}

			transitionMatrixObject.forwardTransitionMap[{it.row(), it.col()}] = transition;
		}

		spitFilterToFile(transitionMatrixObject,	E_SerialObject::TRANSITION_MATRIX,	rts_forward_filename);
	}

	//compute the updated states and permutation and covariance matrices
	if (F.rows() == F.cols())
	{
// 		Instrument	instrument("PPPalgebra0");
		dx = (F * x) - x;
	}

	{
// 		Instrument	instrument("PPPalgebra1");
		Z = (F_z	* Z	* F_z.transpose()			).eval();
	}
	{
// 		Instrument	instrument("PPPalgebra2");
		x = (F		* x								).eval();
	}
	{
// 		Instrument	instrument("PPPalgebra3");
		P = (F		* P * F.transpose()		+ Q0	).eval();
	}
	{
// 		Instrument	instrument("PPPalgebra4");
		Z += Z_plus.asDiagonal();
	}
// 	std::cout << "F_z" << std::endl << F_z << std::endl;
// 	std::cout << "F" << std::endl << F << std::endl;
// 	std::cout << "Z" << std::endl << Z << std::endl;

	//replace the index map with the updated version that corresponds to the updated state
	kfIndexMap = std::move(newKFIndexMap);

	initFilterEpoch();
}

/** Compare variances of measurements and pre-filtered states to detect unreasonable values
* Ref: Wang et al. (1997) - On Quality Control in Hydrographic GPS Surveying
* &  Wieser et al. (2004) - Failure Scenarios to be Considered with Kinematic High Precision Relative GNSS Positioning - http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.573.9628&rep=rep1&type=pdf
*/
void KFState::preFitSigmaCheck(
	Trace&		trace,			///< Trace to output to
	KFMeas&		kfMeas,			///< Measurements, noise, and design matrix
	KFKey&		badStateKey,	///< Key to the state that has worst ratio (only if worse than badMeasIndex)
	int&		badMeasIndex,	///< Index of the measurement that has the worst ratio
	double&		sumOfSqTestStat,	///< Sum of squared test statistics
	int			begX,			///< Index of first state element to process
	int			numX,			///< Number of states elements to process
	int			begH,			///< Index of first measurement to process
	int			numH)			///< Number of measurements to process
{
	auto		v = kfMeas.V.segment(begH, numH);
	auto		R = kfMeas.R.block(begH, begH, numH, numH);
	auto		H = kfMeas.A.block(begH, begX, numH, numX);
	auto		P = this-> P.block(begX, begX, numX, numX);

	ArrayXd		measRatios	= ArrayXd::Zero(numH);
	ArrayXd		stateRatios	= ArrayXd::Zero(numX);

	if (sigma_check)
	{
		//use 'array' for component-wise calculations
		auto		measVariations	= v.array().square();	//delta squared
		auto		measVariances	= (R.diagonal() + (H * P * H.transpose()).diagonal()).array();

		measRatios	= measVariations	/ measVariances;

		trace << std::endl << "DOING PRE SIGMA CHECK: ";
	}
	else if (w_test)
	{
		MatrixXd	Qinv	= (R + H * P * H.transpose()).inverse();
		MatrixXd	H_Qinv	= H.transpose() * Qinv;

		//use 'array' for component-wise calculations
		auto		measVariations	= (Qinv * v)	.array().square();
		auto		stateVariations	= (H_Qinv * v)	.array().square();

		auto		measVariances	=  Qinv			.diagonal().array();
		auto		stateVariances	= (H_Qinv * H)	.diagonal().array();

		measRatios	= measVariations	/ measVariances;
		stateRatios	= stateVariations	/ stateVariances;
		stateRatios	= stateRatios.isFinite().select(stateRatios, 0);	//set ratio to 0 if corresponding variance is 0, e.g. ONE state, clk rate states

		trace << std::endl << "DOING W-TEST: ";
	}

	sumOfSqTestStat	= measRatios.sum() + stateRatios.sum();

	//if any are outside the expected value, flag an error

	Eigen::ArrayXd::Index stateIndex;
	Eigen::ArrayXd::Index measIndex;

	double maxStateRatio	= stateRatios	.maxCoeff(&stateIndex);
	double maxMeasRatio		= measRatios	.maxCoeff(&measIndex);

	//if any are outside the expected values, flag an error
	if	( maxStateRatio > maxMeasRatio
		&&maxStateRatio > SQR(sigma_threshold))
	{
		int chunkIndex = stateIndex + begX;

		auto it = kfIndexMap.begin();
		std::advance(it, stateIndex);

		auto& [key, dummy] = *it;

		trace << std::endl << "LARGE STATE ERROR OF " << maxStateRatio	<< " AT " << chunkIndex << " : " << key;

		badStateKey = key;
	}

	if	(maxMeasRatio > SQR(sigma_threshold))
	{
		int chunkIndex = measIndex + begH;

		trace << std::endl << "LARGE MEAS  ERROR OF " << maxMeasRatio	<< " AT " << chunkIndex << " : " << kfMeas.obsKeys[chunkIndex];

		badMeasIndex = measIndex + begH;
	}
}

/** Compare variances of measurements and filtered states to detect unreasonable values
*/
void KFState::postFitSigmaChecks(
	Trace&		trace,      	///< Trace file to output to
	KFMeas&		kfMeas,			///< Measurements, noise, and design matrix
	VectorXd&	xp,         	///< The post-filter state vector to compare with measurements
	VectorXd&	dx,				///< The innovations from filtering to recalculate the deltas.
	int			iteration,		///< Number of iterations prior to this check
	KFKey&		badStateKey,	///< Key to the state that has worst ratio (only if worse than badMeasIndex)
	int&		badMeasIndex,	///< Index of the measurement that has the worst ratio
	double&		sumOfSqTestStat,	///< Sum of squared test statistics
	int			begX,			///< Index of first state element to process
	int			numX,			///< Number of state elements to process
	int			begH,			///< Index of first measurement to process
	int			numH)			///< Number of measurements to process
{
	auto		H = kfMeas.A.block(begH, begX, numH, numX);
	VectorXd	vv = kfMeas.V.segment(begH, numH) - H * dx.segment(begX, numX);

	//use 'array' for component-wise calculations
	auto		measVariations		= vv					.array().square();	//delta squared
	auto		stateVariations		= dx.segment(begX, numX).array().square();

	auto		measVariances		= kfMeas.	R.block(begH, begH, numH, numH).diagonal().array();
	auto		stateVariances		= 			P.block(begX, begX, numX, numX).diagonal().array();

	auto		measRatios			= measVariations	/ measVariances;
	ArrayXd		stateRatios			= stateVariations	/ stateVariances;
				stateRatios			= stateRatios.isFinite().select(stateRatios, 0);

	if (output_residuals)
	{
		trace << std::endl << "+ Residuals" << std::endl;
		tracepdeex(2, trace, "#\t%2s\t%19s\t%8s\t%3s\t%7s\t%13s\t%13s\t%16s\n", "It", "Time", "Str", "Sat", "Type", "Prefit Res", "Postfit Res", "Meas Variance");

		for (int i = begH; i < begH + numH; i++)
		{
			tracepdeex(2, trace, "%%\t%2d\t%19s\t%20s\t%13.4f\t%13.4f\t%16.9f\n", iteration, time.to_string(0).c_str(), ((string)kfMeas.obsKeys[i]).c_str(), kfMeas.V(i), vv(i - begH), kfMeas.R(i,i));
		}
		trace << "- Residuals" << std::endl;
	}

#	ifdef ENABLE_MONGODB
	if (acsConfig.output_mongo_measurements)
	{
		mongoMeasResiduals(kfMeas.obsKeys, kfMeas.V, vv, kfMeas.R, begH, numH);
	}
#	endif

	trace << std::endl << "DOING SIGMACHECK: ";

	sumOfSqTestStat	= measRatios.sum() + stateRatios.sum();

	//if any are outside the expected values, flag an error

	Eigen::ArrayXd::Index stateIndex;
	Eigen::ArrayXd::Index measIndex;

	double maxStateRatio	= stateRatios	.maxCoeff(&stateIndex);
	double maxMeasRatio		= measRatios	.maxCoeff(&measIndex);

	//if any are outside the expected values, flag an error
	if	( maxStateRatio > maxMeasRatio
		&&maxStateRatio > SQR(sigma_threshold))
	{
		int chunkIndex = stateIndex + begX;

		auto it = kfIndexMap.begin();
		std::advance(it, stateIndex);

		auto& [key, dummy] = *it;

		trace << std::endl << "LARGE STATE ERROR OF " << maxStateRatio	<< " AT " << chunkIndex << " : " << key;

		badStateKey = key;
	}

	if	(maxMeasRatio > SQR(sigma_threshold))
	{
		int chunkIndex = measIndex + begH;

		trace << std::endl << "LARGE MEAS  ERROR OF " << maxMeasRatio	<< " AT " << chunkIndex << " : " << kfMeas.obsKeys[chunkIndex];

		badMeasIndex = measIndex + begH;
	}
}

/** Compute Chi-square increment based on the change of fitting solution
*/
double KFState::stateChiSquare(
	Trace&		trace,      ///< Trace file to output to
	MatrixXd&	Pp,   		///< Post-update covariance of states
	VectorXd&	dx,			///< The innovations from filtering to recalculate the deltas.
	int			begX,		///< Index of first state element to process
	int			numX,		///< Number of states elements to process
	int			begH,		///< Index of first measurement to process
	int			numH)		///< Number of measurements to process
{
	if (begX == 0)	//exclude the One state
	{
		begX  = 1;
		numX -= 1;
	}

	auto		w  = dx.segment(begX, numX);
	MatrixXd	P  = this->P.block(begX, begX, numX, numX);
	// MatrixXd	dP = this->P.block(begX, begX, numX, numX) - Pp.block(begX, begX, numX, numX);	//Ref: Li et al. (2020) - Robust Kalman Filtering Based on Chi-square Increment and Its Application - https://www.mdpi.com/2072-4292/12/4/732/pdf

	double		chiSq = w.transpose() * P.inverse() * w;
	// double		chiSq = w.transpose() * dP.inverse() * w;	//Ref: Li et al. (2020) - Robust Kalman Filtering Based on Chi-square Increment and Its Application - https://www.mdpi.com/2072-4292/12/4/732/pdf
																//numerical instability problem exists for dP.inverse()

	trace << std::endl << "DOING STATE CHI-SQUARE TEST:";
	// for (int i = 0; i < numX; i++)	trace << "dx: " 	<< w(i) << "\tdP: "	<< dP(i, i) << std::endl;

	return chiSq;
}

/** Compute Chi-square increment based on post-fit residuals
*/
double KFState::measChiSquare(
	Trace&		trace,      ///< Trace file to output to
	KFMeas&		kfMeas,		///< Measurements, noise, and design matrix
	VectorXd&	dx,			///< The innovations from filtering to recalculate the deltas.
	int			begX,		///< Index of first state element to process
	int			numX,		///< Number of states elements to process
	int			begH,		///< Index of first measurement to process
	int			numH)		///< Number of measurements to process
{
	auto		w = dx.segment(begX, numX);
	auto		H = kfMeas.A.block(begH, begX, numH, numX);
	VectorXd	v = kfMeas.V.segment(begH, numH) - H * w;
	auto		R = kfMeas.R.block(begH, begH, numH, numH);

	double		chiSq = (v.array().square() / R.diagonal().array()).sum();

	trace << std::endl << "DOING MEASUREMENT CHI-SQUARE TEST:";
	// for (int i = 0; i < numH; i++)	trace << "v(+): "	<< v(i) << "\tR: "		<< R(i, i) << std::endl;

	return chiSq;
}

/** Compute Chi-square increment based on pre-fit residuals (innovations)
*/
double KFState::innovChiSquare(
	Trace&		trace,		///< Trace to output to
	KFMeas&		kfMeas,		///< Measurements, noise, and design matrix
	int			begX,		///< Index of first state element to process
	int			numX,		///< Number of states elements to process
	int			begH,		///< Index of first measurement to process
	int			numH)		///< Number of measurements to process
{
	auto		H = kfMeas.A.block(begH, begX, numH, numX);
	auto		v = kfMeas.V.segment(begH, numH);
	auto		R = kfMeas.R.block(begH, begH, numH, numH);
	auto		P = this->P.block(begX, begX, numX, numX);
	MatrixXd	Q = R + H * P * H.transpose();

	double		chiSq = v.transpose() * Q.inverse() * v;

	trace << std::endl << "DOING INNOVATION CHI-SQUARE TEST:";
	// for (int i = 0; i < numH; i++)	trace << "v(-): "	<< v(i) << "\tS: "		<< Q(i, i) << std::endl;

	return chiSq;
}

/** Kalman filter.
*/
int KFState::kFilter(
	Trace&			trace,		///< Trace to output to
	KFMeas&			kfMeas,		///< Measurements, noise, and design matrices
	VectorXd&		xp,   		///< Post-update state vector
	MatrixXd&		Pp,   		///< Post-update covariance of states
	VectorXd&		dx,			///< Post-update state innovation
	int				begX,		///< Index of first state element to process
	int				numX,		///< Number of state elements to process
	int				begH,		///< Index of first measurement to process
	int				numH)		///< Number of measurements to process
{
	auto& H = kfMeas.A;
	auto& R = kfMeas.R;
	auto& v = kfMeas.V;

	auto subH = H.block(begH, begX, numH, numX);

	MatrixXd HP	= subH	* P.block(begX, begX, numX, numX);
	MatrixXd Q	= HP	* subH.transpose();

	Q += R.block(begH, begH, numH, numH);

	MatrixXd K;

	bool repeat = true;
	while (repeat)
	{
		switch (inverter)
		{
			default:
			case E_Inverter::LDLT:
			{
				auto QQ = Q.triangularView<Eigen::Upper>().adjoint();
				LDLT<MatrixXd> solver;
				solver.compute(QQ);
				if (solver.info() != Eigen::ComputationInfo::Success)
				{
					xp = x;
					Pp = P;
					dx = VectorXd::Zero(xp.rows());

					return 1;
				}

				auto Kt = solver.solve(HP);
				if (solver.info() != Eigen::ComputationInfo::Success)
				{
					tracepdeex(1, trace, "Warning: kalman filter error2\n");
					xp = x;
					Pp = P;
					dx = VectorXd::Zero(xp.rows());

					return 1;
				}

				K = Kt.transpose();

				break;
			}
			case E_Inverter::LLT:
			{
				auto QQ = Q.triangularView<Eigen::Upper>().adjoint();
				LLT<MatrixXd> solver;
				solver.compute(QQ);
				if (solver.info() != Eigen::ComputationInfo::Success)
				{
					inverter = E_Inverter::LDLT;
					continue;
				}

				auto Kt = solver.solve(HP);
				if (solver.info() != Eigen::ComputationInfo::Success)
				{
					inverter = E_Inverter::LDLT;
					continue;
				}

				K = Kt.transpose();

				break;
			}
			case E_Inverter::INV:
			{
				MatrixXd Qinv = Q.inverse();
				K = P * H.transpose() * Qinv;

				break;
			}
		}
		repeat = false;
	}

	dx.segment(begX, numX)	= K * v.segment(begH, numH);
	xp.segment(begX, numX)	= x. segment(begX, numX)
							+ dx.segment(begX, numX);

// 	trace << std::endl << "h "	<< std::endl << subH;
// 	trace << std::endl << "hp "	<< std::endl << HP;
// 	trace << std::endl << "Q "	<< std::endl << Q;
// 	trace << std::endl << "K "	<< std::endl << K;
// 	trace << std::endl << "X "	<< std::endl << x. segment(begX, numX);
// 	trace << std::endl << "DX"	<< std::endl << dx.segment(begX, numX);
// 	trace << std::endl << "xp"	<< std::endl << xp.segment(begX, numX);

// 	if (acsConfig.joseph_stabilisation)
// 	{
// 		MatrixXd IKH = MatrixXd::Identity(P.rows(), P.cols()) - K * H;
// 		Pp = IKH * P * IKH.transpose() + K * R * K.transpose();
// 	}
// 	else
	{
		Pp.block(begX, begX, numX, numX) = P.block(begX, begX, numX, numX) - K * HP;

		Pp.block(begX, begX, numX, numX) = (	  Pp.block(begX, begX, numX, numX)
												+ Pp.block(begX, begX, numX, numX).transpose()	).eval() / 2;
	}

	bool error = xp.segment(begX, numX).array().isNaN().any();
	if (error)
	{
		std::cout << std::endl << "xp:" << std::endl << xp << std::endl;
		std::cout << std::endl << "R :" << std::endl << R << std::endl;
		std::cout << std::endl << "v :" << std::endl << v << std::endl;
		std::cout << std::endl << "K :" << std::endl << K << std::endl;
		std::cout << std::endl << "P :" << std::endl << P << std::endl;
		std::cout << std::endl;
		std::cout << "NAN found. Exiting...";
		std::cout << std::endl;

		exit(0);
	}

	bool pass = true;
	return pass;
}

/** Perform chi squared quality control.
*/
bool KFState::chiQC(
	Trace&		trace,      ///< Trace to output to
	KFMeas&		kfMeas,		///< Measurements, noise, and design matrix
	VectorXd&	xp)         ///< Post filtered state vector
{
	auto& H = kfMeas.A;
	auto W = kfMeas.W(all, 0);

	VectorXd&	y		= kfMeas.Y;
	VectorXd	v		= y - H * xp;
	double		v_Wv	= v.transpose() * W.asDiagonal() * v;

	//trace << std::endl << "chiqcV" << v.rows() << std::endl;	tracematpde(5, trace, v, 15, 5);

	int obsNumber = v.rows() - x.rows() + 1;
	double val = v_Wv / (obsNumber);

	double thres;
	if (obsNumber <= 100) 	thres = chisqr_arr[obsNumber - 1] / (obsNumber);
	else					thres = 3;

	/* chi-square validation */
	if (val > thres)
	{
		tracepdeex(6, trace, " ChiSquare error detected");

// 		auto variations	= v.array().square();
// 		Eigen::MatrixXf::Index index;
// 		trace << " -> LARGEe ERROR OF " << sqrt(variations.maxCoeff(&index)) << " AT " << index;

		return false;
	}

	return true;
}

/** Combine a list of KFMeasEntrys into a single KFMeas object for used in the filter
*/
KFMeas KFState::combineKFMeasList(
	KFMeasEntryList&	kfEntryList,	///< List of input measurements as lists of entries
	GTime				measTime)		///< Time to use for measurements and hence state transitions
{
	int numMeas = kfEntryList.size();

	KFMeas kfMeas;

	kfMeas.time = measTime;

	kfMeas.V.resize(numMeas);
	kfMeas.Y.resize(numMeas);

	kfMeas.R = MatrixXd::Zero(numMeas, numMeas);
	kfMeas.A = MatrixXd::Zero(numMeas, x.rows());

	kfMeas.obsKeys		.resize(numMeas);
	kfMeas.metaDataMaps	.resize(numMeas);

	int meas = 0;
	for (auto& entry: kfEntryList)
	{
		kfMeas.R(meas, meas)	= entry.noise;
		kfMeas.Y(meas)			= entry.value;
		kfMeas.V(meas)			= entry.innov;

		for (auto& [kfKey, value] : entry.designEntryMap)
		{
			int index = getKFIndex(kfKey);
			if (index < 0)
			{
				std::cout << "hhuh?" << kfKey << std::endl;
				return KFMeas();
			}
			kfMeas.A(meas, index) = value;
		}

		kfMeas.obsKeys		[meas] = std::move(entry.obsKey);
		kfMeas.metaDataMaps	[meas] = std::move(entry.metaDataMap);
		meas++;
	}

	if (noiseElementMap.empty() == false)
	{
		VectorXd uncorrelatedNoise = VectorXd::Zero(noiseElementMap.size());

		map<ObsKey, int>	noiseIndexMap;
		int noises = 0;
		for (auto& [obsKey, variance] : noiseElementMap)
		{
			uncorrelatedNoise(noises) = variance;

			noiseIndexMap[obsKey] = noises;
			noises++;
		}

		SparseMatrix<double> R_A = SparseMatrix<double>(numMeas, noiseElementMap.size());

		meas = 0;
		for (auto& entry: kfEntryList)
		{
			for (auto& [obsKey, value] : entry.noiseEntryMap)
			{
				int noiseIndex = noiseIndexMap[obsKey];

				R_A.insert(meas, noiseIndex) = value;
			}

			meas++;
		}

		Instrument instrument("PPPnoisELment");
// 		std::cout << R_A << std::endl;
		kfMeas.R = R_A * uncorrelatedNoise.asDiagonal() * R_A.transpose();

// 		std::cout << std::setprecision(5);
// 		std::cout << "R" << std::endl << kfMeas.R << std::endl;
	}

	return kfMeas;
}

bool KFState::doStateRejectCallbacks(
	Trace&		trace,				///< Trace file for output
	KFMeas&		kfMeas,				///< Measurements that were passed to the filter
	KFKey&		badKey)				///< Index in measurement list that was unsatisfactory
{
	for (auto& callback : stateRejectCallbacks)
	{
		bool keepGoing = callback(trace, *this, kfMeas, badKey);

		if (keepGoing == false)
		{
			return false;
		}
	}

	return true;
}

bool KFState::doMeasRejectCallbacks(
	Trace&		trace,				///< Trace file for output
	KFMeas&		kfMeas,				///< Measurements that were passed to the filter
	int			badIndex)			///< Index in measurement list that was unsatisfactory
{
	for (auto& callback : measRejectCallbacks)
	{
		bool keepGoing = callback(trace, *this, kfMeas, badIndex);

		if (keepGoing == false)
		{
			return false;
		}
	}

	return true;
}

bool ObsKey::operator ==(const ObsKey& b) const
{
	if (str.compare(b.str)	!= 0)		return false;
	if (Sat					!= b.Sat)	return false;
	if (type.compare(b.type)!= 0)		return false;
	if (num					!= b.num)	return false;
	else								return true;
}

bool ObsKey::operator <(const ObsKey& b) const
{
	int typeCompare = type.compare(b.type);
	if (typeCompare < 0)		return true;
	if (typeCompare > 0)		return false;

	int strCompare = str.compare(b.str);
	if (strCompare < 0)		return true;
	if (strCompare > 0)		return false;

	if (Sat < b.Sat)		return true;
	if (Sat > b.Sat)		return false;

	if (num < b.num)		return true;
	if (num > b.num)		return false;

	else					return false;
}

/** Kalman filter operation
*/
int KFState::filterKalman(
	Trace&				trace,					///< Trace file for output
	KFMeas&				kfMeas,					///< Measurement object
	bool				innovReady,				///< Innovation already constructed
	list<FilterChunk>*	filterChunkList_ptr)	///< Optional ist of chunks for parallel processing of sub filters
{
	KFState& kfState = *this;

	if (kfMeas.time != GTime::noTime())
	{
		kfState.time = kfMeas.time;
	}

	if (kfState.rts_filename.empty() == false)
	{
		spitFilterToFile(kfState, E_SerialObject::FILTER_MINUS, kfState.rts_forward_filename);
	}

	if (kfMeas.A.rows() == 0)
	{
		//nothing to be done, clean up and return early

		if (kfState.rts_filename.empty() == false)
		{
			spitFilterToFile(kfState, E_SerialObject::FILTER_PLUS, kfState.rts_forward_filename);
		}
		return 1;
	}

	/* kalman filter measurement update */
	if (innovReady == false)
	{
		kfMeas.V = kfMeas.Y - kfMeas.A * kfState.x;
	}

	list<FilterChunk> dummyFilterChunkList;
	if (filterChunkList_ptr == nullptr)
	{
		filterChunkList_ptr = &dummyFilterChunkList;
	}

	auto& filterChunkList = *filterChunkList_ptr;

	if (filterChunkList.empty())
	{
		FilterChunk filterChunk;
		filterChunk.trace_ptr = &trace;

		filterChunkList.push_back(filterChunk);
	}

	double	prefitSumOfSqTestStat	= 0;
	for (auto& filterChunk : filterChunkList)
	{
		if (filterChunk.numX < 0)	filterChunk.numX = x.rows();
		if (filterChunk.numH < 0)	filterChunk.numH = kfMeas.A.rows();

		double	chunkSumOfSqTestStat	= 0;
		for (int i = 0; i < max_prefit_remv; i++)
		{
			auto& chunkTrace = *filterChunk.trace_ptr;

			if	(  sigma_check	== false
				&& w_test		== false)
			{
				continue;
			}

			KFKey	badState;
			int		badMeasIndex = -1;

			kfState.preFitSigmaCheck(chunkTrace, kfMeas, badState, badMeasIndex, chunkSumOfSqTestStat, filterChunk.begX, filterChunk.numX, filterChunk.begH, filterChunk.numH);

			if (badState.type)		{	chunkTrace << std::endl << "Prefit check failed state test";		bool keepGoing = doStateRejectCallbacks	(chunkTrace, kfMeas, badState);			/*continue;*/	}	//always fallthrough
			if (badMeasIndex >= 0)	{	chunkTrace << std::endl << "Prefit check failed measurement test";	bool keepGoing = doMeasRejectCallbacks	(chunkTrace, kfMeas, badMeasIndex);		continue;		}	//retry next iteration
			else					{	chunkTrace << std::endl << "Prefit check passed";																									break;			}
		}

		prefitSumOfSqTestStat	+= chunkSumOfSqTestStat;
	}

	if	(  sigma_check
		|| w_test)
	{
		trace << std::endl << "Sum-of-squared test statistics (prefit): "	<< prefitSumOfSqTestStat	<< std::endl;
	}

	MatrixXd Pp = P;
	VectorXd xp = x;
			 dx = VectorXd::Zero(x.rows());

	double postfitSumOfSqTestStat = 0;
	for (auto& filterChunk : filterChunkList)
	{
		double chunkSumOfSqTestStat = 0;
		for (int i = 0; i < max_filter_iter; i++)
		{
			auto& chunkTrace = *filterChunk.trace_ptr;

			bool pass = kfState.kFilter(chunkTrace, kfMeas, xp, Pp, dx, filterChunk.begX, filterChunk.numX, filterChunk.begH, filterChunk.numH);

			if (pass == false)
			{
				chunkTrace << "FILTER FAILED" << std::endl;
				return 0;
			}

// 			chunkTrace << "\nFrom " << filterChunk.begH << " for " << filterChunk.numH;
// 			chunkTrace << "\nStat " << filterChunk.begX << " for " << filterChunk.numX;
	// 		outputStates(chunkTrace);

			if (sigma_check == false)
			{
				continue;
			}

			KFKey	badState;
			int		badMeasIndex = -1;

			kfState.postFitSigmaChecks(chunkTrace, kfMeas, xp, dx, i, badState, badMeasIndex, chunkSumOfSqTestStat, filterChunk.begX, filterChunk.numX, filterChunk.begH, filterChunk.numH);

			if (badState.type)		{	chunkTrace << std::endl << "Postfit check failed state test";		bool keepGoing = doStateRejectCallbacks	(chunkTrace, kfMeas, badState);			/*continue;*/	}	//always fallthrough
			if (badMeasIndex >= 0)	{	chunkTrace << std::endl << "Postfit check failed measurement test";	bool keepGoing = doMeasRejectCallbacks	(chunkTrace, kfMeas, badMeasIndex);		continue;		}	//retry next iteration
			else					{	chunkTrace << std::endl << "Postfit check passed";																									break;			}	//all ok, finish
		}

		postfitSumOfSqTestStat += chunkSumOfSqTestStat;
	}

	if (sigma_check)
		trace << std::endl << "Sum-of-squared test statistics (postfit): " << postfitSumOfSqTestStat << std::endl;

	double	chiSq		= 0;
	double	dof			= 0;
	double	chiSqPerDof	= 0;
	double	qc			= 0;
	if (chi_square_test)
	{
		for (auto& filterChunk : filterChunkList)
		{
			auto& chunkTrace = *filterChunk.trace_ptr;

			switch (chi_square_mode)
			{
				case E_ChiSqMode::INNOVATION:	{	chiSq += kfState.innovChiSquare(chunkTrace, kfMeas,     filterChunk.begX, filterChunk.numX, filterChunk.begH, filterChunk.numH);	break;	}
				case E_ChiSqMode::MEASUREMENT:	{	chiSq += kfState.measChiSquare( chunkTrace, kfMeas, dx, filterChunk.begX, filterChunk.numX, filterChunk.begH, filterChunk.numH);	break;	}
				case E_ChiSqMode::STATE:		{	chiSq += kfState.stateChiSquare(chunkTrace, Pp,     dx, filterChunk.begX, filterChunk.numX, filterChunk.begH, filterChunk.numH);	break;	}
				default:							break;
			}
		}

		trace << std::endl << "Number of measurements:" << kfMeas.A.rows() << "\tNumber of states:" << kfState.x.rows() - 1;

		if (chi_square_mode == +E_ChiSqMode::STATE)	dof	= kfState.x.rows() - 1;
		else										dof	= kfMeas. A.rows();

		chiSqPerDof	= chiSq / dof;

		// check against threshold
		boost::math::normal normDist;
		double	alpha = cdf(complement(normDist, sigma_threshold)) * 2;	//two-tailed

		boost::math::chi_squared chiSqDist(dof);
		qc = quantile(complement(chiSqDist, alpha));
		if (chiSq <= qc)	trace << std::endl << "Chi-square test passed";
		else				trace << std::endl << "Chi-square test failed";

		trace << std::endl << "Chi-square increment: " << chiSq << "\tThreshold: " << qc << "\tDegree of freedom: " << dof << "\tChi-square per DOF: " << chiSqPerDof << std::endl;
	}

#	ifdef ENABLE_MONGODB
	if (acsConfig.output_mongo_test_stats)
	{
		mongoTestStat(kfState, prefitSumOfSqTestStat, postfitSumOfSqTestStat, chiSq, qc, dof, chiSqPerDof);
	}
#	endif

// 	if (pass)
	{
		kfState.x = std::move(xp);
		kfState.P = std::move(Pp);
	}

	if (kfState.rts_filename.empty() == false)
	{
		spitFilterToFile(kfState, E_SerialObject::FILTER_PLUS, kfState.rts_forward_filename);
	}

	initFilterEpoch();
	return 1;
}

/** Perform least squares using the measurements in the KFMeas object
*/
int KFState::filterLeastSquares(
	Trace&			trace,		///< [in]		Trace to output to
	KFMeas&			kfMeas)		///< [in]		Measurements, noise, and design matrix
{
	KFState& kfState = *this;

	trace << std::endl << " -------STARTING LS --------" << std::endl;

	//invert measurement noise matrix to get a weight matrix
	kfMeas.W = (1 / kfMeas.R.diagonal().array()).matrix();		//todo, aaron straight inversion?

	if (kfMeas.R.rows() < kfState.x.rows())
	{
		trace << std::endl << "INSUFFICIENT MEASUREMENTS FOR LEAST SQUARES " << kfMeas.R.rows() <<  " " << kfState.x.rows();
		return 0;
	}

	auto& H = kfMeas.A;
	auto& W = kfMeas.W;
	auto& Y = kfMeas.Y;

	//calculate least squares solution
	MatrixXd H_W	= H.transpose() * W.asDiagonal();
	MatrixXd Q		= H_W * H;

	trace << std::endl << " -------DOING LS --------"<< std::endl;

	MatrixXd Qinv = Q.inverse();
	VectorXd x1 = Qinv * H_W * Y;

	bool error = x1.array().isNaN().any();
	if (error)
	{
		std::cout << "NAN found. Exiting...";
		std::cout	<< std::endl
		<< x1		<< std::endl
		<< Qinv		<< std::endl
		<< Q		<< std::endl;

		exit(0);
	}

// 	std::cout << std::endl << "postLSQ" << std::endl;

	chiQCPass = kfState.chiQC(trace, kfMeas, x1);
	if (chiQCPass == false)
	{
// 		return 0;	//todo aaron
	}

	//copy results to outputs
	for (int i = 0; i < kfState.x.rows(); i++)
	{
		if (kfState.P(i,i) == 0)
		{
			kfState.P(i,i)	= Qinv(i,i);
			kfState.x(i)	= x1(i);
		}
	}

	trace << std::endl << " -------STOPPING LS --------"<< std::endl;
	return 1;
}

//todo aaron, remove tgap, add time to filter

/** Least squares estimator for new kalman filter states.
* If new states have been added that do not contain variance values, the filter will assume that these states values and covariances should be
* estimated using least squares.
*
* This function will extract the minimum required states from the existing state vector,
* and the minimum required measurements in order to perform least squares for the uninitialised states.
*/
void KFState::leastSquareInitStates(
	Trace&			trace,				///< [in]		Trace file for output
	KFMeas&			kfMeas,				///< [in]		Measurement object
	bool			initCovars,			///< [in]		Option to also initialise off-diagonal covariance values
	VectorXd*		dx,					///< [out]		Optional output of state deltas
	bool			innovReady)			///< [in]		Perform conversion between V & Y
{
	chiQCPass = false;

	if (innovReady)
	{
		kfMeas.Y = kfMeas.V;
	}

	vector<int> newStateIndicies;

	//find all the states that aren't initialised, they need least squaring.
	for (auto& [key, i] : kfIndexMap)
	{
		if	( (key.type != KF::ONE)
			&&(P(i,i) == 0))
		{
			//this is a new state and needs to be evaluated using least squares
			newStateIndicies.push_back(i);
		}
	}

	//get the subset of the measurement matrix that applies to the uninitialised states
	auto subsetA = kfMeas.A(all, newStateIndicies);

	//find the subset of measurements that are required for the initialisation
	auto usedMeas = subsetA.rowwise().any();

	map<int, bool> pseudoMeasStates;
	vector<int> leastSquareMeasIndicies;

	for (int meas = 0; meas < usedMeas.rows(); meas++)
	{
		//if not used, dont worry about it
		if (usedMeas(meas) == 0)
		{
			continue;
		}

		//this measurement is used to calculate a new state.
		//copy it to a new design matrix
		leastSquareMeasIndicies.push_back(meas);

		//remember make a pseudo measurement of anything it references that is already set
		for (int state = 0; state < kfMeas.A.cols(); state++)
		{
			if	( (kfMeas.A(meas, state)	!= 0)
				&&(P(state,state)			!= 0))
			{
				pseudoMeasStates[state] = true;
			}
		}
	}

	int newMeasCount = leastSquareMeasIndicies.size() + pseudoMeasStates.size();

	//Create new measurement objects with larger size, (using all states for now)
	KFMeas	leastSquareMeas;

	leastSquareMeas.Y = VectorXd::Zero(newMeasCount);
	leastSquareMeas.R = MatrixXd::Zero(newMeasCount, newMeasCount);
	leastSquareMeas.A = MatrixXd::Zero(newMeasCount, kfMeas.A.cols());

	int measCount = leastSquareMeasIndicies.size();

	//copy in the required measurements from the old set
	leastSquareMeas.Y.head		(measCount)		= kfMeas.Y(leastSquareMeasIndicies);
	leastSquareMeas.R.topLeftCorner	(measCount, measCount)	= kfMeas.R(leastSquareMeasIndicies, leastSquareMeasIndicies);
	leastSquareMeas.A.topRows	(measCount)		= kfMeas.A(leastSquareMeasIndicies, all);

	//append any new pseudo measurements to the end
	for (auto& [state, boool] : pseudoMeasStates)
	{
		leastSquareMeas.Y(measCount)				= x(state);
		leastSquareMeas.R(measCount, measCount)		= P(state, state);
		leastSquareMeas.A(measCount, state)			= 1;
		measCount++;
	}

	//find the subset of states required for these measurements
	vector<int> usedCols;
	auto usedStates = leastSquareMeas.A.colwise().any();
	for (int i = 0; i < usedStates.cols(); i++)
	{
		if (usedStates(i) != 0)
		{
			usedCols.push_back(i);
		}
	}

	//create a new meaurement object using only the required states.
	KFMeas	leastSquareMeasSubs;
	leastSquareMeasSubs.Y = leastSquareMeas.Y;
	leastSquareMeasSubs.R = leastSquareMeas.R;
	leastSquareMeasSubs.A = leastSquareMeas.A(all, usedCols);

	//invert measurement noise matrix to get a weight matrix
	leastSquareMeasSubs.W = (1 / leastSquareMeasSubs.R.diagonal().array()).matrix();//todo aaron straight inversion?

	VectorXd w = (1 / leastSquareMeasSubs.R.diagonal().array()).matrix().col(0);	//todo aaron straight inversion?

	for (int i = 0; i < w.rows(); i++)
	{
		if (std::isinf(w(i)))
		{
			w(i) = 0;
		}
	}

	if	( leastSquareMeasSubs.A.cols() == 0
		||leastSquareMeasSubs.A.rows() == 0)
	{
		trace << std::endl << "EMPTY DESIGN MATRIX DURING LEAST SQUARES";
		return;
	}

	if (leastSquareMeasSubs.R.rows() < leastSquareMeasSubs.A.cols())
	{
		trace << std::endl << "INSUFFICIENT MEASUREMENTS FOR LEAST SQUARES " << leastSquareMeasSubs.R.rows() <<  " " << x.rows();
		return;
	}
	auto& H = leastSquareMeasSubs.A;
	auto& Y = leastSquareMeasSubs.Y;

	//calculate least squares solution
	MatrixXd W		= w.asDiagonal();
	MatrixXd H_W	= H.transpose() * W;
	MatrixXd Q		= H_W * H;

	MatrixXd Qinv	= Q.inverse();
	VectorXd x1		= Qinv * H_W * Y;

// 	std::cout << "Q : " << std::endl << Q;
	bool error = x1.array().isNaN().any();
	if (error)
	{
		std::cout << std::endl << "x1:" << std::endl << x1 << std::endl;
		std::cout << std::endl << "w :" << std::endl << w << std::endl;
		std::cout << std::endl << "H :" << std::endl << H << std::endl;
		std::cout << std::endl << "P :" << std::endl << P << std::endl;
		std::cout << std::endl;
		std::cout << "NAN found. Exiting....";
		std::cout	<< std::endl;

		exit(-1);
	}

// 	std::cout << std::endl << "postLSQ" << std::endl;
	chiQCPass = chiQC(trace, leastSquareMeasSubs, x1);
	if (chiQCPass == false)
	{
// 		return 1;	//todo aaron
	}

	if (dx)
	{
		(*dx) = x1;
	}

	for (int i = 0; i < usedCols.size(); i++)
	{
		int stateRowIndex = usedCols[i];

		if (P(stateRowIndex, stateRowIndex) != 0)
		{
			continue;
		}

		double newStateVal = x1(i);
		double newStateCov = Qinv(i,i);

		if (dx)
		{
			x(stateRowIndex)				+=	newStateVal;
			P(stateRowIndex,stateRowIndex)	=	newStateCov;
		}
		else
		{
			x(stateRowIndex)				= newStateVal;
			P(stateRowIndex,stateRowIndex)	= newStateCov;
		}

		if (initCovars)
		{
			for (int j = 0; j < i; j++)
			{
				int stateColIndex = usedCols[j];

				newStateCov = Qinv(i,j);

				P(stateRowIndex,stateColIndex)	= newStateCov;
				P(stateColIndex,stateRowIndex)	= newStateCov;
			}
		}
	}
}
void KFState::leastSquareInitStatesA(
	Trace&			trace,				///< [in]		Trace file for output
	KFMeas&			kfMeas,				///< [in]		Measurement object
	bool			initCovars,			///< [in]		Option to also initialise off-diagonal covariance values
	VectorXd*		dx,					///< [out]		Optional output of state deltas
	bool			innovReady)			///< [in]		Perform conversion between V & Y
{
	chiQCPass = false;

	if (innovReady)
	{
		kfMeas.Y = kfMeas.V;
	}

	vector<int> newStateIndicies;

	//find all the states that aren't initialised, they need least squaring.
	for (auto& [key, i] : kfIndexMap)
	{
		if	( (key.type != KF::ONE)
			&&(P(i,i) == 0))
		{
			//this is a new state and needs to be evaluated using least squares
			newStateIndicies.push_back(i);
		}
	}

	//get the subset of the measurement matrix that applies to the uninitialised states
	auto subsetA = kfMeas.A(all, newStateIndicies);

	//find the subset of measurements that are required for the initialisation
	auto usedMeas = subsetA.rowwise().any();

	map<int, bool> pseudoMeasStates;
	vector<int> leastSquareMeasIndicies;

	for (int meas = 0; meas < usedMeas.rows(); meas++)
	{
		//if not used, dont worry about it
		if (usedMeas(meas) == 0)
		{
			continue;
		}

		//this measurement is used to calculate a new state.
		//copy it to a new design matrix
		leastSquareMeasIndicies.push_back(meas);
	}

	//Create new measurement objects with larger size, (using all states for now)
	//copy in the required measurements from the old set
	KFMeas	leastSquareMeas;
	leastSquareMeas.Y	= kfMeas.Y(leastSquareMeasIndicies);
	leastSquareMeas.V	= kfMeas.V(leastSquareMeasIndicies);
	leastSquareMeas.R	= kfMeas.R(leastSquareMeasIndicies, leastSquareMeasIndicies);
	leastSquareMeas.A	= kfMeas.A(leastSquareMeasIndicies, all);

	//invert measurement noise matrix to get a weight matrix
	leastSquareMeas.W = (1 / leastSquareMeas.R.diagonal().array()).matrix();		//todo aaron straight inversion?

	VectorXd w = (1 / leastSquareMeas.R.diagonal().array()).matrix().col(0);	//todo aaron straight inversion?

	for (int i = 0; i < w.rows(); i++)
	{
		if (std::isinf(w(i)))
		{
			w(i) = 0;
		}
	}

	if (leastSquareMeas.R.rows() < leastSquareMeas.A.cols())
	{
		trace << std::endl << "INSUFFICIENT MEASUREMENTS FOR LEAST SQUARES " << leastSquareMeas.R.rows() <<  " " << x.rows();
		trace << std::endl << "Setting variances to large initial values ";;
		for (int i = 0; i < newStateIndicies.size(); i++)
		{
			int index = newStateIndicies[i];

			P(index, index) = SQR(10000);
		}

		return;
	}
	auto& H = leastSquareMeas.A;
	auto& Y = leastSquareMeas.Y;

	//calculate least squares solution
	MatrixXd W		= w.asDiagonal();
	MatrixXd H_W	= H.transpose() * W;
	MatrixXd Q		= H_W * H;

	MatrixXd Qinv	= Q.inverse();
	VectorXd x1		= Qinv * H_W * Y;

// 	std::cout << "Q : " << std::endl << Q;
	bool error = x1.array().isNaN().any();
	if (error)
	{
		std::cout << std::endl << "x1:" << std::endl << x1 << std::endl;
		std::cout << std::endl << "w :" << std::endl << w << std::endl;
		std::cout << std::endl << "H :" << std::endl << H << std::endl;
		std::cout << std::endl << "P :" << std::endl << P << std::endl;
		std::cout << std::endl;
		std::cout << "NAN found. Exiting....";
		std::cout	<< std::endl;

		exit(-1);
	}

// 	std::cout << std::endl << "postLSQ" << std::endl;
	chiQCPass = chiQC(trace, leastSquareMeas, x1);
	if (chiQCPass == false)
	{
// 		return 1;	//todo aaron
	}

	if (dx)
	{
		(*dx) = x1;
	}

	for (int i = 0; i < newStateIndicies.size(); i++)
	{
		int stateRowIndex = newStateIndicies[i];

		if (P(stateRowIndex, stateRowIndex) != 0)
		{
			continue;
		}

		double newStateVal = x1(i);
		double newStateCov = Qinv(i,i);

		if (dx)
		{
			x(stateRowIndex)				+=	newStateVal;
			P(stateRowIndex,stateRowIndex)	=	newStateCov;
		}
		else
		{
			x(stateRowIndex)				= newStateVal;
			P(stateRowIndex,stateRowIndex)	= newStateCov;
		}

		if (initCovars)
		{
			for (int j = 0; j < i; j++)
			{
				int stateColIndex = newStateIndicies[j];

				newStateCov = Qinv(i,j);

				P(stateRowIndex,stateColIndex)	= newStateCov;
				P(stateColIndex,stateRowIndex)	= newStateCov;
			}
		}
	}
}

/** Get a portion of the state vector by passing a list of keys
*/
VectorXd KFState::getSubState(
	map<KFKey, int>&	kfKeyMap,	///< List of keys to return within substate
	MatrixXd*			covarMat)	///< Optional pointer to a matrix for output of covariance submatrix
{
	vector<int> indices;
	indices.resize(kfKeyMap.size());

	for (auto& [kfKey, mapIndex] : kfKeyMap)
	{
		int stateIndex = getKFIndex(kfKey);
		if (stateIndex >= 0)
		{
			indices[mapIndex] = stateIndex;
		}
	}

	VectorXd subState = x(indices);
	if (covarMat)
	{
		*covarMat = P(indices, indices);
	}
	return subState;
}

/** Output keys and states in human readable format
*/
void KFState::outputStates(
		Trace&		trace,	///< Trace to output to
		int			begX,	///< Index of first state element to process
		int			numX)   ///< Number of state elements to process
{
	tracepdeex(2, trace, "\n\n");

	trace << std::endl << "+ States" << std::endl;

	tracepdeex(2, trace, "#\t%19s\t%20s\t%5s\t%3s\t%3s\t%13s\t%16s\t%10s\n", "Time", "Type", "Str", "Sat", "Num", "State", "Variance", "Adjust");

	int endX;
	if (numX < 0)	endX = x.rows();
	else			endX = begX + numX;

	for (auto& [key, index] : kfIndexMap)
	{
		if (index >= x.rows())
		{
			continue;
		}
		if	( index <  begX
			||index >= endX)
		{
			continue;
		}

		double _x	= x(index);
		double _dx = 0;
		if (index < dx.rows())
			_dx = dx(index);
		double _p	= P(index, index);
		string type	= KF::_from_integral(key.type)._to_string();

		tracepdeex(2, trace, "*\t%19s\t%20s\t%5s\t%3s\t%3d\t%13.4f\t%16.9f\t%10.3f\n", time.to_string(0).c_str(), type.c_str(), key.str.c_str(), key.Sat.id().c_str(), key.num, _x, _p, _dx);
	}
	trace << "- States" << std::endl;
}

void KFState::outputCorrelations(
			Trace&		trace)
{
	tracepdeex(2, trace, "\n\n");

	trace << std::endl << "+ Correlations" << std::endl;

	int skip = 0;
	for (auto& [key, index] : kfIndexMap)
	{
		if (key.type == KF::ONE)
		{
			continue;
		}

		tracepdeex(2, trace, "\n%28s", "");
		for (int i = 0; i < skip; i++)
		{
			tracepdeex(2, trace, "|    ");
		}

		trace << "> " << key;

		skip++;
	}

	for (auto& [key, index] : kfIndexMap)
	{
		if (key.type == KF::ONE)
		{
			continue;
		}

		trace << std::endl << key << "  ";

		for (auto& [key2, index2] : kfIndexMap)
		{
			if (key2.type == KF::ONE)
			{
				continue;
			}

			double v1	= P(index,	index);
			double v2	= P(index2, index2);
			double v12	= P(index,	index2);

			double correlation = v12 / sqrt(v1 * v2) * 100;

			if (index == index2)	tracepdeex(2, trace, "%4.0s ", "");
			else					tracepdeex(2, trace, "%4.0f ", correlation);

		}
	}
	trace << std::endl << "- Correlations" << std::endl;
}

InitialState initialStateFromConfig(
	KalmanModel&	kalmanModel,
	int				index)
{
	InitialState init = {};

	if (index < kalmanModel.apriori_val	.size())		init.x		= 		kalmanModel.apriori_val	[index];
	else												init.x		= 		kalmanModel.apriori_val	.back();
	if (index < kalmanModel.sigma		.size())		init.P		= SQR(	kalmanModel.sigma		[index]);
	else												init.P		= SQR(	kalmanModel.sigma		.back());
	if (index < kalmanModel.proc_noise	.size())		init.Q		= SQR(	kalmanModel.proc_noise	[index]);
	else												init.Q		= SQR(	kalmanModel.proc_noise	.back());
	if (index < kalmanModel.tau			.size())		init.tau	= 		kalmanModel.tau			[index];
	else												init.tau	= 		kalmanModel.tau			.back();
	if (index < kalmanModel.mu			.size())		init.mu		= 		kalmanModel.mu			[index];
	else												init.mu		= 		kalmanModel.mu			.back();

	return init;
}

KFState mergeFilters(
	list<KFState*>& kfStatePointerList,
	bool			includeTrop)
{
	map<KFKey, double>				stateValueMap;
	map<KFKey, map<KFKey, double>>	stateCovarMap;

	for (auto& statePointer : kfStatePointerList)
	{
		KFState& kfState = *statePointer;

		for (auto& [key1, index1] : kfState.kfIndexMap)
		{
			if	( key1.type != KF::REC_POS
				&&key1.type != KF::ONE
				&&key1.type != KF::TROP
				&&key1.type != KF::TROP_GM)
			{
				continue;
			}
			if	(  includeTrop == false
				&&(key1.type == KF::TROP
				|| key1.type == KF::TROP_GM))
			{
				continue;
			}

			stateValueMap[key1] = kfState.x(index1);

			for (auto& [key2, index2] : kfState.kfIndexMap)
			{
				if	( key2.type != KF::REC_POS
					&&key2.type != KF::TROP
					&&key2.type != KF::TROP_GM)
				{
					continue;
				}
				if	(  includeTrop == false
					&&(key2.type == KF::TROP
					|| key2.type == KF::TROP_GM))
				{
					continue;
				}

				if (kfState.P(index1, index2) != 0)
				{
					stateCovarMap[key1][key2] = kfState.P(index1, index2);
				}
			}
		}
	}

	KFState mergedKFState;

	mergedKFState.x		= VectorXd::Zero(stateValueMap.size());
	mergedKFState.dx	= VectorXd::Zero(stateValueMap.size());
	mergedKFState.P		= MatrixXd::Zero(stateValueMap.size(), stateValueMap.size());

	int i = 0;
	for (auto& [key, value] : stateValueMap)
	{
		mergedKFState.kfIndexMap[key]	= i;
		mergedKFState.x(i)				= value;

		i++;
	}

	for (auto& [key1, map2]		: stateCovarMap)
	for (auto& [key2, value]	: map2)
	{
		int index1 = mergedKFState.kfIndexMap[key1];
		int index2 = mergedKFState.kfIndexMap[key2];

		mergedKFState.P(index1, index2) = value;
	}

	return mergedKFState;
}

/** Calculates prefit residual, returning results as a KFMeasEntryList
* KFMeasEntryMap[ObsKey].value = prefit residual value
* KFMeasEntryMap[ObsKey].noise = prefit residual uncertainty
* KFMeasEntryMap[ObsKey].innov = kfMeas.V(i)
*/
KFMeasEntryList KFState::calcPrefitResids(
	Trace&			trace,				///< [out]	Trace file for output
	KFMeas&			kfMeas)				///< [in]	Measurement object
{
	KFState& kfState = *this;

	// Calculate prefit resid & uncertainty S
	Eigen::VectorXd PrefitResid = kfMeas.Y - kfMeas.A * kfState.x;
	Eigen::MatrixXd S = kfMeas.A * kfState.P * kfMeas.A.transpose() + Eigen::MatrixXd(kfMeas.R.asDiagonal());
	int numMeas = PrefitResid.size();
	//assert(kfMeas.R.size() == numMeas);
	assert(S.rows() == numMeas);
	assert(S.cols() == numMeas);
	assert(kfMeas.V.size() == numMeas);

	// Return result within a KFMeasEntryList
	KFMeasEntryList kfMeasEntryList;
	for (int meas=0; meas<numMeas; ++meas)
	{
		KFMeasEntry entry;
		entry.value = PrefitResid(meas);
		//entry.noise = kfMeas.R(meas);
		entry.noise = S(meas,meas);
		entry.innov = kfMeas.V(meas);
		entry.obsKey = kfMeas.obsKeys.at(meas);
		kfMeasEntryList.push_back(entry);
	}
	return kfMeasEntryList;
}


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

#define BOOST_TEST_MODULE pubkey_elgamal_verifiable_test

#undef NDEBUG

#include <string>
#include <type_traits>
#include <functional>

#include <boost/filesystem.hpp>
#include <filesystem>
#include <fstream>
#include <sstream>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/pubkey/algorithm/generate_keypair.hpp>
#include <nil/crypto3/pubkey/algorithm/encrypt.hpp>
#include <nil/crypto3/pubkey/algorithm/decrypt.hpp>
#include <nil/crypto3/pubkey/algorithm/verify_encryption.hpp>
#include <nil/crypto3/pubkey/algorithm/verify_decryption.hpp>
#include <nil/crypto3/pubkey/algorithm/rerandomize.hpp>

#include <nil/crypto3/pubkey/modes/verifiable_encryption.hpp>

#include <nil/crypto3/pubkey/elgamal_verifiable.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/jubjub.hpp>
#include <nil/crypto3/algebra/pairing/bls12.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>

#include <nil/crypto3/random/algebraic_random_device.hpp>
#include <nil/crypto3/random/algebraic_engine.hpp>

#include <nil/crypto3/zk/algorithms/generate.hpp>

#include <nil/crypto3/zk/snark/systems/ppzksnark/r1cs_gg_ppzksnark.hpp>

#include <nil/crypto3/zk/components/voting/encrypted_input_voting.hpp>

#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/primary_input.hpp>
#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/proof.hpp>
#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/verification_key.hpp>
#include <nil/crypto3/marshalling/pubkey/types/elgamal_verifiable.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::algebra;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::pubkey;
using namespace nil::crypto3::random;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp<FieldParams> &e) {
    std::cout << e.data << std::endl;
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp2<FieldParams> &e) {
    std::cout << e.data[0].data << ", " << e.data[1].data << std::endl;
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp3<FieldParams> &e) {
    std::cout << e.data[0].data << ", " << e.data[1].data << ", " << e.data[2].data << std::endl;
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const fields::detail::element_fp12_2over3over2<FieldParams> &e) {
    os << "[[[" << e.data[0].data[0].data[0].data << "," << e.data[0].data[0].data[1].data << "],["
       << e.data[0].data[1].data[0].data << "," << e.data[0].data[1].data[1].data << "],["
       << e.data[0].data[2].data[0].data << "," << e.data[0].data[2].data[1].data << "]],"
       << "[[" << e.data[1].data[0].data[0].data << "," << e.data[1].data[0].data[1].data << "],["
       << e.data[1].data[1].data[0].data << "," << e.data[1].data[1].data[1].data << "],["
       << e.data[1].data[2].data[0].data << "," << e.data[1].data[2].data[1].data << "]]]" << std::endl;
}

template<typename CurveParams, typename Form>
void print_curve_point(std::ostream &os,
                       const curves::detail::curve_element<CurveParams, Form, curves::coordinates::affine> &p) {
    os << "( X: [";
    print_field_element(os, p.X);
    os << "], Y: [";
    print_field_element(os, p.Y);
    os << "] )" << std::endl;
}

template<typename CurveParams, typename Form>
void print_curve_point(
    std::ostream &os,
    const curves::detail::curve_element<CurveParams, Form, curves::coordinates::jacobian_with_a4_0> &p) {
    os << "( X: [";
    print_field_element(os, p.X);
    os << "], Y: [";
    print_field_element(os, p.Y);
    os << "], Z: [";
    print_field_element(os, p.Z);
    os << "] )" << std::endl;
}

template<typename CurveParams, typename Form, typename Coordinates>
typename std::enable_if<std::is_same<Coordinates, curves::coordinates::projective>::value ||
                        std::is_same<Coordinates, curves::coordinates::jacobian_with_a4_0>::value>::type
    print_curve_point(std::ostream &os, const curves::detail::curve_element<CurveParams, Form, Coordinates> &p) {
    os << "( X: [";
    print_field_element(os, p.X);
    os << "], Y: [";
    print_field_element(os, p.Y);
    os << "], Z:[";
    print_field_element(os, p.Z);
    os << "] )" << std::endl;
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<typename CurveParams, typename Form, typename Coordinates>
            struct print_log_value<curves::detail::curve_element<CurveParams, Form, Coordinates>> {
                void operator()(std::ostream &os,
                                curves::detail::curve_element<CurveParams, Form, Coordinates> const &p) {
                    print_curve_point(os, p);
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp<FieldParams>> {
                void operator()(std::ostream &os, typename fields::detail::element_fp<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp2<FieldParams>> {
                void operator()(std::ostream &os, typename fields::detail::element_fp2<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp12_2over3over2<FieldParams>> {
                void operator()(std::ostream &os,
                                typename fields::detail::element_fp12_2over3over2<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

template<typename ValueType, std::size_t N>
typename std::enable_if<std::is_unsigned<ValueType>::value, std::vector<std::array<ValueType, N>>>::type
    generate_random_data(std::size_t leaf_number) {
    std::vector<std::array<ValueType, N>> v;
    for (std::size_t i = 0; i < leaf_number; ++i) {
        std::array<ValueType, N> leaf;
        std::generate(std::begin(leaf), std::end(leaf),
                      [&]() { return std::rand() % (std::numeric_limits<ValueType>::max() + 1); });
        v.emplace_back(leaf);
    }
    return v;
}

template<typename VerificationKey, typename PublicKey, typename Proof, typename PInput, typename CipherText>
struct marshalling_verification_data_groth16_encrypted_input {
    using endianness = nil::marshalling::option::big_endian;
    using proof_marshalling_type =
        nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_proof<nil::marshalling::field_type<endianness>, Proof>;
    using verification_key_marshalling_type =
        nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_extended_verification_key<
            nil::marshalling::field_type<endianness>, VerificationKey>;
    using public_key_marshalling_type =
        nil::crypto3::marshalling::types::elgamal_verifiable_public_key<nil::marshalling::field_type<endianness>,
                                                                        PublicKey>;
    using ct_marshalling_type = nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_encrypted_primary_input<
        nil::marshalling::field_type<endianness>, CipherText>;
    using pinput_marshalling_type =
        nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_primary_input<nil::marshalling::field_type<endianness>,
                                                                          PInput>;

    static inline std::string proof_path_str = "proof.bin";
    static inline std::string vk_path_str = "vkey.bin";
    static inline std::string pubkey_path_str = "pubkey.bin";
    static inline std::string ct_path_str = "ct.bin";
    static inline std::string unenc_pi_path_str = "unenc_pi.bin";
    static inline std::string full_output_path_str = "data_encrypted_input.bin";
    static inline std::string full_output_wrong_ct_path_str = "data_encrypted_input_wrong_ct.bin";

    static inline auto proof_path = std::filesystem::path(proof_path_str);
    static inline auto vk_path = std::filesystem::path(vk_path_str);
    static inline auto pubkey_path = std::filesystem::path(pubkey_path_str);
    static inline auto ct_path = std::filesystem::path(ct_path_str);
    static inline auto unenc_pi_path = std::filesystem::path(unenc_pi_path_str);
    static inline auto full_output_path = std::filesystem::path(full_output_path_str);
    static inline auto full_output_wrong_ct_path = std::filesystem::path(full_output_wrong_ct_path_str);

    template<typename MarshallingType, typename InputObj, typename F>
    static std::vector<std::uint8_t> serialize_obj(const InputObj &in_obj, const std::function<F> &f) {
        MarshallingType filled_val = f(in_obj);
        std::vector<std::uint8_t> blob(filled_val.length());
        auto it = std::begin(blob);
        nil::marshalling::status_type status = filled_val.write(it, blob.size());
        return blob;
    }

    template<typename Path, typename Blob>
    static void write_obj(const Path &path, std::initializer_list<Blob> blobs) {
        std::ofstream out(path, std::ios_base::binary);
        for (const auto &blob : blobs) {
            for (const auto b : blob) {
                out << b;
            }
        }
        out.close();
    }

    static void write_data(const VerificationKey &vk, const PublicKey &pubkey, const Proof &proof, const PInput &pinput,
                           const CipherText &ct) {
        auto proof_blob = serialize_obj<proof_marshalling_type>(
            proof, std::function(nil::crypto3::marshalling::types::fill_r1cs_gg_ppzksnark_proof<Proof, endianness>));
        write_obj(proof_path, {proof_blob});

        auto vk_blob = serialize_obj<verification_key_marshalling_type>(
            vk, std::function(nil::crypto3::marshalling::types::fill_r1cs_gg_ppzksnark_verification_key<VerificationKey,
                                                                                endianness>));
        write_obj(vk_path, {vk_blob});

        auto pubkey_blob = serialize_obj<public_key_marshalling_type>(
            pubkey, std::function(nil::crypto3::marshalling::types::fill_public_key<PublicKey, endianness>));
        write_obj(pubkey_path, {pubkey_blob});

        auto pinput_blob = serialize_obj<pinput_marshalling_type>(
            pinput,
            std::function(nil::crypto3::marshalling::types::fill_r1cs_gg_ppzksnark_primary_input<PInput, endianness>));
        write_obj(unenc_pi_path, {pinput_blob});

        auto ct_blob = serialize_obj<ct_marshalling_type>(
            ct,
            std::function(
                nil::crypto3::marshalling::types::fill_r1cs_gg_ppzksnark_encrypted_primary_input<CipherText,
                                                                                endianness>));
        write_obj(ct_path, {ct_blob});

        nil::crypto3::random::algebraic_random_device<
            typename std::iterator_traits<typename CipherText::iterator>::value_type::group_type>
            d;
        auto ct_wrong = ct;
        ct_wrong[std::rand() % ct.size()] = d();
        auto ct_wrong_blob = serialize_obj<ct_marshalling_type>(
            ct_wrong,
            std::function(
                nil::crypto3::marshalling::types::fill_r1cs_gg_ppzksnark_encrypted_primary_input<CipherText,
                                                                                endianness>));

        write_obj(full_output_path, {proof_blob, vk_blob, pubkey_blob, ct_blob, pinput_blob});
        write_obj(full_output_wrong_ct_path, {proof_blob, vk_blob, pubkey_blob, ct_wrong_blob, pinput_blob});
    }

    template<typename ReturnType, typename MarshallingType, typename Path, typename F>
    static ReturnType read_obj(const Path &path, const std::function<F> &f) {
        std::ifstream in(path, std::ios_base::binary);
        std::stringstream buffer;
        buffer << in.rdbuf();
        auto blob_str = buffer.str();
        std::vector<std::uint8_t> blob(std::cbegin(blob_str), std::cend(blob_str));

        MarshallingType marshalling_obj;
        auto it = std::cbegin(blob);
        nil::marshalling::status_type status = marshalling_obj.read(it, blob.size());
        return f(marshalling_obj);
    }

    static std::tuple<Proof, VerificationKey, PublicKey, PInput, CipherText> read_data() {
        Proof proof = read_obj<Proof, proof_marshalling_type>(
            proof_path,
            std::function(nil::crypto3::marshalling::types::make_r1cs_gg_ppzksnark_proof<Proof, endianness>));
        VerificationKey vk = read_obj<VerificationKey, verification_key_marshalling_type>(
            vk_path,
            std::function(nil::crypto3::marshalling::types::make_r1cs_gg_ppzksnark_verification_key<VerificationKey,
                                                                                endianness>));
        PublicKey pubkey = read_obj<PublicKey, public_key_marshalling_type>(
            pubkey_path, std::function(nil::crypto3::marshalling::types::make_public_key<PublicKey, endianness>));
        PInput pinput = read_obj<PInput, pinput_marshalling_type>(
            unenc_pi_path,
            std::function(nil::crypto3::marshalling::types::make_r1cs_gg_ppzksnark_primary_input<PInput, endianness>));
        CipherText ct = read_obj<CipherText, ct_marshalling_type>(
            ct_path,
            std::function(
                nil::crypto3::marshalling::types::make_r1cs_gg_ppzksnark_encrypted_primary_input<CipherText,
                                                                                endianness>));

        return std::tuple {proof, vk, pubkey, pinput, ct};
    }
};

struct test_policy {
    using pairing_curve_type = curves::bls12_381;
    using curve_type = curves::jubjub;
    using base_points_generator_hash_type = hashes::sha2<256>;
    using hash_params = hashes::find_group_hash_default_params;
    using hash_component = components::pedersen<curve_type, base_points_generator_hash_type, hash_params>;
    using hash_type = typename hash_component::hash_type;
    using merkle_hash_component = hash_component;
    using merkle_hash_type = typename merkle_hash_component::hash_type;
    using field_type = typename hash_component::field_type;
    static constexpr std::size_t arity = 2;
    static constexpr std::size_t tree_depth = 1;
    using voting_component =
        components::encrypted_input_voting<arity, hash_component, merkle_hash_component, field_type>;
    using merkle_proof_component = typename voting_component::merkle_proof_component;
    using encryption_scheme = elgamal_verifiable<pairing_curve_type>;
    using proof_system = typename encryption_scheme::proof_system_type;
    using marshalling_data_type = marshalling_verification_data_groth16_encrypted_input<
        typename proof_system::verification_key_type, typename encryption_scheme::public_key_type,
        typename proof_system::proof_type, typename proof_system::primary_input_type,
        typename encryption_scheme::cipher_type::first_type>;
};

BOOST_AUTO_TEST_SUITE(pubkey_elgamal_verifiable_test_suite)

BOOST_AUTO_TEST_CASE(elgamal_verifiable_auto_test) {
    /* prepare test */
    constexpr std::size_t participants_number = 1 << test_policy::tree_depth;
    auto secret_keys = generate_random_data<bool, test_policy::hash_type::digest_bits>(participants_number);
    std::vector<std::array<bool, test_policy::hash_type::digest_bits>> public_keys;
    for (const auto &sk : secret_keys) {
        std::array<bool, test_policy::hash_type::digest_bits> pk {};
        hash<test_policy::merkle_hash_type>(sk, std::begin(pk));
        public_keys.emplace_back(pk);
    }
    containers::merkle_tree<test_policy::merkle_hash_type, test_policy::arity> tree(public_keys.begin(),
                                                                                public_keys.end());
    std::size_t proof_idx = std::rand() % participants_number;
    containers::merkle_proof<test_policy::merkle_hash_type, test_policy::arity> proof(tree, proof_idx);
    auto tree_pk_leaf = tree[proof_idx];

    std::vector<bool> m = {0, 1, 0, 0, 0, 0, 0};
    std::vector<typename test_policy::pairing_curve_type::scalar_field_type::value_type> m_field;
    for (const auto m_i : m) {
        m_field.emplace_back(std::size_t(m_i));
    }

    const std::size_t eid_size = 64;
    std::vector<bool> eid(eid_size);
    std::generate(eid.begin(), eid.end(), [&]() { return std::rand() % 2; });

    std::vector<bool> eid_sk;
    std::copy(std::cbegin(eid), std::cend(eid), std::back_inserter(eid_sk));
    std::copy(std::cbegin(secret_keys[proof_idx]), std::cend(secret_keys[proof_idx]), std::back_inserter(eid_sk));
    std::vector<bool> sn = hash<test_policy::hash_type>(eid_sk);

    components::blueprint<test_policy::field_type> bp;
    components::block_variable<test_policy::field_type> m_block(bp, m.size());
    components::block_variable<test_policy::field_type> eid_block(bp, eid.size());
    components::digest_variable<test_policy::field_type> sn_digest(bp, test_policy::hash_component::digest_bits);
    components::digest_variable<test_policy::field_type> root_digest(bp,
                                                                     test_policy::merkle_hash_component::digest_bits);
    components::blueprint_variable_vector<test_policy::field_type> address_bits_va;
    address_bits_va.allocate(bp, test_policy::tree_depth);
    test_policy::merkle_proof_component path_var(bp, test_policy::tree_depth);
    components::block_variable<test_policy::field_type> sk_block(bp, secret_keys[proof_idx].size());
    test_policy::voting_component vote_var(bp, m_block, eid_block, sn_digest, root_digest, address_bits_va, path_var,
                                           sk_block, components::blueprint_variable<test_policy::field_type>(0));

    path_var.generate_r1cs_constraints();
    vote_var.generate_r1cs_constraints();

    BOOST_CHECK(!bp.is_satisfied());
    path_var.generate_r1cs_witness(proof);
    BOOST_CHECK(!bp.is_satisfied());
    address_bits_va.fill_with_bits_of_ulong(bp, path_var.address);
    BOOST_CHECK(!bp.is_satisfied());
    auto address = path_var.address;
    BOOST_CHECK(address_bits_va.get_field_element_from_bits(bp) == path_var.address);
    m_block.generate_r1cs_witness(m);
    BOOST_CHECK(!bp.is_satisfied());
    eid_block.generate_r1cs_witness(eid);
    BOOST_CHECK(!bp.is_satisfied());
    sk_block.generate_r1cs_witness(secret_keys[proof_idx]);
    BOOST_CHECK(!bp.is_satisfied());
    vote_var.generate_r1cs_witness(tree.root(), sn);
    BOOST_CHECK(bp.is_satisfied());

    std::cout << "Constraints number: " << bp.num_constraints() << std::endl;

    bp.set_input_sizes(vote_var.get_input_size());

    typename test_policy::proof_system::keypair_type gg_keypair =
        generate<test_policy::proof_system>(bp.get_constraint_system());

    algebraic_random_device<typename test_policy::pairing_curve_type::scalar_field_type> d;
    std::vector<typename test_policy::pairing_curve_type::scalar_field_type::value_type> rnd;
    for (std::size_t i = 0; i < m.size() * 3 + 2; ++i) {
        rnd.emplace_back(d());
    }
    typename test_policy::encryption_scheme::keypair_type keypair =
        generate_keypair<test_policy::encryption_scheme, modes::verifiable_encryption<test_policy::encryption_scheme>>(
            rnd, {gg_keypair, m.size()});

    typename test_policy::encryption_scheme::cipher_type cipher_text =
        encrypt<test_policy::encryption_scheme, modes::verifiable_encryption<test_policy::encryption_scheme>>(
            m_field, {d(), std::get<0>(keypair), gg_keypair, bp.primary_input(), bp.auxiliary_input()});

    typename test_policy::proof_system::primary_input_type pinput = bp.primary_input();
    test_policy::marshalling_data_type::write_data(
        gg_keypair.second, std::get<0>(keypair), cipher_text.second,
        typename test_policy::proof_system::primary_input_type {std::cbegin(pinput) + m.size(), std::cend(pinput)},
        cipher_text.first);

    typename test_policy::encryption_scheme::decipher_type decipher_text =
        decrypt<test_policy::encryption_scheme, modes::verifiable_encryption<test_policy::encryption_scheme>>(
            cipher_text.first, {std::get<1>(keypair), std::get<2>(keypair), gg_keypair});
    BOOST_CHECK(decipher_text.first.size() == m_field.size());
    for (std::size_t i = 0; i < m_field.size(); ++i) {
        BOOST_CHECK(decipher_text.first[i] == m_field[i]);
    }

    bool enc_verification_ans = verify_encryption<test_policy::encryption_scheme>(
        cipher_text.first,
        {std::get<0>(keypair), gg_keypair.second, cipher_text.second,
         typename test_policy::proof_system::primary_input_type {std::cbegin(pinput) + m.size(), std::cend(pinput)}});
    BOOST_CHECK(enc_verification_ans);

    bool dec_verification_ans = verify_decryption<test_policy::encryption_scheme>(
        cipher_text.first, decipher_text.first, {std::get<2>(keypair), gg_keypair, decipher_text.second});
    BOOST_CHECK(dec_verification_ans);

    /// Rerandomized cipher text
    std::vector<typename test_policy::pairing_curve_type::scalar_field_type::value_type> rnd_rerandomization;
    for (std::size_t i = 0; i < 3; ++i) {
        rnd_rerandomization.emplace_back(d());
    }
    typename test_policy::encryption_scheme::cipher_type rerand_cipher_text =
        rerandomize<test_policy::encryption_scheme>(rnd_rerandomization, cipher_text.first,
                                                    {std::get<0>(keypair), gg_keypair, cipher_text.second});

    /// Decryption of the rerandomized cipher text
    typename test_policy::encryption_scheme::decipher_type decipher_rerand_text =
        decrypt<test_policy::encryption_scheme, modes::verifiable_encryption<test_policy::encryption_scheme>>(
            rerand_cipher_text.first, {std::get<1>(keypair), std::get<2>(keypair), gg_keypair});
    BOOST_CHECK(decipher_rerand_text.first.size() == m_field.size());
    for (std::size_t i = 0; i < m_field.size(); ++i) {
        BOOST_CHECK(decipher_rerand_text.first[i] == m_field[i]);
    }

    /// Encryption verification of the rerandomized cipher text
    enc_verification_ans = verify_encryption<test_policy::encryption_scheme>(
        rerand_cipher_text.first,
        {std::get<0>(keypair), gg_keypair.second, rerand_cipher_text.second,
         typename test_policy::proof_system::primary_input_type {std::cbegin(pinput) + m.size(), std::cend(pinput)}});
    BOOST_CHECK(enc_verification_ans);

    /// Decryption verification of the rerandomized cipher text
    dec_verification_ans = verify_decryption<test_policy::encryption_scheme>(
        rerand_cipher_text.first, decipher_rerand_text.first,
        {std::get<2>(keypair), gg_keypair, decipher_rerand_text.second});
    BOOST_CHECK(dec_verification_ans);

    // TODO: add status return
    // /// False-positive tests
    // auto cipher_text_wrong = cipher_text.first;
    // for (auto & c: cipher_text_wrong) {
    //     c = c + std::iterator_traits<typename decltype(cipher_text.first)::iterator>::value_type::one();
    // }
    // typename encryption_scheme::decipher_type decipher_text_wrong =
    //     decrypt<encryption_scheme, modes::verifiable_encryption<encryption_scheme>>(
    //         cipher_text_wrong, {std::get<1>(keypair), std::get<2>(keypair), gg_keypair});
    // BOOST_CHECK(decipher_text.first.size() == m_field.size());
    // bool wrong_decryption_ans = true;
    // for (std::size_t i = 0; i < m_field.size(); ++i) {
    //     wrong_decryption_ans &= (decipher_text.first[i] == m_field[i]);
    // }
    // BOOST_CHECK(!wrong_decryption_ans);
}

BOOST_AUTO_TEST_CASE(elgamal_verifiable_restored_test) {
    auto [proof, vk, pubkey, pinput, ct] = test_policy::marshalling_data_type::read_data();

    bool enc_verification_ans = verify_encryption<test_policy::encryption_scheme>(ct, {pubkey, vk, proof, pinput});
    BOOST_CHECK(enc_verification_ans);
}

BOOST_AUTO_TEST_SUITE_END()