#include "simulation/rope.h"
#include "simulation/basicobjects.h"
#include <iostream>
#include <boost/make_shared.hpp>

using namespace std;

boost::shared_ptr<btGeneric6DofSpringConstraint>  createBendConstraint(
        btScalar len,
        const boost::shared_ptr<btRigidBody> rbA,
        const boost::shared_ptr<btRigidBody>& rbB,
        float damping, float stiffness, float limit)
{
    btTransform tA, tB;
    tA.setIdentity();
    tB.setIdentity();
    tA.setOrigin(btVector3(len / 2, 0,0));
    tB.setOrigin(btVector3(-len / 2, 0,0));

    boost::shared_ptr<btGeneric6DofSpringConstraint> springPtr =
            boost::make_shared<btGeneric6DofSpringConstraint>(*rbA, *rbB, tA, tB, false);
    for (int i = 3; i <= 5; i++)
    {
        springPtr->enableSpring(i, true);
        springPtr->setStiffness(i, stiffness);
        springPtr->setDamping(i, damping);
    }

    springPtr->setAngularLowerLimit(btVector3(-limit, -limit, -limit));
    springPtr->setAngularUpperLimit(btVector3(limit, limit, limit));
    return springPtr;
}

void createRopeTransforms(
        vector<btTransform>& transforms,
        vector<btScalar>& lengths,
        const vector<btVector3>& ctrlPoints)
{
    const int nLinks = ctrlPoints.size() - 1;
    for (int i = 0; i < nLinks; i++)
    {
        btVector3 pt0 = ctrlPoints[i];
        btVector3 pt1 = ctrlPoints[i+1];
        btVector3 midpt = (pt0+pt1)/2;
        btVector3 diff = (pt1-pt0);

        btQuaternion q;
        btScalar ang = diff.angle(btVector3(1,0,0));
        if (ang * ang > 1e-4)
        {
            btVector3 ax = diff.cross(btVector3(1,0,0));
            q = btQuaternion(ax, -ang);
        }
        else
        {
            q = btQuaternion(0, 0, 0, 1);
        }
        btTransform trans(q, midpt);

        float len = diff.length();
        transforms.push_back(trans);
        lengths.push_back(len);
    }
}

CapsuleRope::CapsuleRope(
        const vector<btVector3>& ctrlPoints,
        btScalar radius_, float angStiffness_,
        float angDamping_, float linDamping_, float angLimit_)
    : angStiffness(angStiffness_)
    , angDamping(angDamping_)
    , linDamping(linDamping_)
    , angLimit(angLimit_)
    , radius(radius_)
    , nLinks(ctrlPoints.size() - 1)
{
    vector<btTransform> transforms;
    vector<btScalar> lengths;
    createRopeTransforms(transforms, lengths, ctrlPoints);
    for (int i = 0; i < nLinks; i++)
    {
        btTransform trans = transforms[i];
        btScalar len = lengths[i];
        float mass = 1;
        CapsuleObject::Ptr child(new CapsuleObject(1,radius,len,trans));
        child->rigidBody->setDamping(linDamping,angDamping);
        child->rigidBody->setFriction(1);

        children.push_back(child);

        if (i > 0)
        {
            auto jointPtr = boost::make_shared<btPoint2PointConstraint>(
                        *children[i-1]->rigidBody,
                        *children[i]->rigidBody,
                        btVector3(len / 2.0, 0, 0),
                        btVector3(-len / 2.0, 0, 0));
            joints.push_back(boost::make_shared<BulletConstraint>(jointPtr, true));

            auto springPtr = createBendConstraint(
                        len,
                        children[i-1]->rigidBody,
                        children[i]->rigidBody,
                        angDamping,
                        angStiffness,
                        angLimit);
            joints.push_back(BulletConstraint::Ptr(new BulletConstraint(springPtr, true)));
        }
    }
}

void CapsuleRope::init()
{
    CompoundObject<BulletObject>::init();

    for (int i = 0; i< joints.size(); i++)
    {
        getEnvironment()->addConstraint(joints[i]);
    }
}

void CapsuleRope::destroy()
{
    CompoundObject<BulletObject>::destroy();
}

vector<btVector3> CapsuleRope::getNodes() const
{
    vector<btVector3> out(children.size());
    for (int i = 0; i < children.size(); i++)
    {
        out[i] = children[i]->rigidBody->getCenterOfMassPosition();
    }
    return out;
}

vector<btVector3> CapsuleRope::getControlPoints() const
{
    vector<btVector3> out;
    out.reserve(children.size()+1);
    for (int i = 0; i < children.size(); i++)
    {
        btRigidBody* body = children[i]->rigidBody.get();
        btCapsuleShape* capsule = dynamic_cast<btCapsuleShapeX*>(body->getCollisionShape());
        btTransform tf = body->getCenterOfMassTransform();
        if (i == 0)
        {
            out.push_back(tf.getOrigin() + btMatrix3x3(tf.getRotation()) * btVector3(-capsule->getHalfHeight(), 0, 0));
        }
        out.push_back(tf.getOrigin() + btMatrix3x3(tf.getRotation()) * btVector3(capsule->getHalfHeight(), 0, 0));
    }
    return out;
}


/*
 * adapt3d_test.cpp
 *
 *  Created on: Sep 23, 2009
 *      Author: dberrios
 */

#include <boost/lexical_cast.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <iostream>
#include <ccmc/Adapt3D.h>
#include <ccmc/Interpolator.h>
#include <string>
#include <ctime>

/**
 * adapt3d <inputfile> variable x y z
 */
int main (int argc, char * argv[])
{
	boost::minstd_rand generator(42u);
	boost::uniform_real<> xrandom(-30.0,-27);
	boost::uniform_real<> yrandom(-3.0, 3.0);
	boost::uniform_real<> zrandom(-3.0, 3.0);
	boost::variate_generator<boost::minstd_rand&, boost::uniform_real<> > xrandomGenerator(generator, xrandom);
	boost::variate_generator<boost::minstd_rand&, boost::uniform_real<> > yrandomGenerator(generator, yrandom);
	boost::variate_generator<boost::minstd_rand&, boost::uniform_real<> > zrandomGenerator(generator, zrandom);
	if (argc != 6)
	{
		std::cout << "adapt3d <inputfile> variable x y z" << std::endl;
		exit(1);
	}
	ccmc::Adapt3D adapt3d;
	//adapt3d = new ccmc::Adapt3D();

	std::string filename = argv[1];
	std::string variable = argv[2];
	float x = boost::lexical_cast<float>(argv[3]);
	float y = boost::lexical_cast<float>(argv[4]);
	float z = boost::lexical_cast<float>(argv[5]);

	adapt3d.open(filename);
	adapt3d.loadVariable(variable);

	clock_t begin = clock();
	ccmc::Interpolator * interpolator = adapt3d.createNewInterpolator();
	float upper = 30.0;
	int numValues = 300;
	int numPositions = 1000;
	std::vector<float> xvalues(numPositions);
	std::vector<float> yvalues(numPositions);
	std::vector<float> zvalues(numPositions);
	for (int i = 0; i < numPositions; i++)
	{
		float c0,c1,c2;

		c0 = xrandomGenerator();
		c1 = yrandomGenerator();
		c2 = zrandomGenerator();

		xvalues[i] = c0;
		yvalues[i] = c1;
		zvalues[i] = c2;
		//if (randomGenerator() > .5) { c0 *= -1.0f;};
		//if (randomGenerator() > .5) { c1 *= -1.0f;};
		//if (randomGenerator() > .5) { c2 *= -1.0f;};
		//float value = interpolator->interpolate(variable, c0, c1, c2);
	}
	std::vector<float> previousResults(numPositions);
	for (int num = 0; num < numValues; num++)
	{
		for (int i = 0; i <numPositions; i++)
		{
			float value = interpolator->interpolate(variable, xvalues[i], yvalues[i], zvalues[i]);
			if (num != 0 && previousResults[i] != value)
			{
				std::cout << "Current result (" << value << ") is different from previous result (";
				std::cout << previousResults[i] << ") for position(" << xvalues[i] << "," << yvalues[i] ;
				std::cout << "," << zvalues[i] << ")" << std::endl;
			}
			previousResults[i] = value;

		}
	}
	clock_t end = clock();
	float final = (end-begin)/CLOCKS_PER_SEC;
	std::cout << "elapsed time: " << final << " seconds" << std::endl;
	float value = interpolator->interpolate(variable, x, y, z);
	//float density = interpolator->interpolate("rho", x, y, z);
	delete interpolator;

	std::string units = adapt3d.getNativeUnit(variable);

	std::cout << "interpolated value: " << value << " " << units << std::endl;
	adapt3d.close();
	return 0;

}


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

#include <gtest/gtest.h>
#include <array>
#include <boost/fiber/future/async.hpp>
#include <boost/fiber/future/future.hpp>
#include <iterator>
#include <memory>

#include "algorithm/circuit_loader.h"
#include "base/gate_register.h"
#include "communication/communication_layer.h"
#include "crypto/base_ots/base_ot_provider.h"
#include "crypto/motion_base_provider.h"
#include "crypto/oblivious_transfer/ot_provider.h"
#include "gate/new_gate.h"
#include "protocols/beavy/wire.h"
#include "protocols/gmw/wire.h"
#include "protocols/yao/wire.h"
#include "protocols/yao/yao_provider.h"
#include "utility/logger.h"

using namespace MOTION::proto::yao;

class YaoTest : public ::testing::Test {
 protected:
  void SetUp() override {
    comm_layers_ = MOTION::Communication::make_dummy_communication_layers(2);
    for (std::size_t i = 0; i < 2; ++i) {
      loggers_[i] = std::make_shared<MOTION::Logger>(i, boost::log::trivial::severity_level::trace);
      comm_layers_[i]->set_logger(loggers_[i]);
      base_ot_providers_[i] =
          std::make_unique<MOTION::BaseOTProvider>(*comm_layers_[i], nullptr, nullptr);
      motion_base_providers_[i] =
          std::make_unique<MOTION::Crypto::MotionBaseProvider>(*comm_layers_[i], nullptr);
      ot_provider_managers_[i] = std::make_unique<ENCRYPTO::ObliviousTransfer::OTProviderManager>(
          *comm_layers_[i], *base_ot_providers_[i], *motion_base_providers_[i], nullptr, nullptr);
      gate_registers_[i] = std::make_unique<MOTION::GateRegister>();
      yao_providers_[i] = std::make_unique<YaoProvider>(
          *comm_layers_[i], *gate_registers_[i], circuit_loader_, *motion_base_providers_[i],
          ot_provider_managers_[i]->get_provider(1 - i), loggers_[i]);
    }
  }

  void TearDown() override {
    std::vector<std::future<void>> futs;
    for (std::size_t i = 0; i < 2; ++i) {
      futs.emplace_back(std::async(std::launch::async, [this, i] { comm_layers_[i]->shutdown(); }));
    }
    std::for_each(std::begin(futs), std::end(futs), [](auto& f) { f.get(); });
  }

  const std::size_t garbler_i_ = 0;
  const std::size_t evaluator_i_ = 1;
  ENCRYPTO::ObliviousTransfer::OTProvider& get_garbler_ot_provider() {
    return ot_provider_managers_[garbler_i_]->get_provider(evaluator_i_);
  }
  ENCRYPTO::ObliviousTransfer::OTProvider& get_evaluator_ot_provider() {
    return ot_provider_managers_[evaluator_i_]->get_provider(garbler_i_);
  }

  void run_setup() {
    std::vector<std::future<void>> futs;
    for (std::size_t i = 0; i < 2; ++i) {
      futs.emplace_back(std::async(std::launch::async, [this, i] {
        comm_layers_[i]->start();
        motion_base_providers_[i]->setup();
        base_ot_providers_[i]->ComputeBaseOTs();
        ot_provider_managers_[i]->get_provider(1 - i).SendSetup();
        ot_provider_managers_[i]->get_provider(1 - i).ReceiveSetup();
        yao_providers_[i]->setup();
      }));
    }
    std::for_each(std::begin(futs), std::end(futs), [](auto& f) { f.get(); });
  }

