#include "stdafx.h"
#include "BodyGraph.h"
#include <boost/graph/iteration_macros.hpp>
#include <boost/log/trivial.hpp>

#define _USE_MATH_DEFINES
#include <math.h>
#undef _USE_MATH_DEFINES

#include "nsvr_core_errors.h"

const segment_range segment_range::full = { 0, 1 };
const segment_range segment_range::lower_half = { 0, 0.5 };
const segment_range segment_range::upper_half = { 0.5, 1.0 };

const angle_range angle_range::full = { 0, 360 };
const angle_range angle_range::left_half = { 180, 360 };
const angle_range angle_range::right_half = { 0, 180 };
const angle_range angle_range::front_half = { 270, 90 };
const angle_range angle_range::back_half = { 90, 270 };

BodyGraph::BodyGraph()
{
	using region = subregion::shared_region;

	auto entire_torso = std::shared_ptr<subregion>(new
		subregion(region::identifier_torso, segment_range::full, angle_range::full, {
			subregion(region::identifier_torso_front, segment_range::full, angle_range::front_half,{
				subregion(region::identifier_chest_left,  segment_range{ 0.8, 1.0 }, angle_range{ 340, 360 }),
				subregion(region::identifier_middle_sternum, segment_range{0.45, 0.55}, angle_range{355, 5}),
				subregion(region::identifier_upper_ab_left,  segment_range{ 0.6, 0.8 }, angle_range{ 340, 360 }),
				subregion(region::identifier_middle_ab_left, segment_range{ 0.4, 0.6 }, angle_range{ 340, 360 }),
				subregion(region::identifier_lower_ab_left,  segment_range{ 0.0, 0.4 }, angle_range{ 340, 360 }),
				subregion(region::identifier_chest_right, segment_range{ 0.8, 1.0 }, angle_range{ 0, 20 }),
				subregion(region::identifier_upper_ab_right, segment_range{ 0.6, 0.8 }, angle_range{ 0, 20 }),
				subregion(region::identifier_middle_ab_right, segment_range{ 0.4, 0.6 }, angle_range{ 0, 20 }),
				subregion(region::identifier_lower_ab_right, segment_range{ 0.0, 0.4 }, angle_range{ 0, 20 })
			}),
			subregion(region::identifier_torso_back, segment_range::full, angle_range::back_half,{
				subregion(region::identifier_upper_back_left,  segment_range{ 0.5, 1.0 }, angle_range{ 180, 270 }),
				subregion(region::identifier_upper_back_right,  segment_range{ 0.5, 1.0 }, angle_range{ 90, 180 })
			}),
			subregion(region::identifier_torso_left, segment_range::full, angle_range::left_half),
			subregion(region::identifier_torso_right, segment_range::full, angle_range::right_half)
	}));

	auto upper_arm_left = std::shared_ptr<subregion>(new
		subregion(region::identifier_upper_arm_left, segment_range::full, angle_range::full, {
			subregion(region::identifier_shoulder_left, segment_range{ 0.85, 1.0 }, angle_range::full)
		}));

	auto upper_arm_right = std::shared_ptr<subregion>(new
		subregion(region::identifier_upper_arm_right, segment_range::full, angle_range::full, {
			subregion(region::identifier_shoulder_right, segment_range{ 0.85, 1.0 }, angle_range::full)
		}));

	auto lower_arm_left = std::shared_ptr<subregion>(new
		subregion(region::identifier_lower_arm_left, segment_range::full, angle_range::full));

	auto lower_arm_right = std::shared_ptr<subregion>(new
		subregion(region::identifier_lower_arm_right, segment_range::full, angle_range::full));

	auto upper_leg_left = std::shared_ptr<subregion>(new
		subregion(region::identifier_upper_leg_left, segment_range::full, angle_range::full));

	auto lower_leg_left = std::shared_ptr<subregion>(new
		subregion(region::identifier_lower_leg_left, segment_range::full, angle_range::full));

	auto upper_leg_right = std::shared_ptr<subregion>(new
		subregion(region::identifier_upper_leg_right, segment_range::full, angle_range::full));

	auto lower_leg_right = std::shared_ptr<subregion>(new
		subregion(region::identifier_lower_leg_right, segment_range::full, angle_range::full));

	auto head = std::shared_ptr<subregion>(new
		subregion(region::identifier_head, segment_range::full, angle_range::full));

	auto left_palm = std::shared_ptr<subregion>(new
		subregion(region::identifier_palm_left, segment_range::full, angle_range::full));

auto right_palm = std::shared_ptr<subregion>(new
	subregion(region::identifier_palm_right, segment_range::full, angle_range::full));

	m_bodyparts.emplace(nsvr_bodypart_head, Bodypart(nsvr_bodypart_head, 19.03, head));

	m_bodyparts.emplace(nsvr_bodypart_torso, Bodypart(nsvr_bodypart_torso, 51.9, entire_torso));

	m_bodyparts.emplace(nsvr_bodypart_upperarm_left, Bodypart(nsvr_bodypart_upperarm_left, 29.41, upper_arm_left));

	m_bodyparts.emplace(nsvr_bodypart_lowerarm_left, Bodypart(nsvr_bodypart_lowerarm_left, 27.68, lower_arm_left));

	m_bodyparts.emplace(nsvr_bodypart_upperarm_right, Bodypart(nsvr_bodypart_upperarm_right, 29.41, upper_arm_right));

	m_bodyparts.emplace(nsvr_bodypart_lowerarm_right, Bodypart(nsvr_bodypart_lowerarm_right, 27.68, lower_arm_right));

	m_bodyparts.emplace(nsvr_bodypart_lowerarm_right, Bodypart(nsvr_bodypart_lowerarm_right, 27.68, lower_arm_right));

	m_bodyparts.emplace(nsvr_bodypart_lowerleg_left, Bodypart(nsvr_bodypart_lowerleg_left, 43.25, lower_leg_left));

	m_bodyparts.emplace(nsvr_bodypart_upperleg_left, Bodypart(nsvr_bodypart_upperleg_left, 43.25, upper_leg_left));

	m_bodyparts.emplace(nsvr_bodypart_lowerleg_right, Bodypart(nsvr_bodypart_lowerleg_right, 43.25, lower_leg_right));

	m_bodyparts.emplace(nsvr_bodypart_upperleg_right, Bodypart(nsvr_bodypart_upperleg_right, 43.25, upper_leg_right));

	m_bodyparts.emplace(nsvr_bodypart_palm_left, Bodypart(nsvr_bodypart_palm_left, 10.0, left_palm));
	m_bodyparts.emplace(nsvr_bodypart_palm_right, Bodypart(nsvr_bodypart_palm_right, 10.0, right_palm));

	for (auto& bp : m_bodyparts) {
		bp.second.region->init_backlinks();
	}

}

subregion::subregion()
	: region(shared_region::identifier_unknown)
	, seg{ 0, 0 }
	, ang{ 0,0 }
	, coords{ 0, 0, 0 }
	, children()
	, hardware_defined_regions()
	, parent(nullptr) {}

subregion::subregion(shared_region region, segment_range segment_offset, angle_range angle_range)
	: region(region)
	, seg(segment_offset)
	, ang(angle_range)
	, children()
	, hardware_defined_regions()
	, parent(nullptr) {
	calculateCoordinates();

}

subregion::subregion(shared_region region, segment_range segment_offset, angle_range angle_range, std::vector<subregion> child_regions)
	: region(region)
	, seg(segment_offset)
	, ang(angle_range)
	, children(std::move(child_regions))
	, hardware_defined_regions()
	, parent(nullptr) {

	calculateCoordinates();

}
int BodyGraph::CreateNode(const char * name, nsvr_bodygraph_region * pose)
{
	//To create a new node:
		//First, add a vertex into the graph with the given name.
	boost::add_vertex(name, m_nodes);

	//Then, grab the bodypart corresponding with the given arguments.
	auto& bodypart = m_bodyparts[pose->bodypart];

	//Then ask that bodypart to find the nearest match to the given coordinates.
	subregion::shared_region region = bodypart.region->find_best_match(pose->segment_ratio, pose->rotation).second;

	//Update the vertex with the information that was found
	m_nodes[name] = NodeData(name, *pose, region);

	//Update the bodypart's subregion that corresponded with the match to have a reference to this graph node.
	bodypart.region->find(region)->hardware_defined_regions.emplace_back(name);

	BOOST_LOG_TRIVIAL(info) << "[BodyGraph] Node [" << name << "] registered on region " << region._to_string();
	return 0;
}

int BodyGraph::ConnectNodes(const char* a, const char* b)
{
	if (m_nodes.vertex(a) != LabeledGraph::null_vertex() && m_nodes.vertex(b) != LabeledGraph::null_vertex()) {
		boost::add_edge_by_label(a, b, m_nodes);
		return nsvr_success;
	}
	else {
		return nsvr_error_nosuchnode;
	}
}

int BodyGraph::Associate(const char * node, nsvr_node_id node_id)
{
	if (m_nodes.vertex(node) != LabeledGraph::null_vertex()) {
		m_nodes[node].addNode(node_id);
		return nsvr_success;
	}
	else {
		return nsvr_error_nosuchnode;
	}

}

int BodyGraph::Unassociate(const char * node, nsvr_node_id node_id)
{
	if (m_nodes.vertex(node) != LabeledGraph::null_vertex()) {
		m_nodes[node].removeNode(node_id);
		return nsvr_success;
	}
	else {
		return nsvr_error_nosuchnode;
	}
}

void BodyGraph::ClearAssociations(nsvr_node_id node_id)
{
	BGL_FORALL_VERTICES_T(v, m_nodes, LabeledGraph) {
		m_nodes.graph()[v].removeNode(node_id);
	}
}

std::vector<nsvr_node_id> BodyGraph::getNodesForNamedRegion(subregion::shared_region region) const
{
	std::vector<nsvr_node_id> nodes;

	//Special case for whole body - must refactor, and test these methods better
	if (region._value == subregion::shared_region::identifier_body) {
		const auto& all = getAllNodes();
		for (const auto& kvp : all) {
			nodes.insert(nodes.end(), kvp.second.begin(), kvp.second.end());
		}
		std::sort(nodes.begin(), nodes.end());
		nodes.erase(std::unique(nodes.begin(), nodes.end()), nodes.end());
		return nodes;

	}

	for (auto& bp : m_bodyparts) {
		subregion* ptr = bp.second.region->find(region);
		if (ptr != nullptr) {

			//If this region doesn't have any devices associated with it,
			//Then we need to traverse upwards until we find one that does!
			while (ptr->hardware_defined_regions.empty()) {
				if (ptr->parent != nullptr) {
					ptr = ptr->parent;
				}
				else {
					break;
				}
			}

			//Okay, we found some references - for each one, grab the associated nodes out of the graph
			//and insert into our list.
			for (const std::string& name : ptr->hardware_defined_regions) {
				auto& found = m_nodes[name].nodes;
				nodes.insert(nodes.end(), found.begin(), found.end());
			}
		}
	}

	std::sort(nodes.begin(), nodes.end());
	nodes.erase(std::unique(nodes.begin(), nodes.end()), nodes.end());
	return nodes;

}

std::vector<subregion::shared_region::_enumerated> BodyGraph::getRegionsForNode(nsvr_node_id node) const
{
	std::vector<subregion::shared_region::_enumerated> regions;
	BGL_FORALL_VERTICES_T(v, m_nodes, LabeledGraph) {
		subregion::shared_region region = m_nodes.graph()[v].computed_region;
		auto& nodeList = m_nodes.graph()[v].nodes;
		if (std::find(nodeList.begin(), nodeList.end(), node) != nodeList.end()) {
			regions.push_back(region);
		}

	}
	return regions;
}

std::unordered_map<subregion::shared_region::_enumerated, std::vector<nsvr_node_id>> BodyGraph::getAllNodes() const
{

	std::unordered_map<subregion::shared_region::_enumerated, std::vector<nsvr_node_id>> nodes;

	//Walk through the graph, and add any nodes that were found to the hashtable
	BGL_FORALL_VERTICES_T(v, m_nodes, LabeledGraph) {
		subregion::shared_region region = m_nodes.graph()[v].computed_region;
		auto& nodeList = m_nodes.graph()[v].nodes;
		auto& currentDevices = nodes[region];
		currentDevices.insert(currentDevices.end(), nodeList.begin(), nodeList.end());
	}

	//make sure each list has no duplicates
	for (auto& kvp : nodes) {
		auto& list = kvp.second;
		std::sort(list.begin(), list.end());
		list.erase(std::unique(list.begin(), list.end()), list.end());
	}

	return nodes;
}

