// Copyright(c) 2020-Present, Matthew R. Hennefarth
// Distributed under the MIT License (http://opensource.org/licenses/MIT)

#include "FieldLocations.h"

/* EXTERNAL LIBRARY HEADER FILES */
#include <Eigen/Dense>
#include <matplot/matplot.h>

/* CPET HEADER FILES */
#include "System.h"

namespace cpet {

FieldLocations FieldLocations::fromSimple(
    const std::vector<std::string>& options) {
  FieldLocations fl;
  constexpr auto create_location = [](const std::string& location) -> AtomID {
    return AtomID{location};
  };
  std::transform(options.begin(), options.end(),
                 std::back_inserter(fl.locations_), create_location);
  return fl;
}

FieldLocations FieldLocations::fromBlock(
    const std::vector<std::string>& options) {
  constexpr const char* PLOT_KEY = "plot";
  constexpr const char* LOCATIONS_KEY = "locations";
  constexpr const char* OUTPUT_KEY = "output";

  FieldLocations fl;
  for (const auto& line : options) {
    const auto tokens = util::split(line, ' ');
    if (tokens.size() < 2) {
      continue;
    }

    const auto key = util::tolower(tokens[0]);
    const std::vector<std::string> key_options{tokens.begin() + 1,
                                               tokens.end()};

    if (key == LOCATIONS_KEY) {
      constexpr auto create_location =
          [](const std::string& location) -> AtomID {
        return AtomID{location};
      };

      std::transform(key_options.begin(), key_options.end(),
                     std::back_inserter(fl.locations_), create_location);
    } else if (key == PLOT_KEY) {
      fl.plotStyle_ = decodePlotStyle_(key_options);
    } else if (key == OUTPUT_KEY) {
      fl.output_ = *key_options.begin();
    }
  }
  return fl;
}
void FieldLocations::computeEFieldsWith(
    const std::vector<System>& systems) const {
  std::vector<std::vector<Eigen::Vector3d>> results;
  for (const auto& point : locations_) {
    SPDLOG_INFO("=~=~=~=~[Field at {}]=~=~=~=~", point.ID());
    std::vector<Eigen::Vector3d> fieldTrajectoryAtPoint;

    for (const auto& system : systems) {
      Eigen::Vector3d location;
      if (point.position()) {
        location = *(point.position());
      } else {
        location = system.frame().find(point)->coordinate;
      }

      Eigen::Vector3d field = system.electricFieldAt(location);
      SPDLOG_INFO("{} [{}]", field.transpose(), field.norm());
      fieldTrajectoryAtPoint.emplace_back(field);
    }
    results.push_back(fieldTrajectoryAtPoint);
  }
  if (output_) {
    writeOutput_(results);
  }
  if (showPlots()) {
    plot_(results);
  }
}
void FieldLocations::writeOutput_(
    const std::vector<std::vector<Eigen::Vector3d>>& results) const {
  if (!output_) {
    return;
  }

  std::ofstream outFile(*output_, std::ios::out);
  if (outFile.is_open()) {
    for (size_t i = 0; i < results.size(); i++) {
      outFile << '#' << locations_[i].ID() << '\n';
      for (const Eigen::Vector3d& field : results[i]) {
        outFile << field.transpose() << '\n';
      }
    }
    outFile << std::flush;
  } else {
    SPDLOG_ERROR("Could not open file {}", *output_);
    throw cpet::io_error("Could not open file " + *output_);
  }
}
void FieldLocations::plot_(
    const std::vector<std::vector<Eigen::Vector3d>>& results) const {
  const auto numberOfPlots =
      util::countSetBits(static_cast<unsigned int>(plotStyle_));
  if (numberOfPlots <= 0 || numberOfPlots > 4) {
    SPDLOG_WARN(
        "Number of plots less than 1 or greater than 4! Error in logic");
    return;
  }

  auto figure = matplot::figure();
  if (numberOfPlots < 3) {
    figure->tiledlayout(numberOfPlots, 1);
  } else {
    figure->tiledlayout(2, 2);
  }

  /* can make this an option eventually
   figure->size(500,500); */

  auto current_ax = matplot::nexttile(0);

  for (const auto& data : results) {
    std::array<std::vector<double>, 4> rotatedElectricFields;
    for (size_t index = 0; index < 3; index++) {
      const auto extract_index =
          [&index](const Eigen::Vector3d& vector) -> double {
        return vector[static_cast<long>(index)];
      };

      std::transform(data.begin(), data.end(),
                     std::back_inserter(rotatedElectricFields.at(index)),
                     extract_index);
    }

    constexpr auto compute_magnitude =
        [](const Eigen::Vector3d& vector) -> double { return vector.norm(); };

    std::transform(data.begin(), data.end(),
                   std::back_inserter(*rotatedElectricFields.rbegin()),
                   compute_magnitude);

    size_t plot_index = 0;

    // constexpr std::array<const char*, 4> titles{"X", "Y", "Z", "Magnitude"};
    const auto plot = [&rotatedElectricFields, &plot_index, &current_ax,
                       &figure](size_t index) {
      constexpr std::array<const char*, 4> titles{"X", "Y", "Z", "Magnitude"};
      current_ax = figure->nexttile(plot_index);
      matplot::hold(current_ax, matplot::on);
      matplot::plot(current_ax, rotatedElectricFields.at(index));
      current_ax->xlabel("Frame");
      current_ax->ylabel("Magnitude (V/Ang)");
      current_ax->title(titles.at(index));
      ++plot_index;
    };

    if (plotX_()) {
      plot(0);
    }
    if (plotY_()) {
      plot(1);
    }
    if (plotZ_()) {
      plot(2);
    }
    if (plotM_()) {
      plot(3);
    }
  }

  std::vector<std::string> legend_list;
  constexpr auto get_string_representation = [](const AtomID& aid) {
    return aid.ID();
  };
  std::transform(locations_.begin(), locations_.end(),
                 std::back_inserter(legend_list), get_string_representation);

  current_ax->legend(legend_list);
  matplot::show();
}

}  // namespace cpet


#include <boost/asio/ip/address.hpp>
#include <cstdint>
#include <iostream>
#include <iomanip>
#include <limits>
#include <string>

using boost::asio::ip::address;
using boost::asio::ip::address_v4;
using boost::asio::ip::address_v6;
using boost::asio::ip::make_address;
using boost::asio::ip::make_address_v4;
using boost::asio::ip::make_address_v6;

template<typename uint>
bool parse_int(const std::string& str, int base, uint& n)
{
    try
    {
        size_t pos = 0;
        unsigned long u = stoul(str, &pos, base);
        if (pos != str.length() || u > std::numeric_limits<uint>::max())
            return false;
        n = static_cast<uint>(u);
        return true;
    }
    catch (const std::exception& ex)
    {
        return false;
    }
}

//
// Parse an IP address and port from the given input string.
//
// Throws an exception if the input is not valid.
//
// Valid formats are:
// [ipv6_address]:port
// ipv4_address:port
// ipv4_address
// ipv6_address
//
void parse_ip_address_and_port(const std::string& input, address& addr, uint16_t& port)
{
    size_t pos = input.rfind(':');
    if (pos != std::string::npos && pos > 1 && pos + 1 < input.length()
        && parse_int(input.substr(pos + 1), 10, port) && port > 0)
    {
        if (input[0] == '[' && input[pos - 1] == ']')
        {
            // square brackets so can only be an IPv6 address
            addr = make_address_v6(input.substr(1, pos - 2));
            return;
        }
        else
        {
            try
            {
                // IPv4 address + port?
                addr = make_address_v4(input.substr(0, pos));
                return;
            }
            catch (const std::exception& ex)
            {
                // nope, might be an IPv6 address
            }
        }
    }
    port = 0;
    addr = make_address(input);
}

void print_address_and_port(const address& addr, uint16_t port)
{
    std::cout << std::hex << std::uppercase << std::setfill('0');
    if (addr.is_v4())
    {
        address_v4 addr4 = addr.to_v4();
        std::cout << "address family: IPv4\n";
        std::cout << "address number: " << std::setw(8) << addr4.to_uint() << '\n';
    }
    else if (addr.is_v6())
    {
        address_v6 addr6 = addr.to_v6();
        address_v6::bytes_type bytes(addr6.to_bytes());
        std::cout << "address family: IPv6\n";
        std::cout << "address number: ";
        for (unsigned char byte : bytes)
            std::cout << std::setw(2) << static_cast<unsigned int>(byte);
        std::cout << '\n';
    }
    if (port != 0)
        std::cout << "port: " << std::dec << port << '\n';
    else
        std::cout << "port not specified\n";
}

void test(const std::string& input)
{
    std::cout << "input: " << input << '\n';
    try
    {
        address addr;
        uint16_t port = 0;
        parse_ip_address_and_port(input, addr, port);
        print_address_and_port(addr, port);
    }
    catch (const std::exception& ex)
    {
        std::cout << "parsing failed\n";
    }
    std::cout << '\n';
}

int main(int argc, char** argv)
{
    test("127.0.0.1");
    test("127.0.0.1:80");
    test("::ffff:127.0.0.1");
    test("::1");
    test("[::1]:80");
    test("1::80");
    test("2605:2700:0:3::4713:93e3");
    test("[2605:2700:0:3::4713:93e3]:80");
    return 0;
}


#include<iostream>
// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>

#include"simplevo/visual_odometry.h"
#include "simplevo/config.h"

int main(int argc, char const *argv[]){
if ( argc != 2 ){
        cout<<"usage: example parameter_file"<<endl;
        return 1;
    }
    // get a config file
    simplevo::Config::setParameterFile(argv[1]);
    simplevo::VisualOdometry::Ptr vo(new simplevo::VisualOdometry);  //declare a VO pointer
    string dataset_dir = simplevo::Config::get<string> ( "dataset_dir" );   // config dataset path
     ifstream fin ( dataset_dir+"/associate.txt" );
    int numsRbgFiles =0;

    if ( !fin )  // Make Sure Open the associate txt
    {
        cout<<"please generate the associate file called associate.txt!"<<endl;
        return 1;
    }

    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
 // Read the file until to the end
   while ( !fin.eof() ){
    string rgb_time, rgb_file, depth_time, depth_file;
    fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
    rgb_times.push_back ( atof ( rgb_time.c_str() ) );   // RBG images Timestamp
    depth_times.push_back ( atof ( depth_time.c_str() ) ); // Depth images Timestamps
    rgb_files.push_back ( dataset_dir+"/"+rgb_file );   // RBG FILE
    depth_files.push_back ( dataset_dir+"/"+depth_file ); // Depth FIle
    if ( fin.good() == false )
            break;
    }
    numsRbgFiles = rgb_files.size();
  // Build a Camera Model for Use and translation of coordinate
  simplevo::Camera::Ptr camera_instance (new simplevo::Camera);
  for(int i =0;i<numsRbgFiles;i++){
      Mat color_image = cv::imread(rgb_files[i]);
      Mat depth_image = cv::imread(depth_files[i]);
      if((color_image.data==nullptr)||(depth_image.data==nullptr)){
          break;
      }
      // Build a Frame and Set Parameters
      simplevo::Frame::Ptr pFrame = simplevo::Frame::createFrame();
      pFrame->camera_ = camera_instance;
      pFrame->color_ = color_image;
      pFrame->depth_ = depth_image;
      pFrame-> time_stamp_ = rgb_times[i];

    boost::timer timer;
    vo->addFrame(pFrame);
  }
    return 0;
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(line)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;

  {
    V const p0 = V::make( 0.0f, 0.0f, 0.0f );
    V const p1 = V::make( 2.0f, 0.0f, 0.0f );

    geometry::Line<V> L = geometry::make_line(p0, p1);

    BOOST_CHECK_EQUAL( L.point()(0), 0.0f );
    BOOST_CHECK_EQUAL( L.point()(1), 0.0f );
    BOOST_CHECK_EQUAL( L.point()(2), 0.0f );

    BOOST_CHECK_EQUAL( L.direction()(0), 1.0f );
    BOOST_CHECK_EQUAL( L.direction()(1), 0.0f );
    BOOST_CHECK_EQUAL( L.direction()(2), 0.0f );

  }
  {
    V const p0 = V::make( 0.0f, 0.0f, 0.0f );
    V const p1 = V::make( 2.0f, 0.0f, 0.0f );

    geometry::Line<V> L = geometry::make_line(p0, p1, geometry::FROM_POINTS() );

    BOOST_CHECK_EQUAL( L.point()(0), 0.0f );
    BOOST_CHECK_EQUAL( L.point()(1), 0.0f );
    BOOST_CHECK_EQUAL( L.point()(2), 0.0f );

    BOOST_CHECK_EQUAL( L.direction()(0), 1.0f );
    BOOST_CHECK_EQUAL( L.direction()(1), 0.0f );
    BOOST_CHECK_EQUAL( L.direction()(2), 0.0f );

  }
  {
    V const p = V::make( 0.0f, 0.0f, 0.0f );
    V const d = V::make( 0.5f, 0.0f, 0.0f );

    geometry::Line<V> L = geometry::make_line(p, d, geometry::FROM_DIRECTION() );

    BOOST_CHECK_EQUAL( L.point()(0), 0.0f );
    BOOST_CHECK_EQUAL( L.point()(1), 0.0f );
    BOOST_CHECK_EQUAL( L.point()(2), 0.0f );

    BOOST_CHECK_EQUAL( L.direction()(0), 1.0f );
    BOOST_CHECK_EQUAL( L.direction()(1), 0.0f );
    BOOST_CHECK_EQUAL( L.direction()(2), 0.0f );
  }

}