  void run_gates_setup() {
    auto& gates_g = gate_registers_[garbler_i_]->get_gates();
    auto& gates_e = gate_registers_[evaluator_i_]->get_gates();
    auto fut_g = std::async(std::launch::async, [&gates_g] {
      for (auto& gate : gates_g) {
        if (gate->need_setup()) {
          gate->evaluate_setup();
        }
      }
    });
    auto fut_e = std::async(std::launch::async, [&gates_e] {
      for (auto& gate : gates_e) {
        if (gate->need_setup()) {
          gate->evaluate_setup();
        }
      }
    });
    fut_g.get();
    fut_e.get();
  }

  void run_gates_online() {
    auto& gates_g = gate_registers_[garbler_i_]->get_gates();
    auto& gates_e = gate_registers_[evaluator_i_]->get_gates();
    auto fut_g = std::async(std::launch::async, [&gates_g] {
      for (auto& gate : gates_g) {
        if (gate->need_online()) {
          gate->evaluate_online();
        }
      }
    });
    auto fut_e = std::async(std::launch::async, [&gates_e] {
      for (auto& gate : gates_e) {
        if (gate->need_online()) {
          gate->evaluate_online();
        }
      }
    });
    fut_g.get();
    fut_e.get();
  }

  MOTION::CircuitLoader circuit_loader_;
  std::vector<std::unique_ptr<MOTION::Communication::CommunicationLayer>> comm_layers_;
  std::array<std::unique_ptr<MOTION::BaseOTProvider>, 2> base_ot_providers_;
  std::array<std::unique_ptr<MOTION::Crypto::MotionBaseProvider>, 2> motion_base_providers_;
  std::array<std::unique_ptr<ENCRYPTO::ObliviousTransfer::OTProviderManager>, 2>
      ot_provider_managers_;
  std::array<std::unique_ptr<MOTION::GateRegister>, 2> gate_registers_;
  std::array<std::unique_ptr<YaoProvider>, 2> yao_providers_;
  std::array<std::shared_ptr<MOTION::Logger>, 2> loggers_;
};

static std::vector<ENCRYPTO::BitVector<>> generate_inputs(std::size_t num_wires,
                                                          std::size_t num_simd) {
  std::vector<ENCRYPTO::BitVector<>> inputs;
  std::generate_n(std::back_inserter(inputs), num_wires,
                  [num_simd] { return ENCRYPTO::BitVector<>::Random(num_simd); });
  return inputs;
}

TEST_F(YaoTest, InputGarbler) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);
  ASSERT_EQ(wires_g.size(), num_wires);
  ASSERT_EQ(wires_e.size(), num_wires);
  auto check_num_simd_f = [num_simd](const auto& w) { return w->get_num_simd(); };
  ASSERT_TRUE(std::all_of(std::begin(wires_g), std::end(wires_g), check_num_simd_f));
  ASSERT_TRUE(std::all_of(std::begin(wires_e), std::end(wires_e), check_num_simd_f));

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  // check wire values
  const auto global_offset = yao_providers_[garbler_i_]->get_global_offset();
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& input_bits = inputs.at(wire_i);
    const auto wire_g = std::dynamic_pointer_cast<YaoWire>(wires_g.at(wire_i));
    const auto wire_e = std::dynamic_pointer_cast<YaoWire>(wires_e.at(wire_i));
    const auto& keys_g = wire_g->get_keys();
    const auto& keys_e = wire_e->get_keys();
    ASSERT_EQ(keys_g.size(), num_simd);
    ASSERT_EQ(keys_e.size(), num_simd);
    for (std::size_t simd_i = 0; simd_i < num_simd; ++simd_i) {
      if (input_bits.Get(simd_i)) {
        ASSERT_EQ(keys_e.at(simd_i), keys_g.at(simd_i) ^ global_offset);
      } else {
        ASSERT_EQ(keys_e.at(simd_i), keys_g.at(simd_i));
      }
    }
  }
}

TEST_F(YaoTest, InputEvaluator) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  auto inputs = generate_inputs(num_wires, num_simd);

  auto wires_g =
      yao_providers_[garbler_i_]->make_boolean_input_gate_other(evaluator_i_, num_wires, num_simd);
  auto [input_promise, wires_e] =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_my(evaluator_i_, num_wires, num_simd);
  ASSERT_EQ(wires_g.size(), num_wires);
  ASSERT_EQ(wires_e.size(), num_wires);
  auto check_num_simd_f = [num_simd](const auto& w) { return w->get_num_simd(); };
  ASSERT_TRUE(std::all_of(std::begin(wires_g), std::end(wires_g), check_num_simd_f));
  ASSERT_TRUE(std::all_of(std::begin(wires_e), std::end(wires_e), check_num_simd_f));

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  // check wire values
  const auto global_offset = yao_providers_[garbler_i_]->get_global_offset();
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& input_bits = inputs.at(wire_i);
    const auto wire_g = std::dynamic_pointer_cast<YaoWire>(wires_g.at(wire_i));
    const auto wire_e = std::dynamic_pointer_cast<YaoWire>(wires_e.at(wire_i));
    const auto& keys_g = wire_g->get_keys();
    const auto& keys_e = wire_e->get_keys();
    ASSERT_EQ(keys_g.size(), num_simd);
    ASSERT_EQ(keys_e.size(), num_simd);
    for (std::size_t simd_i = 0; simd_i < num_simd; ++simd_i) {
      if (input_bits.Get(simd_i)) {
        ASSERT_EQ(keys_e.at(simd_i), keys_g.at(simd_i) ^ global_offset);
      } else {
        ASSERT_EQ(keys_e.at(simd_i), keys_g.at(simd_i));
      }
    }
  }
}

TEST_F(YaoTest, OutputGarbler) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);
  auto output_future = yao_providers_[garbler_i_]->make_boolean_output_gate_my(garbler_i_, wires_g);
  yao_providers_[evaluator_i_]->make_boolean_output_gate_other(garbler_i_, wires_e);
  ASSERT_TRUE(output_future.valid());

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  auto outputs = output_future.get();

  // check output values
  ASSERT_EQ(inputs, outputs);
}

TEST_F(YaoTest, OutputEvaluator) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);
  yao_providers_[garbler_i_]->make_boolean_output_gate_other(evaluator_i_, wires_g);
  auto output_future =
      yao_providers_[evaluator_i_]->make_boolean_output_gate_my(evaluator_i_, wires_e);
  ASSERT_TRUE(output_future.valid());

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  auto outputs = output_future.get();

  // check output values
  ASSERT_EQ(inputs, outputs);
}

TEST_F(YaoTest, OutputBoth) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);
  auto output_future_g =
      yao_providers_[garbler_i_]->make_boolean_output_gate_my(MOTION::ALL_PARTIES, wires_g);
  auto output_future_e =
      yao_providers_[evaluator_i_]->make_boolean_output_gate_my(MOTION::ALL_PARTIES, wires_e);
  ASSERT_TRUE(output_future_g.valid());
  ASSERT_TRUE(output_future_e.valid());

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  auto outputs_g = output_future_g.get();
  auto outputs_e = output_future_e.get();

  // check output values
  ASSERT_EQ(inputs, outputs_g);
  ASSERT_EQ(inputs, outputs_e);
}

TEST_F(YaoTest, INV) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);
  MOTION::BitValues expected_output;
  std::transform(std::begin(inputs), std::end(inputs), std::back_inserter(expected_output),
                 [](const auto& bv) { return ~bv; });

  auto [input_promise, wires_g_in] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g_out = yao_providers_[garbler_i_]->make_unary_gate(
      ENCRYPTO::PrimitiveOperationType::INV, wires_g_in);
  auto wires_e_out = yao_providers_[evaluator_i_]->make_unary_gate(
      ENCRYPTO::PrimitiveOperationType::INV, wires_e_in);

  auto output_future_g =
      yao_providers_[garbler_i_]->make_boolean_output_gate_my(garbler_i_, wires_g_out);
  yao_providers_[evaluator_i_]->make_boolean_output_gate_other(garbler_i_, wires_e_out);
  ASSERT_TRUE(output_future_g.valid());

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  auto outputs = output_future_g.get();

  // check output values
  ASSERT_EQ(expected_output, outputs);
}

TEST_F(YaoTest, XOR) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs_a = generate_inputs(num_wires, num_simd);
  const auto inputs_b = generate_inputs(num_wires, num_simd);
  MOTION::BitValues expected_output;
  std::transform(std::begin(inputs_a), std::end(inputs_a), std::begin(inputs_b),
                 std::back_inserter(expected_output),
                 [](const auto& bv_a, const auto& bv_b) { return bv_a ^ bv_b; });

  auto [input_a_promise, wires_g_in_a] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in_a =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);
  auto [input_b_promise, wires_g_in_b] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in_b =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g_out = yao_providers_[garbler_i_]->make_binary_gate(
      ENCRYPTO::PrimitiveOperationType::XOR, wires_g_in_a, wires_g_in_b);
  auto wires_e_out = yao_providers_[evaluator_i_]->make_binary_gate(
      ENCRYPTO::PrimitiveOperationType::XOR, wires_e_in_a, wires_e_in_b);

  auto output_future_g =
      yao_providers_[garbler_i_]->make_boolean_output_gate_my(garbler_i_, wires_g_out);
  yao_providers_[evaluator_i_]->make_boolean_output_gate_other(garbler_i_, wires_e_out);
  ASSERT_TRUE(output_future_g.valid());

  run_setup();
  run_gates_setup();
  input_a_promise.set_value(inputs_a);
  input_b_promise.set_value(inputs_b);
  run_gates_online();

  auto outputs = output_future_g.get();

  // check output values
  ASSERT_EQ(expected_output, outputs);
}

TEST_F(YaoTest, AND) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs_a = generate_inputs(num_wires, num_simd);
  const auto inputs_b = generate_inputs(num_wires, num_simd);
  MOTION::BitValues expected_output;
  std::transform(std::begin(inputs_a), std::end(inputs_a), std::begin(inputs_b),
                 std::back_inserter(expected_output),
                 [](const auto& bv_a, const auto& bv_b) { return bv_a & bv_b; });

  auto [input_a_promise, wires_g_in_a] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in_a =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);
  auto [input_b_promise, wires_g_in_b] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in_b =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g_out = yao_providers_[garbler_i_]->make_binary_gate(
      ENCRYPTO::PrimitiveOperationType::AND, wires_g_in_a, wires_g_in_b);
  auto wires_e_out = yao_providers_[evaluator_i_]->make_binary_gate(
      ENCRYPTO::PrimitiveOperationType::AND, wires_e_in_a, wires_e_in_b);

  auto output_future_g =
      yao_providers_[garbler_i_]->make_boolean_output_gate_my(garbler_i_, wires_g_out);
  yao_providers_[evaluator_i_]->make_boolean_output_gate_other(garbler_i_, wires_e_out);
  ASSERT_TRUE(output_future_g.valid());

  run_setup();
  run_gates_setup();
  input_a_promise.set_value(inputs_a);
  input_b_promise.set_value(inputs_b);
  run_gates_online();

  auto outputs = output_future_g.get();

  // check output values
  ASSERT_EQ(expected_output, outputs);
}

TEST_F(YaoTest, YaoToBooleanGMW) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g_in] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g = yao_providers_[garbler_i_]->convert(MOTION::MPCProtocol::BooleanGMW, wires_g_in);
  auto wires_e = yao_providers_[evaluator_i_]->convert(MOTION::MPCProtocol::BooleanGMW, wires_e_in);

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  // check wire values
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& expected_output_bits = inputs.at(wire_i);
    const auto wire_g =
        std::dynamic_pointer_cast<MOTION::proto::gmw::BooleanGMWWire>(wires_g.at(wire_i));
    const auto wire_e =
        std::dynamic_pointer_cast<MOTION::proto::gmw::BooleanGMWWire>(wires_e.at(wire_i));
    wire_g->wait_online();
    wire_e->wait_online();
    const auto& share_g = wire_g->get_share();
    const auto& share_e = wire_e->get_share();
    ASSERT_EQ(share_g.GetSize(), num_simd);
    ASSERT_EQ(share_e.GetSize(), num_simd);
    ASSERT_EQ(expected_output_bits, share_g ^ share_e);
  }
}