void BodyGraph::NodeData::addNode(nsvr_node_id id)
{
	auto it = std::find(nodes.begin(), nodes.end(), id);
	if (it == nodes.end()) {
		nodes.push_back(id);
	}
}

void BodyGraph::NodeData::removeNode(nsvr_node_id id)
{
	auto it = std::remove(nodes.begin(), nodes.end(), id);
	nodes.erase(it, nodes.end());
}


#include <string>
#include <vector>
#include <set>
#include <fstream>
#include <sstream>
#include <iostream>
#include <memory>
#include <algorithm>
#include <libcxx/sort.hpp>
#include <boost/optional/optional.hpp>
#include "getopt_pp/getopt_pp.h"
#include "kmc_api/kmc_file.h"
//#include "omp.h"
#include "io/kmers/mmapped_reader.hpp"
#include "utils/filesystem/path_helper.hpp"
#include "utils/stl_utils.hpp"
#include "utils/ph_map/perfect_hash_map_builder.hpp"
#include "utils/ph_map/storing_traits.hpp"
#include "utils/kmer_mph/kmer_splitters.hpp"
#include "logger.hpp"

using std::string;
using std::vector;

const string KMER_PARSED_EXTENSION = ".bin";
const string KMER_SORTED_EXTENSION = ".sorted";

class KmerMultiplicityCounter {
    size_t k_ ;
    std::string file_prefix_;

    //TODO: get rid of intermediate .bin file
    string ParseKmc(const string& filename) {
        CKMCFile kmcFile;
        kmcFile.OpenForListing(filename);
        CKmerAPI kmer((unsigned int) k_);
        uint32 count;
        std::string parsed_filename = filename + KMER_PARSED_EXTENSION;
        std::ofstream output(parsed_filename, std::ios::binary);
        while (kmcFile.ReadNextKmer(kmer, count)) {
            RtSeq seq(k_, kmer.to_string());
            seq.BinWrite(output);
            seq_element_type tmp = count;
            output.write((char*) &(tmp), sizeof(seq_element_type));
        }
        output.close();
        return parsed_filename;
    }

    string SortKmersCountFile(const string& filename) {
        MMappedRecordArrayReader<seq_element_type> ins(filename, RtSeq::GetDataSize(k_) + 1, false);
        libcxx::sort(ins.begin(), ins.end(), adt::array_less<seq_element_type>());
        std::string sorted_filename = filename + KMER_SORTED_EXTENSION;
        std::ofstream out(sorted_filename);
        out.write((char*) ins.data(), ins.data_size());
        out.close();
        remove(filename.c_str());
        return sorted_filename;
    }

    bool ReadKmerWithCount(std::ifstream& infile, std::pair<RtSeq, uint32>& res) {
        RtSeq seq(res.first.size());
        if (!seq.BinRead(infile)) {
            return false;
        }
        seq_element_type tmp;
        infile.read((char*) &tmp, sizeof(seq_element_type));
        res = {seq, (uint32) tmp};
        return true;
    }

    fs::TmpFile FilterCombinedKmers(fs::TmpDir workdir, const std::vector<string>& files,
                                    size_t all_min, size_t min_mult) {
        size_t n = files.size();
        vector<std::unique_ptr<ifstream>> infiles;
        infiles.reserve(n);
        for (auto fn : files) {
            INFO("Processing " << fn);
            auto parsed = ParseKmc(fn);
            auto sorted = SortKmersCountFile(parsed);
            infiles.emplace_back(new std::ifstream(sorted));
        }
        vector<std::pair<RtSeq, uint32>> top_kmer(n, {RtSeq(k_), 0});
        vector<bool> alive(n, false);

        for (size_t i = 0; i < n; i++) {
            alive[i] = ReadKmerWithCount(*infiles[i], top_kmer[i]);
        }

        auto kmer_file = fs::tmp::make_temp_file("kmer", workdir);

        typedef uint16_t Mpl;
        std::ofstream output_kmer(*kmer_file, std::ios::binary);
        std::ofstream mpl_file(file_prefix_ + ".bpr", std::ios_base::binary);

        RtSeq::less3 kmer_less;
        while (true) {
            boost::optional<RtSeq> min_kmer;
            size_t cnt_min = 0, num_samples = 0;
            for (size_t i = 0; i < n; ++i) {
                if (alive[i]) {
                    RtSeq& cur_kmer = top_kmer[i].first;
                    if (!min_kmer || kmer_less(cur_kmer, *min_kmer)) {
                        min_kmer = cur_kmer;
                        cnt_min = 0;
                    }
                    if (cur_kmer == *min_kmer) {
                        ++cnt_min;
                    }
                }
            }
            if (!min_kmer) {
                break;
            }
            if (cnt_min >= all_min) {
                std::vector<uint32> cnt_vector(n, 0);
                size_t total_cnt = 0;
                for (size_t i = 0; i < n; ++i) {
                    if (alive[i] && top_kmer[i].first == *min_kmer) {
                        auto cnt = top_kmer[i].second;
                        cnt_vector[i] = cnt;
                        total_cnt += cnt;
                    }
                }
                if (cnt_min > 1 || total_cnt > min_mult) {
                    min_kmer.get().BinWrite(output_kmer);
                    for (size_t mpl : cnt_vector) {
                        mpl_file.write(reinterpret_cast<char *>(&mpl), sizeof(Mpl));
                    }
                }
            }
            for (size_t i = 0; i < n; ++i) {
                if (alive[i] && top_kmer[i].first == *min_kmer) {
                    alive[i] = ReadKmerWithCount(*infiles[i], top_kmer[i]);
                }
            }
        }
        return kmer_file;
    }

    void BuildKmerIndex(fs::TmpDir workdir, fs::TmpFile kmer_file, size_t sample_cnt, size_t nthreads) {
        INFO("Initializing kmer profile index");

        typedef size_t Offset;
        using namespace utils;

        using KMerDiskStorage = kmers::KMerDiskStorage<RtSeq>;
        constexpr size_t read_buffer_size = 0; //FIXME some buffer size
        DeBruijnKMerKMerSplitter<StoringTypeFilter<InvertableStoring>,
                                 KMerDiskStorage::kmer_iterator>
                splitter(workdir, k_, k_, true, read_buffer_size);
        splitter.AddKMers(adt::make_range(KMerDiskStorage::kmer_iterator(*kmer_file, k_), KMerDiskStorage::kmer_iterator()));

        kmers::KMerDiskCounter<RtSeq> counter(workdir, std::move(splitter));
        KeyStoringMap<RtSeq, Offset, kmers::kmer_index_traits<RtSeq>, InvertableStoring> kmer_mpl(k_);
        BuildIndex(kmer_mpl, counter, 16, nthreads);
        INFO("Built index with " << kmer_mpl.size() << " kmers");

        //Building kmer->profile offset index
        std::ifstream kmers_in(*kmer_file, std::ios::binary);
        utils::InvertableStoring::trivial_inverter inverter;
        RtSeq kmer(k_);
        for (Offset offset = 0; ; offset += sample_cnt) {
            kmer.BinRead(kmers_in);
            if (kmers_in.fail()) {
                break;
            }

//            VERIFY(kmer_str.length() == k_);
//            conj_graph_pack::seq_t kmer(k_, kmer_str.c_str());
//            kmer = gp_.kmer_mapper.Substitute(kmer);

            auto kwh = kmer_mpl.ConstructKWH(kmer);
            VERIFY(kmer_mpl.valid(kwh));
            kmer_mpl.put_value(kwh, offset, inverter);
        }

        std::ofstream map_file(file_prefix_ + ".kmm", std::ios_base::binary | std::ios_base::out);
        kmer_mpl.BinWrite(map_file);
        INFO("Saved kmer profile map");
    }

public:
    KmerMultiplicityCounter(size_t k, std::string file_prefix):
        k_(k), file_prefix_(std::move(file_prefix)) {
    }

    void CombineMultiplicities(const vector<string>& input_files, size_t min_samples,
                               size_t min_mult, const string& tmpdir, size_t nthreads = 1) {
        auto workdir = fs::tmp::make_temp_dir(tmpdir, "kmidx");
        auto kmer_file = FilterCombinedKmers(workdir, input_files, min_samples, min_mult);
        BuildKmerIndex(workdir, kmer_file, input_files.size(), nthreads);
    }
private:
    DECL_LOGGER("KmerMultiplicityCounter");
};

void PrintUsageInfo() {
    std::cout << "Usage: kmer_multiplicity_counter [options] -f files_dir" << std::endl;
    std::cout << "Options:" << std::endl;
    std::cout << "-k - kmer length" << std::endl;
    std::cout << "-n - sample count" << std::endl;
    std::cout << "-o - output file prefix" << std::endl;
    std::cout << "-t - number of threads (default: 1)" << std::endl;
    std::cout << "-s - minimal number of samples to contain kmer" << std::endl;
    std::cout << "-m - minimal multiplicity of single-sample kmers" << std::endl;
    std::cout << "files_dir must contain two files (.kmc_pre and .kmc_suf) with kmer multiplicities for each sample from 1 to n" << std::endl;
}

int main(int argc, char *argv[]) {
    using namespace GetOpt;
    create_console_logger();

    size_t k, sample_cnt, min_samples, min_mult, nthreads;
    string output, work_dir;

    try {
        GetOpt_pp ops(argc, argv);
        ops.exceptions_all();
        ops >> Option('k', k)
            >> Option('n', sample_cnt)
            >> Option('m', "min-mult", min_mult, size_t(5))
            >> Option('s', min_samples)
            >> Option('o', output)
            >> Option('t', "threads", nthreads, size_t(1))
            >> Option('f', work_dir)
        ;
    } catch(GetOptEx &ex) {
        PrintUsageInfo();
        exit(1);
    }

    std::vector<string> input_files;
    for (size_t i = 1; i <= sample_cnt; ++i) {
        input_files.push_back(work_dir + "/sample" + std::to_string(i));
    }

    KmerMultiplicityCounter kmcounter(k, output);
    kmcounter.CombineMultiplicities(input_files, min_samples, min_mult, work_dir, nthreads);
    return 0;
}


#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/CPDInterface.h>

#include <stdexcept>
#include <sstream>

#include <boost/foreach.hpp>

#include <spatialops/particles/ParticleFieldTypes.h>
#include <spatialops/Nebo.h>

#include <expression/ClipValue.h>

// expressions we build here
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/L_RHS.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/CPDData.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/C_RHS.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/Gi_RHS.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/Deltai_RHS.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/c0_fun.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/kb.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/kg_i.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/dy_gi.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/MvRHS.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/Mv.h>
#include <CCA/Components/Wasatch/Coal/Devolatilization/CPD/TarProductionRate.h>

using std::vector;
using std::cout;
using std::endl;
using std::ostringstream;
using std::string;