BOOST_AUTO_TEST_SUITE_END();


// As this module uses openVDB which can conflict with the custom math operators used for cuda we
// disable them here.
#define NEW_STYLE
//#define NO_OPERATORS
#define BOOST_USE_WINDOWS_H
#include <SPH/generation/volumeOutlets.cuh>
#include <utility/include_all.h>
#ifdef _WIN32
#pragma warning(push, 0)
#endif
#include <openvdb/Platform.h>
#include <openvdb/openvdb.h>
#include <openvdb/tools/LevelSetFilter.h>
#include <openvdb/tools/ParticlesToLevelSet.h>
#include <openvdb/tools/VolumeToMesh.h>
#include <openvdb/tree/ValueAccessor.h>
#ifdef _WIN32
#pragma warning(pop)
#endif
// At some point this will have to be replaced with <filesystem>
#include <fstream>
#ifdef _WIN32
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#else
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
#endif

#include <utility/generation.h>

// This function is used to load the vdb files from disk and transforms them into cuda 3d textures.
void SPH::Outlet::init(Memory mem) {
  if (!valid(mem)) {
    return;
  }

  auto vols = get<parameters::outletVolumes>();

  std::vector<cudaTextureObject_t> textures;
  std::vector<float4> minAABB;
  std::vector<float4> maxAABB;
  std::vector<int4> dims;
  std::vector<float> rates;

  for (auto boundaryVolume : vols) {
    //std::string vdbFile = get<parameters::config_folder>() + boundaryVolume.fileName.value;
    //auto std_file = fs::path(vdbFile);
    //auto tmp_file = std_file;
    //tmp_file.replace_extension(".vdb_tmp");

    //float4 *host_mem = nullptr;
    //openvdb::Vec3d min, max;
    //int4 dimension;
    //if (!fs::exists(tmp_file) || fs::last_write_time(std_file) > fs::last_write_time(tmp_file)) {
    //  logger(log_level::info) << "Parsing VDB file: " << std_file;
    //  openvdb::initialize();
    //  openvdb::io::File file(vdbFile);

    //  openvdb::FloatGrid::Ptr levelSet = openvdb::FloatGrid::create();
    //  levelSet->setTransform(openvdb::math::Transform::createLinearTransform(0.5));
    //  levelSet->setGridClass(openvdb::GRID_LEVEL_SET);
    //  levelSet->setName("LevelSet");

    //  openvdb::Vec3DGrid::Ptr normalSet = openvdb::Vec3dGrid::create();
    //  normalSet->setTransform(openvdb::math::Transform::createLinearTransform(0.5));
    //  normalSet->setName("NormalsLS");

    //  file.open();
    //  auto grids = file.getGrids();
    //  openvdb::GridBase::Ptr baseGrid;
    //  for (openvdb::io::File::NameIterator nameIter = file.beginName(); nameIter != file.endName(); ++nameIter) {
    //    if (nameIter.gridName() == "surface") {
    //      baseGrid = file.readGrid(nameIter.gridName());
    //      // std::cout << "Found Grid" << std::endl;
    //    }
    //  }
    //  file.close();

    //  levelSet = openvdb::gridPtrCast<openvdb::FloatGrid>(baseGrid);
    //  openvdb::FloatGrid::ConstAccessor c_accessor = levelSet->getConstAccessor();
    //  openvdb::Vec3dGrid::Accessor norm_accessor = normalSet->getAccessor();
    //  openvdb::math::UniformScaleMap map(0.5);

    //  openvdb::FloatGrid::ConstAccessor naccessor = levelSet->getConstAccessor();
    //  openvdb::tools::GridSampler<openvdb::FloatGrid::ConstAccessor, openvdb::tools::BoxSampler> normalSampler(naccessor,
    //                                                                                levelSet->transform());

    //  openvdb::math::ScaleMap scalemap(levelSet->voxelSize());
    //  auto transformer = [&](auto iter, auto accessor) {
    //    openvdb::FloatGrid::ConstAccessor c_accessor = levelSet->getConstAccessor();
    //    openvdb::math::Vec3d isGrad = openvdb::math::ISGradient<openvdb::math::FD_HJWENO5>::result(c_accessor, iter.getCoord());
    //    openvdb::math::Vec3d wsGrad = scalemap.applyIJT(isGrad);
    //    wsGrad = openvdb::math::Gradient<openvdb::math::UniformScaleMap, openvdb::math::CD_6TH>::result(map, c_accessor, iter.getCoord());

    //    openvdb::Coord c = iter.getCoord();
    //    auto sample = [&](int32_t x, int32_t y, int32_t z) {
    //      // c_accessor.getValue
    //      return c_accessor.getValue(openvdb::Coord(c.x() + x, c.y() + y, c.z() + z));
    //    };

    //    openvdb::Coord c2(0, 0, 0);
    //    float max = -FLT_MAX;
    //    for (int32_t x = -1; x <= 1; ++x)
    //      for (int32_t y = -1; y <= 1; ++y)
    //        for (int32_t z = -1; z <= 1; ++z) {
    //          if (abs(x) + abs(y) + abs(z) == 3)
    //            continue;
    //          float s = sample(x, y, z);
    //          if (s > max) {
    //            c2 = openvdb::Coord(x, y, z);
    //            max = s;
    //          }
    //        }
    //    // wsGrad = openvdb::math::Vec3d(c2.x(), c2.y(), c2.z());

    //    float central_x = sample(1, 0, 0) - 2 * sample(0, 0, 0) + sample(-1, 0, 0);
    //    float central_y = sample(0, 1, 0) - 2 * sample(0, 0, 0) + sample(0, -1, 0);
    //    float central_z = sample(0, 0, 1) - 2 * sample(0, 0, 0) + sample(0, 0, -1);
    //    central_x = abs(central_x) > abs(central_y) && abs(central_x) > abs(central_z) ? central_x : 0.f;
    //    central_y = abs(central_y) > abs(central_x) && abs(central_y) > abs(central_z) ? central_y : 0.f;
    //    central_z = abs(central_z) > abs(central_y) && abs(central_z) > abs(central_x) ? central_z : 0.f;

    //    wsGrad.normalize();
    //    if (iter.isVoxelValue()) { // set a single voxel
    //      accessor.setValue(iter.getCoord(), wsGrad);
    //    } else { // fill an entire tile
    //      openvdb::CoordBBox bbox;
    //      iter.getBoundingBox(bbox);
    //      accessor.getTree()->fill(bbox, wsGrad);
    //    }
    //  };
    //  openvdb::tools::transformValues(levelSet->cbeginValueOn(), *normalSet, transformer);
    //  dimension = int4{256, 256, 256, 0};
    //  openvdb::CoordBBox box = levelSet->evalActiveVoxelBoundingBox();
    //  min = levelSet->indexToWorld(box.getStart());
    //  max = levelSet->indexToWorld(box.getEnd());

    //  openvdb::tools::GridSampler<openvdb::FloatGrid, openvdb::tools::BoxSampler> d_sampler(*levelSet);
    //  openvdb::tools::GridSampler<openvdb::Vec3dGrid, openvdb::tools::BoxSampler> n_sampler(*normalSet);

    //  host_mem = (float4 *)malloc(dimension.x * dimension.y * dimension.z * sizeof(float4));

    //  auto getWs = [&](uint32_t x, uint32_t y, uint32_t z) {
    //    openvdb::Vec3d pos = min;
    //    openvdb::Vec3d diff = max - min;
    //    openvdb::Vec3d offs{((double)x) / ((double)dimension.x), ((double)y) / ((double)dimension.y), ((double)z) / ((double)dimension.z)};
    //    return pos + diff * offs;
    //  };
    //  auto getSample = [&](uint32_t x, uint32_t y, uint32_t z) {
    //    auto ws = getWs(x, y, z);
    //    auto d = d_sampler.wsSample(ws);
    //    auto n = n_sampler.wsSample(ws);
    //    return float4{(float)n.x(), (float)n.y(), (float)n.z(), d};
    //  };

    //  for (int32_t x = 0; x < dimension.x; ++x)
    //    for (int32_t y = 0; y < dimension.y; ++y)
    //      for (int32_t z = 0; z < dimension.z; ++z)
    //        host_mem[x + dimension.x * (y + dimension.y * z)] = getSample(x, y, z);

    //  std::ofstream processed_vdb(tmp_file.string(), std::ios::out | std::ios::binary);
    //  processed_vdb.write(reinterpret_cast<char *>(&dimension), sizeof(int4));
    //  processed_vdb.write(reinterpret_cast<char *>(&min), sizeof(openvdb::Vec3d));
    //  processed_vdb.write(reinterpret_cast<char *>(&max), sizeof(openvdb::Vec3d));
    //  processed_vdb.write(reinterpret_cast<char *>(host_mem), dimension.x * dimension.y * dimension.z * sizeof(float4));
    //  processed_vdb.close();
    //} else {
    //  logger(log_level::info) << "Loading TEMP VDB file: " << tmp_file << std::endl;
    //  std::ifstream processed_vdb(tmp_file.string(), std::ios::in | std::ios::binary);
    //  processed_vdb.read(reinterpret_cast<char *>(&dimension), sizeof(int4));
    //  processed_vdb.read(reinterpret_cast<char *>(&min), sizeof(openvdb::Vec3d));
    //  processed_vdb.read(reinterpret_cast<char *>(&max), sizeof(openvdb::Vec3d));
    //  host_mem = (float4 *)malloc(dimension.x * dimension.y * dimension.z * sizeof(float4));
    //  processed_vdb.read(reinterpret_cast<char *>(host_mem), dimension.x * dimension.y * dimension.z * sizeof(float4));
    //  processed_vdb.close();
    //}
    //cudaTextureDesc textureDescription;
    //auto channelDesc = cudaCreateChannelDesc<float4>();
    //memset(&textureDescription, 0, sizeof(cudaTextureDesc));
    //textureDescription.normalizedCoords = true;
    //textureDescription.filterMode = cudaFilterModeLinear;
    //textureDescription.addressMode[0] = cudaAddressModeClamp; // clamp
    //textureDescription.addressMode[1] = cudaAddressModeClamp;
    //textureDescription.addressMode[2] = cudaAddressModeClamp;
    //textureDescription.readMode = cudaReadModeElementType;

    //cudaArray *arr_ptr;
    //cudaPitchedPtr pitched_ptr;
    //cudaTextureObject_t texture;

    //cudaMalloc3DArray(&arr_ptr, &channelDesc, make_cudaExtent(dimension.x, dimension.y, dimension.y));
    //pitched_ptr = make_cudaPitchedPtr(host_mem, dimension.x * sizeof(float4), dimension.x, dimension.y);

    //cudaMemcpy3DParms copyParams;
    //memset(&copyParams,0x00,sizeof(cudaMemcpy3DParms));
    //copyParams.srcPtr = pitched_ptr;
    //copyParams.dstArray = arr_ptr;
    //copyParams.extent = make_cudaExtent(dimension.x, dimension.y, math::weak_get<3>(dimension));
    //copyParams.kind = cudaMemcpyHostToDevice;
    //cudaMemcpy3D(&copyParams);

    //cudaResourceDesc texRes;
    //memset(&texRes, 0, sizeof(cudaResourceDesc));
    //texRes.resType = cudaResourceTypeArray;
    //texRes.res.array.array = arr_ptr;
	//cudaCreateTextureObject(&texture, &texRes, &textureDescription, NULL);

	auto[texture, min, max, dimension, centerOfMass, inertia] = generation::cudaVolume(boundaryVolume.fileName.value);

	min += centerOfMass;
	max += centerOfMass;
    textures.push_back(texture);
    minAABB.push_back(float4{static_cast<float>(min.x), static_cast<float>(min.y), static_cast<float>(min.z), 0.f});
    maxAABB.push_back(float4{static_cast<float>(max.x), static_cast<float>(max.y), static_cast<float>(max.z), 0.f});
    dims.push_back(dimension);
    rates.push_back(boundaryVolume.flowRate.value);
    //free(host_mem);
  }
  if (textures.size() > 0) {
    using dimensions = decltype(info<arrays::volumeOutletDimensions>());
    using volumes = decltype(info<arrays::volumeOutletVolumes>());
    using boundaryMins = decltype(info<arrays::volumeOutletMin>());
    using boundaryMaxs = decltype(info<arrays::volumeOutletMax>());
    using rate = decltype(info<arrays::volumeOutletRate>());
    using rateAcc = decltype(info<arrays::volumeOutletRateAccumulator>());

    get<parameters::volumeOutletCounter>() = (int32_t) textures.size();

    dimensions::allocate(sizeof(int4) * textures.size());
    volumes::allocate(sizeof(cudaTextureObject_t) * textures.size());
    boundaryMins::allocate(sizeof(float4) * textures.size());
    boundaryMaxs::allocate(sizeof(float4) * textures.size());
    rate::allocate(sizeof(float) * textures.size());
    rateAcc::allocate(sizeof(float) * textures.size());

#ifdef UNIFIED_MEMORY
	for (uint32_t i = 0; i < textures.size(); ++i) {
		dimensions::ptr[i] = dims[i];
		volumes::ptr[i] = textures[i];
		boundaryMins::ptr[i] = minAABB[i];
		boundaryMaxs::ptr[i] = maxAABB[i];
        rate::ptr[i] = rates[i];
        rateAcc::ptr[i] = 0.f;
  }
#else
	cudaMemcpy(dimensions::ptr, dims.data(), sizeof(decltype(dims)::value_type) * dims.size(), cudaMemcpyHostToDevice);
	cudaMemcpy(volumes::ptr, textures.data(), sizeof(decltype(textures)::value_type) * textures.size(), cudaMemcpyHostToDevice);
	cudaMemcpy(boundaryMins::ptr, minAABB.data(), sizeof(decltype(minAABB)::value_type) * minAABB.size(), cudaMemcpyHostToDevice);
	cudaMemcpy(boundaryMaxs::ptr, maxAABB.data(), sizeof(decltype(maxAABB)::value_type) * maxAABB.size(), cudaMemcpyHostToDevice);
	cudaMemcpy(rate::ptr, rates.data(), sizeof(decltype(rates)::value_type) * rates.size(), cudaMemcpyHostToDevice);
	cudaMemset(rateAcc::ptr,0x00,sizeof(float) * textures.size());
#endif
  }
  else {
	  using dimensions = decltype(info<arrays::volumeOutletDimensions>());
	  using volumes = decltype(info<arrays::volumeOutletVolumes>());
	  using boundaryMins = decltype(info<arrays::volumeOutletMin>());
	  using boundaryMaxs = decltype(info<arrays::volumeOutletMax>());
      using rate = decltype(info<arrays::volumeOutletRate>());
      using rateAcc = decltype(info<arrays::volumeOutletRateAccumulator>());

	  get<parameters::volumeOutletCounter>() = 0;

	  dimensions::allocate(sizeof(int4) * 1);
	  volumes::allocate(sizeof(cudaTextureObject_t) * 1);
	  boundaryMins::allocate(sizeof(float4) * 1);
	  boundaryMaxs::allocate(sizeof(float4) * 1);
      rate::allocate(sizeof(float) * 1);
      rateAcc::allocate(sizeof(float) * 1);
  }
}


