text:
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <algorithm>
#include <Eigen/Geometry>
#include <stdexcept>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/serialization_eigen.hpp>
#include <arc_utilities/eigen_helpers.hpp>

#ifndef VOXEL_GRID_HPP
#define VOXEL_GRID_HPP

namespace VoxelGrid
{
    struct GRID_INDEX
    {
        int64_t x;
        int64_t y;
        int64_t z;

        GRID_INDEX() : x(-1), y(-1), z(-1) {}

        GRID_INDEX(const int64_t in_x, const int64_t in_y, const int64_t in_z) : x(in_x), y(in_y), z(in_z) {}

        bool operator==(const GRID_INDEX& other) const
        {
            return (x == other.x && y == other.y && z == other.z);
        }
    };

    template<typename T, typename Allocator=std::allocator<T>>
    class VoxelGrid
    {
    protected:

        Eigen::Isometry3d origin_transform_;
        Eigen::Isometry3d inverse_origin_transform_;
        T default_value_;
        T oob_value_;
        std::vector<T, Allocator> data_;
        double cell_x_size_;
        double cell_y_size_;
        double cell_z_size_;
        double inv_cell_x_size_;
        double inv_cell_y_size_;
        double inv_cell_z_size_;
        double x_size_;
        double y_size_;
        double z_size_;
        int64_t stride1_;
        int64_t stride2_;
        int64_t num_x_cells_;
        int64_t num_y_cells_;
        int64_t num_z_cells_;
        bool initialized_;

        inline int64_t GetDataIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return (x_index * stride1_) + (y_index * stride2_) + z_index;
        }

        inline void SetContents(const T& value)
        {
            data_.clear();
            data_.resize(num_x_cells_ * num_y_cells_ * num_z_cells_, value);
        }

        inline void SafetyCheckSizes(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size,const double y_size, const double z_size) const
        {
            if (cell_x_size <= 0.0)
            {
                throw std::invalid_argument("cell_x_size must be positive and non-zero");
            }
            if (std::isnan(cell_x_size))
            {
                throw std::invalid_argument("cell_x_size must not be NaN");
            }
            if (std::isinf(cell_x_size) != 0)
            {
                throw std::invalid_argument("cell_x_size must not be INF");
            }
            if (cell_y_size <= 0.0)
            {
                throw std::invalid_argument("cell_y_size must be positive and non-zero");
            }
            if (std::isnan(cell_y_size))
            {
                throw std::invalid_argument("cell_y_size must not be NaN");
            }
            if (std::isinf(cell_y_size) != 0)
            {
                throw std::invalid_argument("cell_y_size must not be INF");
            }
            if (cell_z_size <= 0.0)
            {
                throw std::invalid_argument("cell_z_size must be positive and non-zero");
            }
            if (std::isnan(cell_z_size))
            {
                throw std::invalid_argument("cell_z_size must not be NaN");
            }
            if (std::isinf(cell_z_size) != 0)
            {
                throw std::invalid_argument("cell_z_size must not be INF");
            }
            if (x_size <= 0.0)
            {
                throw std::invalid_argument("x_size must be positive and non-zero");
            }
            if (y_size <= 0.0)
            {
                throw std::invalid_argument("y_size must be positive and non-zero");
            }
            if (z_size <= 0.0)
            {
                throw std::invalid_argument("z_size must be positive and non-zero");
            }
            if (std::isnan(x_size))
            {
                throw std::invalid_argument("x_size must not be NaN");
            }
            if (std::isnan(y_size))
            {
                throw std::invalid_argument("y_size must not be NaN");
            }
            if (std::isnan(z_size))
            {
                throw std::invalid_argument("z_size must not be NaN");
            }
            if (std::isinf(x_size) != 0)
            {
                throw std::invalid_argument("x_size must not be INF");
            }
            if (std::isinf(y_size) != 0)
            {
                throw std::invalid_argument("y_size must not be INF");
            }
            if (std::isinf(z_size) != 0)
            {
                throw std::invalid_argument("z_size must not be INF");
            }
        }

        inline void SafetyCheckSizes(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells,const int64_t num_y_cells, const int64_t num_z_cells) const
        {
            if (cell_x_size <= 0.0)
            {
                throw std::invalid_argument("cell_x_size must be positive and non-zero");
            }
            if (std::isnan(cell_x_size))
            {
                throw std::invalid_argument("cell_x_size must not be NaN");
            }
            if (std::isinf(cell_x_size) != 0)
            {
                throw std::invalid_argument("cell_x_size must not be INF");
            }
            if (cell_y_size <= 0.0)
            {
                throw std::invalid_argument("cell_y_size must be positive and non-zero");
            }
            if (std::isnan(cell_y_size))
            {
                throw std::invalid_argument("cell_y_size must not be NaN");
            }
            if (std::isinf(cell_y_size) != 0)
            {
                throw std::invalid_argument("cell_y_size must not be INF");
            }
            if (cell_z_size <= 0.0)
            {
                throw std::invalid_argument("cell_z_size must be positive and non-zero");
            }
            if (std::isnan(cell_z_size))
            {
                throw std::invalid_argument("cell_z_size must not be NaN");
            }
            if (std::isinf(cell_z_size) != 0)
            {
                throw std::invalid_argument("cell_z_size must not be INF");
            }
            if (num_x_cells <= 0)
            {
                throw std::invalid_argument("num_x_cells must be positive and non-zero");
            }
            if (num_y_cells <= 0)
            {
                throw std::invalid_argument("num_y_cells must be positive and non-zero");
            }
            if (num_z_cells <= 0)
            {
                throw std::invalid_argument("num_z_cells must be positive and non-zero");
            }
        }