TEST_F(YaoTest, YaoFromBooleanGMW) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g_in] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g_tmp =
      yao_providers_[garbler_i_]->convert(MOTION::MPCProtocol::BooleanGMW, wires_g_in);
  auto wires_e_tmp =
      yao_providers_[evaluator_i_]->convert(MOTION::MPCProtocol::BooleanGMW, wires_e_in);
  auto wires_g = yao_providers_[garbler_i_]->convert(MOTION::MPCProtocol::Yao, wires_g_tmp);
  auto wires_e = yao_providers_[evaluator_i_]->convert(MOTION::MPCProtocol::Yao, wires_e_tmp);

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  // check wire values
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& expected_output_bits = inputs.at(wire_i);
    const auto wire_g = std::dynamic_pointer_cast<YaoWire>(wires_g.at(wire_i));
    const auto wire_e = std::dynamic_pointer_cast<YaoWire>(wires_e.at(wire_i));
    ASSERT_NE(wire_g, nullptr);
    ASSERT_NE(wire_e, nullptr);
    wire_g->wait_setup();
    wire_e->wait_online();
    const auto& keys_g = wire_g->get_keys();
    const auto& keys_e = wire_e->get_keys();
    ASSERT_EQ(keys_g.size(), num_simd);
    ASSERT_EQ(keys_e.size(), num_simd);
    const auto& global_offset = yao_providers_[garbler_i_]->get_global_offset();
    for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
      if (expected_output_bits.Get(simd_j)) {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j) ^ global_offset);
      } else {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j));
      }
    }
  }
}

TEST_F(YaoTest, YaoToBooleanBEAVY) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g_in] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g = yao_providers_[garbler_i_]->convert(MOTION::MPCProtocol::BooleanBEAVY, wires_g_in);
  auto wires_e =
      yao_providers_[evaluator_i_]->convert(MOTION::MPCProtocol::BooleanBEAVY, wires_e_in);

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  // check wire values
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& expected_output_bits = inputs.at(wire_i);
    const auto wire_g =
        std::dynamic_pointer_cast<MOTION::proto::beavy::BooleanBEAVYWire>(wires_g.at(wire_i));
    const auto wire_e =
        std::dynamic_pointer_cast<MOTION::proto::beavy::BooleanBEAVYWire>(wires_e.at(wire_i));
    wire_g->wait_online();
    wire_e->wait_online();
    const auto& sshare_g = wire_g->get_secret_share();
    const auto& sshare_e = wire_e->get_secret_share();
    const auto& pshare_g = wire_g->get_public_share();
    const auto& pshare_e = wire_e->get_public_share();
    ASSERT_EQ(sshare_g.GetSize(), num_simd);
    ASSERT_EQ(sshare_e.GetSize(), num_simd);
    ASSERT_EQ(pshare_g.GetSize(), num_simd);
    ASSERT_EQ(pshare_g, pshare_e);
    ASSERT_EQ(expected_output_bits, pshare_g ^ sshare_g ^ sshare_e);
  }
}

TEST_F(YaoTest, YaoFromBooleanBEAVY) {
  std::size_t num_wires = 8;
  std::size_t num_simd = 10;
  const auto inputs = generate_inputs(num_wires, num_simd);

  auto [input_promise, wires_g_in] =
      yao_providers_[garbler_i_]->make_boolean_input_gate_my(garbler_i_, num_wires, num_simd);
  auto wires_e_in =
      yao_providers_[evaluator_i_]->make_boolean_input_gate_other(garbler_i_, num_wires, num_simd);

  auto wires_g_tmp =
      yao_providers_[garbler_i_]->convert(MOTION::MPCProtocol::BooleanBEAVY, wires_g_in);
  auto wires_e_tmp =
      yao_providers_[evaluator_i_]->convert(MOTION::MPCProtocol::BooleanBEAVY, wires_e_in);
  auto wires_g = yao_providers_[garbler_i_]->convert(MOTION::MPCProtocol::Yao, wires_g_tmp);
  auto wires_e = yao_providers_[evaluator_i_]->convert(MOTION::MPCProtocol::Yao, wires_e_tmp);

  run_setup();
  run_gates_setup();
  input_promise.set_value(inputs);
  run_gates_online();

  // check wire values
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& expected_output_bits = inputs.at(wire_i);
    const auto wire_g = std::dynamic_pointer_cast<YaoWire>(wires_g.at(wire_i));
    const auto wire_e = std::dynamic_pointer_cast<YaoWire>(wires_e.at(wire_i));
    ASSERT_NE(wire_g, nullptr);
    ASSERT_NE(wire_e, nullptr);
    wire_g->wait_setup();
    wire_e->wait_online();
    const auto& keys_g = wire_g->get_keys();
    const auto& keys_e = wire_e->get_keys();
    ASSERT_EQ(keys_g.size(), num_simd);
    ASSERT_EQ(keys_e.size(), num_simd);
    const auto& global_offset = yao_providers_[garbler_i_]->get_global_offset();
    for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
      if (expected_output_bits.Get(simd_j)) {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j) ^ global_offset);
      } else {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j));
      }
    }
  }
}

template <typename T>
class YaoArithmeticConversionTest : public YaoTest {
  using is_enabled_t_ = ENCRYPTO::is_unsigned_int_t<T>;

 public:
  static std::vector<T> generate_inputs(std::size_t num_simd) {
    return MOTION::Helpers::RandomVector<T>(num_simd);
  }
  void run_gates_setup() {
    std::vector<boost::fibers::future<void>> futs;
    for (auto& gate : gate_registers_[garbler_i_]->get_gates()) {
      if (gate->need_setup()) {
        futs.emplace_back(boost::fibers::async([&gate] { gate->evaluate_setup(); }));
      }
    }
    for (auto& gate : gate_registers_[evaluator_i_]->get_gates()) {
      if (gate->need_setup()) {
        futs.emplace_back(boost::fibers::async([&gate] { gate->evaluate_setup(); }));
      }
    }
    std::for_each(std::begin(futs), std::end(futs), [](auto& f) { f.get(); });
  }
  void run_gates_online() {
    std::vector<boost::fibers::future<void>> futs;
    for (auto& gate : gate_registers_[garbler_i_]->get_gates()) {
      if (gate->need_online()) {
        futs.emplace_back(boost::fibers::async([&gate] { gate->evaluate_online(); }));
      }
    }
    for (auto& gate : gate_registers_[evaluator_i_]->get_gates()) {
      if (gate->need_online()) {
        futs.emplace_back(boost::fibers::async([&gate] { gate->evaluate_online(); }));
      }
    }
    std::for_each(std::begin(futs), std::end(futs), [](auto& f) { f.get(); });
  }
};

using integer_types = ::testing::Types<std::uint8_t, std::uint16_t, std::uint32_t, std::uint64_t>;
TYPED_TEST_SUITE(YaoArithmeticConversionTest, integer_types);

// naive transposition of integers into bit vectors
template <typename T>
static std::vector<ENCRYPTO::BitVector<>> int_to_bit_vectors(const std::vector<T>& ints) {
  const auto num_wires = ENCRYPTO::bit_size_v<T>;
  const auto num_simd = ints.size();
  std::vector<ENCRYPTO::BitVector<>> bits;
  std::generate_n(std::back_inserter(bits), num_wires,
                  [num_simd] { return ENCRYPTO::BitVector<>(num_simd); });
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
      auto v = bool(ints[simd_j] & (T(1) << wire_i));
      bits[wire_i].Set(v, simd_j);
    }
  }

  return bits;
}

TYPED_TEST(YaoArithmeticConversionTest, YaoToArithmeticGMW) {
  std::size_t num_wires = ENCRYPTO::bit_size_v<TypeParam>;
  std::size_t num_simd = 1;
  const auto expected_output = this->generate_inputs(num_simd);
  std::vector<ENCRYPTO::BitVector<>> inputs = int_to_bit_vectors(expected_output);

  auto [input_promise, wires_in_0] =
      this->yao_providers_[0]->make_boolean_input_gate_my(0, num_wires, num_simd);
  auto wires_in_1 = this->yao_providers_[1]->make_boolean_input_gate_other(0, num_wires, num_simd);

  auto wires_0 = this->yao_providers_[0]->convert(MOTION::MPCProtocol::ArithmeticGMW, wires_in_0);
  auto wires_1 = this->yao_providers_[1]->convert(MOTION::MPCProtocol::ArithmeticGMW, wires_in_1);

  this->run_setup();
  this->run_gates_setup();
  input_promise.set_value(inputs);
  this->run_gates_online();

  // check wire values
  ASSERT_EQ(wires_0.size(), 1);
  ASSERT_EQ(wires_1.size(), 1);
  const auto wire_0 =
      std::dynamic_pointer_cast<MOTION::proto::gmw::ArithmeticGMWWire<TypeParam>>(wires_0.at(0));
  const auto wire_1 =
      std::dynamic_pointer_cast<MOTION::proto::gmw::ArithmeticGMWWire<TypeParam>>(wires_1.at(0));
  ASSERT_NE(wire_0, nullptr);
  ASSERT_NE(wire_1, nullptr);
  wire_0->wait_online();
  wire_1->wait_online();
  const auto& share_0 = wire_0->get_share();
  const auto& share_1 = wire_1->get_share();
  ASSERT_EQ(share_0.size(), num_simd);
  ASSERT_EQ(share_1.size(), num_simd);
  for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
    ASSERT_EQ(expected_output.at(simd_j), TypeParam(share_0.at(simd_j) + share_1.at(simd_j)));
  }
}

TYPED_TEST(YaoArithmeticConversionTest, YaoFromArithmeticGMW) {
  std::size_t num_wires = ENCRYPTO::bit_size_v<TypeParam>;
  std::size_t num_simd = 1;
  const auto expected_output = this->generate_inputs(num_simd);
  std::vector<ENCRYPTO::BitVector<>> inputs = int_to_bit_vectors(expected_output);

  auto [input_promise, wires_in_g] =
      this->yao_providers_[0]->make_boolean_input_gate_my(0, num_wires, num_simd);
  auto wires_in_e = this->yao_providers_[1]->make_boolean_input_gate_other(0, num_wires, num_simd);

  auto wires_g_tmp =
      this->yao_providers_[0]->convert(MOTION::MPCProtocol::ArithmeticGMW, wires_in_g);
  auto wires_e_tmp =
      this->yao_providers_[1]->convert(MOTION::MPCProtocol::ArithmeticGMW, wires_in_e);
  auto wires_g = this->yao_providers_[0]->convert(MOTION::MPCProtocol::Yao, wires_g_tmp);
  auto wires_e = this->yao_providers_[1]->convert(MOTION::MPCProtocol::Yao, wires_e_tmp);

  this->run_setup();
  this->run_gates_setup();
  input_promise.set_value(inputs);
  this->run_gates_online();

  ASSERT_EQ(wires_g.size(), num_wires);
  ASSERT_EQ(wires_e.size(), num_wires);

  // check wire values
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& expected_output_bits = inputs.at(wire_i);
    const auto wire_g = std::dynamic_pointer_cast<YaoWire>(wires_g.at(wire_i));
    const auto wire_e = std::dynamic_pointer_cast<YaoWire>(wires_e.at(wire_i));
    ASSERT_NE(wire_g, nullptr);
    ASSERT_NE(wire_e, nullptr);
    wire_g->wait_setup();
    wire_e->wait_online();
    const auto& keys_g = wire_g->get_keys();
    const auto& keys_e = wire_e->get_keys();
    ASSERT_EQ(keys_g.size(), num_simd);
    ASSERT_EQ(keys_e.size(), num_simd);
    const auto& global_offset = this->yao_providers_[this->garbler_i_]->get_global_offset();
    for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
      if (expected_output_bits.Get(simd_j)) {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j) ^ global_offset);
      } else {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j));
      }
    }
  }
}