/*******************************************************************************
Copyright(c) 2015-2018 LORD Corporation. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/Wireless/CalCoefficients.h"

#include <boost/test/unit_test.hpp>
using namespace mscl;

BOOST_AUTO_TEST_SUITE(CalCoefficients_Test)

BOOST_AUTO_TEST_CASE(CalCoefficients_DefaultConstructor)
{
    CalCoefficients cals;

    //verify that everything was initialized as it should be
    BOOST_CHECK_EQUAL(cals.equationType(), WirelessTypes::equation_standard);
    BOOST_CHECK_EQUAL(cals.unit(), WirelessTypes::unit_none);
    BOOST_CHECK_EQUAL(cals.linearEquation().slope(), 1.0f);
    BOOST_CHECK_EQUAL(cals.linearEquation().offset(), 0.0f);
}

BOOST_AUTO_TEST_CASE(CalCoefficients_Equals)
{
    CalCoefficients cal1(WirelessTypes::equation_standard, WirelessTypes::unit_accel_g, LinearEquation(3.1245f, 0.07823f));
    BOOST_CHECK_EQUAL(cal1.equationType(), WirelessTypes::equation_standard);
    BOOST_CHECK_EQUAL(cal1.unit(), WirelessTypes::unit_accel_g);
    BOOST_CHECK_EQUAL(cal1.linearEquation().slope(), 3.1245f);
    BOOST_CHECK_EQUAL(cal1.linearEquation().offset(), 0.07823f);

    CalCoefficients cal2(WirelessTypes::equation_standard, WirelessTypes::unit_accel_g, LinearEquation(3.1245f, 0.07823f));
    BOOST_CHECK_EQUAL(cal1 == cal2, true);

    CalCoefficients cal3(WirelessTypes::equation_none, WirelessTypes::unit_accel_g, LinearEquation(3.1245f, 0.07823f));
    BOOST_CHECK_EQUAL(cal1 == cal3, false);

    CalCoefficients cal4(WirelessTypes::equation_standard, WirelessTypes::unit_accel_milliG, LinearEquation(3.1245f, 0.07823f));
    BOOST_CHECK_EQUAL(cal1 == cal4, false);

    CalCoefficients cal5(WirelessTypes::equation_standard, WirelessTypes::unit_accel_g, LinearEquation(5.1245f, 0.07823f));
    BOOST_CHECK_EQUAL(cal1 == cal5, false);

    CalCoefficients cal6(WirelessTypes::equation_standard, WirelessTypes::unit_accel_g, LinearEquation(3.1245f, 0.078231f));
    BOOST_CHECK_EQUAL(cal1 == cal6, false);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @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_FUNCTION_LOGICAL_ANDNOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOGICAL_ANDNOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-boolean
    Function object implementing logical_andnot capabilities

    The operands must share the same @ref cardinal_of value.

    return the logical and of the first parameter and of the negation the second parameter.

    The operands must share the same @ref cardinal_of value.

    The result type is logical type associated to the first parameter.

    @par Semantic:

    @code
    auto r = logical_andnot(x,y);
    @endcode

    is similar to:

    @code
    auto r = x && !y;
    @endcode

    @see  logical_or, logical_xor, logical_notand,
    logical_and, logical_notor, logical_ornot, logical_not

  **/
  as_logical_t<Value0> logical_andnot(Value0 const& x, Value1 const& y);
} }
#endif

#include <boost/simd/function/scalar/logical_andnot.hpp>
#include <boost/simd/function/simd/logical_andnot.hpp>

#endif