        inline void CoreInitialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
            cell_x_size_ = fabs(cell_x_size);
            cell_y_size_ = fabs(cell_y_size);
            cell_z_size_ = fabs(cell_z_size);
            inv_cell_x_size_ = 1.0 / cell_x_size_;
            inv_cell_y_size_ = 1.0 / cell_y_size_;
            inv_cell_z_size_ = 1.0 / cell_z_size_;
            num_x_cells_ = num_x_cells;
            num_y_cells_ = num_y_cells;
            num_z_cells_ = num_z_cells;
            x_size_ = (double)num_x_cells_ * cell_x_size_;
            y_size_ = (double)num_y_cells_ * cell_y_size_;
            z_size_ = (double)num_z_cells_ * cell_z_size_;
            default_value_ = default_value;
            oob_value_ = oob_value;
            stride1_ = num_y_cells_ * num_z_cells_;
            stride2_ = num_z_cells_;
            SetContents(default_value_);
        }

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        inline static uint64_t Serialize(const VoxelGrid<T, Allocator>& grid, std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer)
        {
            return grid.SerializeSelf(buffer, value_serializer);
        }

        inline static std::pair<VoxelGrid<T, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            VoxelGrid<T, Allocator> temp_grid;
            const uint64_t bytes_read = temp_grid.DeserializeSelf(buffer, current, value_deserializer);
            return std::make_pair(temp_grid, bytes_read);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const double x_size, const double y_size, double const z_size, const T& default_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid(const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value)
        {
            Initialize(cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid()
        {
            origin_transform_ = Eigen::Isometry3d::Identity();
            inverse_origin_transform_ = origin_transform_.inverse();
            arc_helpers::RequireAlignment(origin_transform_, 16u);
            arc_helpers::RequireAlignment(inverse_origin_transform_, 16u);
            cell_x_size_ = 0.0;
            cell_y_size_ = 0.0;
            cell_z_size_ = 0.0;
            inv_cell_x_size_ = 0.0;
            inv_cell_y_size_ = 0.0;
            inv_cell_z_size_ = 0.0;
            x_size_ = 0.0;
            y_size_ = 0.0;
            z_size_ = 0.0;
            num_x_cells_ = 0;
            num_y_cells_ = 0;
            num_z_cells_ = 0;
            stride1_ = num_y_cells_ * num_z_cells_;
            stride2_ = num_z_cells_;
            data_.clear();
            initialized_ = false;
        }

        inline void Initialize(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);
            int64_t num_x_cells = (int64_t)(ceil(fabs(x_size) / fabs(cell_x_size)));
            int64_t num_y_cells = (int64_t)(ceil(fabs(y_size) / fabs(cell_y_size)));
            int64_t num_z_cells = (int64_t)(ceil(fabs(z_size) / fabs(cell_z_size)));
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        inline void Initialize(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
            CoreInitialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
            origin_transform_ = origin_transform;
            inverse_origin_transform_ = origin_transform_.inverse();
            arc_helpers::RequireAlignment(origin_transform_, 16u);
            arc_helpers::RequireAlignment(inverse_origin_transform_, 16u);
            initialized_ = true;
        }

        inline void Initialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);
            int64_t num_x_cells = (int64_t)(ceil(fabs(x_size) / fabs(cell_x_size)));
            int64_t num_y_cells = (int64_t)(ceil(fabs(y_size) / fabs(cell_y_size)));
            int64_t num_z_cells = (int64_t)(ceil(fabs(z_size) / fabs(cell_z_size)));
            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        inline void Initialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
            CoreInitialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
            const Eigen::Translation3d origin_translation(-x_size_ * 0.5, -y_size_ * 0.5, -z_size_ * 0.5);
            const Eigen::Isometry3d origin_transform = origin_translation * Eigen::Quaterniond::Identity();
            origin_transform_ = origin_transform;
            inverse_origin_transform_ = origin_transform_.inverse();
            arc_helpers::RequireAlignment(origin_transform_, 16u);
            arc_helpers::RequireAlignment(inverse_origin_transform_, 16u);
            initialized_ = true;
        }

        inline uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer) const
        {
            const uint64_t start_buffer_size = buffer.size();
            // Serialize the initialized
            arc_utilities::SerializeFixedSizePOD<uint8_t>((uint8_t)initialized_, buffer);
            // Serialize the transforms
            arc_utilities::SerializeEigenType<Eigen::Isometry3d>(origin_transform_, buffer);
            arc_utilities::SerializeEigenType<Eigen::Isometry3d>(inverse_origin_transform_, buffer);
            // Serialize the data
            arc_utilities::SerializeVector<T, Allocator>(data_, buffer, value_serializer);
            // Serialize the cell sizes
            arc_utilities::SerializeFixedSizePOD<double>(cell_x_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(cell_y_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(cell_z_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(inv_cell_x_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(inv_cell_y_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(inv_cell_z_size_, buffer);
            // Serialize the grid sizes
            arc_utilities::SerializeFixedSizePOD<double>(x_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(y_size_, buffer);
            arc_utilities::SerializeFixedSizePOD<double>(z_size_, buffer);
            // Serialize the control/bounds values
            arc_utilities::SerializeFixedSizePOD<int64_t>(stride1_, buffer);
            arc_utilities::SerializeFixedSizePOD<int64_t>(stride2_, buffer);
            arc_utilities::SerializeFixedSizePOD<int64_t>(num_x_cells_, buffer);
            arc_utilities::SerializeFixedSizePOD<int64_t>(num_y_cells_, buffer);
            arc_utilities::SerializeFixedSizePOD<int64_t>(num_z_cells_, buffer);
            // Serialize the default value
            value_serializer(default_value_, buffer);
            // Serialize the OOB value
            value_serializer(oob_value_, buffer);
            // Figure out how many bytes were written
            const uint64_t end_buffer_size = buffer.size();
            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
            return bytes_written;
        }

        inline uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
        {
            uint64_t current_position = current;
            // Deserialize the initialized
            const std::pair<uint8_t, uint64_t> initialized_deserialized = arc_utilities::DeserializeFixedSizePOD<uint8_t>(buffer, current_position);
            initialized_ = (bool)initialized_deserialized.first;
            current_position += initialized_deserialized.second;
            // Deserialize the transforms
            const std::pair<Eigen::Isometry3d, uint64_t> origin_transform_deserialized = arc_utilities::DeserializeEigenType<Eigen::Isometry3d>(buffer, current_position);
            origin_transform_ = origin_transform_deserialized.first;
            current_position += origin_transform_deserialized.second;
            const std::pair<Eigen::Isometry3d, uint64_t> inverse_origin_transform_deserialized = arc_utilities::DeserializeEigenType<Eigen::Isometry3d>(buffer, current_position);
            inverse_origin_transform_ = inverse_origin_transform_deserialized.first;
            current_position += inverse_origin_transform_deserialized.second;
            // Deserialize the data
            const std::pair<std::vector<T, Allocator>, uint64_t> data_deserialized = arc_utilities::DeserializeVector<T, Allocator>(buffer, current_position, value_deserializer);
            data_ = data_deserialized.first;
            current_position += data_deserialized.second;
            // Deserialize the cell sizes
            const std::pair<double, uint64_t> cell_x_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            cell_x_size_ = cell_x_size_deserialized.first;
            current_position += cell_x_size_deserialized.second;
            const std::pair<double, uint64_t> cell_y_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            cell_y_size_ = cell_y_size_deserialized.first;
            current_position += cell_y_size_deserialized.second;
            const std::pair<double, uint64_t> cell_z_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            cell_z_size_ = cell_z_size_deserialized.first;
            current_position += cell_z_size_deserialized.second;
            const std::pair<double, uint64_t> inv_cell_x_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            inv_cell_x_size_ = inv_cell_x_size_deserialized.first;
            current_position += inv_cell_x_size_deserialized.second;
            const std::pair<double, uint64_t> inv_cell_y_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            inv_cell_y_size_ = inv_cell_y_size_deserialized.first;
            current_position += inv_cell_y_size_deserialized.second;
            const std::pair<double, uint64_t> inv_cell_z_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            inv_cell_z_size_ = inv_cell_z_size_deserialized.first;
            current_position += inv_cell_z_size_deserialized.second;
            // Deserialize the grid sizes
            const std::pair<double, uint64_t> x_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            x_size_ = x_size_deserialized.first;
            current_position += x_size_deserialized.second;
            const std::pair<double, uint64_t> y_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            y_size_ = y_size_deserialized.first;
            current_position += y_size_deserialized.second;
            const std::pair<double, uint64_t> z_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
            z_size_ = z_size_deserialized.first;
            current_position += z_size_deserialized.second;
            // Deserialize the control/bounds values
            const std::pair<int64_t, uint64_t> stride1_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            stride1_ = stride1_deserialized.first;
            current_position += stride1_deserialized.second;
            const std::pair<int64_t, uint64_t> stride2_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            stride2_ = stride2_deserialized.first;
            current_position += stride2_deserialized.second;
            const std::pair<int64_t, uint64_t> num_x_cells_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            num_x_cells_ = num_x_cells_deserialized.first;
            current_position += num_x_cells_deserialized.second;
            const std::pair<int64_t, uint64_t> num_y_cells_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            num_y_cells_ = num_y_cells_deserialized.first;
            current_position += num_y_cells_deserialized.second;
            const std::pair<int64_t, uint64_t> num_z_cells_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
            num_z_cells_ = num_z_cells_deserialized.first;
            current_position += num_z_cells_deserialized.second;
            // Deserialize the default value
            const std::pair<T, uint64_t> default_value_deserialized = value_deserializer(buffer, current_position);
            default_value_ = default_value_deserialized.first;
            current_position += default_value_deserialized.second;
            // Deserialize the OOB value
            const std::pair<T, uint64_t> oob_value_deserialized = value_deserializer(buffer, current_position);
            oob_value_ = oob_value_deserialized.first;
            current_position += oob_value_deserialized.second;
            // Figure out how many bytes were read
            const uint64_t bytes_read = current_position - current;
            return bytes_read;
        }

        inline bool IsInitialized() const
        {
            return initialized_;
        }

        inline void ResetWithDefault()
        {
            SetContents(default_value_);
        }

        inline void ResetWithNewValue(const T& new_value)
        {
            SetContents(new_value);
        }

        inline void ResetWithNewDefault(const T& new_default)
        {
            default_value_ = new_default;
            SetContents(default_value_);
        }

        inline bool IndexInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            if (x_index >= 0 && y_index >= 0 && z_index >= 0 && x_index < num_x_cells_ && y_index < num_y_cells_ && z_index < num_z_cells_)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline std::pair<const T&, bool> GetImmutable3d(const Eigen::Vector3d& location) const
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
            if (indices.size() == 3)
            {
                return GetImmutable(indices[0], indices[1], indices[2]);
            }
            else
            {
                return std::pair<const T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<const T&, bool> GetImmutable4d(const Eigen::Vector4d& location) const
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
            if (indices.size() == 3)
            {
                return GetImmutable(indices[0], indices[1], indices[2]);
            }
            else
            {
                return std::pair<const T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<const T&, bool> GetImmutable(const double x, const double y, const double z) const
        {
            const Eigen::Vector4d location(x, y, z, 1.0);
            return GetImmutable4d(location);
        }

        inline std::pair<const T&, bool> GetImmutable(const GRID_INDEX& index) const
        {
            return GetImmutable(index.x, index.y, index.z);
        }

        inline std::pair<const T&, bool> GetImmutable(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                return std::pair<const T&, bool>(data_[data_index], true);
            }
            else
            {
                return std::pair<const T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<T&, bool> GetMutable3d(const Eigen::Vector3d& location)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
            if (indices.size() == 3)
            {
                return GetMutable(indices[0], indices[1], indices[2]);
            }
            else
            {
                return std::pair<T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<T&, bool> GetMutable4d(const Eigen::Vector4d& location)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
            if (indices.size() == 3)
            {
                return GetMutable(indices[0], indices[1], indices[2]);
            }
            else
            {
                return std::pair<T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<T&, bool> GetMutable(const double x, const double y, const double z)
        {
            const Eigen::Vector4d location(x, y, z, 1.0);
            return GetMutable4d(location);
        }

        inline std::pair<T&, bool> GetMutable(const GRID_INDEX& index)
        {
            return GetMutable(index.x, index.y, index.z);
        }

        inline std::pair<T&, bool> GetMutable(const int64_t x_index, const int64_t y_index, const int64_t z_index)
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                return std::pair<T&, bool>(data_[data_index], true);
            }
            else
            {
                return std::pair<T&, bool>(oob_value_, false);
            }
        }

        inline bool SetValue3d(const Eigen::Vector3d& location, const T& value)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
            if (indices.size() == 3)
            {
                return SetValue(indices[0], indices[1], indices[2], value);
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue4d(const Eigen::Vector4d& location, const T& value)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
            if (indices.size() == 3)
            {
                return SetValue(indices[0], indices[1], indices[2], value);
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue(const double x, const double y, const double z, const T& value)
        {
            const Eigen::Vector4d location(x, y, z, 1.0);
            return SetValue4d(location, value);
        }

        inline bool SetValue(const GRID_INDEX& index, const T& value)
        {
            return SetValue(index.x, index.y, index.z, value);
        }

        inline bool SetValue(const int64_t x_index, const int64_t y_index, const int64_t z_index, const T& value)
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                data_[data_index] = value;
                return true;
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue3d(const Eigen::Vector3d& location, T&& value)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
            if (indices.size() == 3)
            {
                return SetValue(indices[0], indices[1], indices[2], value);
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue4d(const Eigen::Vector4d& location, T&& value)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
            if (indices.size() == 3)
            {
                return SetValue(indices[0], indices[1], indices[2], value);
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue(const double x, const double y, const double z, T&& value)
        {
            const Eigen::Vector4d location(x, y, z, 1.0);
            return SetValue4d(location, value);
        }

        inline bool SetValue(const GRID_INDEX& index, T&& value)
        {
            return SetValue(index.x, index.y, index.z, value);
        }

        inline bool SetValue(const int64_t x_index, const int64_t y_index, const int64_t z_index, T&& value)
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                data_[data_index] = value;
                return true;
            }
            else
            {
                return false;
            }
        }

        inline double GetXSize() const
        {
            return x_size_;
        }

        inline double GetYSize() const
        {
            return y_size_;
        }

        inline double GetZSize() const
        {
            return z_size_;
        }

        inline std::vector<double> GetCellSizes() const
        {
            return std::vector<double>{cell_x_size_, cell_y_size_, cell_z_size_};
        }

        inline T GetDefaultValue() const
        {
            return default_value_;
        }

        inline T GetOOBValue() const
        {
            return oob_value_;
        }

        inline void SetDefaultValue(const T& default_value)
        {
            default_value_ = default_value;
        }

        inline void SetOOBValue(const T& oob_value)
        {
            oob_value_ = oob_value;
        }

        inline int64_t GetNumXCells() const
        {
            return num_x_cells_;
        }

        inline int64_t GetNumYCells() const
        {
            return num_y_cells_;
        }

        inline int64_t GetNumZCells() const
        {
            return num_z_cells_;
        }

        inline const Eigen::Isometry3d& GetOriginTransform() const
        {
            return origin_transform_;
        }

        inline const Eigen::Isometry3d& GetInverseOriginTransform() const
        {
            return inverse_origin_transform_;
        }

        inline std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const
        {
            const Eigen::Vector4d point(x, y, z, 1.0);
            return LocationToGridIndex4d(point);
        }

        inline std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const
        {
            assert(initialized_);
            const Eigen::Vector3d point_in_grid_frame = inverse_origin_transform_ * location;
            const int64_t x_cell = (int64_t)(point_in_grid_frame.x() * inv_cell_x_size_);
            const int64_t y_cell = (int64_t)(point_in_grid_frame.y() * inv_cell_y_size_);
            const int64_t z_cell = (int64_t)(point_in_grid_frame.z() * inv_cell_z_size_);
            if (IndexInBounds(x_cell, y_cell, z_cell))
            {
                return std::vector<int64_t>{x_cell, y_cell, z_cell};
            }
            else
            {
                return std::vector<int64_t>();
            }
        }

        inline std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const
        {
            assert(initialized_);
            const Eigen::Vector4d point_in_grid_frame = inverse_origin_transform_ * location;
            const int64_t x_cell = (int64_t)(point_in_grid_frame(0) * inv_cell_x_size_);
            const int64_t y_cell = (int64_t)(point_in_grid_frame(1) * inv_cell_y_size_);
            const int64_t z_cell = (int64_t)(point_in_grid_frame(2) * inv_cell_z_size_);
            if (IndexInBounds(x_cell, y_cell, z_cell))
            {
                return std::vector<int64_t>{x_cell, y_cell, z_cell};
            }
            else
            {
                return std::vector<int64_t>();
            }
        }

        inline std::vector<double> GridIndexToLocation(const GRID_INDEX& index) const
        {
            return GridIndexToLocation(index.x, index.y, index.z);
        }

        inline std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const Eigen::Vector3d point_in_grid_frame(cell_x_size_ * ((double)x_index + 0.5), cell_y_size_ * ((double)y_index + 0.5), cell_z_size_ * ((double)z_index + 0.5));
                const Eigen::Vector3d point = origin_transform_ * point_in_grid_frame;
                return std::vector<double>{point.x(), point.y(), point.z()};
            }
            else
            {
                return std::vector<double>();
            }
        }

        inline const std::vector<T>& GetRawData() const
        {
            return data_;
        }

        inline std::vector<T> CopyRawData() const
        {
            return data_;
        }

        inline bool SetRawData(std::vector<T>& data)
        {
            assert(initialized_);
            const int64_t expected_length = num_x_cells_ * num_y_cells_ * num_z_cells_;
            if ((int64_t)data.size() == expected_length)
            {
                data_ = data;
                return true;
            }
            else
            {
                std::cerr << "Failed to load internal data - expected " << expected_length << " got " << data.size() << std::endl;
                return false;
            }
        }

        inline uint64_t HashDataIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return (x_index * stride1_) + (y_index * stride2_) + z_index;
        }
    };
}

namespace std
{
    template <>
    struct hash<VoxelGrid::GRID_INDEX>
    {
        std::size_t operator()(const VoxelGrid::GRID_INDEX& index) const
        {
            using std::size_t;
            using std::hash;
            return ((std::hash<int64_t>()(index.x) ^ (std::hash<int64_t>()(index.y) << 1) >> 1) ^ (std::hash<int64_t>()(index.z) << 1));
        }
    };
}

inline std::ostream& operator<<(std::ostream& strm, const VoxelGrid::GRID_INDEX& index)
{
    strm << "GridIndex: " << index.x << "," << index.y << "," << index.z;
    return strm;
}

#endif // VOXEL_GRID_HPP


text:
#include <string>
#include <vector>

#include <boost/filesystem.hpp>
#include <gmock/gmock.h>
#include <gtest/gtest.h>

#include <irkit/radix_tree.hpp>
#include <irkit/io.hpp>

namespace {

namespace fs = boost::filesystem;

TEST(radix_tree, build_read)
{
    int block_size = 16;
    std::string terms_file("terms.txt");
    std::vector<std::string> lines;
    irk::io::load_lines(fs::path(terms_file), lines);

    // Build
    irk::radix_tree<int> t;
    int idx = 0;
    std::ifstream in_terms(terms_file);
    for (const std::string& term : lines)
    {
        if (idx % block_size == 0) { t.insert(term, idx / block_size); }
        idx++;
    }

    idx = 0;
    in_terms.clear();
    in_terms.seekg(0, std::ios::beg);
    for (const std::string& term : lines)
    {
        auto exact = t.find(term);
        if (idx % block_size == 0) {
            ASSERT_TRUE(exact.has_value());
            ASSERT_EQ(exact.value(), idx / block_size);
        } else {
            ASSERT_FALSE(exact.has_value());
        }
        auto leq = t.seek_le(term);
        ASSERT_EQ(leq.value(), idx / block_size);
        idx++;
    }
}

}  // namespace

int main(int argc, char** argv)
{
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

ext: cpp

max_stars_repo_name: elshize/irkit

max_issues_repo_name: elshize/irkit

max_forks_repo_name: elshize/irkit


text:
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <harp.hpp>
#include <specex_fits.h>
#include <specex_trace.h>
#include <specex_gauss_hermite_psf.h>

using namespace std ;

static void AddRow1(specex::FitsTable& table,const string& PARAM, double wavemin, double wavemax, harp::vector_double& coeff) {
  std::vector<specex::FitsTableEntry> row;
  {specex::FitsTableEntry entry; entry.string_val = PARAM; row.push_back(entry);}
  {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = wavemin; row.push_back(entry);}
  {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = wavemax; row.push_back(entry);}
  {specex::FitsTableEntry entry; entry.double_vals = coeff; row.push_back(entry);}
  table.data.push_back(row);
}

void write_gauss_hermite_psf_fits_version_2(const specex::GaussHermitePSF& psf, fitsfile* fp, int first_hdu, std::vector<specex::Spot_p> *spots) {

  SPECEX_DEBUG("write_gauss_hermite_psf_fits_version_2");

  ////////////////////////////
  string PSFVER = "2";
  ////////////////////////////

  int NPIX_X = psf.ccd_image_n_cols;
  int NPIX_Y = psf.ccd_image_n_rows;
  int BUNDLMIN = 0;
  int BUNDLMAX = 0;
  int FIBERMIN = 0;
  int FIBERMAX = 0;
  int NFIBERS=0;
  for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
      bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {

    if(bundle_it == psf.ParamsOfBundles.begin()) {
      BUNDLMIN = bundle_it->second.bundle_id;
      BUNDLMAX = bundle_it->second.bundle_id;
      FIBERMIN = bundle_it->second.fiber_min;
      FIBERMAX = bundle_it->second.fiber_max;
    }

    BUNDLMIN = min(BUNDLMIN,bundle_it->second.bundle_id);
    BUNDLMAX = max(BUNDLMAX,bundle_it->second.bundle_id);
    FIBERMIN = min(FIBERMIN,bundle_it->second.fiber_min);
    FIBERMAX = max(FIBERMAX,bundle_it->second.fiber_max);

    NFIBERS += (bundle_it->second.fiber_max-bundle_it->second.fiber_min+1);
  }

  SPECEX_DEBUG("BUNDLMIN=" << BUNDLMIN << " BUNDLMAX=" << BUNDLMAX << " FIBERMIN=" << FIBERMIN << " FIBERMAX=" << FIBERMAX << " NFIBERS=" << NFIBERS);

  // number of fibers per bundle from first bundle
  if(NFIBERS != (FIBERMAX+1)) {
    SPECEX_WARNING("will fill with zeros the coefficients of fibers that have not been fit, there are " << (FIBERMAX+1)-NFIBERS << " of those");
    NFIBERS=(FIBERMAX+1);
  }

  int GHDEGX = psf.Degree();
  int GHDEGY = psf.Degree();

  int status = 0;
  fits_movabs_hdu ( fp, first_hdu, NULL, &status ); harp::fits::check ( status );

  // get largest legendre degree of all params in all bundles and check param numbers match !!
  int ncoeff_param_fit=0;
  int nparams=psf.LocalNAllPar();
  int nparams_all = nparams;
  nparams_all += 2; // X and Y
  nparams_all += 1; // GH trivial order zero
#ifdef CONTINUUM
  nparams_all += 1; // continuum
#endif

  for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
      bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {

    if( int(bundle_it->second.AllParPolXW.size()) != nparams ) SPECEX_ERROR("Fatal inconsistency in number of parameters in psf between bundles: AllParPolXW.size=" << bundle_it->second.AllParPolXW.size() << " psf.LocalNAllPar()=" << nparams);

    for(size_t p=0;p<bundle_it->second.AllParPolXW.size();p++) {
      ncoeff_param_fit=max(ncoeff_param_fit,bundle_it->second.AllParPolXW[p]->ydeg+1);
    }
  }

  int ncoeff_xtrace_fit=0;
  int ncoeff_ytrace_fit=0;

  // add traces
  for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
      bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
    const specex::PSF_Params & params_of_bundle = bundle_it->second;
    for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++) {
      ncoeff_xtrace_fit=max(ncoeff_xtrace_fit,int(psf.FiberTraces.find(fiber)->second.X_vs_W.coeff.size()));
      ncoeff_ytrace_fit=max(ncoeff_ytrace_fit,int(psf.FiberTraces.find(fiber)->second.Y_vs_W.coeff.size()));

    }
  }
  int ncoeff=ncoeff_param_fit;
  ncoeff=max(ncoeff,ncoeff_xtrace_fit);
  ncoeff=max(ncoeff,ncoeff_ytrace_fit);

  int delta_deg = 1; // increase degree of legendre polynomials to minimize mapping errors
  ncoeff += delta_deg;

  // no need to add degree for traces because same mapping

  SPECEX_DEBUG("ncoeff = " << ncoeff);

  specex::FitsTable table;

  { // column description
    table.AddColumnDescription("PARAM","8A","","");
    table.AddColumnDescription("WAVEMIN","D","","");
    table.AddColumnDescription("WAVEMAX","D","","");
    char coeff_tform[100];
    sprintf(coeff_tform,"%dD",ncoeff*NFIBERS);
    vector <int> dim; dim.resize(2);
    dim[0]=ncoeff;
    dim[1]=NFIBERS;

    string sdim = table.encode_dimension(dim);
    // debug
    vector <int> dim2 = table.decode_dimension(sdim);
    if (dim2.size()!=2 || dim2[0]!= dim[0]|| dim2[1]!=dim[1]) {
      SPECEX_ERROR("error encode/decode dimension");
    }

    table.AddColumnDescription("COEFF",coeff_tform,sdim,"");
  }

  int LEGWMIN=1000000;
  int LEGWMAX=0;
  vector<string> keys;

  { // data

    // now loop on real psf parameters

    harp::vector_double wave(ncoeff);
    harp::vector_double values(ncoeff);

    // get the max range of wavelength and convert to int

    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	  bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
      const specex::PSF_Params & params_of_bundle = bundle_it->second;
      for(int p=0;p<nparams;p++) {
	LEGWMIN=min(LEGWMIN,int(floor(params_of_bundle.AllParPolXW[p]->ymin)));
	LEGWMAX=max(LEGWMAX,int(floor(params_of_bundle.AllParPolXW[p]->ymax)));
      }
    }

    double wavemin = double(LEGWMIN);
    double wavemax = double(LEGWMAX);
    {
      double wavestep = (wavemax-wavemin)/(ncoeff-1);
      for(int w=0;w<ncoeff;w++) {
	wave[w]   = wavemin + wavestep*w;
      }
    }

    bool need_to_add_first_gh = true;

    harp::vector_double coeff(ncoeff*NFIBERS);

    // first deal with X and Y
    {
      harp::vector_double coeff_y(ncoeff*NFIBERS);
      harp::vector_double values_y(ncoeff);
      coeff.clear();
      coeff_y.clear();
      int fiber_index=0;

      for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	  bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
	const specex::PSF_Params & params_of_bundle = bundle_it->second;
	for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++,fiber_index++) {

	  const specex::Trace &trace = psf.FiberTraces.find(fiber)->second;
	  specex::Legendre1DPol pol1d_x(ncoeff-1,wavemin,wavemax);
	  specex::Legendre1DPol pol1d_y(ncoeff-1,wavemin,wavemax);
	  for(int w=0;w<ncoeff;w++) {
	    values[w]   = trace.X_vs_W.Value(wave[w]);
	    values_y[w] = trace.Y_vs_W.Value(wave[w]);
	  }
	  pol1d_x.Fit(wave,values,0,false);
	  pol1d_y.Fit(wave,values_y,0,false);
	  for(int w = 0; w < ncoeff ; w++) {
	    coeff(fiber*ncoeff+w)   =  pol1d_x.coeff(w);
	    coeff_y(fiber*ncoeff+w) =  pol1d_y.coeff(w);
	  }

	} // end of loop on fiber
      } // end of loop on bundles

      AddRow1(table,"X",LEGWMIN,LEGWMAX,coeff);
      AddRow1(table,"Y",LEGWMIN,LEGWMAX,coeff_y);

    } // end of X Y context

    for(int p=0;p<nparams;p++) {

      coeff.clear();

      string pname = psf.ParamName(p);

      if(need_to_add_first_gh && pname.find("GH-")<pname.npos) { // insert now GH param 0

	for(int fiber=0;fiber<NFIBERS;fiber++)
	  coeff(fiber*ncoeff)=1;
	AddRow1(table,"GH-0-0",LEGWMIN,LEGWMAX,coeff);
	need_to_add_first_gh = false;
      }

      int fiber_index=0;
      for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	  bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
	const specex::PSF_Params & params_of_bundle = bundle_it->second;

	const specex::Pol_p pol2d = params_of_bundle.AllParPolXW[p]; // this is the 2D polynomiald of x_ccd and wave for this param and bundle

	for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++,fiber_index++) {

	  const specex::Trace& trace = psf.FiberTraces.find(fiber)->second; // X_vs_W.Value();

	  // build a Legendre1DPol out of the Legendre2DPol
	  specex::Legendre1DPol pol1d(ncoeff-1,wavemin,wavemax);
	  for(int w=0;w<ncoeff;w++) {
	    values[w] = pol2d->Value(trace.X_vs_W.Value(wave[w]),wave[w]);
	  }
	  pol1d.Fit(wave,values,0,false);

	  // now copy parameters;

	  for(int w = 0; w < ncoeff ; w++) {
	    coeff(fiber*ncoeff+w) = pol1d.coeff(w); // this is the definition of the ordering, (wave,fiber)
	  }

	} // end of loop on fibers of bundle

      } // end of loop on bundles

      AddRow1(table,pname,LEGWMIN,LEGWMAX,coeff);

    } // end of loop on params

#ifdef CONTINUUM
    {
      coeff.clear();
      int fiber_index=0;
      for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	  bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
	const specex::PSF_Params & params_of_bundle = bundle_it->second;
	specex::Legendre1DPol pol1d(ncoeff-1,wavemin,wavemax);
	for(int w=0;w<ncoeff;w++) {
	  values[w]   = params_of_bundle.ContinuumPol.Value(wave[w]);
	}
	pol1d.Fit(wave,values,0,false);
	for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++,fiber_index++) {
	  for(int w = 0; w < ncoeff ; w++) {
	    coeff(fiber*ncoeff+w)   =  pol1d.coeff(w);
	  }
	}
      }
      AddRow1(table,"CONT",LEGWMIN,LEGWMAX,coeff);
    }