namespace CPD{

//------------------------------------------------------------------

vector<string> taglist_names( const Expr::TagList& tags )
{
  vector<string> fieldNames;
  for(  Expr::TagList::const_iterator it=tags.begin(); it!=tags.end(); ++it ){
    fieldNames.push_back( it->name() );
  }
  return fieldNames;
}
//------------------------------------------------------------------

CPDSpecies gas_dev2cpd( const DEV::DEVSpecies cspec )
{
  CPDSpecies s;
  switch( cspec ){
    case DEV::CO2:  s=CO2;             break;
    case DEV::H2O:  s=H2O;             break;
    case DEV::CO :  s=CO;              break;
    case DEV::HCN:  s=HCN;             break;
    case DEV::NH3:  s=NH3;             break;
    case DEV::CH4:  s=CH4;             break;
    case DEV::H2 :  s=H2;              break;
    default:   s=INVALID_SPECIES; break;
  }
  return s;
}

//--------------------------------------------------------------------

template< typename FieldT >
CPDInterface<FieldT>::CPDInterface( GraphCategories& gc,
                                    const CoalType   coalType,
                                    const Expr::Tag  pTempTag,
                                    const Expr::Tag  pMassTag,
                                    const Expr::Tag  pMass0Tag )
  : DEV::DevolatilizationBase(),
    cpdInfo_   ( coalType ),
    coalComp_  ( cpdInfo_.get_coal_composition() ),
    c0_        ( c0_fun( coalComp_.get_C(), coalComp_.get_O() )),
    vMassFrac0_( coalComp_.get_vm() ),
    tar0_      ( tar_0(cpdInfo_, c0_) ),
    pTempTag_  ( pTempTag  ),
    pMassTag_  ( pMassTag  ),
    pMass0Tag_ ( pMass0Tag ),
    kbTag_     ( Coal::StringNames::self().cpd_kb, Expr::STATE_NONE ),
    sNames_    ( Coal::StringNames::self() ),
    gc_        ( gc )
{
  if( pTempTag == Expr::Tag() ){
    ostringstream msg;
    msg << __FILE__ << " : " << __LINE__ << endl
        << "temperature tag is invalid." << endl;
    throw std::runtime_error( msg.str() );
  }

  haveRegisteredExprs_ = false;

  parse_equations();
  set_tags();
  register_expressions();
}

//----------------------------------------------------------------------

template< typename FieldT >
void
CPDInterface<FieldT>::
parse_equations()
{
  gEqns_.    clear();
  deltaEqns_.clear();

  gTags_.    clear();
  deltaTags_.clear();

  g_rhsTags_.    clear();
  delta_rhsTags_.clear();

  const vector<double> delta0 = deltai_0(cpdInfo_, c0_ );

  /*
   * Parse equations for coal functional groups
   */
  const int nspec = cpdInfo_.get_nspec();
  for( int i=0; i<nspec; ++i ){
    ostringstream name1, name2;
    name1 << sNames_.cpd_g     << i;
    name2 << sNames_.cpd_delta << i;

    gEqns_.    push_back( new Coal::CoalEquation(name1.str(), 0.0, gc_) );
    deltaEqns_.push_back( new Coal::CoalEquation(name2.str(), pMassTag_, delta0[i] , gc_) );

    gTags_.    push_back( gEqns_    [i]->solution_variable_tag() );
    deltaTags_.push_back( deltaEqns_[i]->solution_variable_tag() );

    g_rhsTags_.    push_back( gEqns_    [i]->rhs_tag() );
    delta_rhsTags_.push_back( deltaEqns_[i]->rhs_tag() );
  }

  /*
   * Parse equations for volatiles, tar, labile bridge, and labile bridge population.
   */
  const double lFrac0 = cpdInfo_.get_l0_mass() * vMassFrac0_; // initial mass fraction of labile bridge in the particle.

  volatilesEqn_    = new Coal::CoalEquation( sNames_.dev_mv, pMassTag_, vMassFrac0_*(1.0-c0_), gc_ );
  tarEqn_          = new Coal::CoalEquation( sNames_.cpd_tar, pMassTag_, vMassFrac0_*tar0_,    gc_ );
  lbEqn_           = new Coal::CoalEquation( sNames_.cpd_l,  pMassTag_, lFrac0,                gc_ );
  lbPopulationEqn_ = new Coal::CoalEquation( sNames_.cpd_lbPopulation, cpdInfo_.get_lbPop0(),  gc_ );

  // put pointers to equations in a vector
  eqns_.clear();
  eqns_.insert( eqns_.end(), gEqns_.begin(),     gEqns_.end()     );
  eqns_.insert( eqns_.end(), deltaEqns_.begin(), deltaEqns_.end() );
  eqns_.push_back( volatilesEqn_    );
  eqns_.push_back( tarEqn_          );
  eqns_.push_back( lbEqn_           );
  eqns_.push_back( lbPopulationEqn_ );
}
//----------------------------------------------------------------------
template< typename FieldT >
void
CPDInterface<FieldT>::
set_tags()
{
  // set tags for functional group rate constants
  const int nspec = cpdInfo_.get_nspec();
  for( int i=0; i<nspec; ++i ){

    ostringstream name1;
    name1 << sNames_.cpd_kg << i;
    kgTags_.push_back( Expr::Tag(name1.str(),Expr::STATE_NONE) );
  }

  // set tags for composition and species sources.
  speciesSrcTags_.clear();
  const SpeciesSum& specSum = SpeciesSum::self();
  const int ncomp = specSum.get_ncomp();
  for( int i=1; i<=ncomp; i++ ){

    ostringstream name2;
    name2 << sNames_.cpd_dy << i;
    speciesSrcTags_.push_back( Expr::Tag(name2.str(),Expr::STATE_NONE) );
  }

  // set tags needed for particle-to-cell source terms (and a few others)
  charSrcTag_      = Expr::Tag(sNames_.cpd_charProd_rhs, Expr::STATE_NONE);

  lbTag_           = lbEqn_           ->solution_variable_tag();
  lb_rhsTag_       = lbEqn_           ->rhs_tag();

  lbpTag_          = lbPopulationEqn_->solution_variable_tag();
  lbp_rhsTag_      = lbPopulationEqn_->rhs_tag();

  tarTag_          = tarEqn_          ->solution_variable_tag();
  tarSrcTag_       = tarEqn_          ->rhs_tag();

  volatilesTag_    = volatilesEqn_    ->solution_variable_tag();
  volatilesSrcTag_ = volatilesEqn_    ->rhs_tag();
}

//----------------------------------------------------------------------

template< typename FieldT >
void
CPDInterface<FieldT>::
register_expressions()
{
  Expr::ExpressionFactory& factory = *(gc_[WasatchCore::ADVANCE_SOLUTION]->exprFactory);

  factory.register_expression( new typename kb<FieldT>::
                                                Builder(kbTag_, pTempTag_, cpdInfo_));

  factory.register_expression( new typename kg_i<FieldT>::
                                                Builder(kgTags_, gTags_ ,pTempTag_, pMass0Tag_, cpdInfo_) );

  factory.register_expression( new typename TarProductionRate<FieldT>::
                                                Builder(tarSrcTag_, lbpTag_, lbp_rhsTag_,
                                                        pMass0Tag_, vMassFrac0_, tar0_, cpdInfo_ ) );

  factory.register_expression( new typename L_RHS<FieldT>::
                                                Builder(lb_rhsTag_, kbTag_, lbTag_ ) );

  factory.register_expression( new typename C_RHS<FieldT>::
                                                Builder(charSrcTag_,  kbTag_, lbTag_, cpdInfo_ ));

  factory.register_expression( new typename Gi_RHS<FieldT>::
                                                Builder(g_rhsTags_, kbTag_, kgTags_,deltaTags_,
                                                        lbTag_ ,cpdInfo_) );

  factory.register_expression( new typename Deltai_RHS<FieldT>::
                                                Builder(delta_rhsTags_, kbTag_, kgTags_, deltaTags_,
                                                        lbTag_ ,cpdInfo_) );

  factory.register_expression( new typename MvRHS<FieldT>::
                                                Builder(volatilesSrcTag_, charSrcTag_, speciesSrcTags_ ));

  factory.register_expression( new typename dy_gi<FieldT>::
                                                Builder(speciesSrcTags_, g_rhsTags_ ) );

  factory.register_expression( new typename L_RHS<FieldT>::
                                                Builder(lbp_rhsTag_, kbTag_, lbpTag_ ) );

  // adds tar production rate to volatiles production rate
  factory.attach_dependency_to_expression( tarSrcTag_, volatilesSrcTag_ );

  // Ensure non-negative values
  typedef Expr::ClipValue<FieldT> Clipper;
  BOOST_FOREACH( Coal::CoalEquation* const& eqn, eqns_ ){
    Expr::Tag tag = eqn->solution_variable_tag();
    const Expr::Tag clip( tag.name()+"_clip", Expr::STATE_NONE );
    factory.register_expression( new typename Clipper::Builder( clip, 0.0, 0.0, Clipper::CLIP_MIN_ONLY ) );
    factory.attach_modifier_expression( clip, tag );
  }

  haveRegisteredExprs_ = true;
}

//--------------------------------------------------------------------

template< typename FieldT >
const Expr::Tag
CPDInterface<FieldT>::
gas_species_src_tag( const DEV::DEVSpecies devspec ) const
{
  const CPDSpecies spec = gas_dev2cpd(devspec);
  if( spec == INVALID_SPECIES ) return Expr::Tag();
  return speciesSrcTags_[ spec ];
}

//====================================================================
// Explicit template instantiation
template class CPDInterface< SpatialOps::Particle::ParticleField >;
//====================================================================

} // namespace CPD


#include "StdInc.h"

#include <ClientHttpHandler.h>

#include <ClientRegistry.h>
#include <GameServer.h>

#include <ServerInstanceBase.h>

#include <ResourceManager.h>
#include <ResourceEventComponent.h>
#include <ResourceCallbackComponent.h>

#include <boost/random/random_device.hpp>
#include <boost/uuid/random_generator.hpp>
#include <boost/uuid/uuid_io.hpp>

#include <IteratorView.h>

#include <botan/base64.h>
#include <botan/sha160.h>
#include <botan/pubkey.h>
#include <botan/rsa.h>
#include <botan/ber_dec.h>

#include <ctime>

#include <ClientDeferral.h>

#include <ServerIdentityProvider.h>

#include <MonoThreadAttachment.h>

#include <json.hpp>

#define FOLLY_NO_CONFIG

#ifdef _WIN32
#undef ssize_t
#else
#include <sys/types.h>
#endif

#include <folly/String.h>
#include <boost/algorithm/string.hpp>

using json = nlohmann::json;

static std::forward_list<fx::ServerIdentityProviderBase*> g_serverProviders;
static std::map<std::string, fx::ServerIdentityProviderBase*> g_providersByType;

namespace fx
{
void RegisterServerIdentityProvider(ServerIdentityProviderBase* provider)
{
	g_serverProviders.push_front(provider);
	g_providersByType.insert({ provider->GetIdentifierPrefix(), provider });
}
}

static std::mutex g_ticketMapMutex;
static std::unordered_set<std::tuple<uint64_t, uint64_t>> g_ticketList;
static std::chrono::milliseconds g_nextTicketGc;

static bool VerifyTicket(const std::string& guid, const std::string& ticket, std::string* error = nullptr)
{
	auto ticketData = Botan::base64_decode(ticket);

	// validate ticket length
	if (ticketData.size() < 20 + 4 + 128)
	{
		if (error)
		{
			*error = "Invalid ticket length.";
		}

		return false;
	}

	uint32_t length = *(uint32_t*)&ticketData[0];

	if (length != 16)
	{
		if (error)
		{
			*error = "Invalid ticket length. (2)";
		}

		return false;
	}

	uint64_t ticketGuid = *(uint64_t*)&ticketData[4];
	uint64_t ticketExpiry = *(uint64_t*)&ticketData[12];

	// check expiration

	// get UTC time
	std::time_t timeVal;
	std::time(&timeVal);

	// verify
	if (ticketExpiry < timeVal)
	{
		if (error)
		{
			*error = "Ticket expired. Please check your server's system time.";
		}

		console::DPrintf("server", "Connecting player: ticket expired\n");
		return false;
	}

	// check the GUID
	uint64_t realGuid = strtoull(guid.c_str(), nullptr, 10);

	if (realGuid != ticketGuid)
	{
		if (error)
		{
			*error = "Mismatching GUID.";
		}

		console::DPrintf("server", "Connecting player: ticket GUID not matching\n");
		return false;
	}

	{
		std::unique_lock<std::mutex> _(g_ticketMapMutex);

		if (g_ticketList.find({ ticketExpiry, ticketGuid }) != g_ticketList.end())
		{
			if (error)
			{
				*error = "Reused ticket.";
			}

			return false;
		}

		if (msec() > g_nextTicketGc)
		{
			g_ticketList.clear();
			g_nextTicketGc = msec() + std::chrono::minutes(30);
		}

		g_ticketList.insert({ ticketExpiry, ticketGuid });
	}

	// check the RSA signature
	uint32_t sigLength = *(uint32_t*)&ticketData[length + 4];

	if (sigLength != 128)
	{
		if (error)
		{
			*error = "Invalid signature length.";
		}

		return false;
	}

	Botan::SHA_160 hashFunction;
	auto result = hashFunction.process(&ticketData[4], length);

	std::vector<uint8_t> msg(result.size() + 1);
	msg[0] = 2;
	memcpy(&msg[1], &result[0], result.size());

	auto modulus = Botan::base64_decode("1DNT1go22VUAU3BON+jCfXxs7Ow9Zxwng4ARTX/vrv6I65bsSYbdBrcc"
										"w/50Fu7AJr8zy8+sXK8wUO4gx00frtA0adaGeZOeBqNq7/K3Gprv98wc"
										"ftbxWjUv75pVl9Ush5yxpBPbuYUnGR/Nh2+K3GRrIrKxWYpNSF1JZYzE"
										"+5k=");

	auto exponent = Botan::base64_decode("AQAB");

	Botan::BigInt n(modulus.data(), modulus.size());
	Botan::BigInt e(exponent.data(), exponent.size());

	auto pk = Botan::RSA_PublicKey(n, e);

	auto signer = std::make_unique<Botan::PK_Verifier>(pk, "EMSA_PKCS1(SHA-1)");

	bool valid = signer->verify_message(msg.data(), msg.size(), &ticketData[length + 4 + 4], sigLength);

	if (!valid)
	{
		if (error)
		{
			*error = "Invalid ticket signature.";
		}

		console::DPrintf("server", "Connecting player: ticket RSA signature not matching\n");
		return false;
	}

	return true;
}