TYPED_TEST(YaoArithmeticConversionTest, YaoToArithmeticBEAVY) {
  std::size_t num_wires = ENCRYPTO::bit_size_v<TypeParam>;
  std::size_t num_simd = 1;
  const auto expected_output = this->generate_inputs(num_simd);
  std::vector<ENCRYPTO::BitVector<>> inputs = int_to_bit_vectors(expected_output);

  auto [input_promise, wires_in_0] =
      this->yao_providers_[0]->make_boolean_input_gate_my(0, num_wires, num_simd);
  auto wires_in_1 = this->yao_providers_[1]->make_boolean_input_gate_other(0, num_wires, num_simd);

  auto wires_0 = this->yao_providers_[0]->convert(MOTION::MPCProtocol::ArithmeticBEAVY, wires_in_0);
  auto wires_1 = this->yao_providers_[1]->convert(MOTION::MPCProtocol::ArithmeticBEAVY, wires_in_1);

  this->run_setup();
  this->run_gates_setup();
  input_promise.set_value(inputs);
  this->run_gates_online();

  // check wire values
  ASSERT_EQ(wires_0.size(), 1);
  ASSERT_EQ(wires_1.size(), 1);
  const auto wire_0 =
      std::dynamic_pointer_cast<MOTION::proto::beavy::ArithmeticBEAVYWire<TypeParam>>(
          wires_0.at(0));
  const auto wire_1 =
      std::dynamic_pointer_cast<MOTION::proto::beavy::ArithmeticBEAVYWire<TypeParam>>(
          wires_1.at(0));
  ASSERT_NE(wire_0, nullptr);
  ASSERT_NE(wire_1, nullptr);
  wire_0->wait_online();
  wire_1->wait_online();
  const auto& public_share_0 = wire_0->get_public_share();
  const auto& public_share_1 = wire_1->get_public_share();
  const auto& secret_share_0 = wire_0->get_secret_share();
  const auto& secret_share_1 = wire_1->get_secret_share();
  ASSERT_EQ(public_share_0.size(), num_simd);
  ASSERT_EQ(public_share_1.size(), num_simd);
  ASSERT_EQ(secret_share_0.size(), num_simd);
  ASSERT_EQ(secret_share_1.size(), num_simd);
  ASSERT_EQ(public_share_0, public_share_1);
  for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
    ASSERT_EQ(expected_output.at(simd_j),
              TypeParam(public_share_0.at(simd_j) - secret_share_0.at(simd_j) -
                        secret_share_1.at(simd_j)));
  }
}

TYPED_TEST(YaoArithmeticConversionTest, YaoFromArithmeticBEAVY) {
  std::size_t num_wires = ENCRYPTO::bit_size_v<TypeParam>;
  std::size_t num_simd = 1;
  const auto expected_output = this->generate_inputs(num_simd);
  std::vector<ENCRYPTO::BitVector<>> inputs = int_to_bit_vectors(expected_output);

  auto [input_promise, wires_in_g] =
      this->yao_providers_[0]->make_boolean_input_gate_my(0, num_wires, num_simd);
  auto wires_in_e = this->yao_providers_[1]->make_boolean_input_gate_other(0, num_wires, num_simd);

  auto wires_g_tmp =
      this->yao_providers_[0]->convert(MOTION::MPCProtocol::ArithmeticBEAVY, wires_in_g);
  auto wires_e_tmp =
      this->yao_providers_[1]->convert(MOTION::MPCProtocol::ArithmeticBEAVY, wires_in_e);
  auto wires_g = this->yao_providers_[0]->convert(MOTION::MPCProtocol::Yao, wires_g_tmp);
  auto wires_e = this->yao_providers_[1]->convert(MOTION::MPCProtocol::Yao, wires_e_tmp);

  this->run_setup();
  this->run_gates_setup();
  input_promise.set_value(inputs);
  this->run_gates_online();

  ASSERT_EQ(wires_g.size(), num_wires);
  ASSERT_EQ(wires_e.size(), num_wires);

  // check wire values
  for (std::size_t wire_i = 0; wire_i < num_wires; ++wire_i) {
    const auto& expected_output_bits = inputs.at(wire_i);
    const auto wire_g = std::dynamic_pointer_cast<YaoWire>(wires_g.at(wire_i));
    const auto wire_e = std::dynamic_pointer_cast<YaoWire>(wires_e.at(wire_i));
    ASSERT_NE(wire_g, nullptr);
    ASSERT_NE(wire_e, nullptr);
    wire_g->wait_setup();
    wire_e->wait_online();
    const auto& keys_g = wire_g->get_keys();
    const auto& keys_e = wire_e->get_keys();
    ASSERT_EQ(keys_g.size(), num_simd);
    ASSERT_EQ(keys_e.size(), num_simd);
    const auto& global_offset = this->yao_providers_[this->garbler_i_]->get_global_offset();
    for (std::size_t simd_j = 0; simd_j < num_simd; ++simd_j) {
      if (expected_output_bits.Get(simd_j)) {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j) ^ global_offset);
      } else {
        ASSERT_EQ(keys_e.at(simd_j), keys_g.at(simd_j));
      }
    }
  }
}


/*
 *  @file   rnxToGtsam.cpp
 *  @author Ryan
 *  @brief  Script to convert Rinex File format to format utilized by GTSAM.

 * To do ::
 * Currenlty only works with SP3
 * Need to get nom. pos. from rinex header
 * Get DOY from header for trop estimation
 * need to write to file. Currently only prints to screen.
 */

// GPSTK
#include <gtsam/gpstk/MJD.hpp>
#include <gtsam/gpstk/PowerSum.hpp>
#include <gtsam/gpstk/Decimate.hpp>
#include <gtsam/gpstk/TropModel.hpp>
#include <gtsam/gpstk/BasicModel.hpp>
#include <gtsam/gpstk/CommonTime.hpp>
#include <gtsam/gpstk/PCSmoother.hpp>
#include <gtsam/gpstk/CodeSmoother.hpp>
#include <gtsam/gpstk/SimpleFilter.hpp>
#include <gtsam/gpstk/MWCSDetector.hpp>
#include <gtsam/gpstk/SatArcMarker.hpp>
#include <gtsam/gpstk/DCBDataReader.hpp>
#include <gtsam/gpstk/ComputeWindUp.hpp>
#include <gtsam/gpstk/Rinex3NavData.hpp>
#include <gtsam/gpstk/GNSSconstants.hpp>
#include <gtsam/gpstk/ComputeLinear.hpp>
#include <gtsam/gpstk/GPSWeekSecond.hpp>
#include <gtsam/gpstk/LICSDetector2.hpp>
#include <gtsam/gpstk/DataStructures.hpp>
#include <gtsam/gpstk/Rinex3ObsStream.hpp>
#include <gtsam/gpstk/Rinex3NavStream.hpp>
#include <gtsam/gpstk/ComputeTropModel.hpp>
#include <gtsam/gpstk/SP3EphemerisStore.hpp>
#include <gtsam/gpstk/ComputeSatPCenter.hpp>
#include <gtsam/gpstk/EclipsedSatFilter.hpp>
#include <gtsam/gpstk/GPSEphemerisStore.hpp>
#include <gtsam/gpstk/CorrectCodeBiases.hpp>
#include <gtsam/gpstk/ComputeSatPCenter.hpp>
#include <gtsam/gpstk/RequireObservables.hpp>
#include <gtsam/gpstk/CorrectObservables.hpp>
#include <gtsam/gpstk/LinearCombinations.hpp>
#include <gtsam/gpstk/GravitationalDelay.hpp>
#include <gtsam/gpstk/PhaseCodeAlignment.hpp>

// GTSAM
#include <gtsam/slam/dataset.h>
#include <gtsam/gnssNavigation/GnssData.h>

// BOOST
#include <boost/program_options.hpp>

// STD
#include <iomanip>
#include <iostream>

using namespace std;
using namespace gpstk;
using namespace gtsam;
using namespace boost;