//==================================================================================================
/*!
  @file

    @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_FUNCTION_ONEMINUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ONEMINUS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object returns  one minus the entry.

    @par Header <boost/simd/function/oneminus.hpp>

    @par Notes
    Using `oneminus(x)` is similar to `1-x`

    @par Decorator

    _saturated_ is available,  but for unsigned integral types the saturated_
    function is merely  equivalent to `x == 0 ? 1 : 0`

    @par Example:

      @snippet oneminus.cpp oneminus

    @par Possible output:

      @snippet oneminus.txt oneminus

  **/
  Value oneminus(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/simd/function/simd/oneminus.hpp>

#endif


#ifndef REMAINDERTREE_SRC_ALGORITHMS_FACTORIALENGINE_H_
#define REMAINDERTREE_SRC_ALGORITHMS_FACTORIALENGINE_H_

// included by rem_factorial.tpp

#include <functional>
#include <vector>

#include <NTL/ZZ_pX.h>
#include <NTL/matrix.h>

//TODO: make these all const
template<typename T, typename U>
T naive_factorial(long n, const U& m, const std::function<std::vector<T> (long, long)>& get_A);

NTL::ZZ_p poly_factorial(long n, const NTL::ZZ& m, const NTL::ZZ_pX& poly);

NTL::Mat<NTL::ZZ_p> matrix_factorial(long n, const NTL::ZZ& m, const NTL::Mat<NTL::ZZ_pX>& matrix);

#include "factorial_engine.tpp"

#endif //REMAINDERTREE_SRC_ALGORITHMS_FACTORIALENGINE_H_


#ifndef CONTRACTOR_PROCESSOR_SYMMETRIZER_HPP_
#define CONTRACTOR_PROCESSOR_SYMMETRIZER_HPP_

#include "terms/Index.hpp"
#include "terms/Tensor.hpp"
#include "terms/Term.hpp"
#include "utils/HeapsAlgorithm.hpp"

#include <cassert>
#include <numeric>
#include <optional>
#include <type_traits>
#include <unordered_set>
#include <vector>

#include <boost/range/join.hpp>

namespace Contractor::Processor {

namespace details {
	template< Terms::Index::Type Type > struct is_index_type {
		bool operator()(const Terms::Index &index) const { return index.getType() == Type; }
	};

	std::optional< Terms::Tensor::index_list_t::const_iterator > find_index_by_name(const Terms::Index &index,
																					const Terms::Term &term) {
		for (const Terms::Tensor &tensor : term.getTensors()) {
			auto it =
				std::find_if(tensor.getIndices().begin(), tensor.getIndices().end(), [index](const Terms::Index &idx) {
					return idx.getSpace() == index.getSpace() && idx.getID() == index.getID();
				});

			if (it != tensor.getIndices().end()) {
				return std::optional(it);
			}
		}

		return std::nullopt;
	}

	void adaptSubstitutionToTerm(Terms::IndexSubstitution &sub, const Terms::Term &term) {
		for (Terms::IndexSubstitution::index_pair_t &currentPair : sub.accessSubstitutions()) {
			std::optional< Terms::Tensor::index_list_t::const_iterator > it =
				find_index_by_name(currentPair.first, term);

			assert(it.has_value());

			currentPair.first = *it.value();

			it = find_index_by_name(currentPair.second, term);

			assert(it.has_value());

			currentPair.second = *it.value();

			// Make sure that we are only exchanging the index name, not the spin (retaining the proper type is taken
			// care of by the IndexSubstitution class itself).
			currentPair.second.setSpin(currentPair.first.getSpin());
		}
	}
}; // namespace details

template< typename term_t > class Symmetrizer {
public:
	Symmetrizer()  = default;
	~Symmetrizer() = default;

	const std::vector< term_t > &antisymmetrize(const term_t &term) {
		static_assert(std::is_base_of_v< Terms::Term, term_t >);

		m_resultingTerms.clear();

		std::vector< Terms::Index > creators;
		std::vector< Terms::Index > annihilators;

		for (const Terms::Index &current : term.getResult().getIndices()) {
			switch (current.getType()) {
				case Terms::Index::Type::Creator:
					creators.push_back(current);
					break;
				case Terms::Index::Type::Annihilator:
					annihilators.push_back(current);
					break;
				case Terms::Index::Type::None:
					// Do nothing
					break;
			}
		}

		std::vector< Terms::IndexSubstitution > creatorSubs     = antisymmetrize(creators, term.getResult());
		std::vector< Terms::IndexSubstitution > annihilatorSubs = antisymmetrize(annihilators, term.getResult());

		Terms::PermutationGroup symmetry = term.getResult().getSymmetry();
		for (const Terms::IndexSubstitution &current : boost::join(creatorSubs, annihilatorSubs)) {
			if (!current.isIdentity()) {
				symmetry.addGenerator(current, false);
			}
		}

		// Create all possible combinations of the both substitution categories
		for (const Terms::IndexSubstitution &currentCreatorSub : creatorSubs) {
			for (const Terms::IndexSubstitution &currentAnnihilatorSub : annihilatorSubs) {
				term_t termCopy = term;

				Terms::IndexSubstitution::factor_t factor = 1;

				// Now also apply it to the rest of the Tensors in the Term
				for (Terms::Tensor &currentTensor : termCopy.accessTensors()) {
					factor = 1;
					factor *= currentCreatorSub.apply(currentTensor);
					factor *= currentAnnihilatorSub.apply(currentTensor);
				}

				termCopy.setPrefactor(factor * term.getPrefactor());

				symmetry.setRootSequence(termCopy.getResult().getIndices());
				termCopy.accessResult().setSymmetry(symmetry);

				m_resultingTerms.push_back(std::move(termCopy));
			}
		}

		assert(!m_resultingTerms.empty());
		return m_resultingTerms;
	}

	double getPrefactor() const { return 1.0 / m_resultingTerms.size(); }

	std::vector< Terms::IndexSubstitution > antisymmetrize(const std::vector< Terms::Index > &indices,
														   const Terms::Tensor &tensor) {
		std::vector< Terms::IndexSubstitution > substitutions;

		// The identity substitution should always be present
		substitutions.push_back(Terms::IndexSubstitution::identity());

		std::vector< Terms::Index > indicesCopy = indices;
		Utils::HeapsAlgorithm heapAlg(indicesCopy);

		do {
			Terms::IndexSubstitution::substitution_list currentSubs;
			std::unordered_set< Terms::Index > processedIndices;

			// Create index mapping from the original indices to the current permutation
			for (std::size_t i = 0; i < indices.size(); ++i) {
				if (processedIndices.find(indices[i]) == processedIndices.end()
					&& processedIndices.find(indicesCopy[i]) == processedIndices.end()) {
					currentSubs.push_back(Terms::IndexSubstitution::index_pair_t(indices[i], indicesCopy[i]));

					processedIndices.insert(indices[i]);
					processedIndices.insert(indicesCopy[i]);
				} else {
#ifndef NDEBUG
					// Verify that the previous mapping is the reverse of the current one. Everything else is unexpected
					bool duplicateSubstitutionIsReverseOfExisting = false;
					for (const Terms::IndexSubstitution::index_pair_t &currentPair : currentSubs) {
						if (currentPair.first == indicesCopy[i] && currentPair.second == indices[i]) {
							duplicateSubstitutionIsReverseOfExisting = true;
							break;
						}
					}

					assert(duplicateSubstitutionIsReverseOfExisting);
#endif
				}
			}

			int sign = heapAlg.getParity() ? 1 : -1;

			Terms::IndexSubstitution sub = Terms::IndexSubstitution::createPermutation(std::move(currentSubs), sign);

			// First check whether the given Tensor already has this symmetry and only consider it, if it doesn't
			if (!tensor.getSymmetry().contains(sub)) {
				substitutions.push_back(std::move(sub));
			}
		} while (heapAlg.nextPermutation());

		return substitutions;
	}

	const std::vector< term_t > &symmetrize(const term_t &term, bool ignoreExistingSymmetries = false) {
		m_resultingTerms.clear();

		std::vector< Terms::IndexSubstitution > symmetrizations =
			generateSymmetrizations(term, ignoreExistingSymmetries);

		// We expect at least the identity operation to always be present
		assert(!symmetrizations.empty());

		// Copy Terms so we can access it
		term_t termCopy = term;

		for (const Terms::IndexSubstitution &currentSymmetry : symmetrizations) {
			// "Inform" the result Tensor about its (potentially) new symmetry
			termCopy.accessResult().accessSymmetry().addGenerator(currentSymmetry);
		}

		for (Terms::IndexSubstitution &currentSymmetrization : symmetrizations) {
			assert(currentSymmetrization.appliesTo(termCopy.getResult()));

			term_t currentCopy = termCopy;

			Terms::IndexSubstitution::factor_t factor = 1;

			// We have to "adapt" the current symmetrization to the given  Term in order to make
			// sure that we can also properly symmetrize spin-summed terms such as
			// O2[aⁿ⁺bⁿ⁺iⁿ⁻jⁿ⁻] += 0.125 * B_T2[a🠑⁺i🠑⁻qⁿ] * B_T2[b🠑⁺j🠑⁻qⁿ]
			// where we e.g. want to exchange a and b but these indices have different spins in the result
			// Tensor (which the original symmetrization is based on) and on the rhs of the term.
			// The "adaption" essentially ensures that we are exchanging indices "by name" which means
			// ignoring index type and spin.
			details::adaptSubstitutionToTerm(currentSymmetrization, termCopy);

			// Also apply to all contained Tensors
			for (Terms::Tensor &currenTensor : currentCopy.accessTensors()) {
				factor = currentSymmetrization.apply(currenTensor);
			}

			termCopy.setPrefactor(currentCopy.getPrefactor() * factor);

			m_resultingTerms.push_back(std::move(currentCopy));
		}

		return m_resultingTerms;
	}

	std::vector< Terms::IndexSubstitution > generateSymmetrizations(const term_t &term, bool ignoreExistingSymmetries) {
		std::vector< Terms::IndexSubstitution > symmetrizations;
		// The identity operation is always contained in order to retain the original term as-is
		symmetrizations.push_back(Terms::IndexSubstitution::identity());

		// Gather information about index distribution
		// We assume indices are ordered as Creators, Annihilators, Other
		const Terms::Tensor::index_list_t &indices = term.getResult().getIndices();
		auto creatorBegin                          = indices.begin();
		auto annihilatorBegin =
			std::find_if(creatorBegin, indices.end(), details::is_index_type< Terms::Index::Type::Annihilator >{});
		auto annihilatorEnd =
			std::find_if(annihilatorBegin, indices.end(), details::is_index_type< Terms::Index::Type::None >{});
		auto creatorEnd = annihilatorBegin;

		std::size_t nCreators     = std::distance(creatorBegin, creatorEnd);
		std::size_t nAnnihilators = std::distance(annihilatorBegin, annihilatorEnd);

		if (nCreators != nAnnihilators) {
			throw std::runtime_error("Can't symmetrize Tensor with different amounts of creators and annihilators!");
		}

		// Symmetrization works by arranging all creator/annihilator pairs in all possible orders
		std::vector< std::size_t > pairPositions;
		pairPositions.resize(nCreators);
		std::iota(pairPositions.begin(), pairPositions.end(), 0);

		// We always start with the first permutation of pairPositions in order to avoid the case where all
		// index positions are mapped to themselves (identity operation) as that is already explicitly added
		// above.
		while (std::next_permutation(pairPositions.begin(), pairPositions.end())) {
			Terms::IndexSubstitution::substitution_list subs;
			subs.reserve(nCreators);

			for (std::size_t i = 0; i < nCreators; ++i) {
				// Swap the i-th creator/annihilator with the pairPositions[i]-th one such that
				// the pair that is at position i in the original result Tensor is moved to the
				// pairPositions[i]-th position after the substitution.
				subs.push_back({ *(creatorBegin + i), *(creatorBegin + pairPositions[i]) });
				subs.push_back({ *(annihilatorBegin + i), *(annihilatorBegin + pairPositions[i]) });
			}

			// This symmetrization is performed without change in sign
			Terms::IndexSubstitution currentSubstitution(std::move(subs));

			assert(!currentSubstitution.isIdentity());

			if (!ignoreExistingSymmetries && term.getResult().getSymmetry().contains(currentSubstitution)) {
				// This symmetry is already contained, so we don't want to explicitly symmetrize over it again because
				// we were NOT instructed to ignore existing symmetries
				continue;
			}

			symmetrizations.push_back(std::move(currentSubstitution));
		}

		return symmetrizations;
	}

protected:
	std::vector< term_t > m_resultingTerms;
};

}; // namespace Contractor::Processor

#endif // CONTRACTOR_PROCESSOR_SYMMETRIZER_HPP_


#include <igl/embree/bone_visible.h>

#include <igl/matlab/MexStream.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>
#include <algorithm>
#include <functional>

// Parse right hand side arguments for a matlab mex function.
//
// Inputs:
//   nrhs  number of right hand side arguments
//   prhs  pointer to right hand side arguments
// Outputs:
//   V  n by dim list of mesh vertex positions
//   F  m by dim list of mesh face indices
//   s  1 by dim bone source vertex position
//   d  1 by dim bone dest vertex position
// "Throws" matlab errors if dimensions are not sane.
void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  Eigen::VectorXd & s,
  Eigen::VectorXd & d)
{
  using namespace std;
  if(nrhs < 4)
  {
    mexErrMsgTxt("nrhs < 4");
  }
  const int dim = mxGetN(prhs[0]);
  if(dim != 3)
  {
    mexErrMsgTxt("Mesh vertex list must be #V by 3 list of vertex positions");
  }
  if(dim != (int)mxGetN(prhs[1]))
  {
   mexErrMsgTxt("Mesh facet size must equal dimension");
  }
  if(dim != (int)mxGetN(prhs[2]))
  {
   mexErrMsgTxt("Source dim must equal vertex dimension");
  }
  if(dim != (int)mxGetN(prhs[3]))
  {
   mexErrMsgTxt("Dest dim must equal vertex dimension");
  }
  // set number of mesh vertices
  const int n = mxGetM(prhs[0]);
  // set vertex position pointers
  double * Vp = mxGetPr(prhs[0]);
  // set number of faces
  const int m = mxGetM(prhs[1]);
  // set face index list pointer
  double * Fp = mxGetPr(prhs[1]);
  // set source and dest pointers
  double * sp = mxGetPr(prhs[2]);
  double * dp = mxGetPr(prhs[3]);
  // resize output to transpose
  V.resize(n,dim);
  copy(Vp,Vp+n*dim,V.data());
  // resize output to transpose
  F.resize(m,dim);
  // Q: Is this doing a cast?
  // A: Yes.
  copy(Fp,Fp+m*dim,F.data());
  // http://stackoverflow.com/a/4461466/148668
  transform(F.data(),F.data()+m*dim,F.data(),
    bind2nd(std::plus<double>(),-1.0));
  // resize output to transpose
  s.resize(dim);
  copy(sp,sp+dim,s.data());
  d.resize(dim);
  copy(dp,dp+dim,d.data());
}