struct TicketData
{
	std::optional<std::array<uint8_t, 20>> entitlementHash;
	std::optional<std::string> extraJson;
};

static std::optional<TicketData> VerifyTicketEx(const std::string& ticket)
{
	auto ticketData = Botan::base64_decode(ticket);

	// validate ticket length
	if (ticketData.size() < 20 + 4 + 128 + 4)
	{
		return {};
	}

	uint32_t length = *(uint32_t*)&ticketData[20 + 4 + 128];

	// validate full length
	if (ticketData.size() < 20 + 4 + 128 + 4 + length)
	{
		return {};
	}

	// copy extra data
	std::vector<uint8_t> extraData(length);

	if (!extraData.empty())
	{
		memcpy(&extraData[0], &ticketData[20 + 4 + 128 + 4], length);
	}

	// check the RSA signature
	uint32_t sigLength = *(uint32_t*)&ticketData[20 + 4 + 128 + 4 + length];

	if (sigLength != 128)
	{
		return {};
	}

	Botan::SHA_160 hashFunction;
	auto result = hashFunction.process(&ticketData[4], ticketData.size() - 128 - 4 - 4);

	std::vector<uint8_t> msg(result.size() + 1);
	msg[0] = 2;
	memcpy(&msg[1], &result[0], result.size());

	auto modulus = Botan::base64_decode("1DNT1go22VUAU3BON+jCfXxs7Ow9Zxwng4ARTX/vrv6I65bsSYbdBrcc"
		"w/50Fu7AJr8zy8+sXK8wUO4gx00frtA0adaGeZOeBqNq7/K3Gprv98wc"
		"ftbxWjUv75pVl9Ush5yxpBPbuYUnGR/Nh2+K3GRrIrKxWYpNSF1JZYzE"
		"+5k=");

	auto exponent = Botan::base64_decode("AQAB");

	Botan::BigInt n(modulus.data(), modulus.size());
	Botan::BigInt e(exponent.data(), exponent.size());

	auto pk = Botan::RSA_PublicKey(n, e);

	auto signer = std::make_unique<Botan::PK_Verifier>(pk, "EMSA_PKCS1(SHA-1)");

	bool valid = signer->verify_message(msg.data(), msg.size(), &ticketData[length + 4 + 4 + 128 + 20 + 4], sigLength);

	if (!valid)
	{
		console::DPrintf("server", "Connecting player: ticket RSA signature not matching\n");
		return {};
	}

	TicketData outData;

	if (length >= 20)
	{
		std::array<uint8_t, 20> entitlementHash;
		memcpy(entitlementHash.data(), &extraData[0], entitlementHash.size());

		outData.entitlementHash = entitlementHash;
	}

	if (length >= 24)
	{
		uint32_t extraJsonLength = *(uint32_t*)&extraData[20];

		if (length >= (24 + extraJsonLength))
		{
			outData.extraJson = std::string{ (char*)&extraData[24], extraJsonLength };
		}
	}

	return outData;
}

extern std::shared_ptr<ConVar<bool>> g_oneSyncVar;
std::string g_enforcedGameBuild;