#endif

  }

  // write table
  table.Write(fp);

  // write keywords
  {

    fits_write_comment(fp,"------------------------------------------------------------------------",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF generated by specex, https://github.com/julienguy/specex",&status); harp::fits::check ( status );

    {
      char date_comment[80];
      time_t t = time(0);   // get time now
      struct tm * now = localtime( & t );
      sprintf(date_comment,"PSF fit date %04d-%02d-%02d",(now->tm_year + 1900),(now->tm_mon + 1),now->tm_mday);
      fits_write_comment(fp,date_comment,&status); harp::fits::check ( status );
    }
    ///////////////////////xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx////////
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"Each row of the table contains the data vector of one PSF parameter",&status); harp::fits::check ( status );
    fits_write_comment(fp,"The size of the vector is ((FIBERMAX-FIBERMIN+1)*(LEGDEG+1))",&status); harp::fits::check ( status );
    fits_write_comment(fp,"Description of  the NPARAMS parameters : ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"X        : CCD column coordinate (as a function of fiber and wavelength)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"Y        : CCD row coordinate (as a function of fiber and wavelength)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"         (X,Y)=(0,0) means that PSF is centered on center of first pixel",&status); harp::fits::check ( status );
    fits_write_comment(fp,"GHSIGX   : Sigma of first Gaussian along CCD columns for PSF core",&status); harp::fits::check ( status );
    fits_write_comment(fp,"GHSIGY   : Sigma of first Gaussian along CCD rows for PSF core",&status); harp::fits::check ( status );
    fits_write_comment(fp,"GH-i-j   : Hermite pol. coefficents, i along columns, j along rows,",&status); harp::fits::check ( status );
    fits_write_comment(fp,"         i is integer from 0 to GHDEGX, j is integer from 0 to GHDEGY,",&status); harp::fits::check ( status );
    fits_write_comment(fp,"         there are (GHDEGX+1)*(GHDEGY+1) such coefficents.",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILAMP  : Amplitude of PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILCORE : Size in pixels of PSF tail saturation in PSF core",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILXSCA : Scaling apply to CCD coordinate along columns for PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILYSCA : Scaling apply to CCD coordinate along rows for PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILINDE : Asymptotic power law index of PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"CONT     : Continuum flux in arc image (not part of PSF)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_core(X,Y) = [ SUM_ij (GH-i-j)*HERM(i,X/GHSIGX)*HERM(j,Y/GHSIGX) ]",&status); harp::fits::check ( status );
    fits_write_comment(fp,"                                       *GAUS(X,GHSIGX)*GAUS(Y,GHSIGY)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_tail(X,Y) = TAILAMP*R^2/(TAILCORE^2+R^2)^(1+TAILINDE/2)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"                with R^2=(X/TAILXSCA)^2+(Y/TAILYSCA)^2",&status); harp::fits::check ( status );
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_core is integrated in pixel",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_tail is not, it is evaluated at center of pixel",&status); harp::fits::check ( status );
    fits_write_comment(fp,"------------------------------------------------------------------------",&status); harp::fits::check ( status );
    harp::fits::key_write(fp,"EXTNAME","PSF","");
    harp::fits::key_write(fp,"PSFTYPE","GAUSS-HERMITE","");
    harp::fits::key_write(fp,"PSFVER",PSFVER,"");

    harp::fits::key_write(fp,"MJD",(long long int)psf.mjd,"MJD of arc lamp exposure");
    harp::fits::key_write(fp,"PLATEID",psf.plate_id,"plate ID of arc lamp exposure");
    harp::fits::key_write(fp,"CAMERA",psf.camera_id,"camera ID");
    harp::fits::key_write(fp,"ARCEXP",psf.arc_exposure_id,"ID of arc lamp exposure used to fit PSF");

    harp::fits::key_write(fp,"NPIX_X",(long long int)NPIX_X,"number of columns in input CCD image");
    harp::fits::key_write(fp,"NPIX_Y",(long long int)NPIX_Y,"number of rows in input CCD image");
    harp::fits::key_write(fp,"HSIZEX",(long long int)psf.hSizeX,"Half size of PSF in fit, NX=2*HSIZEX+1");
    harp::fits::key_write(fp,"HSIZEY",(long long int)psf.hSizeY,"Half size of PSF in fit, NY=2*HSIZEY+1");
    harp::fits::key_write(fp,"BUNDLMIN",(long long int)BUNDLMIN,"first bundle of fibers (starting at 0)");
    harp::fits::key_write(fp,"BUNDLMAX",(long long int)BUNDLMAX,"last bundle of fibers (included)");
    harp::fits::key_write(fp,"FIBERMIN",(long long int)FIBERMIN,"first fiber (starting at 0)");
    harp::fits::key_write(fp,"FIBERMAX",(long long int)FIBERMAX,"last fiber (included)");
    harp::fits::key_write(fp,"NPARAMS",(long long int)nparams_all,"number of PSF parameters");
    harp::fits::key_write(fp,"LEGDEG",(long long int)(ncoeff-1),"degree of Legendre pol.(wave) for parameters");
    harp::fits::key_write(fp,"PALEGDEG",(long long int)(ncoeff_param_fit-1),"fitted degree of Legendre pol. for parameters");
    harp::fits::key_write(fp,"TXLEGDEG",(long long int)(ncoeff_xtrace_fit-1),"fitted degree of Legendre pol. for trace along x");
    harp::fits::key_write(fp,"TYLEGDEG",(long long int)(ncoeff_ytrace_fit-1),"fitted degree of Legendre pol. for trace along y");
    harp::fits::key_write(fp,"GHDEGX",(long long int)GHDEGX,"degree of Hermite polynomial along CCD columns");
    harp::fits::key_write(fp,"GHDEGY",(long long int)GHDEGY,"degree of Hermite polynomial along CCD rows");

    // add chi2
    harp::fits::key_write(fp,"PSFERROR",psf.psf_error,"assumed PSF fractional error in chi2");
    harp::fits::key_write(fp,"READNOIS",psf.readout_noise,"assumed read out noise in chi2");
    harp::fits::key_write(fp,"GAIN",psf.gain,"assumed gain in chi2");

    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {

      const specex::PSF_Params & params_of_bundle = bundle_it->second;

      int ndf = params_of_bundle.ndata - params_of_bundle.nparams;
      double chi2pdf = 0;
      if(ndf>0) chi2pdf = params_of_bundle.chi2/ndf;

      char key[20];
      char comment[800];

      sprintf(key,"B%02dRCHI2",params_of_bundle.bundle_id);
      sprintf(comment,"best fit chi2/ndf for fiber bundle %d",params_of_bundle.bundle_id);
      harp::fits::key_write(fp,key,chi2pdf,comment);

      sprintf(key,"B%02dNDATA",params_of_bundle.bundle_id);
      sprintf(comment,"number of pixels in fit for fiber bundle %d",params_of_bundle.bundle_id);
      harp::fits::key_write(fp,key,(long long int)params_of_bundle.ndata,comment);

      sprintf(key,"B%02dNPAR",params_of_bundle.bundle_id);
      sprintf(comment,"number of parameters in fit for fiber bundle %d",params_of_bundle.bundle_id);
      harp::fits::key_write(fp,key,(long long int)params_of_bundle.nparams,comment);

    }

    // writing only few key words in the previous hdu
    fits_movrel_hdu ( fp, -1, NULL, &status ); harp::fits::check ( status );
    harp::fits::key_write(fp,"PSFTYPE","GAUSS-HERMITE","");
    harp::fits::key_write(fp,"PSFVER",PSFVER,"");
    fits_write_comment(fp,"PSF generated by specex, https://github.com/julienguy/specex",&status); harp::fits::check ( status );
    fits_write_comment(fp,"Data are in the form a binary table in the next hdu",&status); harp::fits::check ( status );

  } // end of write key words

  if( spots != NULL ) { // write spots

    fits_movrel_hdu ( fp, 1, NULL, &status ); harp::fits::check ( status );

    specex::FitsTable table;
    { // column description
      table.AddColumnDescription("X","D","","");
      table.AddColumnDescription("Y","D","","");
      table.AddColumnDescription("FLUX","D","","");
      table.AddColumnDescription("EFLUX","D","","");
      table.AddColumnDescription("WAVE","D","","");
      table.AddColumnDescription("FIBER","D","","");
      table.AddColumnDescription("BUNDLE","D","","");
      table.AddColumnDescription("CHI2","D","","");
      table.AddColumnDescription("STATUS","D","","");
    }

    for(size_t s=0;s<spots->size();s++) {
      const specex::Spot_p spot=(*spots)[s];
      std::vector<specex::FitsTableEntry> row;
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->xc; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->yc; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->flux; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->eflux; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->wavelength; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->fiber; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->fiber_bundle; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->chi2; row.push_back(entry);}
      {specex::FitsTableEntry entry; entry.double_vals.resize(1); entry.double_vals[0] = spot->status; row.push_back(entry);}
      table.data.push_back(row);
    }
    table.Write(fp);

    harp::fits::key_write(fp,"EXTNAME","SPOTS","");
    fits_write_comment(fp,"Fitted spots properties",&status); harp::fits::check ( status );

  }
} // end of routine