// Prepare left hand side
//
// Inputs:
//   nlhs  number of left hand side arguments
//   plhs  pointer to left hand side arguments
//   no  number of origins
// Outputs:
//   S  no by 1  list of solid angles
void prepare_lhs(
  const int nlhs,
  mxArray *plhs[],
  const int no,
  double *& S)
{
  // Create pointers for output arrays
  if(nlhs > 0)
  {
    // solid angles
    plhs[0] = mxCreateDoubleMatrix(no,1, mxREAL);
    S = mxGetPr(plhs[0]);
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd V;
  MatrixXi F;
  VectorXd s;
  VectorXd d;
  parse_rhs(nrhs,prhs,V,F,s,d);

  //// Set up openmp
  //int nProcessors=omp_get_max_threads();
  ////std::cout<<nProcessors<<std::endl;
  //omp_set_num_threads(min(nProcessors,2));

  // List of flag
  Matrix<bool,Dynamic,1> flag;
  igl::embree::bone_visible(V,F,s,d,flag);

  nlhs = 1;
  plhs[0] = mxCreateLogicalMatrix(V.rows(),1);
  mxLogical * flagp = mxGetLogicals(plhs[0]);
  //copy(&flag.data()[0],&flag.data()[0]+flag.size(),flagp);
  // safe copy
  for(int f = 0;f<flag.size();f++)
  {
    flagp[f] = flag(f);
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}


//此源码被清华学神尹成大魔王专业翻译分析并修改
//尹成QQ77025077
//尹成微信18510341407
//尹成所在QQ群721929980
//尹成邮箱 yinc13@mails.tsinghua.edu.cn
//尹成毕业于清华大学,微软区块链领域全球最有价值专家
//https://mvp.microsoft.com/zh-cn/PublicProfile/4033620
//————————————————————————————————————————————————————————————————————————————————————————————————————————————————
/*
    此文件是Rippled的一部分：https://github.com/ripple/rippled
    版权所有（c）2012-2017 Ripple Labs Inc.

    使用、复制、修改和/或分发本软件的权限
    特此授予免费或不收费的目的，前提是
    版权声明和本许可声明出现在所有副本中。

    本软件按“原样”提供，作者不作任何保证。
    关于本软件，包括
    适销性和适用性。在任何情况下，作者都不对
    任何特殊、直接、间接或后果性损害或任何损害
    因使用、数据或利润损失而导致的任何情况，无论是在
    合同行为、疏忽或其他侵权行为
    或与本软件的使用或性能有关。
**/

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

#include <test/jtx/Env.h>
#include <ripple/beast/utility/temp_dir.h>
#include <ripple/crypto/csprng.h>
#include <boost/filesystem.hpp>
#include <fstream>
#include <streambuf>

namespace ripple {

class CryptoPRNG_test : public beast::unit_test::suite
{
    void
    testGetValues()
    {
        testcase ("Get Values");
        try
        {
            auto& engine = crypto_prng();
            auto rand_val = engine();
            BEAST_EXPECT(rand_val >= engine.min());
            BEAST_EXPECT(rand_val <= engine.max());

            uint16_t twoByte {0};
            engine(&twoByte, sizeof(uint16_t));
            pass();
        }
        catch(std::exception&)
        {
            fail();
        }
    }

public:
    void run () override
    {
        testGetValues();
    }
};

BEAST_DEFINE_TESTSUITE (CryptoPRNG, core, ripple);

}  //涟漪


//headers in this package
#include <geographic_map_server.h>

//headers in boost
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

//headers in tf
#include <tf/transform_datatypes.h>

geographic_map_server::geographic_map_server()
{
  is_fix_velocity_recieved_ = false;
  nh_.getParam(ros::this_node::getName()+"/osm_filepath", osm_filepath_);
  nh_.param<double>(ros::this_node::getName()+"/earth_radius", earth_radius_, 6378137);
  parse_osm();
  imu_sub_ = nh_.subscribe("/fix_velocity", 1, &geographic_map_server::fix_velocity_callback, this);
  fix_sub_ = nh_.subscribe("/fix", 1, &geographic_map_server::fix_callback, this);
}

geographic_map_server::~geographic_map_server()
{

}

void geographic_map_server::fix_callback(sensor_msgs::NavSatFix msg)
{
  if(is_fix_velocity_recieved_ == true)
  {

  }
}

void geographic_map_server::fix_velocity_callback(geometry_msgs::Vector3Stamped msg)
{
  is_fix_velocity_recieved_ = true;
  //ship_direction =
}

void geographic_map_server::parse_osm()
{
  using namespace boost::property_tree;
  ptree pt;
  read_xml(osm_filepath_, pt);
  //parse osm file and get osm_node infomation
  for(auto itr : pt.get_child("osm"))
  {
    if(itr.first == "node")
    {
      double lat = itr.second.get<double>("<xmlattr>.lat");
      double lon = itr.second.get<double>("<xmlattr>.lon");
      long id = itr.second.get<long>("<xmlattr>.id");
      osm_node* osm_node_ptr = new osm_node(lat,lon,id);
      osm_nodes_.push_back(osm_node_ptr);
    }
    //parse osm file and get osm_way infomation
    if(itr.first == "way")
    {
      std::vector<long> node_ids;
      std::vector<osm_node*> way_nodes;
      bool is_water = false;
      for(auto way_data_itr : itr.second )
      {
        if(way_data_itr.first == "nd")
        {
          long id = way_data_itr.second.get<long>("<xmlattr>.ref");
          node_ids.push_back(id);
        }
        //parse tag
        if(way_data_itr.first == "tag")
        {
          //get areas of natural = water
          if(way_data_itr.second.get<std::string>("<xmlattr>.k") == "natural" && way_data_itr.second.get<std::string>("<xmlattr>.v") == "water")
          {
            is_water = true;
          }
        }
        //check if the area is water or not
        if(is_water == true)
        {
          //std::vector<*osm_node> node_list;
          for(auto node_id = node_ids.begin(); node_id != node_ids.end(); ++node_id)
          {
            for(int i = 0; i<osm_nodes_.size() ;i++)
            {
              if(osm_nodes_[i]->id_ == *node_id)
              {
                way_nodes.push_back(osm_nodes_[i]);
              }
            }
          }
        }
      }
      osm_ways_.push_back(new osm_way(way_nodes));
    }
  }
}


#pragma once
#include <GL/glew.h>
#include <boost/optional.hpp>
#include "../eigen_def.hh"

namespace kt84 {

namespace graphics_util {
    enum struct ParamName {
        #if defined(KT84_USE_OPENGL_4)
        #include "ParamName4.inl"
        #elif defined(KT84_USE_OPENGL_3_3)
        #include "ParamName3.3.inl"
        #else
        #include "ParamName2.1.inl"
        #endif
    };
    inline int    glGet1i(ParamName pname) { int    value; glGetIntegerv(static_cast<GLenum>(pname), &value); return value; }
    inline float  glGet1f(ParamName pname) { float  value; glGetFloatv  (static_cast<GLenum>(pname), &value); return value; }
    inline double glGet1d(ParamName pname) { double value; glGetDoublev (static_cast<GLenum>(pname), &value); return value; }
    template <class EigenType> inline EigenType glGetXi(ParamName pname) { EigenType value; glGetIntegerv(static_cast<GLenum>(pname), value.data()); return value; }
    template <class EigenType> inline EigenType glGetXf(ParamName pname) { EigenType value; glGetFloatv  (static_cast<GLenum>(pname), value.data()); return value; }
    template <class EigenType> inline EigenType glGetXd(ParamName pname) { EigenType value; glGetDoublev (static_cast<GLenum>(pname), value.data()); return value; }

    inline Eigen::Matrix4d get_modelview_matrix() {
        return glGetXd<Eigen::Matrix4d>(ParamName::MODELVIEW_MATRIX);
    }
    inline Eigen::Matrix4d get_projection_matrix() {
        return glGetXd<Eigen::Matrix4d>(ParamName::PROJECTION_MATRIX);
    }
    inline Eigen::Vector4i get_viewport() {
        return glGetXi<Eigen::Vector4i>(ParamName::VIEWPORT);
    }
    inline float read_depth(int x, int y) {
        float z = 0;
    	glReadPixels(x, y, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &z);
        return z;
    }
    inline Eigen::Vector3d unproject(const Eigen::Vector3d& win_xyz,
                                     const Eigen::Matrix4d& modelview_matrix,
                                     const Eigen::Matrix4d& projection_matrix,
                                     const Eigen::Vector4i& viewport)
    {
        Eigen::Vector3d obj_xyz;
    	gluUnProject( win_xyz.x(),  win_xyz.y(),  win_xyz.z(),     modelview_matrix.data(), projection_matrix.data(), viewport.data(),
                     &obj_xyz.x(), &obj_xyz.y(), &obj_xyz.z());
        return obj_xyz;
    }
    inline Eigen::Vector3d unproject(const Eigen::Vector3d& win_xyz) {
        return unproject(win_xyz, get_modelview_matrix(), get_projection_matrix(), get_viewport());
    }
    inline boost::optional<Eigen::Vector3d> read_and_unproject(int x, int y) {
        auto modelview_matrix  = get_modelview_matrix();
        auto projection_matrix = get_projection_matrix();
        auto viewport          = get_viewport();

        float z = read_depth(x, y);
        if (z == 1.0f)
            return boost::none;

        return unproject(Eigen::Vector3d(x, y, z));
    }
    inline Eigen::Vector3d project(const Eigen::Vector3d& obj_xyz,
                                   const Eigen::Matrix4d& modelview_matrix,
                                   const Eigen::Matrix4d& projection_matrix,
                                   const Eigen::Vector4i& viewport)
    {
        Eigen::Vector3d win_xyz;
    	gluProject( obj_xyz.x(),  obj_xyz.y(),  obj_xyz.z(),    modelview_matrix.data(), projection_matrix.data(), viewport.data(),
                   &win_xyz.x(), &win_xyz.y(), &win_xyz.z());
        return win_xyz;
    }
    inline Eigen::Vector3d project(const Eigen::Vector3d& obj_xyz) {
        return project(obj_xyz, get_modelview_matrix(), get_projection_matrix(), get_viewport());
    }

    inline void glColor3f (const Eigen::Vector3f & color) { ::glColor3fv (&color[0]); }
    inline void glColor3d (const Eigen::Vector3d & color) { ::glColor3dv (&color[0]); }
    //inline void glColor3b (const Eigen::Vector3c & color) { ::glColor3bv (&color[0]); }       // Weird error: cannot convert parameter 1 from 'const char *' to 'const GLbyte *'
    inline void glColor3i (const Eigen::Vector3i & color) { ::glColor3iv (&color[0]); }
    inline void glColor3ub(const Eigen::Vector3uc& color) { ::glColor3ubv(&color[0]); }
    inline void glColor3ui(const Eigen::Vector3ui& color) { ::glColor3uiv(&color[0]); }
    inline void glColor4f (const Eigen::Vector4f & color) { ::glColor4fv (&color[0]); }
    inline void glColor4d (const Eigen::Vector4d & color) { ::glColor4dv (&color[0]); }
    //inline void glColor4b (const Eigen::Vector4c & color) { ::glColor4bv (&color[0]); }       // Weird error: cannot convert parameter 1 from 'const char *' to 'const GLbyte *'
    inline void glColor4i (const Eigen::Vector4i & color) { ::glColor4iv (&color[0]); }
    inline void glColor4ub(const Eigen::Vector4uc& color) { ::glColor4ubv(&color[0]); }
    inline void glColor4ui(const Eigen::Vector4ui& color) { ::glColor4uiv(&color[0]); }
    inline void glColor4f (const Eigen::Vector3f& color_rgb, float  color_alpha) { ::glColor4f(color_rgb[0], color_rgb[1], color_rgb[2], color_alpha); }
    inline void glColor4d (const Eigen::Vector3d& color_rgb, double color_alpha) { ::glColor4d(color_rgb[0], color_rgb[1], color_rgb[2], color_alpha); }
    inline void glVertex2f(const Eigen::Vector2f& vertex) { ::glVertex2fv(&vertex[0]); }
    inline void glVertex2d(const Eigen::Vector2d& vertex) { ::glVertex2dv(&vertex[0]); }
    inline void glVertex3f(const Eigen::Vector3f& vertex) { ::glVertex3fv(&vertex[0]); }
    inline void glVertex3d(const Eigen::Vector3d& vertex) { ::glVertex3dv(&vertex[0]); }
    inline void glNormal3f(const Eigen::Vector3f& normal) { ::glNormal3fv(&normal[0]); }
    inline void glNormal3d(const Eigen::Vector3d& normal) { ::glNormal3dv(&normal[0]); }
    inline void glTexCoord2f(const Eigen::Vector2f& texCoord) { ::glTexCoord2fv(&texCoord[0]); }
    inline void glTexCoord2d(const Eigen::Vector2d& texCoord) { ::glTexCoord2dv(&texCoord[0]); }
    inline void glTexCoord3f(const Eigen::Vector3f& texCoord) { ::glTexCoord3fv(&texCoord[0]); }
    inline void glTexCoord3d(const Eigen::Vector3d& texCoord) { ::glTexCoord3dv(&texCoord[0]); }
    inline void glTexCoord4f(const Eigen::Vector4f& texCoord) { ::glTexCoord4fv(&texCoord[0]); }
    inline void glTexCoord4d(const Eigen::Vector4d& texCoord) { ::glTexCoord4dv(&texCoord[0]); }
    inline void glTexCoord4f(const Eigen::Vector3f& texCoord_str, float  texCoord_q) { ::glTexCoord4f(texCoord_str[0], texCoord_str[1], texCoord_str[2], texCoord_q); }
    inline void glTexCoord4d(const Eigen::Vector3d& texCoord_str, double texCoord_q) { ::glTexCoord4d(texCoord_str[0], texCoord_str[1], texCoord_str[2], texCoord_q); }
    inline void glScalef(const Eigen::Vector3f& v) { ::glScalef(v[0], v[1], v[2]); }
    inline void glScaled(const Eigen::Vector3d& v) { ::glScaled(v[0], v[1], v[2]); }
    inline void glScalef(float  s) { ::glScalef(s, s, s); }
    inline void glScaled(double s) { ::glScaled(s, s, s); }
    inline void glTranslatef(const Eigen::Vector3f& v) { ::glTranslatef(v[0], v[1], v[2]); }
    inline void glTranslated(const Eigen::Vector3d& v) { ::glTranslated(v[0], v[1], v[2]); }
    inline void gluLookAt(const Eigen::Vector3d& eye, const Eigen::Vector3d& center, const Eigen::Vector3d& up) { ::gluLookAt(eye.x(), eye.y(), eye.z(), center.x(), center.y(), center.z(), up.x(), up.y(), up.z()); }
#ifdef OPENMESH_VECTOR_HH
    // for OpenMesh::VectorT
    inline void glColor3f (const OpenMesh::Vec3f& color) { ::glColor3fv (&color[0]); }
    inline void glColor3d (const OpenMesh::Vec3d& color) { ::glColor3dv (&color[0]); }
    inline void glColor3i (const OpenMesh::Vec3i& color) { ::glColor3iv (&color[0]); }
    inline void glColor3ub(const OpenMesh::Vec3uc& color) { ::glColor3ubv(&color[0]); }
    inline void glColor3ui(const OpenMesh::Vec3ui& color) { ::glColor3uiv(&color[0]); }
    inline void glColor4f (const OpenMesh::Vec4f & color) { ::glColor4fv (&color[0]); }
    inline void glColor4d (const OpenMesh::Vec4d & color) { ::glColor4dv (&color[0]); }
    inline void glColor4i (const OpenMesh::Vec4i & color) { ::glColor4iv (&color[0]); }
    inline void glColor4ub(const OpenMesh::Vec4uc& color) { ::glColor4ubv(&color[0]); }
    inline void glColor4ui(const OpenMesh::Vec4ui& color) { ::glColor4uiv(&color[0]); }
    inline void glColor4f (const OpenMesh::Vec3f& color_rgb, float  color_alpha) { ::glColor4f(color_rgb[0], color_rgb[1], color_rgb[2], color_alpha); }
    inline void glColor4d (const OpenMesh::Vec3d& color_rgb, double color_alpha) { ::glColor4d(color_rgb[0], color_rgb[1], color_rgb[2], color_alpha); }
    inline void glVertex2f(const OpenMesh::Vec2f& vertex) { ::glVertex2fv(&vertex[0]); }
    inline void glVertex2d(const OpenMesh::Vec2d& vertex) { ::glVertex2dv(&vertex[0]); }
    inline void glVertex3f(const OpenMesh::Vec3f& vertex) { ::glVertex3fv(&vertex[0]); }
    inline void glVertex3d(const OpenMesh::Vec3d& vertex) { ::glVertex3dv(&vertex[0]); }
    inline void glNormal3f(const OpenMesh::Vec3f& normal) { ::glNormal3fv(&normal[0]); }
    inline void glNormal3d(const OpenMesh::Vec3d& normal) { ::glNormal3dv(&normal[0]); }
    inline void glTexCoord2f(const OpenMesh::Vec2f& texCoord) { ::glTexCoord2fv(&texCoord[0]); }
    inline void glTexCoord2d(const OpenMesh::Vec2d& texCoord) { ::glTexCoord2dv(&texCoord[0]); }
    inline void glTexCoord3f(const OpenMesh::Vec3f& texCoord) { ::glTexCoord3fv(&texCoord[0]); }
    inline void glTexCoord3d(const OpenMesh::Vec3d& texCoord) { ::glTexCoord3dv(&texCoord[0]); }
#endif

}

}


/*
 *  Copyright (c) 2014, Facebook, Inc.
 *  All rights reserved.
 *
 *  This source code is licensed under the BSD-style license found in the
 *  LICENSE file in the root directory of this source tree. An additional grant
 *  of patent rights can be found in the PATENTS file in the same directory.
 *
 */

#include <boost/range/adaptor/reversed.hpp>
#include <lua.hpp>
#include <matio.h>

#include <fblualib/LuaUtils.h>
#include <folly/Range.h>
#include <folly/ScopeGuard.h>
#include <thpp/Tensor.h>

using namespace thpp;
using namespace fblualib;

namespace {

// Push a tensor (of type T) on the Lua stack, obtained from the Matlab
// variable var
template <class T>
void pushTensor(lua_State* L, mat_t* fp, matvar_t* var) {
  auto range = folly::range(var->dims, var->dims + var->rank);
  auto revRange = boost::adaptors::reverse(range);
  Tensor<T> tensor(LongStorage(revRange.begin(), revRange.end()));

  int r = Mat_VarReadDataLinear(fp, var, static_cast<void*>(tensor.data()),
                                0, 1, tensor.size());
  if (r != 0) {
    luaL_error(L, "Variable %s: unable to read Matlab data", var->name);
  }

  // Matlab is column-major, Lua is row-major. Transpose, and make a copy
  // to give contiguous output to Lua.
  tensor.transpose();
  tensor.force(Tensor<T>::CONTIGUOUS);

  luaPushTensor(L, std::move(tensor));
}

// Load tensors from one Matlab file
int load(lua_State* L) {
  auto path = luaGetStringChecked(L, 1).str();

  auto fp = Mat_Open(path.c_str(), MAT_ACC_RDONLY);
  if (!fp) {
    return luaL_error(L, "Unable to open Matlab file %s", path.c_str());
  }
  SCOPE_FAIL {
    Mat_Close(fp);  // ignore error, we're throwing an exception
  };

  // Create table to hold loaded variables
  lua_newtable(L);
  int tableIdx = lua_gettop(L);

  // We need to read info for all variables first, as matio relies on
  // not changing the file pointer between calls to Mat_VarReadNextInfo, and
  // data-reading functions do that.

  {
    std::vector<matvar_t*> vars;
    SCOPE_EXIT {
      for (auto var : vars) {
        Mat_VarFree(var);
      }
    };

    // Read info for all variables
    matvar_t* var;
    while ((var = Mat_VarReadNextInfo(fp)) != nullptr) {
      vars.push_back(var);
    }

    for (auto var : vars) {
      lua_pushstring(L, var->name);

      // Dispatch by type
      switch (var->class_type) {
#define X(tname, type) \
      case MAT_C_##tname: pushTensor<type>(L, fp, var); break;
      X(DOUBLE, double)
      X(SINGLE, float)
      X(UINT8, uint8_t)
      X(INT16, int16_t)
      X(INT32, int32_t)
      X(INT64, int64_t)
#undef X
      default:
        return luaL_error(L, "Variable %s: unsupported Matlab class %d",
                          var->name,
                          int(var->class_type));
      }

      // Add to table
      lua_rawset(L, tableIdx);
    }
  }

  if (Mat_Close(fp) != 0) {
    luaL_error(L, "Unable to close Matlab file");
  }

  return 1;
}

// Save one tensor to Matlab file
template <class T>
void saveTensor(lua_State* L,
                const Tensor<T>& srcTensor, mat_t* fp,
                const char* name,
                matio_classes classType,
                matio_types dataType) {
  // Sanity check
  DCHECK_EQ(sizeof(T), Mat_SizeOf(dataType));
  DCHECK_EQ(sizeof(T), Mat_SizeOfClass(classType));

  // Matlab is column-major, Lua is row-major. Transpose (likely making a
  // copy).
  //
  // We need the tensor to be contiguous; matio doesn't support our
  // stride gymnastics.
  Tensor<T> tensor(srcTensor);
  tensor.transpose();
  tensor.force(Tensor<T>::CONTIGUOUS);

  auto sizes = tensor.sizes();
  auto revSizes = boost::adaptors::reverse(sizes);
  std::vector<size_t> sizesSizeT(revSizes.begin(), revSizes.end());

  auto var = Mat_VarCreate(
      name,
      classType,
      dataType,
      sizesSizeT.size(),
      sizesSizeT.data(),
      static_cast<void*>(tensor.data()),
      MAT_F_DONT_COPY_DATA);
  CHECK(var);

  SCOPE_EXIT {
    Mat_VarFree(var);
  };

  if (Mat_VarWrite(fp, var, MAT_COMPRESSION_NONE) != 0) {
    luaL_error(L, "Unable to write tensor");
  }
}

// Save tensor at index idx on the Lua stack to the Matlab file under
// the given name
void saveTensorAtIndex(lua_State* L, int idx, mat_t* fp, const char* name) {
  // Dispatch by type
#define X(tname, type) { \
    auto tensor = luaGetTensor<type>(L, idx); \
    if (tensor) { \
      saveTensor(L, *tensor, fp, name, MAT_C_##tname, MAT_T_##tname); \
      return; \
    } \
  }
  X(DOUBLE, double)
  X(SINGLE, float)
  X(UINT8, uint8_t)
  X(INT16, int16_t)
  X(INT32, int32_t)
  X(INT64, int64_t)
#undef X
  luaL_error(L, "Not a tensor");
}

// Create a Matlab file from the path stored at pathIdx on the Lua stack
// with the output version stored at verIdx on the Lua stack
mat_t* createMatlabFile(lua_State* L, int pathIdx, int verIdx) {
  auto path = luaGetStringChecked(L, pathIdx).str();

  mat_ft version = MAT_FT_DEFAULT;

  if (!lua_isnil(L, verIdx)) {
    auto verStr = luaGetStringChecked(L, verIdx);
    if (!verStr.empty()) {
      if (verStr == "7.3") {
        version = MAT_FT_MAT73;
      } else if (verStr == "5") {
        version = MAT_FT_MAT5;
      } else if (verStr == "4") {
        version = MAT_FT_MAT4;
      } else {
        luaL_error(L, "Unsupported Matlab version %s", verStr.str().c_str());
      }
    }
  }

  auto fp = Mat_CreateVer(path.c_str(), nullptr, version);
  if (!fp) {
    luaL_error(L, "Unable to create Matlab file %s", path.c_str());
  }

  return fp;
}

// Save tensor: save(path, tensor [, version])
int save(lua_State* L) {
  auto fp = createMatlabFile(L, 1, 3);
  SCOPE_FAIL {
    Mat_Close(fp);  // ignore error, we're throwing an exception
  };

  // save tensor in a dummy var named x
  saveTensorAtIndex(L, 2, fp, "x");

  if (Mat_Close(fp) != 0) {
    return luaL_error(L, "Unable to close Matlab file");
  }

  return 0;
}

// Save all tensors in a table: save(path, table [, tensor])
int saveTable(lua_State* L) {
  auto fp = createMatlabFile(L, 1, 3);
  SCOPE_FAIL {
    Mat_Close(fp);  // ignore error, we're throwing an exception
  };

  // Table at index 2
  lua_pushnil(L);  // first key
  while (lua_next(L, 2) != 0) {
    // key at index -2, value at index -1
    auto name = luaGetStringChecked(L, -2, /*strict*/ true).str();
    saveTensorAtIndex(L, -1, fp, name.c_str());
    lua_pop(L, 1);  // pop value (tensor)
  }

  if (Mat_Close(fp) != 0) {
    return luaL_error(L, "Unable to close Matlab file");
  }

  return 0;
}

const struct luaL_reg matlab[] = {
  {"load", load},
  {"saveTensor", save},
  {"saveTable", saveTable},
  {nullptr, nullptr},  // sentinel
};

}  // namespace

extern "C" int LUAOPEN(lua_State* L) {
  lua_newtable(L);
  luaL_register(L, nullptr, matlab);
  return 1;
}


/*
 * =====================================================================================
 *
 *       Filename:  matrixtest.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  05/26/2014 12:46:33 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Anup Pillai (), anupgpillai@gmail.com
 *   Organization:  IISER Pune
 *
 * =====================================================================================
 */

#include <iostream>
#include <vector>
//#include "/opt/boost/boost_1_55_0/boost/numeric/ublas/vector.hpp" /* Specifying the full path in qoutes for a header file */
//#include <boost/numeric/ublas/matrix.hpp> /* Specifying only the path from root directory: must have an -I option when calling compiler when used this way example: g++ -I /opt/boost/boost_1_55_0 matrixtest.cpp -o matrixtest */
#include <boost/numeric/ublas/io.hpp>
using namespace std;
using namespace boost::numeric::ublas;
using namespace boost::numeric::odeint;

main()
{
   // Create a uBLAS matrix using boost
   //matrix<double > myMat (3,3,2.5);
   v
   string prompt("What is your name: "), name;
   cout << prompt;
   getline(cin, name);
   cout << "Your name is: " << name << endl;
   cout << "Your name is " << name.length() << " characters long" << endl;
   // Create a uBLAS vector using boost
   boost::numeric::ublas::vector<double > myVec (4,45);
   std::cout << myMat << "\n";
   std::cout << myVec << "\n";
   std::cout << myMat(2,2) << "\n";
   /* Create a lambda function : inline functions in c++ */
   auto mylambda = [] (double a, double b) {double c = a*b; return (c);};
   cout << mylambda(4,3) << endl;
   std::cout << "Current version of c++: " <<  __cplusplus << "\n";
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/Numerik/numsoft/kaskade7/                        */
/*                                                                           */
/*  Copyright (C) 2002-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ERROR_DISTRIBUTION_HH
#define ERROR_DISTRIBUTION_HH

#include <string>

#include <boost/utility.hpp>
#include <boost/fusion/include/as_vector.hpp>

#include "fem/assemble.hh"
#include "fem/functionspace.hh"
#include "fem/lagrangespace.hh"
#include "fem/shapefunctioncache.hh"

// forward declarations
namespace Dune
{
  template <class,int> class FieldVector;
  template <class,int,int> class FieldMatrix;
  template <class, int> class QuadratureRule;
}

namespace Kaskade
{
  using namespace boost::fusion;
  using namespace AssemblyDetail;

  enum class ErrorNorm { Energy, L2, H1, H1_half };

  template <class OriginalEvaluators, class ExtensionEvaluators, class Functional,
            class ArgYH, class ArgYE, class ArgUH, class ArgUE, class ArgPH, class ArgPE>
  void evaluateData(OriginalEvaluators& originalEvaluators, ExtensionEvaluators& extendedEvaluators, Functional const& f,
                    ArgYH& yl, ArgYE& yh, ArgUH& ul, ArgUE& uh, ArgPH& pl, ArgPE& ph, double w)
  {
      if( f.considerControlVariable_ )
      {
        uh.value += w * at_c<Functional::uIdx>(f.errorEstimateH.data).value(at_c<Functional::uSHIdx>(extendedEvaluators));
        ul.value += w * at_c<Functional::uIdx>(f.errorEstimateL.data).value(at_c<Functional::uSLIdx>(originalEvaluators));
        uh.gradient += w * at_c<Functional::uIdx>(f.errorEstimateH.data).gradient(at_c<Functional::uSHIdx>(extendedEvaluators));
        ul.gradient += w * at_c<Functional::uIdx>(f.errorEstimateL.data).gradient(at_c<Functional::uSLIdx>(originalEvaluators));
      }
      if( f.considerStateVariable_ )
      {
        yh.value += w * at_c<Functional::yIdx>(f.errorEstimateH.data).value(at_c<Functional::ySHIdx>(extendedEvaluators));
        yl.value += w * at_c<Functional::yIdx>(f.errorEstimateL.data).value(at_c<Functional::ySLIdx>(originalEvaluators));
        yh.gradient += w * at_c<Functional::yIdx>(f.errorEstimateH.data).gradient(at_c<Functional::ySHIdx>(extendedEvaluators));
        yl.gradient += w * at_c<Functional::yIdx>(f.errorEstimateL.data).gradient(at_c<Functional::ySLIdx>(originalEvaluators));
      }
      if( f.considerAdjointVariable_ )
      {
        ph.value += w * at_c<Functional::pIdx>(f.errorEstimateH.data).value(at_c<Functional::pSHIdx>(extendedEvaluators));
        pl.value += w * at_c<Functional::pIdx>(f.errorEstimateL.data).value(at_c<Functional::pSLIdx>(originalEvaluators));
        ph.gradient += w * at_c<Functional::pIdx>(f.errorEstimateH.data).gradient(at_c<Functional::pSHIdx>(extendedEvaluators));
        pl.gradient += w * at_c<Functional::pIdx>(f.errorEstimateL.data).gradient(at_c<Functional::pSLIdx>(extendedEvaluators));
      }
  }

  template <class ArgYL, class ArgYH, class ArgUL, class ArgUH, class ArgPL, class ArgPH>
  void clearVarArgs(ArgYL& yl, ArgYH& yh, ArgUL& ul, ArgUH& uh, ArgPL& pl, ArgPH& ph)
  {
    ul.value = 0;  ul.gradient = 0;
    yl.value = 0;  yl.gradient = 0;
    pl.value = 0;  pl.gradient = 0;
    uh.value = 0;  uh.gradient = 0;
    yh.value = 0;  yh.gradient = 0;
    ph.value = 0;  ph.gradient = 0;
  }

  template <class ArgU, class ArgY, class ArgP>
  double computeL2Error(ArgU const& u, ArgY const& y, ArgP const& p)
  {
    return u.value*u.value + y.value*y.value + p.value*p.value;
  }

  template <class ArgU, class ArgY, class ArgP>
  double computeH1HalfError(ArgU const& u, ArgY const& y, ArgP const& p)
  {
    LinAlg::EuclideanScalarProduct sp;
    return sp(u.gradient,u.gradient) + sp(y.gradient,y.gradient) + sp(p.gradient,p.gradient);
  }

  template <class Functional, class ExtendedAnsatzVars >
  class ErrorDistribution
  {
    typedef typename Functional::AnsatzVars OriginalAnsatzVars;
    typedef typename OriginalAnsatzVars::Grid Grid;
    typedef ShapeFunctionCache<Grid, typename Functional::Scalar> SfCache;
    typedef ShapeFunctionCache<Grid, typename Functional::Scalar> SfCache2;
    typedef typename OriginalAnsatzVars::Spaces OriginalSpaces;
    typedef typename ExtendedAnsatzVars::Spaces ExtendedSpaces;
    typedef typename result_of::as_vector<typename result_of::transform<OriginalSpaces, GetEvaluators<SfCache> >::type>::type OriginalEvaluators;
    typedef typename result_of::as_vector<typename result_of::transform<ExtendedSpaces, GetEvaluators<SfCache2> >::type>::type ExtendedEvaluators;
    typedef typename Grid::ctype CoordType;
    typedef Dune::QuadratureRule<typename Functional::AnsatzVars::Grid::ctype, Functional::AnsatzVars::Grid::dimension> QuadRule;
    typedef Dune::QuadratureRule<typename Functional::AnsatzVars::Grid::ctype, Functional::AnsatzVars::Grid::dimension-1> FaceQuadRule;
  public:
    typedef typename Functional::Scalar Scalar;
    typedef FEFunctionSpace<DiscontinuousLagrangeMapper<Scalar,typename Grid::LeafGridView> > AnsatzSpace;
    typedef boost::fusion::vector<AnsatzSpace const*> AnsatzSpaces;
    typedef Variable<SpaceIndex<0>,Components<1>,VariableId<0> > AnsatzVariableInformation;
    typedef boost::fusion::vector<AnsatzVariableInformation> VariableDescriptions;
    typedef VariableSetDescription<AnsatzSpaces,VariableDescriptions> AnsatzVars;
    typedef typename AnsatzVars::template CoefficientVectorRepresentation<0,1>::type ErrorVector;
    typedef AnsatzVars TestVars;
    typedef OriginalAnsatzVars OriginVars;
    //typedef AnsatzVars OriginVars;
    static int const dim = Grid::dimension;
    static ProblemType const type = Functional::type;

    static constexpr int yIdx = getStateId<Functional>();
    static constexpr int uIdx = getControlId<Functional>();
    static constexpr int pIdx = getAdjointId<Functional>();
    static constexpr int uSLIdx = result_of::value_at_c<typename OriginalAnsatzVars::Variables, uIdx>::type::spaceIndex;
    static constexpr int ySLIdx = result_of::value_at_c<typename OriginalAnsatzVars::Variables, yIdx>::type::spaceIndex;
    static constexpr int pSLIdx = result_of::value_at_c<typename OriginalAnsatzVars::Variables, pIdx>::type::spaceIndex;
    static constexpr int uSHIdx = result_of::value_at_c<typename ExtendedAnsatzVars::Variables, uIdx>::type::spaceIndex;
    static constexpr int ySHIdx = result_of::value_at_c<typename ExtendedAnsatzVars::Variables, yIdx>::type::spaceIndex;
    static constexpr int pSHIdx = result_of::value_at_c<typename ExtendedAnsatzVars::Variables, pIdx>::type::spaceIndex;

    class DomainCache
    {
    public:
      DomainCache(ErrorDistribution const& f_, typename OriginVars::VariableSet const& /*vars*/, int flags=7)
        : f(f_), domainCache(f.functional,f.iterate,flags)
      {
      }

      template <class Entity>
      void moveTo(Entity const &entity)
      {
        e  = &entity;
        domainCache.moveTo(entity);
      }

      template <class Position, class Evaluators>
      void evaluateAt(Position const&, Evaluators const& evaluators)
      {
        clearVarArgs(yl,yh,ul,uh,pl,ph);
        OriginalEvaluators originalEvaluators(transform(f.iterate.descriptions.spaces,GetEvaluators<SfCache>(&sfCache)));
        ExtendedEvaluators extendedEvaluators(transform(f.errorEstimateH.descriptions.spaces,GetEvaluators<SfCache>(&extendedSFCache)));
        QuadRule qr = QuadratureTraits<QuadRule>().rule(e->type(),f.qOrder);
        moveEvaluatorsToCell(originalEvaluators,*e);
        moveEvaluatorsToCell(extendedEvaluators,*e);
        useQuadratureRuleInEvaluators(originalEvaluators,qr,0);
        useQuadratureRuleInEvaluators(extendedEvaluators,qr,0);

        size_t nQuadPos = qr.size();
        for (size_t g=0; g<nQuadPos; ++g)
        {
          // pos of integration point
          Dune::FieldVector<CoordType,dim> quadPos = qr[g].position();
          // for all spaces involved, update the evaluators associated
          // to this quadrature point
          moveEvaluatorsToIntegrationPoint(originalEvaluators,quadPos);
          moveEvaluatorsToIntegrationPoint(extendedEvaluators,quadPos);
          // prepare evaluation of functional
          domainCache.evaluateAt(qr[g].position(),originalEvaluators);

          evaluateData(originalEvaluators, extendedEvaluators, f, yl, yh, ul, uh, pl, ph, qr[g].weight());
        }
      }

      Scalar d0() const { return 0; }

      template<int row, int dim>
      Dune::FieldVector<Scalar, TestVars::template Components<row>::m>
      d1 (VariationalArg<Scalar,dim> const& arg) const
      {
        VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<yIdx>::m> y(yh);
        VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<uIdx>::m> u(uh);
        VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<pIdx>::m> p(ph);

        if( !f.onlyH_ )
        {
          u.value += ul.value;
          u.gradient += ul.gradient;
          y.value += yl.value;
          y.gradient += yl.gradient;
          p.value += pl.value;
          p.gradient += pl.gradient;
        }

        if( f.errorNorm == ErrorNorm::L2 ) return computeL2Error(u,y,p);
        if( f.errorNorm == ErrorNorm::H1_half ) return computeH1HalfError(u,y,p);
        if( f.errorNorm == ErrorNorm::H1 ) return computeL2Error(u,y,p) + computeH1HalfError(u,y,p);

        Scalar result = 0;
        result += Functional::template D2<yIdx,yIdx>::present ? domainCache.template d2_impl<yIdx,yIdx>(y,y) : 0.;
        result += Functional::template D2<yIdx,uIdx>::present ? domainCache.template d2_impl<yIdx,uIdx>(y,u) : 0.;
        result += Functional::template D2<uIdx,yIdx>::present ? domainCache.template d2_impl<uIdx,yIdx>(u,y) : 0.;
        result += Functional::template D2<uIdx,uIdx>::present ? domainCache.template d2_impl<uIdx,uIdx>(u,u) : 0.;
        if( !f.useStateNormForAdjoint_ )
        {
          result += Functional::template D2<yIdx,pIdx>::present ? domainCache.template d2_impl<yIdx,pIdx>(y,p) : 0.;
          result += Functional::template D2<pIdx,yIdx>::present ? domainCache.template d2_impl<pIdx,yIdx>(p,y) : 0.;
          result += Functional::template D2<pIdx,uIdx>::present ? domainCache.template d2_impl<pIdx,uIdx>(p,u) : 0.;
          result += Functional::template D2<uIdx,pIdx>::present ? domainCache.template d2_impl<uIdx,pIdx>(u,p) : 0.;
          result += Functional::template D2<pIdx,pIdx>::present ? domainCache.template d2_impl<pIdx,pIdx>(p,p) : 0.;
        }
        else
        {
          result += Functional::template D2<pIdx,pIdx>::present ? domainCache.template d2_impl<yIdx,yIdx>(p,p) : 0.;
        }
        return result*arg.value[0];
      }

      template<int row, int col, int dim>
      Dune::FieldMatrix<Scalar,1,1>	d2 (VariationalArg<Scalar,dim> const&, VariationalArg<Scalar,dim> const&) const { return Dune::FieldMatrix<Scalar,1,1>(0); }

    private:
      ErrorDistribution const& f;
      typename Functional::DomainCache domainCache;
      typename AnsatzVars::Grid::template Codim<0>::Entity const* e;
      VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<yIdx>::m> yl;
      VariationalArg<Scalar,dim,ExtendedAnsatzVars::template Components<yIdx>::m> yh;
      VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<uIdx>::m> ul;
      VariationalArg<Scalar,dim,ExtendedAnsatzVars::template Components<uIdx>::m> uh;
      VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<pIdx>::m> pl;
      VariationalArg<Scalar,dim,ExtendedAnsatzVars::template Components<pIdx>::m> ph;
      SfCache sfCache;
      SfCache2 extendedSFCache;
    };

    class BoundaryCache
    {
    public:
      BoundaryCache(ErrorDistribution const& f_, typename OriginVars::VariableSet const& vars, int flags=7)
        : f(f_), boundaryCache(f.functional,f.iterate,flags)
      {}

      template <class FaceIterator>
      void moveTo(FaceIterator const& entity)
      {
        e = &entity;
        boundaryCache.moveTo(entity);
      }

      template <class Evaluators>
      void evaluateAt(Dune::FieldVector<typename AnsatzVars::Grid::ctype, AnsatzVars::Grid::dimension-1> const& xlocal, Evaluators const& evaluators)
      {
        clearVarArgs(yl,yh,ul,uh,pl,ph);
        OriginalEvaluators originalEvaluators(transform(f.iterate.descriptions.spaces,GetEvaluators<SfCache>(&sfCache)));
        ExtendedEvaluators extendedEvaluators(transform(f.errorEstimateH.descriptions.spaces,GetEvaluators<SfCache>(&extendedSFCache)));
        FaceQuadRule qr = QuadratureTraits<FaceQuadRule>().rule((*e)->geometryInInside().type(),f.qOrder);
        moveEvaluatorsToCell(originalEvaluators,*at_c<ySLIdx>(evaluators).cell_);
        moveEvaluatorsToCell(extendedEvaluators,*at_c<ySLIdx>(evaluators).cell_);
        moveEvaluatorsToCell(originalEvaluators,*e);
        moveEvaluatorsToCell(extendedEvaluators,*e);
        useQuadratureRuleInEvaluators(originalEvaluators,qr,(*e)->indexInInside());
        useQuadratureRuleInEvaluators(extendedEvaluators,qr,(*e)->indexInInside());

        size_t nQuadPos = qr.size();
        for (size_t g=0; g<nQuadPos; ++g)
        {
          // pos of integration point
          Dune::FieldVector<CoordType,dim> quadPos = (*e)->geometryInInside().global(qr[g].position());
          // for all spaces involved, update the evaluators associated
          // to this quadrature point
          moveEvaluatorsToIntegrationPoint(originalEvaluators,quadPos);
          moveEvaluatorsToIntegrationPoint(extendedEvaluators,quadPos);
          // prepare evaluation of functional
          boundaryCache.evaluateAt(qr[g].position(),originalEvaluators);

          evaluateData(originalEvaluators, extendedEvaluators, f, yl, yh, ul, uh, pl, ph, qr[g].weight());
        }
      }

      Scalar d0() const { return 0; }

      template<int row, int dim>
      Dune::FieldVector<Scalar,1> d1 (VariationalArg<Scalar,dim> const& arg) const
      {
        return Dune::FieldVector<Scalar,1>(0);
        VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<yIdx>::m> y(yh);
        VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<uIdx>::m> u(uh);
        VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<pIdx>::m> p(ph);

        if( !f.onlyH_ )
        {
          u.value += ul.value;
          u.gradient += ul.gradient;
          y.value += yl.value;
          y.gradient += yl.gradient;
          p.value += pl.value;
          p.gradient += pl.gradient;
        }

        if( f.errorNorm != ErrorNorm::Energy ) return computeL2Error(u,y,p);

        Scalar result = Functional::template D2<yIdx,yIdx>::present ? boundaryCache.template d2_impl<yIdx,yIdx>(y,y) : 0.;
        result += Functional::template D2<yIdx,uIdx>::present ? boundaryCache.template d2_impl<yIdx,uIdx>(y,u) : 0.;
        result += Functional::template D2<uIdx,yIdx>::present ? boundaryCache.template d2_impl<uIdx,yIdx>(u,y) : 0.;
        result += Functional::template D2<uIdx,uIdx>::present ? boundaryCache.template d2_impl<uIdx,uIdx>(u,u) : 0.;
        if( !f.useStateNormForAdjoint_ )
        {
          result += Functional::template D2<yIdx,pIdx>::present ? boundaryCache.template d2_impl<yIdx,pIdx>(y,p) : 0.;
          result += Functional::template D2<pIdx,yIdx>::present ? boundaryCache.template d2_impl<pIdx,yIdx>(p,y) : 0.;
          result += Functional::template D2<pIdx,uIdx>::present ? boundaryCache.template d2_impl<pIdx,uIdx>(p,u) : 0.;
          result += Functional::template D2<uIdx,pIdx>::present ? boundaryCache.template d2_impl<uIdx,pIdx>(u,p) : 0.;
          result += Functional::template D2<pIdx,pIdx>::present ? boundaryCache.template d2_impl<pIdx,pIdx>(p,p) : 0.;
        }
        else
        {
          result += Functional::template D2<pIdx,pIdx>::present ? boundaryCache.template d2_impl<yIdx,yIdx>(p,p) : 0.;
        }
        return result*arg.value[0];
      }

      template<int row, int col, int dim>
      Dune::FieldMatrix<Scalar,1,1> d2 (VariationalArg<Scalar,dim> const&, VariationalArg<Scalar,dim> const&) const { return Dune::FieldMatrix<Scalar,1,1>(0); }

    private:
      ErrorDistribution const& f;
      typename Functional::BoundaryCache boundaryCache;
      typename AnsatzVars::Grid::LeafGridView::IntersectionIterator const* e;
      VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<yIdx>::m> yl;
      VariationalArg<Scalar,dim,ExtendedAnsatzVars::template Components<yIdx>::m> yh;
      VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<uIdx>::m> ul;
      VariationalArg<Scalar,dim,ExtendedAnsatzVars::template Components<uIdx>::m> uh;
      VariationalArg<Scalar,dim,OriginalAnsatzVars::template Components<pIdx>::m> pl;
      VariationalArg<Scalar,dim,ExtendedAnsatzVars::template Components<pIdx>::m> ph;
      SfCache sfCache;
      SfCache2 extendedSFCache;
    };

    ErrorDistribution(Functional const& functional_, typename OriginalAnsatzVars::VariableSet const& iterate_,
                      typename OriginalAnsatzVars::VariableSet const& errorEstimateL_, typename ExtendedAnsatzVars::VariableSet const& errorEstimateH_)
      : functional(functional_), iterate(iterate_), errorEstimateL(errorEstimateL_), errorEstimateH(errorEstimateH_), ansatzSpace(boost::fusion::at_c<0>(iterate.descriptions.spaces)->gridManager(), iterate.descriptions.gridView,0),
        ansatzSpaces(&ansatzSpace), varName( {"error"} ), ansatzVars(ansatzSpaces,varName)

    {}

    template <int row>
    struct D1
    {
      static bool const present   = true;
      static bool const constant  = false;
    };

    template <int row, int col>
    struct D2
    {
      static bool const present = false;
      static bool const symmetric = false;
      static bool const lumped = false;
    };

    template <class Cell>
    int integrationOrder(Cell const&, int, bool) const { return 0; }

    AnsatzSpaces const& getSpaces() const { return ansatzSpaces; }

    AnsatzVars const& getVariableSetDescription() const { return ansatzVars; }

    void ignoreLowerOrderError(bool ignore) { onlyH_ = ignore; }
    void considerStateVariable(bool consider) { considerStateVariable_ = consider; }
    void considerControlVariable(bool consider) { considerControlVariable_ = consider; }
    void considerAdjointVariable(bool consider) { considerAdjointVariable_ = consider; }
    void setErrorNorm(ErrorNorm errNorm) { errorNorm = errNorm; }
    void useStateNormForAdjoint(bool useStateNorm) { useStateNormForAdjoint_ = useStateNorm; }

    friend class DomainCache;
    friend class BoundaryCache;

    Functional const& functional;
    typename OriginalAnsatzVars::VariableSet const& iterate;
    typename OriginalAnsatzVars::VariableSet const& errorEstimateL;
    typename ExtendedAnsatzVars::VariableSet const& errorEstimateH;
    AnsatzSpace ansatzSpace;
    AnsatzSpaces ansatzSpaces;
    std::string varName[1];
    AnsatzVars ansatzVars;
    ErrorNorm errorNorm = ErrorNorm::Energy;
    bool onlyH_ = false, considerStateVariable_ = true, considerControlVariable_ = true, considerAdjointVariable_ = false, useStateNormForAdjoint_ = false;
    int qOrder = 6;
  };
}
#endif