static InitFunction initFunction([]()
{
	fx::ServerInstanceBase::OnServerCreate.Connect([](fx::ServerInstanceBase* instance)
	{
		auto minTrustVar = instance->AddVariable<int>("sv_authMinTrust", ConVar_None, 1);
		minTrustVar->GetHelper()->SetConstraints(1, 5);

		auto maxVarianceVar = instance->AddVariable<int>("sv_authMaxVariance", ConVar_None, 5);
		maxVarianceVar->GetHelper()->SetConstraints(1, 5);

		auto shVar = instance->AddVariable<bool>("sv_scriptHookAllowed", ConVar_ServerInfo, false);
		auto ehVar = instance->AddVariable<bool>("sv_enhancedHostSupport", ConVar_ServerInfo, false);

		// list of space-separated endpoints that can but don't have to include a port
		// for example: sv_endpoints "123.123.123.123 124.124.124.124"
		auto srvEndpoints = instance->AddVariable<std::string>("sv_endpoints", ConVar_None, "");
		auto lanVar = instance->AddVariable<bool>("sv_lan", ConVar_ServerInfo, false);

		g_enforcedGameBuild = "1604";
		auto enforceGameBuildVar = instance->AddVariable<std::string>("sv_enforceGameBuild", ConVar_ReadOnly | ConVar_ServerInfo, "1604", &g_enforcedGameBuild);

		instance->GetComponent<fx::GameServer>()->OnTick.Connect([instance, enforceGameBuildVar]()
		{
			if (instance->GetComponent<fx::GameServer>()->GetGameName() == fx::GameName::RDR3)
			{
				if (g_enforcedGameBuild == "1604")
				{
					enforceGameBuildVar->GetHelper()->SetRawValue("1311");
				}
			}

			auto clientRegistry = instance->GetComponent<fx::ClientRegistry>();

			clientRegistry->ForAllClients([](const fx::ClientSharedPtr& client)
			{
				auto deferralAny = client->GetData("deferralPtr");

				if (deferralAny.has_value())
				{
					auto weakDeferral = std::any_cast<std::weak_ptr<fx::ClientDeferral>>(deferralAny);

					if (!weakDeferral.expired())
					{
						client->Touch();
					}
				}
			});
		});

		instance->GetComponent<fx::ClientMethodRegistry>()->AddHandler("getEndpoints", [instance, srvEndpoints](const std::map<std::string, std::string>& postMap, const fwRefContainer<net::HttpRequest>& request, const std::function<void(const json&)>& cb)
		{
			auto sendError = [=](const std::string& error)
			{
				cb(json::object({ { "error", error } }));
				cb(json(nullptr));
			};

			auto tokenIt = postMap.find("token");

			if (tokenIt == postMap.end())
			{
				sendError("fields missing");
				return;
			}

			auto clientRegistry = instance->GetComponent<fx::ClientRegistry>();
			auto client = clientRegistry->GetClientByConnectionToken(tokenIt->second);
			if (!client)
			{
				cb(false);
			}
			else
			{
				auto endpointList = srvEndpoints->GetValue();
				if (endpointList.empty())
				{
					cb(json::array());
				}
				else
				{
					json endpoints;
					for (auto item :
						fx::GetIteratorView(
							std::make_pair(
								boost::algorithm::make_split_iterator(
									endpointList,
									boost::algorithm::token_finder(
										boost::algorithm::is_space(),
										boost::algorithm::token_compress_on
									)
								),
								boost::algorithm::split_iterator<std::string::iterator>()
							)
						)
					)
					{
						auto endpoint = folly::range(&*item.begin(), &*item.end());
						endpoints += endpoint;
					}
					cb(endpoints);
				}
			}

			cb(json(nullptr));
		});

		instance->GetComponent<fx::ClientMethodRegistry>()->AddHandler("initConnect", [=](const std::map<std::string, std::string>& postMap, const fwRefContainer<net::HttpRequest>& request, const std::function<void(const json&)>& cb)
		{
			auto sendError = [=](const std::string& error)
			{
				cb(json::object({ { "error", error } }));
				cb(json(nullptr));
			};

			auto gameServer = instance->GetComponent<fx::GameServer>();

			if (!gameServer->HasSettled())
			{
				sendError("The server is starting up.");
				return;
			}

			auto nameIt = postMap.find("name");
			auto guidIt = postMap.find("guid");
			auto gameBuildIt = postMap.find("gameBuild");
			auto gameNameIt = postMap.find("gameName");

			auto protocolIt = postMap.find("protocol");

			if (nameIt == postMap.end() || guidIt == postMap.end() || protocolIt == postMap.end())
			{
				sendError("fields missing");
				return;
			}

			auto name = nameIt->second;
			auto guid = guidIt->second;
			auto protocol = atoi(protocolIt->second.c_str());
			auto gameBuild = (gameBuildIt != postMap.end()) ? gameBuildIt->second : "0";
			auto gameName = (gameNameIt != postMap.end()) ? gameNameIt->second : "";

			if (protocol < 6)
			{
				sendError("Client/server version mismatch.");
				return;
			}

			if (fx::IsOneSync())
			{
				if (protocol < 11)
				{
					sendError("Client/server version mismatch.");
					return;
				}
			}

			// verify game name
			bool validGameName = false;
			std::string intendedGameName;

			switch (instance->GetComponent<fx::GameServer>()->GetGameName())
			{
			case fx::GameName::GTA4:
				intendedGameName = "gta4";

				if (gameName == "gta4")
				{
					validGameName = true;
				}
				break;
			case fx::GameName::GTA5:
				intendedGameName = "gta5";

				if (gameName.empty() || gameName == "gta5")
				{
					validGameName = true;
				}
				break;
			case fx::GameName::RDR3:
				intendedGameName = "rdr3";

				if (gameName == "rdr3")
				{
					validGameName = true;
				}
				break;
			}

			if (!validGameName)
			{
				sendError(fmt::sprintf("Client/Server game mismatch: %s/%s", gameName, intendedGameName));
				return;
			}

			// limit name length
			if (name.length() >= 200)
			{
				// TODO: cut this off at a sane UTF-8 position
				name = name.substr(0, 200);
			}

			TicketData ticketData;

			if (!lanVar->GetValue())
			{
				auto ticketIt = postMap.find("cfxTicket");

				if (ticketIt == postMap.end())
				{
					sendError("No authentication ticket was specified.");
					return;
				}

				try
				{
					std::string ticketError;

					if (!VerifyTicket(guid, ticketIt->second, &ticketError))
					{
						sendError(fmt::sprintf("Ticket authorization failed. %s", ticketError));
						return;
					}

					auto optionalTicket = VerifyTicketEx(ticketIt->second);

					if (!optionalTicket)
					{
						sendError("Ticket authorization failed. (2)");
						return;
					}

					ticketData = *optionalTicket;
				}
				catch (const std::exception& e)
				{
					sendError(fmt::sprintf("Parsing error while verifying ticket. %s", e.what()));
					return;
				}
			}

			std::string token = boost::uuids::to_string(boost::uuids::basic_random_generator<boost::random_device>()());

			json data = json::object();
			data["protocol"] = 5;
			data["bitVersion"] = 0x202103292050;
			data["sH"] = shVar->GetValue();
			data["enhancedHostSupport"] = ehVar->GetValue() && !fx::IsOneSync();
			data["onesync"] = fx::IsOneSync();
			data["onesync_big"] = fx::IsBigMode();
			data["onesync_lh"] = fx::IsLengthHack();
			data["token"] = token;
			data["gamename"] = gameName;

			auto clientRegistry = instance->GetComponent<fx::ClientRegistry>();

			data["netlibVersion"] = gameServer->GetNetLibVersion();
			data["maxClients"] = atoi(gameServer->GetVariable("sv_maxclients").c_str());

			{
				auto oldClient = clientRegistry->GetClientByGuid(guid);

				if (oldClient)
				{
					gameServer->DropClient(oldClient, "Reconnecting");
				}
			}

			auto ra = request->GetRemoteAddress();

			static std::atomic<uint32_t> g_tempIds;
			auto tempId = g_tempIds.fetch_add(1);

			auto client = clientRegistry->MakeClient(guid);
			client->SetName(name);
			client->SetConnectionToken(token);
			client->SetTcpEndPoint(ra.substr(0, ra.find_last_of(':')));
			client->SetNetId(0x10000 + tempId);

			// add the entitlement hash if needed
			if (ticketData.entitlementHash)
			{
				auto& hash = *ticketData.entitlementHash;
				client->SetData("entitlementHash", fmt::sprintf("%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
					hash[0], hash[1], hash[2], hash[3], hash[4], hash[5], hash[6], hash[7], hash[8], hash[9],
					hash[10], hash[11], hash[12], hash[13], hash[14], hash[15], hash[16], hash[17], hash[18], hash[19]));
			}

			bool gameNameMatch = false;

			if (ticketData.extraJson)
			{
				try
				{
					json json = json::parse(*ticketData.extraJson);

					if (json["gn"].is_string())
					{
						auto sentGameName = json["gn"].get<std::string>();

						if (sentGameName == intendedGameName)
						{
							gameNameMatch = true;
						}
					}
				}
				catch (std::exception& e)
				{

				}

				client->SetData("entitlementJson", *ticketData.extraJson);
			}

			if (lanVar->GetValue())
			{
				gameNameMatch = true;
			}

			if (!gameNameMatch)
			{
				sendError("CitizenFX ticket authorization failed. (3)");
				return;
			}

			client->Touch();

			auto it = g_serverProviders.begin();

			fx::ClientWeakPtr clientWeak{ client };
			auto done = [=]()
			{
				auto lockedClient = clientWeak.lock();
				if (!lockedClient)
				{
					return;
				}

				auto didSucceed = std::make_shared<bool>(false);
				auto weakSuccess = std::weak_ptr<bool>(didSucceed);

				auto allowClient = [=]()
				{
					auto client = clientWeak.lock();

					if (client)
					{
						client->SetData("deferralPtr", std::any());
						client->SetData("passedValidation", true);
						client->SetData("canBeDead", false);
					}

					auto success = weakSuccess.lock();

					if (success)
					{
						*success = true;
					}
				};

				auto addData = [](json& data, const std::shared_ptr<fx::ClientDeferral>& deferrals)
				{
					json handoverData = json::object();

					for (const auto& [ key, value ] : deferrals->GetHandoverData())
					{
						handoverData[key] = json::parse(value);
					}

					data["handover"] = std::move(handoverData);
				};

				int maxTrust = INT_MIN;
				int minVariance = INT_MAX;

				for (const auto& identifier : lockedClient->GetIdentifiers())
				{
					std::string idType = identifier.substr(0, identifier.find_first_of(':'));

					auto provider = g_providersByType[idType];

					if (provider)
					{
						maxTrust = std::max(provider->GetTrustLevel(), maxTrust);
						minVariance = std::min(provider->GetVarianceLevel(), minVariance);
					}
				}

				if (maxTrust < minTrustVar->GetValue() || minVariance > maxVarianceVar->GetValue())
				{
					clientRegistry->RemoveClient(lockedClient);

					sendError("You can not join this server due to your identifiers being insufficient. Please try starting Steam or another identity provider and try again.");
					return;
				}

				auto svGame = instance->GetComponent<fx::GameServer>()->GetGameName();
				bool canEnforceBuild = (svGame == fx::GameName::GTA5 || svGame == fx::GameName::RDR3);

				if (canEnforceBuild && !enforceGameBuildVar->GetValue().empty() && enforceGameBuildVar->GetValue() != gameBuild)
				{
					clientRegistry->RemoveClient(lockedClient);

					sendError(
						fmt::sprintf(
							"This server requires a different game build (%s) from the one you're using (%s).%s",
							enforceGameBuildVar->GetValue(),
							gameBuild,
							(svGame == fx::GameName::GTA5) ? " Tell the server owner to remove this check." : ""
						)
					);

					return;
				}

				auto resman = instance->GetComponent<fx::ResourceManager>();
				auto eventManager = resman->GetComponent<fx::ResourceEventManagerComponent>();
				auto cbComponent = resman->GetComponent<fx::ResourceCallbackComponent>();

				// TODO: replace with event stacks once implemented
				auto noReason = std::make_shared<std::shared_ptr<std::string>>();
				*noReason = std::make_shared<std::string>("Resource prevented connection.");

				auto deferrals = std::make_shared<std::shared_ptr<fx::ClientDeferral>>();
				*deferrals = std::make_shared<fx::ClientDeferral>(instance, lockedClient);

				lockedClient->SetData("deferralPtr", std::weak_ptr<fx::ClientDeferral>(*deferrals));

				// *copy* the callback into a *shared* reference
				auto cbRef = std::make_shared<std::shared_ptr<std::decay_t<decltype(cb)>>>(std::make_shared<std::decay_t<decltype(cb)>>(cb));

				(*deferrals)->SetMessageCallback([cbRef](const std::string& message)
				{
					auto ref1 = *cbRef;

					if (ref1)
					{
						(*ref1)(json::object({ { "defer", true }, { "message", message }, { "deferVersion", 2 } }));
					}
				});

				(*deferrals)->SetCardCallback([cbRef, token](const std::string& card)
				{
					auto ref1 = *cbRef;

					if (ref1)
					{
						(*ref1)(json::object({ { "defer", true }, { "card", card }, { "token", token }, { "deferVersion", 2 } }));
					}
				});

				(*deferrals)->SetResolveCallback([addData, data, deferrals, cbRef, allowClient]()
				{
					allowClient();

					json dataNew = data;

					if (deferrals && *deferrals)
					{
						addData(dataNew, *deferrals);
					}

					auto ref1 = *cbRef;

					if (ref1)
					{
						(**cbRef)(dataNew);
						(**cbRef)(json(nullptr));
					}

					*cbRef = nullptr;
					*deferrals = nullptr;
				});

				auto earlyReject = std::make_shared<bool>(false);
				auto weakEarlyReject = std::weak_ptr(earlyReject);
				auto weakNoReason = std::weak_ptr(noReason);

				(*deferrals)->SetRejectCallback([deferrals, cbRef, clientWeak, clientRegistry, weakEarlyReject, weakNoReason](const std::string& message)
				{
					auto earlyReject = weakEarlyReject.lock();
					auto noReason = weakNoReason.lock();

					if (earlyReject && noReason)
					{
						*noReason = std::make_shared<std::string>(message);
						*earlyReject = true;
					}

					auto newLockedClient = clientWeak.lock();
					if (newLockedClient)
					{
						clientRegistry->RemoveClient(newLockedClient);

						auto ref1 = *cbRef;

						if (ref1)
						{
							(**cbRef)(json::object({ { "error", message } }));
							(**cbRef)(json(nullptr));
						}
					}

					*cbRef = nullptr;
					*deferrals = nullptr;
				});

				request->SetCancelHandler([cbRef, deferrals, clientWeak, clientRegistry, didSucceed]()
				{
					auto newLockedClient = clientWeak.lock();
					if (!*didSucceed && newLockedClient)
					{
						clientRegistry->RemoveClient(newLockedClient);
					}

					*cbRef = nullptr;
					*deferrals = nullptr;
				});

				gscomms_execute_callback_on_main_thread([=]
				{
					auto deferralsRef = *deferrals;

					/*NETEV playerConnecting SERVER
					/#*
					 * A server-side event that is triggered when a player is trying to connect.
					 *
					 * This event can be canceled to reject the player *instantly*, assuming you haven't yielded.
					 *
					 * @param playerName - The display name of the player connecting.
					 * @param setKickReason - A function used to set a reason message for when the event is canceled.
					 * @param deferrals - An object to control deferrals.
					 * @param source - The player's *temporary* NetID (a number in Lua/JS), **not a real argument, use [FromSource] or source**.
					 #/
					declare function playerConnecting(playerName: string, setKickReason: (reason: string) => void, deferrals: {
						/#*
						 * `deferrals.defer` will initialize deferrals for the current resource. It is required to wait for at least a tick after calling defer before calling `update`, `presentCard` or `done`.
						 #/
						defer(): void,

						/#*
						 * `deferrals.update` will send a progress message to the connecting client.
						 *
						 * @param message - The string to send to the client.
						 #/
						update(message: string): void,

						/#*
						 * `deferrals.presentCard` will send an [Adaptive Card](https://adaptivecards.io/) to the client.
						 *
						 * @param card - An object containing card data, or a serialized JSON string with the card information.
						 * @param cb - If present, will be invoked on an `Action.Submit` event from the Adaptive Card.
						 #/
						presentCard(
							card: object | string,
							cb?:
							/#*
							 * A callback to be invoked for `Action.Submit`.
							 *
							 * @param data - A parsed version of the data sent from the card.
							 * @param rawData - A JSON string containing the data sent from the card.
							 #/
							  (data: any, rawData: string) => void
						): void,

						/#*
						 * `deferrals.done` finalizes a deferral. It is required to wait for at least a tick before calling `done` after calling a prior deferral method.
						 *
						 * @param failureReason - If specified, the connection will be refused, and the user will see the specified message as a result. If this is not specified, the user will be allowed to connect.
						 #/
						done(failureReason?: string): void,

						/#*
						 * `deferrals.handover` adds handover data for the client to be able to use at a later point.
						 *
						 * @param data - Data to pass to the connecting client.
						 #/
						handover(data: { [key: string]: any }): void,
					}, source: string): void;
					*/
					bool shouldAllow = (deferralsRef) ? (eventManager->TriggerEvent2("playerConnecting", { fmt::sprintf("internal-net:%d", lockedClient->GetNetId()) }, lockedClient->GetName(), cbComponent->CreateCallback([noReason](const msgpack::unpacked& unpacked)
					{
						auto obj = unpacked.get().as<std::vector<msgpack::object>>();

						if (obj.size() == 1)
						{
							**noReason = obj[0].as<std::string>();
						}
					}),
					deferralsRef->GetCallbacks())) : false;

					if (!shouldAllow)
					{
						clientRegistry->RemoveClient(lockedClient);

						sendError(**noReason);
						return;
					}

					if (*earlyReject)
					{
						clientRegistry->RemoveClient(lockedClient);

						sendError(**noReason);
						return;
					}

					// was the deferral already completed/canceled this frame? if so, just don't respond at all
					if (!deferralsRef)
					{
						return;
					}

					if (!deferralsRef->IsDeferred())
					{
						allowClient();

						json dataNew = data;
						addData(dataNew, *deferrals);

						*cbRef = nullptr;
						*deferrals = nullptr;

						cb(dataNew);
						cb(json(nullptr));
					}
				});
			};

			// seriously C++?
			auto runOneIdentifier = std::make_shared<std::unique_ptr<std::function<void(decltype(g_serverProviders.begin()))>>>();
			*runOneIdentifier = std::make_unique<std::function<void(decltype(g_serverProviders.begin()))>>([=](auto it)
			{
				if (it == g_serverProviders.end())
				{
					done();

					// unset the callback
					*runOneIdentifier = nullptr;
				}
				else
				{
					auto auth = (*it);
					auto thisIt = ++it;

					// if the client randomly disconnects, let's just bail
					auto clientLocked = clientWeak.lock();
					if (!clientLocked)
					{
						// unset the callback
						*runOneIdentifier = nullptr;

						return;
					}

					auth->RunAuthentication(clientLocked, request, postMap, [=](boost::optional<std::string> err)
					{
						// if the client randomly disconnects, let's just bail (again)
						auto newClientLocked = clientWeak.lock();
						if (!newClientLocked)
						{
							// unset the callback
							*runOneIdentifier = nullptr;

							return;
						}

						// if an auth method fails, bail
						if (err)
						{
							clientRegistry->RemoveClient(newClientLocked);

							sendError(*err);

							// unset the callback
							*runOneIdentifier = nullptr;
						}
						else
						{
							(**runOneIdentifier)(thisIt);
						}
					});
				}
			});

			(**runOneIdentifier)(it);
		});

		instance->GetComponent<fx::ClientMethodRegistry>()->AddHandler("submitCard", [=](const std::map<std::string, std::string>& postMap, const fwRefContainer<net::HttpRequest>& request, const std::function<void(const json&)>& cb)
		{
			auto dataIt = postMap.find("data");
			auto tokenIt = postMap.find("token");

			if (dataIt == postMap.end() || tokenIt == postMap.end())
			{
				cb(json::object({ {"error", "fields missing"} }));
				cb(json(nullptr));
				return;
			}

			auto clientRegistry = instance->GetComponent<fx::ClientRegistry>();
			auto client = clientRegistry->GetClientByConnectionToken(tokenIt->second);

			if (!client)
			{
				cb(json::object({ {"error", "no client"} }));
				cb(json(nullptr));
				return;
			}

			auto deferralRef = client->GetData("deferralPtr");

			if (deferralRef.has_value())
			{
				auto deferralPtr = std::any_cast<std::weak_ptr<fx::ClientDeferral>>(deferralRef);
				auto deferrals = deferralPtr.lock();

				if (deferrals)
				{
					deferrals->HandleCardResponse(dataIt->second);
				}
			}

			cb(json::object({ { "result", "ok" } }));
			cb(json(nullptr));
		});
	}, 50);
});


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2013, 2014.
// Modifications copyright (c) 2013, 2014 Oracle and/or its affiliates.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