void read_gauss_hermite_psf_fits_version_2(specex::PSF_p& psf, fitsfile* fp, int hdu, int first_bundle, int last_bundle) {

  SPECEX_DEBUG("read_gauss_hermite_psf_fits_version_2 hdu=" << hdu << " first,last bundle=" << first_bundle << "," << last_bundle);

  int status = 0;
  fits_movabs_hdu ( fp, hdu, NULL, &status ); harp::fits::check ( status );

  int GHDEGX;  harp::fits::key_read(fp,"GHDEGX",GHDEGX);
  int GHDEGY;  harp::fits::key_read(fp,"GHDEGY",GHDEGY);
  if(GHDEGX != GHDEGY) {
    SPECEX_ERROR("expect GHDEGX=GHDEGY");
  }

  // psf is now already created
  // psf = specex::PSF_p(new specex::GaussHermitePSF(GHDEGX));
  ((specex::GaussHermitePSF*)(&(*psf)))->SetDegree(GHDEGX);

  harp::fits::key_read(fp,"MJD",psf->mjd);
  harp::fits::key_read(fp,"PLATEID",psf->plate_id);
  harp::fits::key_read(fp,"CAMERA",psf->camera_id);
  harp::fits::key_read(fp,"ARCEXP",psf->arc_exposure_id);

  int nx; harp::fits::key_read(fp,"NPIX_X",nx);
  psf->ccd_image_n_cols = nx;
  int ny; harp::fits::key_read(fp,"NPIX_Y",ny);
  psf->ccd_image_n_rows = ny;
  harp::fits::key_read(fp,"HSIZEX",psf->hSizeX);
  harp::fits::key_read(fp,"HSIZEY",psf->hSizeY);

  int bundlemin; harp::fits::key_read(fp,"BUNDLMIN",bundlemin);
  int bundlemax; harp::fits::key_read(fp,"BUNDLMAX",bundlemax);
  int fibermin; harp::fits::key_read(fp,"FIBERMIN",fibermin);
  int fibermax; harp::fits::key_read(fp,"FIBERMAX",fibermax);
  int nparams; harp::fits::key_read(fp,"NPARAMS",nparams);
  int legdeg; harp::fits::key_read(fp,"LEGDEG",legdeg);
  int legdeg_param_fit=0;
  int legdeg_xtrace_fit=0;
  int legdeg_ytrace_fit=0;
  try {
    harp::fits::key_read(fp,"PALEGDEG",legdeg_param_fit);
    harp::fits::key_read(fp,"TXLEGDEG",legdeg_xtrace_fit);
    harp::fits::key_read(fp,"TYLEGDEG",legdeg_ytrace_fit);
  } catch(...) {
    SPECEX_DEBUG("No key PALEGDEG,TXLEGDEG or TYLEGDEG");
  }
  if(legdeg_param_fit>0) {
    SPECEX_DEBUG("Will use fitted legendre degrees");
  }else{
    legdeg_param_fit=legdeg-1;
    legdeg_xtrace_fit=legdeg-1;
    legdeg_ytrace_fit=legdeg-1;
  }

  SPECEX_DEBUG("legdeg_param_fit  = " << legdeg_param_fit);
  SPECEX_DEBUG("legdeg_xtrace_fit = " << legdeg_xtrace_fit);
  SPECEX_DEBUG("legdeg_ytrace_fit = " << legdeg_ytrace_fit);

  specex::FitsTable table;
  table.Read(fp);
  //
  int param_col = table.columns["PARAM"].col;
  int wmin_col  = table.columns["WAVEMIN"].col;
  int wmax_col  = table.columns["WAVEMAX"].col;
  int coeff_col = table.columns["COEFF"].col;
  std::vector<std::string> params;
  std::map<std::string,int> param_row;
  std::map<std::string,double> param_wavemin;
  std::map<std::string,double> param_wavemax;
  std::map<std::string,harp::vector_double > param_coeff;
  for(int i=0;i<table.data.size();i++) {
    std::string pname=table.data[i][param_col].string_val;
    boost::trim(pname);
    SPECEX_DEBUG("read_gauss_hermite_psf " << i << " '" << pname << "'");
    params.push_back(pname);
    param_row[pname]=i;
    param_wavemin[pname]=table.data[i][wmin_col].double_vals[0];
    param_wavemax[pname]=table.data[i][wmax_col].double_vals[0];
    param_coeff[pname]=table.data[i][coeff_col].double_vals;
  }

  if(first_bundle>=0 && last_bundle>=first_bundle && (bundlemin<first_bundle || bundlemax>last_bundle)) {
    SPECEX_DEBUG("Truncating input PSF bundles [" << bundlemin << "," << bundlemax << "] -> ["
		 << first_bundle << "," << last_bundle << "]");
    int original_nfibers  = (fibermax-fibermin)+1;
    int original_nbundles = (bundlemax-bundlemin)+1;
    int nfibers_per_bundle = original_nfibers/original_nbundles; // not safe ...
    int new_fiber_begin = first_bundle*nfibers_per_bundle;
    int new_fiber_end   = (last_bundle+1)*nfibers_per_bundle;
    for(size_t s=0;s<params.size();s++) {
      string pname=params[s];
      //SPECEX_DEBUG("Truncated parameter " << pname);
      int ncoef_per_fiber = param_coeff[pname].size()/original_nfibers;
      param_coeff[pname] = ublas::project(param_coeff[pname],ublas::range(new_fiber_begin*ncoef_per_fiber,new_fiber_end*ncoef_per_fiber));
    }
    fibermin=new_fiber_begin;
    fibermax=new_fiber_end-1;
    bundlemin=first_bundle;
    bundlemax=last_bundle;
    SPECEX_DEBUG("Truncated FIBERMIN " << fibermin);
    SPECEX_DEBUG("Truncated FIBERMAX " << fibermax);
    SPECEX_DEBUG("Truncated BUNDLEMIN " << bundlemin);
    SPECEX_DEBUG("Truncated BUNDLEMAX " << bundlemax);
  }

  SPECEX_DEBUG("Reading and converting FiberTraces");
  // FiberTraces
  // check size makes sense
  int nfibers = fibermax-fibermin+1;
  int ncoeff  = nfibers*(legdeg+1);
  if( int(param_coeff["X"].size()) != ncoeff)
    SPECEX_ERROR("XCOEFF SIZE INCONSISTENT WITH LEGDEG AND NUMBER OF FIBERS");
  int index=0;
  for(int fiber=fibermin;fiber<=fibermax; fiber++,index++) {

    SPECEX_DEBUG("Fiber #" << fiber);

    specex::Trace trace;
    trace.fiber=fiber;
    trace.mask=0; //?
    trace.X_vs_W = specex::Legendre1DPol(legdeg_xtrace_fit,param_wavemin["X"],param_wavemax["X"]);
    trace.Y_vs_W = specex::Legendre1DPol(legdeg_ytrace_fit,param_wavemin["Y"],param_wavemax["Y"]);
    for(int i=0;i<legdeg_xtrace_fit+1;i++) {
      trace.X_vs_W.coeff[i] = param_coeff["X"][i+index*(legdeg+1)];
    }
    for(int i=0;i<legdeg_ytrace_fit+1;i++) {
      trace.Y_vs_W.coeff[i] = param_coeff["Y"][i+index*(legdeg+1)];
    }
    trace.X_vs_Y.deg  = trace.Y_vs_W.deg + 1; // add one for inversion
    trace.W_vs_Y.deg  = trace.Y_vs_W.deg + 1; // add one for inversion
    int npts=100;
    harp::vector_double w(npts);
    harp::vector_double x(npts);
    harp::vector_double y(npts);
    for(int i=0;i<npts;i++) {
      w[i]=param_wavemin["Y"]+((param_wavemax["Y"]-param_wavemin["Y"])/(npts-1))*i;
      x[i]=trace.X_vs_W.Value(w[i]);
      y[i]=trace.Y_vs_W.Value(w[i]);
    }
    trace.X_vs_Y.Fit(y,x);
    trace.W_vs_Y.Fit(y,w);
    trace.synchronized = true;
    psf->FiberTraces[fiber] = trace;
  }

  // Now that's more complicated, need to loop on bundles
  int nbundles = (bundlemax-bundlemin)+1;
  int nfibers_per_bundle = nfibers/nbundles; // not safe ...
  SPECEX_DEBUG("Number of bundles= " << nbundles);
  SPECEX_DEBUG("Number of fibers per bundle= " << nfibers_per_bundle);

  for(int bundle = bundlemin ; bundle <= bundlemax ; bundle++) {

    //SPECEX_DEBUG("Fitting pols of bundle " << bundle);

    int bundle_fibermin=bundle*nfibers_per_bundle;
    int bundle_fibermax=(bundle+1)*nfibers_per_bundle-1;
    if(bundle_fibermin<fibermin || bundle_fibermax>fibermax) {
      SPECEX_ERROR("FIBERMIN,FIBERMAX,BUNDLEMIN,BUNDLEMAX ARE INCONSISTENT");
    }
    specex::PSF_Params bundle_params;
    bundle_params.bundle_id=bundle;
    bundle_params.fiber_min=bundle_fibermin;
    bundle_params.fiber_max=bundle_fibermax;

    double xmin=100000;
    double xmax=0;
    double wavemin = param_wavemin["X"]; // or any other param
    double wavemax = param_wavemax["X"]; // or any other param

    for(int fiber=bundle_fibermin;fiber<=bundle_fibermax;fiber++) {
      const specex::Trace& trace = psf->FiberTraces[fiber];
      for(double wave=wavemin;wave<=wavemax;wave+=10) {
	double x = trace.X_vs_W.Value(wave);
	xmin=min(xmin,x);
	xmax=max(xmax,x);
      }
    }
    SPECEX_DEBUG("bundle=" << bundle << " xmin xmax : " << xmin << " " << xmax);
#ifdef CONTINUUM
    // dealing with continuum
    bundle_params.ContinuumPol = specex::Legendre1DPol(legdeg,param_wavemin["CONT"],param_wavemax["CONT"]);
    for(int i=0;i<legdeg+1;i++) {
      bundle_params.ContinuumPol.coeff[i] = param_coeff["CONT"][i+(bundle_params.fiber_min-fibermin)*(legdeg+1)];
    }
#endif

    // need to set : bundle_params.AllParPolXW  and bundle_params.FitParPolXW
    for(int i=0;i<(int)params.size();i++) {
      const string& pname=params[i];

      SPECEX_DEBUG("Fitting pol of parameter " << pname << " in bundle " << bundle);

      if(pname=="X") continue; // trace
      if(pname=="Y") continue; // trace
      if(pname=="GH-0-0") continue; // not used in c++ version
      if(pname=="CONT") continue; // not a local param
      // now we need to fit a 2D legendre polynomial of X and wave
      // for the subset of fibers of this bundle

      // NOTE : we remove 1 deg along wave, because 1 was added in the c++/xml -> fits conversion X->fiber
      specex::Pol_p pol(new specex::Pol(nfibers_per_bundle-1,xmin,xmax,legdeg_param_fit,wavemin,wavemax));
      pol->name = pname;
      pol->Fill(true); // sparse or not sparse

      int npar = pol->Npar();
      harp::matrix_double A(npar,npar);
      harp::vector_double B(npar);
      A *= 0;
      B *= 0;
      for(int fiber=bundle_fibermin;fiber<=bundle_fibermax;fiber++) {
	const specex::Trace& trace = psf->FiberTraces[fiber];
	specex::Legendre1DPol fiberpol(legdeg,param_wavemin[pname],param_wavemax[pname]);
	for(int i=0;i<=legdeg;i++)
	  fiberpol.coeff[i]=param_coeff[pname][i+(fiber-fibermin)*(legdeg+1)];
	for(double wave=wavemin;wave<=wavemax;wave+=(wavemax-wavemin)/(legdeg_param_fit+2)) {
	  double x    = trace.X_vs_W.Value(wave);
	  double pval = fiberpol.Value(wave);
	  harp::vector_double der = pol->Monomials(x,wave);
	  specex::syr(1.,der,A); // A += der*der.transposed;
	  specex::axpy(pval,der,B); // B += pval*der;
	}
      }
      // now need to solve
      int status = specex::cholesky_solve(A,B);
      if(status != 0)
	SPECEX_ERROR("Failed to convert LegPol(fiber,wave) -> LegPol(x,wave) for bundle " << bundle << " and parameter " << pname);

      pol->coeff = B;
      //SPECEX_DEBUG("Fit of parameter " << pname << " in bundle " << bundle << " done");

      bundle_params.AllParPolXW.push_back(pol);

    }

    psf->ParamsOfBundles[bundle] = bundle_params;
  }

  //SPECEX_ERROR("Finish implementation");
}