namespace po = boost::program_options;

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

        bool usingP1 = false;
        int dec_int, itsBelowThree = 0, count = 0;
        string rnx_file, nav_file, sp3_file, out_file;

        cout << fixed << setprecision(12); // Set a proper output format

        po::options_description desc("Available options");
        desc.add_options()
                ("help,h", "Print help message")
                ("obs", po::value<string>(&rnx_file)->default_value(""),
                "Observation file to read")
                ("sp3", po::value<string>(&sp3_file)->default_value(""),
                "SP3 file to read.")
                ("out", po::value<string>(&out_file)->default_value(""),
                "output file.")
                ("usingP1", "Are you using P1 instead of C1?")
                ("dec", po::value<int>(&dec_int)->default_value(0),
                "decimate input obs file");
        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
        po::notify(vm);

        usingP1 = (vm.count("usingP1")>0);

        if ( rnx_file.empty() )
        {
                cout << " Must pass in obs file !!! Try --obs " << desc << endl;
                exit(1);
        }
        if ( nav_file.empty() && sp3_file.empty() )
        {
                cout << " Must pass in ephemeris file !!! Try --sp3 or --nav" << desc << endl;
                exit(1);
        }

        string obs_path = findExampleDataFile(rnx_file);
        string nav_path = findExampleDataFile(sp3_file);

        // Create the input observation file stream
        Rinex3ObsStream rin(obs_path);

        // Declare a "SP3EphemerisStore" object to handle precise ephemeris
        SP3EphemerisStore SP3EphList;

        // Set flags to reject satellites with bad or absent positional
        // values or clocks
        SP3EphList.rejectBadPositions(true);
        SP3EphList.rejectBadClocks(true);

        // Load all the SP3 ephemerides files
        SP3EphList.loadFile(nav_path);

        // BELL station nominal position
        Position nominalPos(856295.3346, -4843033.4111, 4048017.6649);

        CorrectCodeBiases corrCode;
        corrCode.setDCBFile("p1p2.DCB", "p1c1.DCB");

        if (!usingP1) {
                corrCode.setUsingC1(true);
        }

        // This is the GNSS data structure that will hold all the
        // GNSS-related information
        gnssRinex gRin;

        RequireObservables requireObs;
        requireObs.addRequiredType(TypeID::L1);
        requireObs.addRequiredType(TypeID::L2);

        SimpleFilter pObsFilter;
        pObsFilter.setFilteredType(TypeID::C1);

        if ( usingP1 )
        {
                requireObs.addRequiredType(TypeID::P1);
                pObsFilter.addFilteredType(TypeID::P1);
                requireObs.addRequiredType(TypeID::P2);
                pObsFilter.addFilteredType(TypeID::P2);
        }
        else
        {
                requireObs.addRequiredType(TypeID::C1);
                pObsFilter.addFilteredType(TypeID::C1);
                requireObs.addRequiredType(TypeID::P2);
                pObsFilter.addFilteredType(TypeID::P2);
        }

        // Declare a couple of basic modelers
        BasicModel basic(nominalPos, SP3EphList);

        // Object to correct for SP3 Sat Phase-center offset
        AntexReader antexread;
        antexread.open( "antenna_corr.atx" );
        ComputeSatPCenter svPcenter(SP3EphList, nominalPos);
        svPcenter.setAntexReader( antexread );

        // Objects to mark cycle slips
        MWCSDetector markCSMW;  // Checks Merbourne-Wubbena cycle slip

        // Declare an object to correct observables
        CorrectObservables corr(SP3EphList);

        // object def several linear combinations
        LinearCombinations comb;

        // Object to compute linear combinations for cycle slip detection
        ComputeLinear linear1;
        if ( usingP1 )
        {
                linear1.addLinear(comb.pdeltaCombination);
                linear1.addLinear(comb.mwubbenaCombination);
        }
        else
        {
                linear1.addLinear(comb.pdeltaCombWithC1);
                linear1.addLinear(comb.mwubbenaCombWithC1);
        }
        linear1.addLinear(comb.ldeltaCombination);
        linear1.addLinear(comb.liCombination);

        ComputeLinear linear2;

        // Read if we should use C1 instead of P1
        if ( usingP1 )
        {
                linear2.addLinear(comb.pcCombination);
        }
        else
        {
                // WARNING: When using C1 instead of P1 to compute PC combination,
                //          be aware that instrumental errors will NOT cancel,
                //          introducing a bias that must be taken into account by
                //          other means. This won't be taken into account in this
                //          example.
                linear2.addLinear(comb.pcCombWithC1);
        }
        linear2.addLinear(comb.lcCombination);

        LICSDetector2 markCSLI2;       // Checks LI cycle slips

        // Object to keep track of satellite arcs
        SatArcMarker markArc;
        markArc.setDeleteUnstableSats(true);

        // Objects to compute gravitational delay effects
        GravitationalDelay grDelay(nominalPos);

        // Object to align phase with code measurements
        PhaseCodeAlignment phaseAlign;

        // Object to remove eclipsed satellites
        EclipsedSatFilter eclipsedSV;

        //Object to compute wind-up effect
        ComputeWindUp windup( SP3EphList,
                              nominalPos );

        // Object to compute prefit-residuals
        ComputeLinear linear3(comb.pcPrefit);
        linear3.addLinear(comb.lcPrefit);

        CodeSmoother smoothC1;

        TypeIDSet tset;
        tset.insert(TypeID::prefitC);
        tset.insert(TypeID::prefitL);

        // Declare a NeillTropModel object, setting the defaults
        NeillTropModel neillTM( 310, 39.09, 319);

        // Objects to compute the tropospheric data
        ComputeTropModel computeTropo(neillTM);

        SimpleFilter pcFilter;
        pcFilter.setFilteredType(TypeID::PC);

        // Loop over all data epochs
        while(rin >> gRin)
        {
                TimeSystem sys;
                sys.fromString("GPS");
                CommonTime time(gRin.header.epoch);
                time.setTimeSystem(sys);
                GPSWeekSecond gpstime( time );

                try
                {
                        gRin >> requireObs // Check if required observations are present
                        >> pObsFilter // Filter out spurious data
                        >> linear1 // Compute linear combinations to detect CS
                        >> markCSLI2 // Mark cycle slips
                        >> markCSMW // Mark cycle slips: Melbourne-Wubbena
                        >> markArc // Keep track of satellite arcs
                        >> basic // Compute the basic components of model
                        >> eclipsedSV // Remove satellites in eclipse
                        >> svPcenter // Computer delta for sat. phase center
                        >> linear2  // Compute ionosphere-free combinations
                        >> corr // SP3 Corrections
                        >> corrCode // Correct for differential code biases
                        >> windup // phase windup correction
                        >> grDelay // Compute gravitational delay
                        >> computeTropo // neill trop function
                        >> pcFilter  // screen PC
                        >> phaseAlign; // Align phases with codes

                }
                catch(Exception& e)
                {
                        //cerr << "Exception at epoch: " << time << "; " << e << endl;
                        continue;
                }
                catch(...)
                {
                        cerr << "Unknown exception at epoch: " << time << endl;
                        continue;
                }
                TypeIDSet types;
                types.insert(TypeID::satX);
                types.insert(TypeID::satY);
                types.insert(TypeID::satZ);
                types.insert(TypeID::PC);
                types.insert(TypeID::LC);
                types.insert(TypeID::rho);
                types.insert(TypeID::tropo);
                types.insert(TypeID::tropoSlant);
                types.insert(TypeID::dtSat);
                types.insert(TypeID::rel);
                types.insert(TypeID::gravDelay);
                types.insert(TypeID::instC1);
                types.insert(TypeID::instC2);
                types.insert(TypeID::satArc);
                types.insert(TypeID::satPCenter);
                types.insert(TypeID::windUp);
                gRin.keepOnlyTypeID(types);

                // Iterate through the GNSS Data Structure
                satTypeValueMap::const_iterator it;
                typeValueMap::const_iterator itObs;
                if (gRin.numSats() > 4)
                {
                        if ( itsBelowThree > 0 )
                        {
                                itsBelowThree = 0;
                                continue;
                        }
                        for (it = gRin.body.begin(); it!= gRin.body.end(); it++)
                        {
                                cout << gpstime.week << " ";
                                cout << gpstime.sow << " ";
                                cout << count << " ";
                                cout << (*it).first << " ";

                                typeValueMap::const_iterator itObs;
                                for( itObs  = (*it).second.begin(); itObs != (*it).second.end(); itObs++ )
                                {
                                        //cout << (*itObs).first << " ";
                                        cout << (*itObs).second << " ";
                                }
                                cout << endl;
                        }
                        count++;
                }
                else { itsBelowThree++; }
        }
        return 0;
}


/*
 * Lightmap generator structures file
 *
 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns)
 * See "SoftPixelEngine.hpp" for license information.
 */

#include "Framework/Tools/LightmapGenerator/spLightmapGeneratorStructs.hpp"

#ifdef SP_COMPILE_WITH_LIGHTMAPGENERATOR

#include "Framework/Tools/LightmapGenerator/spLightmapGenerator.hpp"
#include "Base/spMathCollisionLibrary.hpp"
#include "Base/spMathRasterizer.hpp"
#include "Base/spSharedObjects.hpp"
#include "Platform/spSoftPixelDeviceOS.hpp"
#include "SceneGraph/spSceneManager.hpp"

#include <boost/foreach.hpp>