#ifndef BOOST_GEOMETRY_ALGORITHMS_COVERED_BY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_COVERED_BY_HPP

#include <cstddef>

#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/variant_fwd.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/strategies/cartesian/point_in_box.hpp>
#include <boost/geometry/strategies/cartesian/box_in_box.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace covered_by {

struct use_point_in_geometry
{
    template <typename Geometry1, typename Geometry2, typename Strategy>
    static inline bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2, Strategy const& strategy)
    {
        return detail::within::point_in_geometry(geometry1, geometry2, strategy) >= 0;
    }
};

struct use_relate
{
    template <typename Geometry1, typename Geometry2, typename Strategy>
    static inline bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2, Strategy const& /*strategy*/)
    {
        return Strategy::apply(geometry1, geometry2);
    }
};

}} // namespace detail::covered_by
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type
>
struct covered_by
    : not_implemented<Tag1, Tag2>
{};

template <typename Point, typename Box>
struct covered_by<Point, Box, point_tag, box_tag>
{
    template <typename Strategy>
    static inline bool apply(Point const& point, Box const& box, Strategy const& strategy)
    {
        ::boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(point, box);
    }
};

template <typename Box1, typename Box2>
struct covered_by<Box1, Box2, box_tag, box_tag>
{
    template <typename Strategy>
    static inline bool apply(Box1 const& box1, Box2 const& box2, Strategy const& strategy)
    {
        assert_dimension_equal<Box1, Box2>();
        ::boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(box1, box2);
    }
};

// P/P

template <typename Point1, typename Point2>
struct covered_by<Point1, Point2, point_tag, point_tag>
    : public detail::covered_by::use_point_in_geometry
{};

template <typename Point, typename MultiPoint>
struct covered_by<Point, MultiPoint, point_tag, multi_point_tag>
    : public detail::covered_by::use_point_in_geometry
{};

// P/L

template <typename Point, typename Segment>
struct covered_by<Point, Segment, point_tag, segment_tag>
    : public detail::covered_by::use_point_in_geometry
{};

template <typename Point, typename Linestring>
struct covered_by<Point, Linestring, point_tag, linestring_tag>
    : public detail::covered_by::use_point_in_geometry
{};

template <typename Point, typename MultiLinestring>
struct covered_by<Point, MultiLinestring, point_tag, multi_linestring_tag>
    : public detail::covered_by::use_point_in_geometry
{};

// P/A

template <typename Point, typename Ring>
struct covered_by<Point, Ring, point_tag, ring_tag>
    : public detail::covered_by::use_point_in_geometry
{};

template <typename Point, typename Polygon>
struct covered_by<Point, Polygon, point_tag, polygon_tag>
    : public detail::covered_by::use_point_in_geometry
{};

template <typename Point, typename MultiPolygon>
struct covered_by<Point, MultiPolygon, point_tag, multi_polygon_tag>
    : public detail::covered_by::use_point_in_geometry
{};

// L/L

template <typename Linestring1, typename Linestring2>
struct covered_by<Linestring1, Linestring2, linestring_tag, linestring_tag>
    : public detail::covered_by::use_relate
{};

template <typename Linestring, typename MultiLinestring>
struct covered_by<Linestring, MultiLinestring, linestring_tag, multi_linestring_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiLinestring, typename Linestring>
struct covered_by<MultiLinestring, Linestring, multi_linestring_tag, linestring_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiLinestring1, typename MultiLinestring2>
struct covered_by<MultiLinestring1, MultiLinestring2, multi_linestring_tag, multi_linestring_tag>
    : public detail::covered_by::use_relate
{};

// L/A

template <typename Linestring, typename Ring>
struct covered_by<Linestring, Ring, linestring_tag, ring_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiLinestring, typename Ring>
struct covered_by<MultiLinestring, Ring, multi_linestring_tag, ring_tag>
    : public detail::covered_by::use_relate
{};

template <typename Linestring, typename Polygon>
struct covered_by<Linestring, Polygon, linestring_tag, polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiLinestring, typename Polygon>
struct covered_by<MultiLinestring, Polygon, multi_linestring_tag, polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename Linestring, typename MultiPolygon>
struct covered_by<Linestring, MultiPolygon, linestring_tag, multi_polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiLinestring, typename MultiPolygon>
struct covered_by<MultiLinestring, MultiPolygon, multi_linestring_tag, multi_polygon_tag>
    : public detail::covered_by::use_relate
{};

// A/A

template <typename Ring1, typename Ring2>
struct covered_by<Ring1, Ring2, ring_tag, ring_tag>
    : public detail::covered_by::use_relate
{};

template <typename Ring, typename Polygon>
struct covered_by<Ring, Polygon, ring_tag, polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename Polygon, typename Ring>
struct covered_by<Polygon, Ring, polygon_tag, ring_tag>
    : public detail::covered_by::use_relate
{};

template <typename Polygon1, typename Polygon2>
struct covered_by<Polygon1, Polygon2, polygon_tag, polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename Ring, typename MultiPolygon>
struct covered_by<Ring, MultiPolygon, ring_tag, multi_polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiPolygon, typename Ring>
struct covered_by<MultiPolygon, Ring, multi_polygon_tag, ring_tag>
    : public detail::covered_by::use_relate
{};

template <typename Polygon, typename MultiPolygon>
struct covered_by<Polygon, MultiPolygon, polygon_tag, multi_polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiPolygon, typename Polygon>
struct covered_by<MultiPolygon, Polygon, multi_polygon_tag, polygon_tag>
    : public detail::covered_by::use_relate
{};

template <typename MultiPolygon1, typename MultiPolygon2>
struct covered_by<MultiPolygon1, MultiPolygon2, multi_polygon_tag, multi_polygon_tag>
    : public detail::covered_by::use_relate
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

struct covered_by
{
    template <typename Geometry1, typename Geometry2, typename Strategy>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Strategy const& strategy)
    {
        concept::within::check
            <
                typename tag<Geometry1>::type,
                typename tag<Geometry2>::type,
                typename tag_cast<typename tag<Geometry2>::type, areal_tag>::type,
                Strategy
            >();
        concept::check<Geometry1 const>();
        concept::check<Geometry2 const>();
        assert_dimension_equal<Geometry1, Geometry2>();

        return dispatch::covered_by<Geometry1, Geometry2>::apply(geometry1,
                                                                 geometry2,
                                                                 strategy);
    }

    template <typename Geometry1, typename Geometry2>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             default_strategy)
    {
        typedef typename point_type<Geometry1>::type point_type1;
        typedef typename point_type<Geometry2>::type point_type2;

        typedef typename strategy::covered_by::services::default_strategy
            <
                typename tag<Geometry1>::type,
                typename tag<Geometry2>::type,
                typename tag<Geometry1>::type,
                typename tag_cast<typename tag<Geometry2>::type, areal_tag>::type,
                typename tag_cast
                    <
                        typename cs_tag<point_type1>::type, spherical_tag
                    >::type,
                typename tag_cast
                    <
                        typename cs_tag<point_type2>::type, spherical_tag
                    >::type,
                Geometry1,
                Geometry2
            >::type strategy_type;

        return covered_by::apply(geometry1, geometry2, strategy_type());
    }
};

} // namespace resolve_strategy

namespace resolve_variant {

template <typename Geometry1, typename Geometry2>
struct covered_by
{
    template <typename Strategy>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Strategy const& strategy)
    {
        return resolve_strategy::covered_by
                               ::apply(geometry1, geometry2, strategy);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T), typename Geometry2>
struct covered_by<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>, Geometry2>
{
    template <typename Strategy>
    struct visitor: boost::static_visitor<bool>
    {
        Geometry2 const& m_geometry2;
        Strategy const& m_strategy;

        visitor(Geometry2 const& geometry2, Strategy const& strategy)
        : m_geometry2(geometry2), m_strategy(strategy) {}

        template <typename Geometry1>
        bool operator()(Geometry1 const& geometry1) const
        {
            return covered_by<Geometry1, Geometry2>
                   ::apply(geometry1, m_geometry2, m_strategy);
        }
    };

    template <typename Strategy>
    static inline bool
    apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry1,
          Geometry2 const& geometry2,
          Strategy const& strategy)
    {
        return boost::apply_visitor(visitor<Strategy>(geometry2, strategy), geometry1);
    }
};

template <typename Geometry1, BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct covered_by<Geometry1, boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    template <typename Strategy>
    struct visitor: boost::static_visitor<bool>
    {
        Geometry1 const& m_geometry1;
        Strategy const& m_strategy;

        visitor(Geometry1 const& geometry1, Strategy const& strategy)
        : m_geometry1(geometry1), m_strategy(strategy) {}

        template <typename Geometry2>
        bool operator()(Geometry2 const& geometry2) const
        {
            return covered_by<Geometry1, Geometry2>
                   ::apply(m_geometry1, geometry2, m_strategy);
        }
    };

    template <typename Strategy>
    static inline bool
    apply(Geometry1 const& geometry1,
          boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry2,
          Strategy const& strategy)
    {
        return boost::apply_visitor(visitor<Strategy>(geometry1, strategy), geometry2);
    }
};

template <
    BOOST_VARIANT_ENUM_PARAMS(typename T1),
    BOOST_VARIANT_ENUM_PARAMS(typename T2)
>
struct covered_by<
    boost::variant<BOOST_VARIANT_ENUM_PARAMS(T1)>,
    boost::variant<BOOST_VARIANT_ENUM_PARAMS(T2)>
>
{
    template <typename Strategy>
    struct visitor: boost::static_visitor<bool>
    {
        Strategy const& m_strategy;

        visitor(Strategy const& strategy): m_strategy(strategy) {}

        template <typename Geometry1, typename Geometry2>
        bool operator()(Geometry1 const& geometry1,
                        Geometry2 const& geometry2) const
        {
            return covered_by<Geometry1, Geometry2>
                   ::apply(geometry1, geometry2, m_strategy);
        }
    };

    template <typename Strategy>
    static inline bool
    apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T1)> const& geometry1,
          boost::variant<BOOST_VARIANT_ENUM_PARAMS(T2)> const& geometry2,
          Strategy const& strategy)
    {
        return boost::apply_visitor(visitor<Strategy>(strategy), geometry1, geometry2);
    }
};

} // namespace resolve_variant

/*!
\brief \brief_check12{is inside or on border}
\ingroup covered_by
\details \details_check12{covered_by, is inside or on border}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry which might be inside or on the border of the second geometry
\param geometry2 \param_geometry which might cover the first geometry
\return true if geometry1 is inside of or on the border of geometry2,
    else false
\note The default strategy is used for covered_by detection

\qbk{[include reference/algorithms/covered_by.qbk]}

 */
template<typename Geometry1, typename Geometry2>
inline bool covered_by(Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    return resolve_variant::covered_by<Geometry1, Geometry2>
                          ::apply(geometry1, geometry2, default_strategy());
}