ext: cc

max_stars_repo_name: marcelo-alvarez/specex

max_issues_repo_name: marcelo-alvarez/specex

max_forks_repo_name: marcelo-alvarez/specex


text:
// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef PYTHON_INTERFACE_HH
#define PYTHON_INTERFACE_HH

#include <boost/python.hpp>
#include <boost/python/type_id.hpp>

#include <functional>

namespace std
{
    template<>
    struct hash<boost::python::object>
    {
        size_t operator()(const boost::python::object& o) const
        {
            return boost::python::extract<size_t>(o.attr("__hash__")());
        }
    };
}

namespace boost
{
    size_t hash_value(const boost::python::object& o);
}

#include <boost/graph/graph_traits.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include <type_traits>

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_selectors.hh"
#include "numpy_bind.hh"

// this file includes a simple python interface for the internally kept
// graph. It defines a PythonVertex, PythonEdge and PythonIterator template
// classes, which contain the proper member functions for graph traversal. These
// types are then specialized for each version of the adapted graph (directed,
// undirected, filtered, reversed).

namespace graph_tool
{

// generic iterator adaptor which can be used to iterate vertices, edges,
// out_edges and in_edges through python
template <class Descriptor, class Iterator>
class PythonIterator
{
public:
    PythonIterator(const boost::python::object& g,
                   std::pair<Iterator,Iterator> e)
        : _wg(g), _g(g()), _e(e) {}
    Descriptor Next()
    {
        if (_e.first == _e.second)
            boost::python::objects::stop_iteration_error();
        Descriptor e(_wg, *_e.first);
        ++_e.first;
        return e;
    }
private:
    boost::python::object _wg;
    boost::python::object _g;
    std::pair<Iterator,Iterator> _e;
};

// forward declaration of PythonEdge
template <class Graph>
class PythonEdge;

// below are classes related to the PythonVertex type
class PythonVertex
{
public:
    PythonVertex(const boost::python::object& g, GraphInterface::vertex_t v):
        _g(g), _v(v), _valid(true)
    {}