namespace sp
{

extern video::RenderSystem* GlbRenderSys;
extern scene::SceneGraph* GlbSceneGraph;

namespace tool
{

namespace LightmapGen
{

/*
 * SVertex structure
 */

SVertex::SVertex() :
    Surface (0),
    Index   (0)
{
}
SVertex::SVertex(const SModel* Model, const u32 VertexSurface, const u32 VertexIndex) :
    Surface (VertexSurface  ),
    Index   (VertexIndex    )
{
    video::MeshBuffer* Surface = Model->Mesh->getMeshBuffer(VertexSurface);

    Position    = Model->Matrix * Surface->getVertexCoord(Index);
    Normal      = Model->NormalMatrix * Surface->getVertexNormal(Index);
    Color       = Surface->getVertexColor(Index);

    Normal.normalize();

    for (u32 i = 0; i < MAX_COUNT_OF_TEXTURES && i < Surface->getTextureCount(); ++i)
        TexCoord[i] = Surface->getVertexTexCoord(Index, i);
}
SVertex::~SVertex()
{
}

bool SVertex::adjacency(const STriangle &OpTriangle) const
{
    return
        Position.equal(OpTriangle.Vertices[0].Position) ||
        Position.equal(OpTriangle.Vertices[1].Position) ||
        Position.equal(OpTriangle.Vertices[2].Position);
}

void SVertex::scaleProj(const dim::size2df &Scale)
{
    LMapCoord.X = static_cast<s32>(Scale.Width * LMapCoord.X);
    LMapCoord.Y = static_cast<s32>(Scale.Height * LMapCoord.Y);
}

dim::point2df SVertex::getMapCoord(const dim::size2di &MaxLightmapSize) const
{
    return dim::point2df(
        static_cast<f32>(LMapCoord.X) / MaxLightmapSize.Width,
        static_cast<f32>(LMapCoord.Y) / MaxLightmapSize.Height
    );
}

dim::vector3df SVertex::getVectorColor(const video::color &Color)
{
    return dim::vector3df(Color.Red, Color.Green, Color.Blue) / 255;
}

/*
 * STriangle structure
 */

STriangle::STriangle() :
    Surface (0),
    Index   (0),
    Face    (0)
{
}
STriangle::STriangle(
    const SModel* Model, u32 TriangleSurface, u32 TriangleIndex, u32 DefIndices[3]) :
    Surface (TriangleSurface),
    Index   (TriangleIndex  ),
    Face    (0              )
{
    for (s32 i = 0; i < 3; ++i)
        Vertices[i] = SVertex(Model, TriangleSurface, DefIndices[i]);

    Plane = dim::plane3df(Vertices[0].Position, Vertices[1].Position, Vertices[2].Position);
}
STriangle::~STriangle()
{
}

bool STriangle::adjacency(const STriangle &OpTriangle) const
{
    if (Surface != OpTriangle.Surface)
        return false;

    return
        Vertices[0].adjacency(OpTriangle) ||
        Vertices[1].adjacency(OpTriangle) ||
        Vertices[2].adjacency(OpTriangle);
}

f32 STriangle::getDistance(const dim::vector3df &Point) const
{
    return math::getDistance(
        math::CollisionLibrary::getClosestPoint(
            dim::triangle3df(Vertices[0].Position, Vertices[1].Position, Vertices[2].Position), Point
        ),
        Point
    );
}

dim::point2df STriangle::getProjection(
    const dim::vector3df &Point, const dim::vector3df &Normal, const f32 Density)
{
    const dim::vector3df AbsNormal(Normal.getAbs());

    if (AbsNormal.X >= AbsNormal.Y && AbsNormal.X >= AbsNormal.Z)
        return dim::point2df(Point.Z, -Point.Y) * Density;
    else if (AbsNormal.Y >= AbsNormal.X && AbsNormal.Y >= AbsNormal.Z)
        return dim::point2df(Point.X, -Point.Z) * Density;

    return dim::point2df(Point.X, -Point.Y) * Density;
}

//!TODO! -> replace this with a technique using barycentric coordinates!!!
void STriangle::computeInterpolation(
    const scene::SIntersectionContact &Contact, u32 Indices[3], const u8 Layer,
    dim::point2df &TexCoord, dim::vector3df &Color, f32 &Alpha)
{
    // Get triangle construction
    if (!Contact.Face)
        return;

    const dim::triangle3df Triangle(Contact.Triangle);
    scene::Mesh* Mesh = Contact.Face->Mesh;

    // Temporary variables
    const dim::vector3df u(Triangle.PointB - Triangle.PointA);
    const dim::vector3df v(Triangle.PointC - Triangle.PointA);

    const f32 lenu = u.getLength();
    const f32 lenv = v.getLength();

    // Get the texture coordinates & vertex colors
    video::MeshBuffer* Surface = Mesh->getMeshBuffer(Mesh->getMeshBufferCount() - 1);

    const dim::point2df TexCoordA(Surface->getVertexTexCoord(Indices[0], Layer));
    const dim::point2df TexCoordB(Surface->getVertexTexCoord(Indices[1], Layer));
    const dim::point2df TexCoordC(Surface->getVertexTexCoord(Indices[2], Layer));

    const video::color RealColorA(Surface->getVertexColor(Indices[0]));
    const video::color RealColorB(Surface->getVertexColor(Indices[1]));
    const video::color RealColorC(Surface->getVertexColor(Indices[2]));

    const dim::vector3df ColorA(SVertex::getVectorColor(RealColorA));
    const dim::vector3df ColorB(SVertex::getVectorColor(RealColorB));
    const dim::vector3df ColorC(SVertex::getVectorColor(RealColorC));

    const f32 AlphaA = static_cast<f32>(RealColorA.Alpha) / 255;
    const f32 AlphaB = static_cast<f32>(RealColorB.Alpha) / 255;
    const f32 AlphaC = static_cast<f32>(RealColorC.Alpha) / 255;

    // Compute the first coordinates
    dim::vector3df pa, pb;

    math::CollisionLibrary::getLineLineIntersection(
        dim::line3df(Triangle.PointA, Triangle.PointB), dim::line3df(Contact.Point, Contact.Point - v), pa
    );
    math::CollisionLibrary::getLineLineIntersection(
        dim::line3df(Triangle.PointA, Triangle.PointC), dim::line3df(Contact.Point, Contact.Point - u), pb
    );

    const dim::point2df map(math::getDistance(Contact.Point, pb), math::getDistance(Contact.Point, pa));

    // Get the texture coordinate
    TexCoord    = TexCoordA + (TexCoordB    - TexCoordA ) * map.X / lenu + (TexCoordC   - TexCoordA ) * map.Y / lenv;
    Color       = ColorA    + (ColorB       - ColorA    ) * map.X / lenu + (ColorC      - ColorA    ) * map.Y / lenv;
    Alpha       = AlphaA    + (AlphaB       - AlphaA    ) * map.X / lenu + (AlphaC      - AlphaA    ) * map.Y / lenv;
}

/*
 * SFace structure
 */

SFace::SFace(SAxisData* FaceAxis) :
    Density     (0.1f       ),
    Surface     (0          ),
    Lightmap    (0          ),
    RootLightmap(0          ),
    Axis        (FaceAxis   )
{
}
SFace::~SFace()
{
}

void SFace::computeDensityAverage(f32 DefaultDensity)
{
    if (!Triangles.empty())
    {
        Density = 0.0f;

        foreach (const STriangle &Tri, Triangles)
            Density += (Axis->Model->TrianglesDensity[Tri.Surface])[Tri.Index];

        if (math::equal(Density, 0.0f))
            Density = DefaultDensity;
        else
            Density /= Triangles.size();
    }
}

void SFace::updateVertexProjection(const dim::size2di &MaxLightmapSize)
{
    dim::point2di TexCoord;

    dim::point2di Min = 999999;
    dim::point2di Max = -999999;

    // Compute the vertices' lightmap-texture-coordinates and the faces' lightmap space's bounding box
    foreach (STriangle &Tri, Triangles)
    {
        for (s32 i = 0; i < 3; ++i)
        {
            TexCoord = (STriangle::getProjection(Tri.Vertices[i].Position, Tri.Plane.Normal, Density) + 0.5).cast<s32>();

            if (TexCoord.X < Min.X) Min.X = TexCoord.X;
            if (TexCoord.Y < Min.Y) Min.Y = TexCoord.Y;

            if (TexCoord.X > Max.X) Max.X = TexCoord.X;
            if (TexCoord.Y > Max.Y) Max.Y = TexCoord.Y;

            Tri.Vertices[i].LMapCoord = TexCoord;
        }
    }

    // Fit the lightmap-texture-coordinates to the bounding box
    foreach (STriangle &Tri, Triangles)
    {
        Tri.Vertices[0].LMapCoord -= Min;
        Tri.Vertices[1].LMapCoord -= Min;
        Tri.Vertices[2].LMapCoord -= Min;
    }

    Size.Width  = Max.X - Min.X;
    Size.Height = Max.Y - Min.Y;

    // Resize the face if to big for lightmap
    if (Size.Width > MaxLightmapSize.Width - 2 || Size.Height > MaxLightmapSize.Height - 2)
        resizeVertexProjection(Size.getScaledSize(MaxLightmapSize - 2));
}

void SFace::resizeVertexProjection(const dim::size2di &NewSize)
{
    const dim::size2df Scale(
        static_cast<f32>(NewSize.Width) / Size.Width,
        static_cast<f32>(NewSize.Height) / Size.Height
    );

    foreach (STriangle &Tri, Triangles)
    {
        Tri.Vertices[0].scaleProj(Scale);
        Tri.Vertices[1].scaleProj(Scale);
        Tri.Vertices[2].scaleProj(Scale);
    }

    Size = NewSize;
}

// !INCOMPLETE AND SLOW!
bool SFace::adjacency(const SFace &OpFace) const
{
    foreach (const STriangle &TriA, Triangles)
    {
        foreach (const STriangle &TriB, OpFace.Triangles)
        {
            if (TriA.adjacency(TriB))
                return true;
        }
    }

    return false;
}

void SFace::build(scene::Mesh* Mesh, const dim::size2di &MaxLightmapSize)
{
    u32 i, j, c = 0;
    u32 VertexIndex;

    scene::Mesh* SourceMesh = Axis->Model->Mesh;
    video::MeshBuffer* OldSurface = SourceMesh->getMeshBuffer(Surface);

    const u32 TextureCount = math::Min(
        static_cast<u32>(OldSurface->getVertexFormat()->getTexCoords().size() - 1),
        OldSurface->getTextureCount()
    );

    // Create the face with its vertices & triangles
    video::MeshBuffer* NewSurface = Mesh->createMeshBuffer(
        OldSurface->getVertexFormat(), OldSurface->getIndexFormat()->getDataType()
    );

    foreach (STriangle &Tri, Triangles)
    {
        for (i = 0; i < 3; ++i)
        {
            VertexIndex = NewSurface->addVertex(
                Tri.Vertices[i].Position,
                Tri.Vertices[i].Normal,
                Tri.Vertices[i].getMapCoord(MaxLightmapSize),
                Tri.Vertices[i].Color
            );

            for (j = 0; j < TextureCount; ++j)
                NewSurface->setVertexTexCoord(VertexIndex, Tri.Vertices[i].TexCoord[j], j);
        }

        NewSurface->addTriangle(c, c + 1, c + 2);
        c += 3;
    }

    // Add the original textures
    for (i = 0; i < TextureCount; ++i)
    {
        NewSurface->addTexture(OldSurface->getTexture(i));

        video::TextureLayer* NewTexLayer = NewSurface->getTextureLayer(i);
        video::TextureLayer* OldTexLayer = OldSurface->getTextureLayer(i);

        if (NewTexLayer && OldTexLayer)
            NewTexLayer->setTexture(OldTexLayer->getTexture());
    }

    // Add the lightmap texture
    NewSurface->addTexture(RootLightmap->Texture);
}

/*
 * SAxisData structure
 */

SAxisData::SAxisData() :
    Model(0)
{
}
SAxisData::~SAxisData()
{
    foreach (SFace &Face, Faces)
        MemoryManager::deleteMemory(Face.Lightmap);
}

// !THE NEXT TWO FUNCTIONS (createFaces & optimizeFaces) ARE NOT OPTIMIZED AND VERY SLOW!
void SAxisData::createFaces()
{
    // Temporary variables
    bool FoundList;
    std::list<STriangle>::iterator ita, itb;
    std::list<SFace>::iterator itFace;

    // Create the faces and insert the triangles
    for (ita = Triangles.begin(); ita != Triangles.end() && LightmapGenerator::processRunning(); ++ita)
    {
        FoundList = false;

        // Loop for each face and check if the current triangle is adjacency to the face's triangle list
        for (itFace = Faces.begin(); itFace != Faces.end() && !FoundList; ++itFace)
        {
            for (itb = itFace->Triangles.begin(); itb != itFace->Triangles.end(); ++itb)
            {
                // If adjacency add the current triangle (ita) to the face's triangle list
                if (ita->adjacency(*itb))
                {
                    itFace->Triangles.push_back(*ita);
                    FoundList = true;
                    break;
                }
            }
        }

        // If the triangle was not adjacency to any of the faces' triangle list create a new face
        if (!FoundList)
        {
            SFace NewFace(this);
            NewFace.Triangles.push_back(*ita);
            Faces.push_back(NewFace);
        }
    }
}

void SAxisData::optimizeFaces()
{
    // Temporary variables
    std::list<SFace>::iterator ita, itb;
    std::list<STriangle>::iterator itTri;

    // Summarize the faces as far as possible
    for (ita = Faces.begin(); ita != Faces.end(); ++ita)
    {
        for (itb = Faces.begin(); itb != Faces.end();)
        {
            if (ita != itb && ita->adjacency(*itb))
            {
                // Add the triangle to the first list
                for (itTri = itb->Triangles.begin(); itTri != itb->Triangles.end(); ++itTri)
                    ita->Triangles.push_back(*itTri);

                // Delete the old list
                itb = Faces.erase(itb);
            }
            else
                ++itb;
        }
    }
}

void SAxisData::completeFaces(const dim::size2di &MaxLightmapSize, f32 DefaultDensity)
{
    // Complete each face in the axis (average vertex projection)
    foreach (SFace &Face, Faces)
    {
        Face.computeDensityAverage(DefaultDensity);
        Face.updateVertexProjection(MaxLightmapSize);

        if (!Face.Triangles.empty())
            Face.Surface = Face.Triangles.begin()->Surface;

        // Complete the triangles' face connection
        foreach (STriangle &Tri, Face.Triangles)
            Tri.Face = &Face;
    }
}

/*
 * SModel structure
 */

SModel::SModel(
    scene::Mesh* ObjMesh, bool DefStayAlone, const std::vector< std::vector<f32> > &InitTrianglesDensity) :
    Mesh            (ObjMesh                        ),
    DefaultDensity  (0.1f                           ),
    Matrix          (Mesh->getTransformMatrix(true) ),
    MatrixInv       (Matrix.getInverse()            ),
    NormalMatrix    (Matrix.getRotationMatrix()     ),
    StayAlone       (DefStayAlone                   ),
    TrianglesDensity(InitTrianglesDensity           )
{
    for (s32 i = 0; i < 6; ++i)
        Axles[i].Model = this;

    Triangles.resize(Mesh->getMeshBufferCount());
    for (u32 s = 0; s < Mesh->getMeshBufferCount(); ++s)
        Triangles[s].resize(Mesh->getMeshBuffer(s)->getTriangleCount());
}
SModel::~SModel()
{
}

void SModel::partitionMesh(const dim::size2di &MaxLightmapSize, f32 DefaultDensity)
{
    createAxles();

    for (s32 i = 0; i < 6 && LightmapGenerator::processRunning(); ++i)
    {
        Axles[i].createFaces();
        Axles[i].optimizeFaces();
        Axles[i].completeFaces(MaxLightmapSize, DefaultDensity);
        linkAxisTriangles(i);
    }
}

void SModel::createAxles()
{
    u32 Indices[3] = { 0 };
    STriangle CurTriangle;
    video::MeshBuffer* Surface = 0;

    dim::EAxisTypes AxisType;

    for (u32 s = 0, i; s < Mesh->getMeshBufferCount(); ++s)
    {
        Surface = Mesh->getMeshBuffer(s);

        for (i = 0; i < Surface->getTriangleCount(); ++i)
        {
            Surface->getTriangleIndices(i, Indices);

            CurTriangle = STriangle(this, s, i, Indices);
            AxisType    = CurTriangle.Plane.Normal.getDominantAxis();

            Axles[AxisType].Triangles.push_back(CurTriangle);
        }
    }
}

void SModel::linkAxisTriangles(const s32 Axis)
{
    foreach (SFace &Face, Axles[Axis].Faces)
    {
        foreach (STriangle &Tri, Face.Triangles)
            (Triangles[Tri.Surface])[Tri.Index] = &Tri;
    }
}

void SModel::buildFaces(scene::Mesh* Mesh, const dim::size2di &MaxLightmapSize)
{
    for (s32 i = 0; i < 6; ++i)
    {
        foreach (SFace &Face, Axles[i].Faces)
            Face.build(Mesh, MaxLightmapSize);
    }
}

/*
 * SLightmapTexel strucuture
 */

SLightmapTexel::SLightmapTexel() :
    Color       (0),
    OrigColor   (0),
    Face        (0)
{
}
SLightmapTexel::~SLightmapTexel()
{
}

/*
 * SLightmapTexelLoc strucuture
 */

SLightmapTexelLoc::SLightmapTexelLoc()
{
}
SLightmapTexelLoc::~SLightmapTexelLoc()
{
}

/*
 * SLightmap strucutre
 */

SLightmap::SLightmap(const dim::size2di &ImageSize, bool UseTexelBuffer, bool UseTexelLocBuffer) :
    Size            (ImageSize  ),
    TexelBuffer     (0          ),
    TexelLocBuffer  (0          ),
    Texture         (0          ),
    RectNode        (0          )
{
    if (UseTexelBuffer)
        TexelBuffer = MemoryManager::createBuffer<SLightmapTexel>(Size.getArea(), "SLightmap::TexelBuffer");
    if (UseTexelLocBuffer)
        TexelLocBuffer = MemoryManager::createBuffer<SLightmapTexelLoc>(Size.getArea(), "SLightmap::TexelLocBuffer");
}
SLightmap::~SLightmap()
{
    delete [] TexelBuffer;
    delete [] TexelLocBuffer;
}

video::Texture* SLightmap::createTexture(const video::color &AmbientColor)
{
    if (!TexelBuffer)
        return 0;

    // Create texture if not already done
    if (!Texture)
    {
        Texture = GlbRenderSys->createTexture(Size, video::PIXELFORMAT_RGB);
        Texture->setFilename("Lightmap");
    }
    else
        Texture->setSize(Size);

    // Create temporary image buffer
    const s32 FormatSize = Texture->getImageBuffer()->getFormatSize();
    const s32 ImageBufferSize = Size.Width * Size.Height * FormatSize;

    u8* ImageBuffer = MemoryManager::createBuffer<u8>(
        ImageBufferSize, "SLightmap -> ImageBuffer"
    );

    const s32 AmbColor[3] = { AmbientColor.Red, AmbientColor.Green, AmbientColor.Blue };

    for (s32 i = 0, j = 0; i < ImageBufferSize; i += FormatSize, ++j)
    {
        ImageBuffer[i + 0] = math::MinMax(AmbColor[0] + TexelBuffer[j].Color.Red  , 0, 255);
        ImageBuffer[i + 1] = math::MinMax(AmbColor[1] + TexelBuffer[j].Color.Green, 0, 255);
        ImageBuffer[i + 2] = math::MinMax(AmbColor[2] + TexelBuffer[j].Color.Blue , 0, 255);
        if (FormatSize == 4)
            ImageBuffer[i + 3] = 255;
    }

    // Update texture's image buffer
    Texture->setupImageBuffer(ImageBuffer);

    // Delete temporary image buffer
    MemoryManager::deleteBuffer(ImageBuffer);

    return Texture;
}

void SLightmap::copyImageBuffers()
{
    if (TexelBuffer)
    {
        for (s32 i = 0, n = Size.Width * Size.Height; i < n; ++i)
            TexelBuffer[i].OrigColor = TexelBuffer[i].Color;
    }
}

void SLightmap::reduceBleeding()
{
    if (!TexelBuffer)
        return;

    s32 x, y;

    SLightmapTexel* Texel = TexelBuffer;

    for (y = 0; y < Size.Height; ++y)
    {
        for (x = 0; x < Size.Width; ++x, ++Texel)
        {
            if (!Texel->Face)
                Texel->Color = video::color(getAverageColor(x, y), false);
        }
    }
}

dim::point2df SLightmap::getTexCoord(const dim::point2di &RealPos) const
{
    return dim::point2df(
        static_cast<f32>(RealPos.X) / Size.Width,
        static_cast<f32>(RealPos.Y) / Size.Height
    );
}

dim::vector3df SLightmap::getAverageColor(s32 X, s32 Y) const
{
    s32 c = 0;
    dim::vector3df Color;

    getAverageColorPart(X - 1, Y - 1, Color, c);
    getAverageColorPart(X    , Y - 1, Color, c);
    getAverageColorPart(X + 1, Y - 1, Color, c);

    getAverageColorPart(X - 1, Y    , Color, c);
    getAverageColorPart(X + 1, Y    , Color, c);

    getAverageColorPart(X - 1, Y + 1, Color, c);
    getAverageColorPart(X    , Y + 1, Color, c);
    getAverageColorPart(X + 1, Y + 1, Color, c);

    return c ? Color / static_cast<f32>(c) : 0.0f;
}

void SLightmap::getAverageColorPart(s32 X, s32 Y, dim::vector3df &Color, s32 &Counter) const
{
    if (X >= 0 && X < Size.Width && Y >= 0 && Y < Size.Height)
    {
        const SLightmapTexel* Texel = &getTexel(X, Y);

        if (Texel->Face)
        {
            Color += Texel->Color.getVector();
            ++Counter;
        }
    }
}

/*
 * SLight structure
 */

SLight::SLight(const SLightmapLight &LightData) :
    Type                    (LightData.Type                     ),
    Matrix                  (LightData.Matrix                   ),
    Position                (Matrix.getPosition()               ),
    Color                   (LightData.Color.getVector(true)    ),
    Attn0                   (LightData.Attn0                    ),
    Attn1                   (LightData.Attn1                    ),
    Attn2                   (LightData.Attn2                    ),
    InnerConeAngle          (LightData.InnerConeAngle / math::PI),
    OuterConeAngle          (LightData.OuterConeAngle / math::PI),
    FixedDirection          (
        (Matrix.getRotationMatrix() * dim::vector3df(0, 0, 1)).normalize()
    ),
    FixedVolumetricRadius   (getAttenuationRadius()),
    FixedVolumetric         (
        !math::equal(Attn0, 1.0f) || !math::equal(Attn1, 0.0f) || !math::equal(Attn2, 0.0f)
    )
{
}
SLight::~SLight()
{
}

f32 SLight::getIntensity(
    const dim::vector3df &Point, const dim::vector3df &Normal) const
{
    /* Directional light calculations */
    if (Type == scene::LIGHT_DIRECTIONAL)
        return math::Max(0.0f, -FixedDirection.dot(Normal));

    /* General light calculations */
    f32 Intensity = 0.0f;

    const dim::vector3df RayDirection((Point - Position).normalize());

    Intensity = -RayDirection.dot(Normal);

    /* Volumetric light calculations */
    if (FixedVolumetric)
    {
        const f32 Distance = math::getDistance(Position, Point);

        Intensity /= ( Attn0 + Attn1*Distance + Attn2*Distance*Distance );
    }

    /* Spot light calculations */
    if (Type == scene::LIGHT_SPOT)
    {
        const f32 Angle = RayDirection.getAngle(FixedDirection);

        if (Angle > OuterConeAngle)
            Intensity = 0.0f;
        else if (Angle > InnerConeAngle)
        {
            /* Calculate splot-light cone angle linear-interpolation (lerp) */
            const f32 ConeAngleLerp = (Angle - InnerConeAngle) / (OuterConeAngle - InnerConeAngle);
            Intensity *= (1.0f - ConeAngleLerp);
        }
    }

    return math::Max(0.0f, Intensity);
}

f32 SLight::getAttenuationRadius() const
{
    // Last 5 black texel levels (5, 5, 5) to (0, 0, 0) are not rendered -> for optimization
    static const f32 COLOR_PRECISE = 5.0f;

    if (!FixedVolumetric || Type == scene::LIGHT_DIRECTIONAL)
        return math::OMEGA;

    // Attenuation calculation backwards using the pq-formula
    return -(Attn1/Attn2)/2 + sqrt(math::pow2((Attn1/Attn2)/2) + (255.0f - COLOR_PRECISE*Attn0)/(COLOR_PRECISE*Attn2));
}

bool SLight::checkVisibility(const STriangle &Triangle) const
{
    return (
        Type == scene::LIGHT_DIRECTIONAL ?
        ( FixedDirection.dot(-Triangle.Plane.Normal) > 0.0f ) :
        ( Triangle.Plane.isPointFrontSide(Position) && ( !FixedVolumetric || Triangle.getDistance(Position) < FixedVolumetricRadius ) )
    );
}

/*
 * SRasterizerVertex structure
 */

SRasterizerVertex::SRasterizerVertex()
{
}
SRasterizerVertex::SRasterizerVertex(
    const dim::vector3df &InitPosition,
    const dim::vector3df &InitNormal,
    const dim::point2di &InitScreenCoord) :
    Position    (InitPosition   ),
    Normal      (InitNormal     ),
    ScreenCoord (InitScreenCoord)
{
}
SRasterizerVertex::~SRasterizerVertex()
{
}

SRasterizerVertex& SRasterizerVertex::operator = (const SRasterizerVertex &Other)
{
    Position = Other.Position;
    Normal = Other.Normal;
    return *this;
}

SRasterizerVertex& SRasterizerVertex::operator += (const SRasterizerVertex &Other)
{
    Position += Other.Position;
    Normal += Other.Normal;
    return *this;
}
SRasterizerVertex& SRasterizerVertex::operator -= (const SRasterizerVertex &Other)
{
    Position -= Other.Position;
    Normal -= Other.Normal;
    return *this;
}

SRasterizerVertex& SRasterizerVertex::operator *= (f32 Factor)
{
    Position *= Factor;
    Normal *= Factor;
    return *this;
}
SRasterizerVertex& SRasterizerVertex::operator /= (f32 Factor)
{
    Position /= Factor;
    Normal /= Factor;
    return *this;
}

s32 SRasterizerVertex::getScreenCoordX() const
{
    return ScreenCoord.X;
}
s32 SRasterizerVertex::getScreenCoordY() const
{
    return ScreenCoord.Y;
}

} // /namespace LightmapGen

} // /namespace tool

} // /namespace sp