/*!
\brief \brief_check12{is inside or on border} \brief_strategy
\ingroup covered_by
\details \details_check12{covered_by, is inside or on border}, \brief_strategy. \details_strategy_reasons
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry which might be inside or on the border of the second geometry
\param geometry2 \param_geometry which might cover the first geometry
\param strategy strategy to be used
\return true if geometry1 is inside of or on the border of geometry2,
    else false

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/covered_by.qbk]}

*/
template<typename Geometry1, typename Geometry2, typename Strategy>
inline bool covered_by(Geometry1 const& geometry1, Geometry2 const& geometry2,
        Strategy const& strategy)
{
    return resolve_variant::covered_by<Geometry1, Geometry2>
                          ::apply(geometry1, geometry2, strategy);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_COVERED_BY_HPP


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/map.hpp>
#include <stf.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE("map TO DO")
{
  STF_FAIL("TO DO");

}


#include "sim_map.h"
#include "sim_robot.h"
#include "sim_lidar.h"
#include "sim_window.h"
#include "pfilter.h"
#include "sim_landmark.h"
#include <cstdio>
#include <armadillo>
#include <SDL2/SDL.h>
#include <cassert>
#include <iostream>
#include <cstdlib>

using namespace std;
using namespace arma;

// This is a simpler landmark extractor with the goal of extracting the location of the landmarks easily
void sim_tag_extract(mat &tag_landmarks,
    mat &sensor_vals,
    vector<sim_landmark> &landmarks,
    sim_robot &robot,
    sim_map &map) {
  tag_landmarks = mat(2, landmarks.size());
  for (int lid = 0; lid < landmarks.size(); lid++) {
    tag_landmarks.col(lid) = landmarks[lid].sense(robot);
  }
}

icube doubleImageSize(icube &image) {
  icube newframe(image.n_rows/2, image.n_cols/2, 3);
  for (uword i = 0; i < image.n_rows / 2; i++) {
    for (uword j = 0; j < image.n_cols / 2; j++) {
      newframe(i, j, 0) = (image(i*2, j*2, 0) + image(i*2+1, j*2, 0) + image(i*2, j*2+1, 0) + image(i*2+1, j*2+1, 0)) / 4;
      newframe(i, j, 1) = (image(i*2, j*2, 1) + image(i*2+1, j*2, 1) + image(i*2, j*2+1, 1) + image(i*2+1, j*2+1, 1)) / 4;
      newframe(i, j, 2) = (image(i*2, j*2, 2) + image(i*2+1, j*2, 2) + image(i*2, j*2+1, 2) + image(i*2+1, j*2+1, 2)) / 4;
    }
  }
  return newframe;
}

icube partial_frame(icube frame, int x, int y, int width, int height) {
  icube partial_frame(height, width, 3, fill::zeros);
  for (int i = 0; i < (int)partial_frame.n_rows; i++) {
    for (int j = 0; j < (int)partial_frame.n_cols; j++) {
      int x_ = x - width/2 + j;
      int y_ = y - height/2 + i;
      if (x_ < 0 || x_ >= (int)frame.n_cols ||
          y_ < 0 || y_ >= (int)frame.n_rows) {
        continue;
      }
      partial_frame(i, j, 0) = frame(y_, x_, 0);
      partial_frame(i, j, 1) = frame(y_, x_, 1);
      partial_frame(i, j, 2) = frame(y_, x_, 2);
    }
  }
  return partial_frame;
}

int main() {
  srand(getpid());

  // load the map (custom)
  sim_map map;
  map.load("bengC.jpg");

  // create the landmarks (custom)
  vector<sim_landmark> landmarks;
  landmarks.push_back(sim_landmark(88, 202));
  landmarks.push_back(sim_landmark(601, 617));
  landmarks.push_back(sim_landmark(0, 171));
  landmarks.push_back(sim_landmark(98, 627));
  landmarks.push_back(sim_landmark(384, 627));
  landmarks.push_back(sim_landmark(1225, 627));
  landmarks.push_back(sim_landmark(3400, 513));
  landmarks.push_back(sim_landmark(3442, 583));
  landmarks.push_back(sim_landmark(1961, 539));
  landmarks.push_back(sim_landmark(762, 539));

  // load the robot
  sim_robot robot(&map);
  robot.set_size(8);
  robot.set_pose(70, 48, M_PI_2);
  robot.set_noise(0.2, 0.05);

  // load the lidar
  sim_lidar lidar(&robot);
  lidar.set_noise(1.0, 0.1);

/////////////////////////
//  START OF THE BOT
/////////////////////////

  // create the particle filter
  pfilter pf(1000, &map, landmarks, 70, 48, M_PI_2);
  pf.set_size(12);
  pf.set_noise(0.2, 0.05);

  // start up the window
  SDL_Surface *screen = sim_window::init(400, 400);
  icube frame(map.n_rows, map.n_cols, 3, fill::zeros), newframe;
  bool forward = false;
  bool backward = false;
  bool turn_left = false;
  bool turn_right = false;
  bool strafe_left = false;
  bool strafe_right = false;

  for (;;) {
    // see if something is about to quit
    SDL_Event *e;
    bool done = false;
    while ((e = sim_window::get_event())) {
      if (e->type == SDL_QUIT) {
        done = true;
      } else if (e->type == SDL_KEYDOWN) {
        // attempt to see if the user wants to make the robot move
        if (e->key.keysym.sym == SDLK_w)  { forward = true; }
        if (e->key.keysym.sym == SDLK_s)  { backward = true; }
        if (e->key.keysym.sym == SDLK_q)  { turn_left = true; }
        if (e->key.keysym.sym == SDLK_e)  { turn_right = true; }
        if (e->key.keysym.sym == SDLK_a)  { strafe_left = true; }
        if (e->key.keysym.sym == SDLK_d)  { strafe_right = true; }
      } else if (e->type == SDL_KEYUP) {
        if (e->key.keysym.sym == SDLK_w)  { forward = false; }
        if (e->key.keysym.sym == SDLK_s)  { backward = false; }
        if (e->key.keysym.sym == SDLK_q)  { turn_left = false; }
        if (e->key.keysym.sym == SDLK_e)  { turn_right = false; }
        if (e->key.keysym.sym == SDLK_a)  { strafe_left = false; }
        if (e->key.keysym.sym == SDLK_d)  { strafe_right = false; }
      }
    }
    if (done) {
      break;
    }

    // update the robot
    robot.move((strafe_right - strafe_left) * 4, (forward - backward) * 4, (turn_left - turn_right) * .1);
    pf.move((strafe_right - strafe_left) * 4, (forward - backward) * 4, (turn_left - turn_right) * .1);
    mat sensor_values = lidar.sense();
    mat tag_landmarks;
    sim_tag_extract(tag_landmarks, sensor_values, landmarks, robot, map);
    pf.observe(tag_landmarks);

    // predict the position
    vec mu;
    mat sigma;
    pf.predict(mu, sigma);
    cout << "position: " << mu(0) << ", " << mu(1) << ", angle: " << mu(2) * 180 / M_PI << ", error: \n" << sigma << endl;

    // put stuff on the screen
    map.blit(frame);
    for (sim_landmark &lm : landmarks) {
      lm.blit(frame);
    }
    //lidar.blit(frame);
    pf.blit(frame);
    //robot.blit(frame);
    newframe = partial_frame(frame, (int)round(robot.x), (int)round(robot.y), screen->w, screen->h);
    sim_window::blit(screen, newframe);
    SDL_Delay(25);

    // draw the screen
    sim_window::update();
  }

  // clean up
  sim_window::destroy();
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Math/EulerAngles.h>
#include <vw/Camera/CameraSolve.h>
#include <asp/Core/StereoSettings.h>
#include <asp/Camera/RPCModel.h>
#include <asp/Camera/RPC_XML.h>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace asp {

// -----------------------------------------------------------------
// LinescanDGModel class functions

template <class PositionFuncT, class PoseFuncT>
vw::camera::PinholeModel LinescanDGModel<PositionFuncT, PoseFuncT>::linescan_to_pinhole(double y) const {

  double t = this->m_time_func( y );
  return vw::camera::PinholeModel(this->m_position_func(t),  this->m_pose_func(t).rotation_matrix(),
				  this->m_focal_length, -this->m_focal_length,
				  -this->m_detector_origin[0], y - this->m_detector_origin[1]
				  );
}

template <class PositionFuncT, class PoseFuncT>
vw::Vector3 LinescanDGModel<PositionFuncT, PoseFuncT>::get_local_pixel_vector(vw::Vector2 const& pix) const {
  vw::Vector3 local_vec(pix[0]+m_detector_origin[0], m_detector_origin[1], m_focal_length);
  return normalize(local_vec);
}

// Here we use an initial guess for the line number
template <class PositionFuncT, class PoseFuncT>
vw::Vector2 LinescanDGModel<PositionFuncT, PoseFuncT>::point_to_pixel(vw::Vector3 const& point, double starty) const {

  // Use the uncorrected function to get a fast but good starting seed.
  vw::camera::CameraGenericLMA model( this, point );
  int status;
  vw::Vector2 start = point_to_pixel_uncorrected(point, starty);

  // Run the solver
  vw::Vector3 objective(0, 0, 0);
  const double ABS_TOL = 1e-16;
  const double REL_TOL = 1e-16;
  const int    MAX_ITERATIONS = 1e+5;
  vw::Vector2 solution = vw::math::levenberg_marquardtFixed<vw::camera::CameraGenericLMA, 2,3>(model, start, objective, status,
                                                       ABS_TOL, REL_TOL, MAX_ITERATIONS);
  VW_ASSERT( status > 0,
          vw::camera::PointToPixelErr() << "Unable to project point into LinescanDG model" );

  return solution;
}

// Computing the uncorrected pixel location is much faster.
template <class PositionFuncT, class PoseFuncT>
vw::Vector2 LinescanDGModel<PositionFuncT, PoseFuncT>::point_to_pixel_uncorrected(vw::Vector3 const& point, double starty) const {

  // Solve for the correct line number to use
  LinescanLMA model( this, point );
  int status;
  vw::Vector<double> objective(1), start(1);
  start[0] = m_image_size.y()/2;
  // Use a refined guess, if available, otherwise the center line.
  if (starty >= 0)
    start[0] = starty;

  // Run the solver
  const double ABS_TOL = 1e-16;
  const double REL_TOL = 1e-16;
  const int    MAX_ITERATIONS = 1e+5;
  vw::Vector<double> solution = vw::math::levenberg_marquardt(model, start, objective, status,
                                                              ABS_TOL, REL_TOL, MAX_ITERATIONS);

  VW_ASSERT( status > 0, vw::camera::PointToPixelErr() << "Unable to project point into LinescanDG model" );

  // Solve for sample location now that we know the correct line
  double      t  = m_time_func( solution[0] );
  vw::Vector3 pt = inverse( m_pose_func(t) ).rotate( point - m_position_func(t) );
  pt *= m_focal_length / pt.z();

  return vw::Vector2(pt.x() - m_detector_origin[0], solution[0]);
}

// -----------------------------------------------------------------
// LinescanDGModel solver functions

// Function to minimize with the no-correction LMA optimizer.
template <class PositionFuncT, class PoseFuncT>
typename LinescanDGModel<PositionFuncT, PoseFuncT>::LinescanLMA::result_type
LinescanDGModel<PositionFuncT, PoseFuncT>::LinescanLMA::operator()( domain_type const& y ) const {
  double       t        = m_model->get_time_at_line(y[0]);
  vw::Quat     pose     = m_model->get_camera_pose_at_time(t);
  vw::Vector3  position = m_model->m_position_func(t);

  // Get point in camera's frame and rescale to pixel units
  vw::Vector3 pt = vw::camera::point_to_camera_coord(position, pose, m_point);
  pt *= m_model->m_focal_length / pt.z();
  result_type result(1);
  result[0] = pt.y() - m_model->m_detector_origin[1]; // Error against the location of the detector
  return result;
}

// -----------------------------------------------------------------
// LinescanDGModel supporting functions

class SecondsFrom
{
  boost::posix_time::ptime m_reference;
public:
  inline SecondsFrom( boost::posix_time::ptime const& time ) : m_reference(time) {}

  inline double operator()( boost::posix_time::ptime const& time ) const {
    return double( (time - m_reference).total_microseconds() ) / 1e6;
  }
};

inline boost::posix_time::ptime parse_time(std::string str)
{
  try{
    return boost::posix_time::time_from_string(str);
  }catch(...){
    vw::vw_throw(vw::ArgumentErr() << "Failed to parse time from string: " << str
		 << ". If you are not using Digital Globe images, you may need to specify the session type, such as -t rpc, -t rpcmaprpc, -t aster, etc.\n");
  }
  return boost::posix_time::time_from_string(str); // Never reached!
}

boost::shared_ptr<DGCameraModel> load_dg_camera_model_from_xml(std::string const& path)
{
  //vw_out() << "DEBUG - Loading DG camera file: " << camera_file << std::endl;

  // Parse the Digital Globe XML file
  GeometricXML geo;
  AttitudeXML  att;
  EphemerisXML eph;
  ImageXML     img;
  RPCXML       rpc;

  try {
    read_xml( path, geo, att, eph, img, rpc );
  } catch ( const std::exception& e ){
    vw::vw_throw(vw::ArgumentErr() << "Invalid Digital Globe XML file: " << path
		 << ". If you are not using Digital Globe images, you may need to specify the session type, such as -t rpc, -t rpcmaprpc, -t aster, etc.\n"
		 << e.what() << "\n");
  }

  // Get an estimate of the surface elevation from the corners specified in the file.
  // - Not every file has this information, in which case we will just use zero.
  double mean_ground_elevation = 0;
  vw::BBox3 bbox = rpc.get_lon_lat_height_box();
  if (!bbox.empty())
    mean_ground_elevation = (bbox.min()[2] + bbox.max()[2]) / 2.0;

  // Convert measurements in millimeters to pixels.
  geo.principal_distance /= geo.detector_pixel_pitch;
  geo.detector_origin    /= geo.detector_pixel_pitch;

  // Convert all time measurements to something that boost::date_time can read.
  boost::replace_all( eph.start_time,            "T", " " );
  boost::replace_all( img.tlc_start_time,        "T", " " );
  boost::replace_all( img.first_line_start_time, "T", " " );
  boost::replace_all( att.start_time,            "T", " " );

  // Convert UTC time measurements to line measurements. Ephemeris
  // start time will be our reference frame to calculate seconds against.
  SecondsFrom convert( parse_time( eph.start_time ) );

  // I'm going make the assumption that EPH and ATT are sampled at the same rate and time.
  VW_ASSERT( eph.position_vec.size() == att.quat_vec.size(),
	     vw::MathErr() << "Ephemeris and Attitude don't have the same number of samples." );
  VW_ASSERT( eph.start_time == att.start_time && eph.time_interval == att.time_interval,
	     vw::MathErr() << "Ephemeris and Attitude don't seem to sample with the same t0 or dt." );

  // Convert ephemeris to be position of camera. Change attitude to
  // be the rotation from camera frame to world frame. We also add an
  // additional rotation to the camera frame so X is the horizontal
  // direction to the picture and +Y points down the image (in the direction of flight).
  vw::Quat sensor_coordinate = vw::math::euler_xyz_to_quaternion(vw::Vector3(0,0,geo.detector_rotation - M_PI/2));
  for ( size_t i = 0; i < eph.position_vec.size(); i++ ) {
    eph.position_vec[i] += att.quat_vec[i].rotate( geo.perspective_center );
    att.quat_vec[i] = att.quat_vec[i] * geo.camera_attitude * sensor_coordinate;
  }

  //vw::vw_out() << "DG model load - sensor_coordinate = " << sensor_coordinate << std::endl;
  //geo.printDebugInfo(); // DEBUG INFO

  // Load up the time interpolation class. If the TLCList only has
  // one entry ... then we have to manually drop in the slope and offset.
  if ( img.tlc_vec.size() == 1 ) {
    double direction = 1;
    if ( boost::to_lower_copy( img.scan_direction ) != "forward" ) {
      direction = -1;
    }
    img.tlc_vec.push_back( std::make_pair(img.tlc_vec.front().first +
					  img.avg_line_rate, direction) );
  }

  // Build the TLCTimeInterpolation object and do a quick sanity check.
  vw::camera::TLCTimeInterpolation tlc_time_interpolation( img.tlc_vec,
							       convert( parse_time( img.tlc_start_time ) ) );
  VW_ASSERT( fabs( convert( parse_time( img.first_line_start_time ) ) -
  tlc_time_interpolation( 0 ) ) < fabs( 1.0 / (10.0 * img.avg_line_rate ) ),
	     vw::MathErr()
	     << "First Line Time and output from TLC lookup table "
	     << "do not agree of the ephemeris time for the first line of the image. "
	     << "If your XML camera files are not from the WorldView satellites, "
	     << "you may try the switch -t rpc to use the RPC camera model.\n"
	     << "The first image line ephemeris time is: "
  	     << convert( parse_time( img.first_line_start_time ) ) << ".\n"
	     << "The TLC look up table time is: " << tlc_time_interpolation( 0 ) << ".\n"
	     << "Maximum allowed difference is 1/10 of avg line rate, which is: "
	     << fabs( 1.0 / (10.0 * img.avg_line_rate ))
	     << ".\n"
    );

  vw::Vector2 final_detector_origin
    = subvector(inverse(sensor_coordinate).rotate(vw::Vector3(geo.detector_origin[0],
							      geo.detector_origin[1],
							      0)), 0, 2);

  double et0 = convert( parse_time( eph.start_time ) );
  double at0 = convert( parse_time( att.start_time ) );
  double edt = eph.time_interval;
  double adt = att.time_interval;

  // This is where we could set the Earth radius if we have that info.

  typedef boost::shared_ptr<DGCameraModel> CameraModelPtr;
  return CameraModelPtr(new DGCameraModel(vw::camera::PiecewiseAPositionInterpolation(eph.position_vec, eph.velocity_vec, et0, edt ),
					                                vw::camera::LinearPiecewisePositionInterpolation(eph.velocity_vec, et0, edt),
					                                vw::camera::SLERPPoseInterpolation(att.quat_vec, at0, adt),
					                                tlc_time_interpolation, img.image_size,
					                                final_detector_origin,
					                                geo.principal_distance,
					                                mean_ground_elevation,
					                                !stereo_settings().disable_correct_velocity_aberration,
					                                !stereo_settings().disable_correct_atmospheric_refraction)
		    );
} // End function load_dg_camera_model()

} // end namespace asp


#include "sim_inter.h"
#include <Eigen/Geometry>
#include <gflags/gflags.h>

DEFINE_double(starttime, 0, "Process time at which to start the simulation");
DEFINE_double(endtime, 9999, "Process time at which to end the simulation");

namespace {
constexpr double kAirmarOffset = -.0;

};

namespace sailbot {
namespace sim {

SimulatorNode::SimulatorNode(double reset_period, bool set_pos)
    : Node(dt),
      reset_period_(reset_period),
      set_pos_(set_pos),
      // impl_(new SimulatorSaoud2013(dt)),
      impl_(new TrivialDynamics(dt)), sdot_(0), rdot_(0), bdot_(0),
      state_queue_("sim_true_boat_state", true),
      state_msg_(AllocateMessage<msg::BoatState>()),
      wind_queue_("sim_true_wind", true),
      wind_msg_(AllocateMessage<msg::Vector3f>()) {
    RegisterHandler<msg::SailCmd>(
        "sail_cmd",
        std::bind(&SimulatorNode::ProcessSail, this, std::placeholders::_1));
    RegisterHandler<msg::RudderCmd>(
        "rudder_cmd",
        std::bind(&SimulatorNode::ProcessRudder, this, std::placeholders::_1));
    RegisterHandler<msg::BallastCmd>(
        "ballast_cmd",
        std::bind(&SimulatorNode::ProcessBallast, this, std::placeholders::_1));
    RegisterHandler<msg::Vector3f>("sim_set_wind",
                                   [this](const msg::Vector3f &msg) {
      set_wind(msg.x(), msg.y(), msg.z());
      // Don't actually need to correct for airmar, because this is the true,
      // not apparent, wind.
      // float ang = std::atan2(msg.y(), msg.x());
      // float speed = std::sqrt(msg.x() * msg.x() + msg.y() * msg.y());
      // ang -= kAirmarOffset; // not sure of sign
      // set_wind(ang, speed);
    });
    // For when doing log replay, retrieve logged state.
    // And set sail appropriately.
    RegisterHandler<msg::BoatState>("orig_boat_state",
                                    [this](const msg::BoatState &msg) {
      std::unique_lock<std::mutex> lck(last_state_mutex_);
      last_state_ = msg;
      last_state_.mutable_euler()->set_yaw(msg.euler().yaw() - kAirmarOffset);

      deltas_ = msg.internal().sail();
    });
}

void SimulatorNode::ProcessSail(const msg::SailCmd& cmd) {
  if (cmd.has_vel() && !std::isnan(cmd.vel())) {
    sdot_ = cmd.vel();
  } else if (cmd.has_pos() && !std::isnan(cmd.pos())) {
    double pos = cmd.pos();
    sdot_ = impl_->get_sdot_for_goal(pos);
  }
}

void SimulatorNode::ProcessRudder(const msg::RudderCmd& cmd) {
  deltar_ = cmd.pos();
  if (cmd.has_vel() && !std::isnan(cmd.vel())) {
    rdot_ = cmd.vel();
  } else if (cmd.has_pos() && !std::isnan(cmd.pos())) {
    rdot_ = impl_->get_rdot_for_goal(cmd.pos());
  }
}

void SimulatorNode::ProcessBallast(const msg::BallastCmd& cmd) {
  if (cmd.has_vel() && !std::isnan(cmd.vel())) {
    bdot_ = cmd.vel();
  } else if (cmd.has_voltage()) {
    bdot_ = cmd.voltage() / 12.0;
  }
}

void SimulatorNode::Run() {
  std::thread t_internal(&FakeInternalSensors::Run, &internal_sensors_);
  std::thread t_airmar(&FakeAirmar::Run, &fake_airmar_);
  Node::Run();
  t_airmar.join();
  t_internal.join();
}

void SimulatorNode::Iterate() {
  const double systime =
      std::chrono::nanoseconds(util::monotonic_clock::now().time_since_epoch())
          .count() /
      1e9;
  if (systime < FLAGS_starttime) {
    return;
  }
  if (systime > FLAGS_endtime) {
    util::RaiseShutdown();
    return;
  }
  if (reset_period_ > 0 && systime > next_reset_) {
    started_ = false;
    next_reset_ = systime + reset_period_;
  }
  if (!started_) {
    std::unique_lock<std::mutex> lck(last_state_mutex_);
    impl_->set_from_state(last_state_);
    if (last_state_.has_pos()) {
      fake_airmar_.set_gps_zero(last_state_.pos().y(), last_state_.pos().x());
    } else {
      fake_airmar_.set_gps_zero(42.276126, -71.756934);
    }
    started_ = true;
  }
  usleep(dt * 1e6 / 2.);
  static float time = 0;
  VLOG(2) << "Time: " << (time += dt);
  VLOG(1) << "s, r: " << sdot_ << ", " << rdot_;
  //if (time > .05) std::exit(0);
  if (set_pos_) {
    impl_->Update(deltas_, deltar_);
  } else {
    impl_->Update(sdot_, rdot_, bdot_);
  }
  Eigen::Vector3d omega = impl_->get_omega();
  Eigen::Vector3d x = impl_->get_x();
  Eigen::Vector3d v = impl_->get_v();
  Eigen::Quaternion<double> rot(impl_->get_RBI());

  Eigen::Vector3d rollpitchyaw = util::GetRollPitchYaw(impl_->get_RBI());

  msg::Vector3d *pos = state_msg_->mutable_pos();
  pos->set_x(x(0));
  pos->set_y(x(1));
  pos->set_z(x(2));

  msg::Vector3f *vel = state_msg_->mutable_vel();
  vel->set_x(v(0));
  vel->set_y(v(1));
  vel->set_z(v(2));

  msg::Vector3f *wmsg = state_msg_->mutable_omega();
  wmsg->set_x(omega(0));
  wmsg->set_y(omega(1));
  wmsg->set_z(omega(2));

  msg::Quaternion *qmsg = state_msg_->mutable_orientation();
  qmsg->set_w(rot.w());
  qmsg->set_x(rot.x());
  qmsg->set_y(rot.y());
  qmsg->set_z(rot.z());

  msg::EulerAngles *euler = state_msg_->mutable_euler();
  euler->set_roll(rollpitchyaw(0, 0));
  euler->set_pitch(rollpitchyaw(1, 0));
  euler->set_yaw(rollpitchyaw(2, 0));

  state_msg_->mutable_internal()->set_sail(std::abs(impl_->get_deltas()));
  // XXX(james): Resolve abs vs. not for deltas
  state_msg_->mutable_internal()->set_sail(impl_->get_deltas());
  state_msg_->mutable_internal()->set_rudder(impl_->get_deltar());
  state_msg_->mutable_internal()->set_ballast(impl_->get_deltab());
  state_msg_->mutable_internal()->set_saildot(sdot_);
  state_msg_->mutable_internal()->set_rudderdot(rdot_);
  state_msg_->mutable_internal()->set_ballastdot(bdot_);

  state_queue_.send(state_msg_);
}

}  // sim
}  // sailbot