    bool IsValid() const
    {
        if (_g().ptr() == Py_None)
            return false;
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        return _valid &&
            (_v != boost::graph_traits<GraphInterface::multigraph_t>::null_vertex()) &&
            (_v < num_vertices(*gi._mg));
    }

    void SetValid(bool valid)
    {
        _valid = valid;
    }

    void CheckValid() const
    {
        if (!IsValid())
            throw ValueException("invalid vertex descriptor: " +
                                 boost::lexical_cast<string>(_v));
    }

    boost::python::object GetGraph() const
    {
        return _g();
    }

    GraphInterface::vertex_t GetDescriptor() const
    {
        return _v;
    }

    template <class DegSelector>
    struct get_degree
    {
        template<class Graph>
        void operator()(const Graph& g,
                        typename boost::graph_traits<Graph>::vertex_descriptor v,
                        size_t& deg) const
        {
            deg = DegSelector()(v, g);
        }

        template<class Graph, class PMap>
        void operator()(const Graph& g,
                        typename boost::graph_traits<Graph>::vertex_descriptor v,
                        const PMap& weight,
                        boost::python::object& deg) const
        {
            deg = boost::python::object(DegSelector()(v, g, weight));
        }
    };

    size_t GetInDegree() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        size_t in_deg;
        run_action<>()(gi, std::bind(get_degree<in_degreeS>(),
                                     std::placeholders::_1, _v,
                                     std::ref(in_deg)))();
        return in_deg;
    }