#endif

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


#include "lingua/chat.hxx"
#include <FlexLexer.h>
#include <re2/re2.h>
#include <boost/tokenizer.hpp>
#include <sys/stat.h>
#include <fcntl.h>
#include <algorithm>
#include <random>
#include <fstream>
#include <iostream>
#include <cstdio>
#include <cmath>
#include <cfloat>
#include <chrono>

namespace lingua {
    float operator*(const SemanticVector &lhs, const SemanticVector &rhs) {
        float result = 0;
        auto vl = ChatEngine::instance->getVectorLength();
        for (auto i = 0; i < vl; ++i)
            result += lhs.values[i] * rhs.values[i];
        return result;
    }

    float cosine(const SemanticVector &lhs, const SemanticVector &rhs) {
        return (lhs * rhs) / (lhs.magnitude() * rhs.magnitude());
    }

    SemanticVector::SemanticVector() : values(new float[ChatEngine::instance->getVectorLength()]) {
        auto vl = ChatEngine::instance->getVectorLength();
        auto seed = std::chrono::system_clock::now().time_since_epoch().count();
        std::default_random_engine gen(seed);
        std::uniform_real_distribution<float> dist(-1.0, 1.0);

        for (auto i = 0; i < vl; ++i)
            values[i] = dist(gen);
    }

    SemanticVector::SemanticVector(const float *vs) : values(new float[ChatEngine::instance->getVectorLength()]) {
        auto vl = ChatEngine::instance->getVectorLength();

        for (auto i = 0; i < vl; ++i)
            values[i] = vs[i];
    }

    SemanticVector& SemanticVector::operator=(const SemanticVector &other) {
        values = other.values;
        return *this;
    }

    const float* SemanticVector::getValues() const {
        return values;
    }

    float SemanticVector::getValue(size_t ix) const {
        return values[ix];
    }

    void SemanticVector::setValues(const float *vs) {
        auto vl = ChatEngine::instance->getVectorLength();

        for (auto i = 0; i < vl; ++i)
            values[i] = vs[i];
    }

    void SemanticVector::setValue(size_t ix, float value) {
        values[ix] = value;
    }

    float SemanticVector::magnitude() const {
        auto vl = ChatEngine::instance->getVectorLength();
        float result = 0;

        for (auto i = 0; i < vl; ++i)
            result += values[i] * values[i];

        return std::sqrt(result);
    }

    WordInfo::WordInfo(tag_t t, const std::string &txt) : tag(t), text(txt), semEmb(new SemanticVector), ctxEmb(new SemanticVector) { }

    tag_t WordInfo::getTag() const {
        return tag;
    }

    std::string WordInfo::getText() const {
        return text;
    }

    SemanticVector* WordInfo::getSemanticEmbedding() {
        return semEmb;
    }

    SemanticVector* WordInfo::getContextEmbedding() {
        return ctxEmb;
    }

    void WordInfo::setSemanticEmbedding(const SemanticVector &sv) {
        if (semEmb)
            *semEmb = sv;
        else {
            semEmb = new SemanticVector(sv);
        }
    }

    void WordInfo::setContextEmbedding(const SemanticVector &cv) {
        if (ctxEmb)
            *ctxEmb = cv;
        else {
            ctxEmb = new SemanticVector(cv);
        }
    }

    std::string ChatEngine::Tokenizer::currentTitle = "";

    std::string ChatEngine::Tokenizer::currentBody;

    ChatEngine* ChatEngine::instance = nullptr;

    ChatEngine::ChatEngine(unsigned pvl, unsigned pcn, unsigned pnr) : sourceFile("-"), docs(), pVectorLength(pvl), pContextNeighborhood(pcn), pNoiseRatio(pnr), learningRate(PARAM_DEFAULT_INITIAL_LEARNING_RATE), dict(), infotbl() { }