#include <boost/graph/prim_minimum_spanning_tree.hpp>


#pragma once

#pragma GCC diagnostic ignored "-Wshift-count-overflow"
#pragma GCC diagnostic ignored "-Woverflow"

#include <array>
#include <cstdint>
#include <cstddef>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/tuple/elem.hpp>

#include <giecs/memory/block.hpp>

namespace giecs
{

template <std::size_t N>
struct bittype_tag
{
    typedef uintmax_t type;
};

#define GEN_BITTYPE_TAG(z, n, data) \
	template <> struct bittype_tag< BOOST_PP_TUPLE_ELEM(2, 0, data) + n > {typedef BOOST_PP_TUPLE_ELEM(2, 1, data) type;};

BOOST_PP_REPEAT(8, GEN_BITTYPE_TAG, (0x01, uint8_t))
BOOST_PP_REPEAT(8, GEN_BITTYPE_TAG, (0x09, uint16_t))
BOOST_PP_REPEAT(16, GEN_BITTYPE_TAG, (0x11, uint32_t))
BOOST_PP_REPEAT(32, GEN_BITTYPE_TAG, (0x21, uint64_t))

#undef GEN_BITTYPE_TAG

template <std::size_t N>
class Bits
{
    public:
        Bits()
        {
            this->value = 0;
        }