    boost::python::object GetWeightedInDegree(boost::any pmap) const
    {
        if (!belongs<edge_scalar_properties>()(pmap))
            throw ValueException("edge weight property must be of scalar type");
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        boost::python::object in_deg;
        run_action<>()(gi, std::bind(get_degree<in_degreeS>(),
                                     std::placeholders::_1, _v,
                                     std::placeholders::_2,
                                     std::ref(in_deg)),
                       edge_scalar_properties())(pmap);
        return in_deg;
    }

    size_t GetOutDegree() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        size_t out_deg;
        run_action<>()(gi, std::bind(get_degree<out_degreeS>(),
                                     std::placeholders::_1, _v,
                                     std::ref(out_deg)))();
        return out_deg;
    }

    boost::python::object GetWeightedOutDegree(boost::any pmap) const
    {
        if (!belongs<edge_scalar_properties>()(pmap))
            throw ValueException("edge weight property must be of scalar type");
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        boost::python::object out_deg;
        run_action<>()(gi, std::bind(get_degree<out_degreeS>(),
                                     std::placeholders::_1, _v,
                                     std::placeholders::_2,
                                     std::ref(out_deg)),
                       edge_scalar_properties())(pmap);
        return out_deg;
    }

    // provide iterator support for out_edges

    struct get_out_edges
    {
        template<class Graph>
        void operator()(const Graph& g, const boost::python::object& pg,
                        typename boost::graph_traits<Graph>::vertex_descriptor v,
                        boost::python::object& iter) const
        {
            typedef typename boost::graph_traits<Graph>::out_edge_iterator
                out_edge_iterator;
            iter = boost::python::object(PythonIterator<PythonEdge<Graph>,
                                                 out_edge_iterator>
                                  (pg, out_edges(v, g)));
        }
    };

    boost::python::object OutEdges() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        boost::python::object iter;
        run_action<>()(gi, std::bind(get_out_edges(), std::placeholders::_1,
                                     std::ref(_g), _v, std::ref(iter)))();
        return iter;
    }

    struct get_in_edges
    {
        template<class Graph>
        void operator()(const Graph& g, const boost::python::object& pg,
                        typename boost::graph_traits<Graph>::vertex_descriptor v,
                        boost::python::object& iter) const
        {
            typedef typename in_edge_iteratorS<Graph>::type
                in_edge_iterator;
            iter = boost::python::object
                (PythonIterator<PythonEdge<Graph>,in_edge_iterator>
                 (pg, in_edge_iteratorS<Graph>::get_edges(v, g)));
        }
    };

    boost::python::object InEdges() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        boost::python::object iter;
        run_action<>()(gi, std::bind(get_in_edges(), placeholders::_1,
                                     std::ref(_g), _v,
                                     std::ref(iter)))();
        return iter;
    }

    std::string GetString() const
    {
        CheckValid();
        return boost::lexical_cast<std::string>(_v);
    }

    size_t GetHash() const
    {
        return std::hash<size_t>()(_v);
    }

    size_t GetIndex() const
    {
        return _v;
    }

private:
    boost::python::object _g;
    GraphInterface::vertex_t _v;
    bool _valid;
};

// below are classes related to the PythonEdge type

class EdgeBase {}; // useful to unite all edge types

template <class Graph>
class PythonEdge : public EdgeBase
{
public:
    typedef typename boost::graph_traits<Graph>::edge_descriptor edge_descriptor;
    PythonEdge(const boost::python::object& g, edge_descriptor e)
        : _g(g), _e(e), _valid(true)
    {
    }

    bool IsValid() const
    {
        boost::python::object g = _g();
        if (g.ptr() == Py_None || !_valid)
            return false;
        GraphInterface& gi = boost::python::extract<GraphInterface&>(g.attr("_Graph__graph"));
        GraphInterface::edge_t e(_e);

        typename GraphInterface::multigraph_t::vertex_t s, t;
        s = source(e, *gi._mg);
        t = target(e, *gi._mg);

        bool valid = ((s != boost::graph_traits<GraphInterface::multigraph_t>::null_vertex()) &&
                      (s < num_vertices(*gi._mg)) &&
                      (t != boost::graph_traits<GraphInterface::multigraph_t>::null_vertex()) &&
                      (t < num_vertices(*gi._mg)) &&
                      gi.GetEdgeIndex()[e] <= gi.GetMaxEdgeIndex());

        return valid;
    }

    void SetValid(bool valid)
    {
        _valid = valid;
    }

    void CheckValid() const
    {
        if (!IsValid())
            throw ValueException("invalid edge descriptor");
    }

    boost::python::object GetGraph() const
    {
        return _g();
    }

    GraphInterface::edge_t GetDescriptor() const
    {
        return _e;
    }

    struct get_source
    {
        template<class GraphType>
        void operator()(const GraphType& g, const boost::python::object& pg,
                        const edge_descriptor& edge, boost::python::object& vertex)
            const
        {
            typedef typename boost::graph_traits<GraphType>::edge_descriptor edge_t;
            vertex = boost::python::object(PythonVertex(pg, source(edge_t(edge), g)));
        }
    };

    boost::python::object GetSource() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        boost::python::object v;
        run_action<>()(gi, std::bind(get_source(), std::placeholders::_1,
                                     std::ref(_g), std::ref(_e),
                                     std::ref(v)))();
        return v;
    }

    struct get_target
    {
        template<class GraphType>
        void operator()(const GraphType& g, const boost::python::object& pg,
                        const edge_descriptor& edge, boost::python::object& vertex)
            const
        {
            typedef typename boost::graph_traits<GraphType>::edge_descriptor edge_t;
            vertex = boost::python::object(PythonVertex(pg, target(edge_t(edge), g)));
        }
    };

    boost::python::object GetTarget() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        boost::python::object v;
        run_action<>()(gi, std::bind(get_target(), std::placeholders::_1,
                                     std::ref(_g), std::ref(_e), std::ref(v)))();
        return v;
    }

    std::string GetString() const
    {
        PythonVertex src = boost::python::extract<PythonVertex>(GetSource());
        PythonVertex tgt = boost::python::extract<PythonVertex>(GetTarget());
        return "(" + src.GetString() + ", " + tgt.GetString() + ")";
    }

    size_t GetHash() const
    {
        CheckValid();
        GraphInterface& gi = boost::python::extract<GraphInterface&>(_g().attr("_Graph__graph"));
        return std::hash<size_t>()(gi._edge_index[_e]);
    }

private:
    boost::python::object _g;
    edge_descriptor _e;
    bool _valid;
};

// metafunction to determine wether or not to return copies or internal
// references to property types
struct return_reference
{
    template <class ValueType>
    struct apply
    {
        // return actual references only for non-string and non-python::object
        // classes

        typedef typename boost::mpl::if_<
            typename boost::mpl::and_<
                std::is_class<ValueType>,
                typename boost::mpl::and_<
                    typename boost::mpl::not_<std::is_same<ValueType,
                                                           string> >::type,
                    typename boost::mpl::not_<std::is_same<ValueType,
                                                           boost::python::object> >::type>::type
                >::type,
            boost::mpl::bool_<true>,
            boost::mpl::bool_<false> >::type type;
    };
};

template <class PropertyMap>
class PythonPropertyMap
{
public:
    PythonPropertyMap(const PropertyMap& pmap)
        : _pmap(pmap) {}

    typedef typename boost::property_traits<PropertyMap>::value_type value_type;

    typedef typename boost::mpl::if_<
        typename return_reference::apply<value_type>::type,
        value_type&,
        value_type>::type reference;

    template <class PythonDescriptor>
    reference GetValue(const PythonDescriptor& key)
    {
        key.CheckValid();
        return get(_pmap, key.GetDescriptor());
    }

    // in this case, val should be a copy, not a reference. This is to avoid a
    // problem with vector-valued property maps
    template <class PythonDescriptor>
    void SetValue(const PythonDescriptor& key, value_type val)
    {
        set_value(key, val,
                  std::is_convertible<typename boost::property_traits<PropertyMap>::category,
                                      boost::writable_property_map_tag>());
    }

    template <class PythonDescriptor>
    void set_value(const PythonDescriptor& key, const value_type& val,
                   std::true_type)
    {
        key.CheckValid();
        put(_pmap, key.GetDescriptor(), val);
    }