    ChatEngine::ChatEngine(const std::string &src, unsigned pvl, unsigned pcn, unsigned pnr) : sourceFile(src), docs(), pVectorLength(pvl), pContextNeighborhood(pcn), pNoiseRatio(pnr), learningRate(PARAM_DEFAULT_INITIAL_LEARNING_RATE), dict(), infotbl() { }

    void ChatEngine::initialize(unsigned pvl, unsigned pcn, unsigned pnr) {
        instance = new ChatEngine(pvl, pcn, pnr);
    }

    void ChatEngine::initialize(const std::string &src, unsigned pvl, unsigned pvn, unsigned pnr) {
        instance = new ChatEngine(src, pvl, pvn, pnr);
    }

    void ChatEngine::analyzeSemantics() {
        preprocess();
        learnSemantics();
    }

    void ChatEngine::learnSemantics() {
        for (auto it = docs.begin(); it != docs.end(); ++it)
            trainOn(*it);
    }

    void ChatEngine::trainOn(const Document &doc) {
        auto toks = doc.getTokens();
        std::size_t targetIndex = pContextNeighborhood;
        std::size_t lastTargetIndex = toks.size() - pContextNeighborhood - 1;

        while (targetIndex < lastTargetIndex) {
            auto beginContext = toks.begin() + targetIndex - pContextNeighborhood;
            auto targetPos = toks.begin() + targetIndex;
            auto endContext = toks.begin() + targetIndex + pContextNeighborhood;
            WordInfo targetInfo = infotbl[toks[targetIndex]];
            auto targetEmbedding = targetInfo.getSemanticEmbedding();
            std::vector<WordInfo> contextInfo;
            std::vector<WordInfo> noiseInfo;
            std::vector<SemanticVector*> contextEmbeddings;
            std::vector<SemanticVector*> noiseEmbeddings;
            std::vector<tag_t> contextTags(beginContext, targetPos);
            std::vector<tag_t> backContextTags(targetPos + 1, endContext);
            std::vector<tag_t> noiseTags;

            contextTags.resize(contextTags.size() << 1);
            contextTags.insert(contextTags.end(), backContextTags.begin(), backContextTags.end());

            noiseTags = generateNoise(contextTags);

            contextInfo.resize(contextTags.size());
            std::transform(contextTags.begin(), contextTags.end(),
                           contextInfo.begin(), [&](auto tag) { return this->getInfotbl()[tag]; });

            contextEmbeddings.resize(contextInfo.size());
            std::transform(contextInfo.begin(), contextInfo.end(),
                           contextEmbeddings.begin(), [&](auto ci) { return ci.getSemanticEmbedding(); });

            noiseInfo.resize(noiseTags.size());
            std::transform(noiseTags.begin(), noiseTags.end(),
                           noiseInfo.begin(), [&](auto tag) { return this->getInfotbl()[tag]; });

            noiseEmbeddings.resize(noiseInfo.size());
            std::transform(noiseInfo.begin(), noiseInfo.end(),
                           noiseEmbeddings.begin(), [&](auto ni) { return ni.getSemanticEmbedding(); });

            trainTargetEmbedding(targetEmbedding, contextEmbeddings, noiseEmbeddings);
            ++targetIndex;
            break; // DEBUG
        }
    }

    void ChatEngine::trainTargetEmbedding(SemanticVector *targetEmbedding, const std::vector<SemanticVector*> &contextEmbeddings, const std::vector<SemanticVector*> &noiseEmbeddings) {

    }

    std::vector<tag_t> ChatEngine::generateNoise(const std::vector<tag_t> &ctx) {
        auto seed = std::chrono::system_clock::now().time_since_epoch().count();
        std::default_random_engine gen(seed);
        std::uniform_int_distribution<tag_t> dist;
        auto limit = infotbl.size();
        auto needed = ctx.size() * pNoiseRatio;
        std::vector<tag_t> noise;

        while (noise.size() < needed) {
            auto candidate = dist(gen) % limit;
            if (std::find(ctx.cbegin(), ctx.cend(), candidate) == ctx.cend())
                noise.push_back(candidate);
        }

        return noise;
    }

    std::string ChatEngine::getSourceFile() const {
        return sourceFile;
    }

    std::vector<Document> ChatEngine::getDocs() const {
        return docs;
    }

    unsigned ChatEngine::getVectorLength() const {
        return pVectorLength;
    }

    unsigned ChatEngine::getContextNeighborhood() const {
        return pContextNeighborhood;
    }

    unsigned ChatEngine::getNoiseRatio() const {
        return pNoiseRatio;
    }

    float ChatEngine::getLearningRate() const {
        return learningRate;
    }

    Dictionary ChatEngine::getDict() const {
        return dict;
    }

    Infotbl ChatEngine::getInfotbl() const {
        return infotbl;
    }

    void ChatEngine::setSourceFile(const std::string &src) {
        sourceFile = src;
    }

    void ChatEngine::addDoc(const Document &doc) {
        docs.push_back(doc);
    }

    Document ChatEngine::tokenize(const std::string &text) {
        boost::tokenizer<> tknzr(text);
        std::vector<tag_t> toks;

        for (auto it = tknzr.begin(); it != tknzr.end(); ++it) {
            if (dict.find(*it) != dict.end())
                toks.push_back(dict[*it]);
            else {
                tag_t tkn = dict.size();
                dict[*it] = tkn;
                infotbl[tkn] = WordInfo(tkn, *it);
                toks.push_back(tkn);
            }
        }

        return Document(toks);
    }

    void ChatEngine::printDocuments() const {
        for (auto it = docs.begin(); it != docs.end(); ++it)
            it->printTokens();
    }

    void ChatEngine::preprocess() {
        std::ifstream fh(sourceFile);
        std::ofstream nullDev("/dev/null");
        FlexLexer *lexer = new yyFlexLexer(fh, nullDev);

        lexer->yylex();
    }
}


//==================================================================================================
/**
  Copyright 2016 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)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ABS_S_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ABS_S_HPP_INCLUDED

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( abs_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::unsigned_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()(const saturated_tag &, T const& a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( abs_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::floating_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()(const saturated_tag &, T const& a) const BOOST_NOEXCEPT
    {
      return bs::abs(a);
    }
  };
} } }

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to run a CNN based dog face detector using dlib.  The
    example loads a pretrained model and uses it to find dog faces in images.
    We also use the dlib::shape_predictor to find the location of the eyes and
    nose and then draw glasses and a mustache onto each dog found :)

    Users who are just learning about dlib's deep learning API should read the
    dnn_introduction_ex.cpp and dnn_introduction2_ex.cpp examples to learn how
    the API works.  For an introduction to the object detection method you
    should read dnn_mmod_ex.cpp

    TRAINING THE MODEL
        Finally, users interested in how the dog face detector was trained should
        read the dnn_mmod_ex.cpp example program.  It should be noted that the
        dog face detector used in this example uses a bigger training dataset and
        larger CNN architecture than what is shown in dnn_mmod_ex.cpp, but
        otherwise training is the same.  If you compare the net_type statements
        in this file and dnn_mmod_ex.cpp you will see that they are very similar
        except that the number of parameters has been increased.

        Additionally, the following training parameters were different during
        training: The following lines in dnn_mmod_ex.cpp were changed from
            mmod_options options(face_boxes_train, 40*40);
            trainer.set_iterations_without_progress_threshold(300);
        to the following when training the model used in this example:
            mmod_options options(face_boxes_train, 80*80);
            trainer.set_iterations_without_progress_threshold(8000);

        Also, the random_cropper was left at its default settings,  So we didn't
        call these functions:
            cropper.set_chip_dims(200, 200);
            cropper.set_min_object_height(0.2);

        The training data used to create the model is also available at
        http://dlib.net/files/data/CU_dogs_fully_labeled.tar.gz

        Lastly, the shape_predictor was trained with default settings except we
        used the following non-default settings: cascade depth=20, tree
        depth=5, padding=0.2
*/

#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>

using namespace std;
using namespace dlib;

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

template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,5,5,1,1,SUBNET>;

template <typename SUBNET> using downsampler  = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<affine<con5<45,SUBNET>>>;

using net_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

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

int main(int argc, char** argv) try
{
    if (argc < 3)
    {
        cout << "Call this program like this:" << endl;
        cout << "./dnn_mmod_dog_hipsterizer mmod_dog_hipsterizer.dat faces/dogs.jpg" << endl;
        cout << "\nYou can get the mmod_dog_hipsterizer.dat file from:\n";
        cout << "http://dlib.net/files/mmod_dog_hipsterizer.dat.bz2" << endl;
        return 0;
    }

    // load the models as well as glasses and mustache.
    net_type net;
    shape_predictor sp;
    matrix<rgb_alpha_pixel> glasses, mustache;
    deserialize(argv[1]) >> net >> sp >> glasses >> mustache;
    pyramid_up(glasses);
    pyramid_up(mustache);

    image_window win1(glasses);
    image_window win2(mustache);
    image_window win_wireframe, win_hipster;

    // Now process each image, find dogs, and hipsterize them by drawing glasses and a
    // mustache on each dog :)
    for (int i = 2; i < argc; ++i)
    {
        matrix<rgb_pixel> img;
        load_image(img, argv[i]);

        // Upsampling the image will allow us to find smaller dog faces but will use more
        // computational resources.
        //pyramid_up(img);

        auto dets = net(img);
        win_wireframe.clear_overlay();
        win_wireframe.set_image(img);
        // We will also draw a wireframe on each dog's face so you can see where the
        // shape_predictor is identifying face landmarks.
        std::vector<image_window::overlay_line> lines;
        for (auto&& d : dets)
        {
            // get the landmarks for this dog's face
            auto shape = sp(img, d.rect);

            const rgb_pixel color(0,255,0);
            auto top  = shape.part(0);
            auto lear = shape.part(1);
            auto leye = shape.part(2);
            auto nose = shape.part(3);
            auto rear = shape.part(4);
            auto reye = shape.part(5);

            // The locations of the left and right ends of the mustache.
            auto lmustache = 1.3*(leye-reye)/2 + nose;
            auto rmustache = 1.3*(reye-leye)/2 + nose;

            // Draw the glasses onto the image.
            std::vector<point> from = {2*point(176,36), 2*point(59,35)}, to = {leye, reye};
            auto tform = find_similarity_transform(from, to);
            for (long r = 0; r < glasses.nr(); ++r)
            {
                for (long c = 0; c < glasses.nc(); ++c)
                {
                    point p = tform(point(c,r));
                    if (get_rect(img).contains(p))
                        assign_pixel(img(p.y(),p.x()), glasses(r,c));
                }
            }

            // Draw the mustache onto the image right under the dog's nose.
            auto mrect = get_rect(mustache);
            from = {mrect.tl_corner(), mrect.tr_corner()};
            to = {rmustache, lmustache};
            tform = find_similarity_transform(from, to);
            for (long r = 0; r < mustache.nr(); ++r)
            {
                for (long c = 0; c < mustache.nc(); ++c)
                {
                    point p = tform(point(c,r));
                    if (get_rect(img).contains(p))
                        assign_pixel(img(p.y(),p.x()), mustache(r,c));
                }
            }

            // Record the lines needed for the face wire frame.
            lines.push_back(image_window::overlay_line(leye, nose, color));
            lines.push_back(image_window::overlay_line(nose, reye, color));
            lines.push_back(image_window::overlay_line(reye, leye, color));
            lines.push_back(image_window::overlay_line(reye, rear, color));
            lines.push_back(image_window::overlay_line(rear, top, color));
            lines.push_back(image_window::overlay_line(top, lear,  color));
            lines.push_back(image_window::overlay_line(lear, leye,  color));
        }

        win_wireframe.add_overlay(lines);
        win_hipster.set_image(img);

        cout << "Hit enter to process the next image." << endl;
        cin.get();
    }
}
catch(std::exception& e)
{
    cout << e.what() << endl;
}