        template <std::size_t N2>
        Bits(Bits<N2> const& b)
        {
            this->value = b.getValue();
        }

        template <typename T>
        Bits(T v = 0)
        {
            this->value = reinterpret_cast<typename bittype_tag<N>::type&>(v);
        }

        inline typename bittype_tag<N>::type getValue(void) const
        {
            return (this->value & std::intmax_t((1 << N) - 1));
        }

        template <typename T>
        operator T () const
        {
            uintmax_t a = this->getValue();
            T r = reinterpret_cast<T&>(a);
            return r;
        }

        operator Bits& () const
        {
            return *this;
        }

        template <typename T>
        bool operator == (T const& v) const
        {
            return (std::uintmax_t(this->getValue()) == std::uintmax_t(v));
        }

        Bits operator ~ () const
        {
            return Bits(~this->getValue());
        }

#define OPERATOR(op) \
        template <typename T> \
        Bits operator op (T const v) const \
        { \
            return Bits(this->getValue() op v); \
        } \
        Bits operator op (Bits const v) const \
        { \
            return Bits(this->getValue() op v.getValue()); \
        }

#define OPERATOR_EQ(op) \
        template <typename T> \
        Bits& operator op (T const v) \
        { \
            this->value op v; \
            return *this; \
        } \
        Bits& operator op (Bits const v) \
        { \
            this->value op v.value; \
            return *this; \
        }

        OPERATOR(&)
        OPERATOR(|)
        OPERATOR(^)
        OPERATOR(<<)
        OPERATOR(>>)

        OPERATOR_EQ(=)
        OPERATOR_EQ(&=)
        OPERATOR_EQ(|=)
        OPERATOR_EQ(^=)
        OPERATOR_EQ(<<=)
        OPERATOR_EQ(>>=)

#undef OPERATOR
#undef OPERATOR_EQ

    protected:
        typename bittype_tag<N>::type value;
};

template <typename T>
constexpr std::size_t bitsize(void)
{
    return sizeof(T) * 8;
}

#define DEF_BITSIZE(z, N, data) \
  template <> constexpr std::size_t bitsize< Bits<N> >(void) { return N; }

BOOST_PP_REPEAT(64, DEF_BITSIZE,)

#undef DEF_BITSIZE

template <typename T>
constexpr std::size_t bitsize(T)
{
    return bitsize<T>();
}

template <std::size_t N>
std::size_t hash_value(Bits<N> const& a)
{
    return size_t(a);
}

namespace memory
{

template <std::size_t page_size, typename align_t, typename val_t, typename cs>
void read_block(TypeBlock<page_size, align_t, val_t, cs> const& b, std::ptrdiff_t i, std::ptrdiff_t const end, std::array<align_t, page_size>& buf, std::ptrdiff_t off)
{
    constexpr std::size_t N_align = bitsize<align_t>();
    constexpr std::size_t N_val = bitsize<val_t>();
    TypeBlock<page_size, align_t, Bits<N_val>, cs> const& block = reinterpret_cast<TypeBlock<page_size, align_t, Bits<N_val>, cs> const&>(b);

    int bitoff = 0;
    if(off < 0)
    {
        int z = off * N_align;
        i -= z / N_val;
        bitoff += z % N_val;
        off = 0;
    }

    while(off < std::ptrdiff_t(page_size) && i <= end)
    {
        Bits<N_align> a = Bits<N_align>();

        if(bitoff > 0 && i > 0)
            a = Bits<N_align>(block[i-1] >> (N_val - bitoff));

        while(bitoff < int(N_align) && i < end)
        {
            if(bitoff < 0)
                a = Bits<N_align>(block[i] >> (-bitoff));
            else
                a |= Bits<N_align>(block[i]) << bitoff;

            i++;
            bitoff += N_val;
        }
        bitoff -= N_align;
        buf[off++] |= align_t(a);
    }
}

template <std::size_t page_size, typename align_t, typename val_t, typename cs>
void write_block(TypeBlock<page_size, align_t, val_t, cs> const& b, std::ptrdiff_t i, std::ptrdiff_t const end, std::array<align_t, page_size> const& buf, std::ptrdiff_t off)
{
    constexpr std::size_t N_align = bitsize<align_t>();
    constexpr std::size_t N_val = bitsize<val_t>();
    TypeBlock<page_size, align_t, Bits<N_val>, cs> const& block = reinterpret_cast<TypeBlock<page_size, align_t, Bits<N_val>, cs> const&>(b);

    int bitoff = 0;
    int tbitoff = off * N_align + bitoff;

    if(off < 0)
    {
        i -= tbitoff / N_val;
        bitoff += tbitoff % N_val;
        off = 0;
        tbitoff = -bitoff;
    }

    Bits<N_val> mask = -1;
    if(N_val > N_align)
        mask = (1 << N_align)-1;

    while(off < std::ptrdiff_t(page_size) && i <= end)
    {
        Bits<N_align> a(buf[off++]);
        if(bitoff > 0 && i > 0)// && tbitoff >= range.first && tbitoff < range.second)
        {
            block[i-1] &= ~(mask << (N_val-bitoff) );
            block[i-1] |= Bits<N_val>(a) << (N_val-bitoff);
        }

        while(bitoff < int(N_align) && i < end)
        {
            // if(tbitoff >= range.first && tbitoff < range.second)
            {
                if(bitoff < 0)
                {
                    block[i] &= ~(mask >> (-bitoff));
                    block[i] |= Bits<N_val>(a >> (-bitoff));
                }
                else
                {
                    block[i] &= ~( mask << bitoff );
                    block[i] |= Bits<N_val>(a) << bitoff;
                }
            }

            ++i;
            bitoff += N_val;
        }
        bitoff -= N_align;
        tbitoff += N_align;
    }
}

} // namespace memory

} // namespace giecs


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/sym_equiv.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>

namespace cctbx { namespace miller { namespace boost_python {

namespace {

  struct sym_equiv_index_wrappers
  {
    typedef sym_equiv_index w_t;
    typedef hendrickson_lattman<> h_l;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("sym_equiv_index", no_init)
        .def("h", &w_t::h)
        .def("hr", &w_t::hr, ccr())
        .def("ht", &w_t::ht)
        .def("t_den", &w_t::t_den)
        .def("ht_angle", &w_t::ht_angle, (arg("deg")=false))
        .def("friedel_flag", &w_t::friedel_flag)
        .def("mate", &w_t::mate, (arg("i_mate")=1))
        .def("phase_eq",
          (double(w_t::*)(double const&, bool) const) &w_t::phase_eq, (
            arg("phi_in"), arg("deg")=false))
        .def("phase_in",
          (double(w_t::*)(double, bool) const) &w_t::phase_in, (
            arg("phi_eq"), arg("deg")=false))
        .def("complex_eq",
          (std::complex<double>(w_t::*)(std::complex<double> const&) const)
          &w_t::complex_eq, (arg("f_in")))
        .def("complex_in",
          (std::complex<double>(w_t::*)(std::complex<double> const&) const)
          &w_t::complex_in, (arg("f_eq")))
        .def("hendrickson_lattman_eq",
          (h_l(w_t::*) (h_l const&) const) &w_t::hendrickson_lattman_eq, (
            arg("hl_in")))
        .def("hendrickson_lattman_in",
          (h_l(w_t::*) (h_l) const) &w_t::hendrickson_lattman_in, (
            arg("hl_eq")))
      ;
    }
  };

  struct sym_equiv_indices_wrappers
  {
    typedef sym_equiv_indices w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("sym_equiv_indices", no_init)
        .def(init<sgtbx::space_group const&, index<> const&>((
          arg("space_group"), arg("h_in"))))
        .def("phase_restriction", &w_t::phase_restriction)
        .def("is_centric", &w_t::is_centric)
        .def("indices", &w_t::indices, ccr())
        .def("multiplicity", &w_t::multiplicity, (arg("anomalous_flag")))
        .def("f_mates", &w_t::f_mates, (arg("anomalous_flag")))
        .def("epsilon", &w_t::epsilon)
        .def("__call__",
          (sym_equiv_index(w_t::*)(std::size_t) const) &w_t::operator(), (
            arg("i")))
        .def("is_valid_phase", &w_t::is_valid_phase, (
          arg("phi"), arg("deg")=false, arg("tolerance")=1e-5))
        .def("p1_listing", &w_t::p1_listing, (arg("anomalous_flag")))
      ;
    }
  };

} // namespace <anoymous>

  void wrap_sym_equiv()
  {
    sym_equiv_index_wrappers::wrap();
    sym_equiv_indices_wrappers::wrap();
  }

}}} // namespace cctbx::miller::boost_python