    template <class PythonDescriptor>
    void set_value(const PythonDescriptor&, const value_type&,
                   std::false_type)
    {
        throw ValueException("property is read-only");
    }

    size_t GetHash() const
    {
        return std::hash<size_t>()(size_t(this));
    }

    std::string GetType() const
    {
        using boost::python::detail::gcc_demangle;
        if (std::is_same<typename boost::mpl::find<value_types,value_type>::type,
                         typename boost::mpl::end<value_types>::type>::value)
            return gcc_demangle(typeid(value_type).name());
        else
            return type_names[boost::mpl::find<value_types,
                                        value_type>::type::pos::value];
    }

    boost::any GetMap() const
    {
        return _pmap;
    }

    boost::any GetDynamicMap() const
    {
        return (boost::dynamic_property_map*)
            (new boost::detail::dynamic_property_map_adaptor<PropertyMap>
             (_pmap));
    }

    boost::python::object GetArray(size_t size)
    {
        typedef typename boost::mpl::or_<
            typename boost::mpl::or_<
                std::is_same<PropertyMap,
                             GraphInterface::vertex_index_map_t>,
                std::is_same<PropertyMap,
                             GraphInterface::edge_index_map_t> >::type,
            typename boost::mpl::not_<
                typename boost::mpl::has_key<numpy_types, value_type>::type >
            ::type>::type isnt_vector_map;
        return get_array(size, isnt_vector_map());
    }

    boost::python::object get_array(size_t size, boost::mpl::bool_<false>)
    {
        _pmap.reserve(size);
        return wrap_vector_not_owned(_pmap.get_storage());
    }

    boost::python::object get_array(size_t, boost::mpl::bool_<true>)
    {
        return boost::python::object();
    }

    bool IsWritable() const
    {
        return std::is_convertible<typename boost::property_traits<PropertyMap>::category,
                                   boost::writable_property_map_tag>::value;
    }

private:
    PropertyMap _pmap; // hold an internal copy, since it's cheap
};

//
// Create new properties
//

struct new_property_map
{
    template <class ValueType, class IndexMap>
    void operator()(ValueType, IndexMap index, const string& type_name,
                     boost::any pmap, boost::python::object& new_prop, bool& found) const
    {
        size_t i = boost::mpl::find<value_types,ValueType>::type::pos::value;
        if (type_name == type_names[i])
        {
            typedef typename property_map_type::apply<ValueType, IndexMap>::type
                map_t;
            map_t prop;
            if (pmap.empty())
                prop = map_t(index);
            else
                prop = boost::any_cast<map_t>(pmap);

            new_prop = boost::python::object(PythonPropertyMap<map_t>(prop));
            found = true;
        }
    }
};

template <class IndexMap>
boost::python::object new_property(const string& type, IndexMap index_map,
                                   boost::any pmap)
{
    boost::python::object prop;
    bool found = false;
    boost::mpl::for_each<value_types>(std::bind(new_property_map(),
                                                std::placeholders::_1, index_map,
                                                std::ref(type), pmap, std::ref(prop),
                                                std::ref(found)));
    if (!found)
        throw ValueException("Invalid property type: " + type);
    return prop;
}

//
// Python IO streams (minimal access to c++ streams)
//

class OStream
{
public:
    OStream(std::ostream& s): _s(s) {}

    void Write(const std::string& s, size_t n)
    {
        _s.write(s.c_str(), long(n));
    }

    void Flush()
    {
        _s.flush();
    }

private:
    std::ostream& _s;
};

class IStream
{
public:
    IStream(std::istream& s): _s(s) {}

    boost::python::object Read(size_t n)
    {
        std::string buf;
        buf.resize(n);
        _s.read(&buf[0], n);
        buf.resize(_s.gcount());

#if (PY_MAJOR_VERSION >= 3)
        // in python 3 we need to construct a 'bytes' instance
        PyObject* bytes = PyBytes_FromStringAndSize(&buf[0], buf.size());
        boost::python::handle<> x(bytes);
        boost::python::object pbuf(x);
#else
        boost::python::str pbuf(buf);
#endif
        return pbuf;
    }

private:
    std::istream& _s;
};

} //graph_tool namespace

#endif

ext: hh

max_stars_repo_name: johankaito/fufuka

max_issues_repo_name: johankaito/fufuka

max_forks_repo_name: johankaito/fufuka


text:
// Copyright (c) 2017-2019 The Multiverse developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "mode/rpc_config.h"

#include <boost/algorithm/algorithm.hpp>
#include "mode/config_macro.h"

namespace multiverse
{
namespace po = boost::program_options;
namespace fs = boost::filesystem;
using tcp = boost::asio::ip::tcp;

/////////////////////////////////////////////////////////////////////
// CMvRPCBasicConfig

CMvRPCBasicConfig::CMvRPCBasicConfig()
{
    po::options_description desc("RPCBasic");

    CMvRPCBasicConfigOption::AddOptionsImpl(desc);

    AddOptions(desc);
}

CMvRPCBasicConfig::~CMvRPCBasicConfig() {}

bool CMvRPCBasicConfig::PostLoad()
{
    if (fHelp)
    {
        return true;
    }

    if (nRPCPortInt <= 0 || nRPCPortInt > 0xFFFF)
    {
        nRPCPort = (fTestNet ? DEFAULT_TESTNET_RPCPORT : DEFAULT_RPCPORT);
    }
    else
    {
        nRPCPort = (unsigned short)nRPCPortInt;
    }

    if (!strRPCCAFile.empty())
    {
        if (!fs::path(strRPCCAFile).is_complete())
        {
            strRPCCAFile = (pathRoot / strRPCCAFile).string();
        }
    }

    if (!strRPCCertFile.empty())
    {
        if (!fs::path(strRPCCertFile).is_complete())
        {
            strRPCCertFile = (pathRoot / strRPCCertFile).string();
        }
    }

    if (!strRPCPKFile.empty())
    {
        if (!fs::path(strRPCPKFile).is_complete())
        {
            strRPCPKFile = (pathRoot / strRPCPKFile).string();
        }
    }

    return true;
}

std::string CMvRPCBasicConfig::ListConfig() const
{
    std::ostringstream oss;
    oss << CMvRPCBasicConfigOption::ListConfigImpl();
    oss << "RPCPort: " << nRPCPort << "\n";
    return oss.str();
}

std::string CMvRPCBasicConfig::Help() const
{
    return CMvRPCBasicConfigOption::HelpImpl();
}

/////////////////////////////////////////////////////////////////////
// CMvRPCServerConfig

CMvRPCServerConfig::CMvRPCServerConfig()
{
    po::options_description desc("RPCServer");

    CMvRPCServerConfigOption::AddOptionsImpl(desc);

    AddOptions(desc);
}

CMvRPCServerConfig::~CMvRPCServerConfig() {}

bool CMvRPCServerConfig::PostLoad()
{
    if (fHelp)
    {
        return true;
    }

    CMvRPCBasicConfig::PostLoad();

    if(fRPCListen4 || (!fRPCListen4 && !fRPCListen6))
    {
        epRPC = tcp::endpoint(!vRPCAllowIP.empty()
                              ? boost::asio::ip::address_v4::any()
                              : boost::asio::ip::address_v4::loopback(),
                          nRPCPort);
    }

    if(fRPCListen6)
    {
        epRPC = tcp::endpoint(!vRPCAllowIP.empty()
                              ? boost::asio::ip::address_v6::any()
                              : boost::asio::ip::address_v6::loopback(),
                          nRPCPort);
    }

    if (nRPCThreadNumber <= 0)
    {
        nRPCThreadNumber = 1;
    }

    return true;
}

std::string CMvRPCServerConfig::ListConfig() const
{
    std::ostringstream oss;
    oss << CMvRPCServerConfigOption::ListConfigImpl();
    oss << "epRPC: " << epRPC << "\n";
    return CMvRPCBasicConfig::ListConfig() + oss.str();
}

std::string CMvRPCServerConfig::Help() const
{
    return CMvRPCBasicConfig::Help() + CMvRPCServerConfigOption::HelpImpl();
}

/////////////////////////////////////////////////////////////////////
// CMvRPCClientConfig

CMvRPCClientConfig::CMvRPCClientConfig()
{
    po::options_description desc("RPCClient");

    CMvRPCClientConfigOption::AddOptionsImpl(desc);

    AddOptions(desc);
}

CMvRPCClientConfig::~CMvRPCClientConfig() {}

bool CMvRPCClientConfig::PostLoad()
{
    if (fHelp)
    {
        return true;
    }

    CMvRPCBasicConfig::PostLoad();

    try
    {
        boost::asio::ip::address addr(boost::asio::ip::address::from_string(strRPCConnect));
        if(fRPCListen4 || (!fRPCListen4 && !fRPCListen6))
        {
            if(addr.is_loopback())
            {
                strRPCConnect = boost::asio::ip::address_v4::loopback().to_string();
            }
        }

        if(fRPCListen6)
        {
            if(addr.is_loopback())
            {
                strRPCConnect = boost::asio::ip::address_v6::loopback().to_string();
            }
        }
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << std::endl;
        return false;
    }

    if (nRPCConnectTimeout == 0)
    {
        nRPCConnectTimeout = 1;
    }

    return true;
}

std::string CMvRPCClientConfig::ListConfig() const
{
    std::ostringstream oss;
    oss << CMvRPCClientConfigOption::ListConfigImpl();
    return CMvRPCBasicConfig::ListConfig() + oss.str();
}

std::string CMvRPCClientConfig::Help() const
{
    return CMvRPCBasicConfig::Help() + CMvRPCClientConfigOption::HelpImpl();
}

}  // namespace multiverse

max_stars_repo_name: FissionAndFusion/FnFnMvWallet-Pre

max_issues_repo_name: FissionAndFusion/FnFnMvWallet-Pre

max_forks_repo_name: FissionAndFusion/FnFnMvWallet-Pre


text: #include <boost/interprocess/windows_shared_memory.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino