#ifndef BOOST_SAFE_FLOAT_HPP
#define BOOST_SAFE_FLOAT_HPP

#include <iostream>

#include <boost/safe_float/convenience.hpp>
#include <boost/safe_float/policy/on_fail_throw.hpp>

namespace boost
{
namespace safe_float
{
template<class FP, template<class T> class CHECK = policy::check_all, class ERROR_HANDLING = policy::on_fail_throw,
         template<class T> class CAST = policy::cast_from_primitive::same>
class safe_float : private CHECK<FP>, ERROR_HANDLING
{
    FP number;

    using pol = CHECK<FP>;
    using traits = policy::policy_traits<FP, CHECK<FP>>;
    pol& policy() noexcept { return static_cast<pol&>(*this); }
    ERROR_HANDLING& handler() noexcept { return static_cast<ERROR_HANDLING&>(*this); }

public:

    using value_type = FP;
    using check_policy = pol;
    using report_policy = ERROR_HANDLING;
    using cast_policy = CAST<safe_float>;

    static_assert(std::is_floating_point<FP>::value,
                  "First template parameter in safe_float has to be floating point data type");

    safe_float() : safe_float((FP)0.0f) {}

    // Explicit constructor for FP in case the cast policy doesn't allow construction already
    template<
        typename OtherFP,
        std::enable_if_t<
            std::is_same_v<
                FP,
                OtherFP> && !CAST<safe_float>::template can_explicitly_cast_from<OtherFP> && !CAST<safe_float>::template can_cast_from<OtherFP>,
            int> = 0>
    explicit safe_float(OtherFP f) : number{f}
    {}

    // Explicit constructors available through the cast policy
    template<typename T, std::enable_if_t<CAST<safe_float>::template can_explicitly_cast_from<T>, int> = 0>
    explicit safe_float(T source)
    {
        policy::cast_helper<FP, CAST<safe_float>>::template construct_explicitly(number, source);
    }

    // Implicit constructors available through the cast policy
    template<typename T, std::enable_if_t<CAST<safe_float>::template can_cast_from<T>, int> = 0>
    safe_float(T source)
    {
        policy::cast_helper<FP, CAST<safe_float>>::template construct_implicitly(number, source);
    }

    // Conversion operators
    template<typename T, std::enable_if_t<CAST<safe_float>::template can_cast_to<T>, int> = 0>
    operator T () {
        return policy::cast_helper<FP, CAST<safe_float>>::template convert_implicitly<T>(number);
    }

    template<typename T, std::enable_if_t<CAST<safe_float>::template can_explicitly_cast_to<T>, int> = 0>
    explicit operator T () {
        return policy::cast_helper<FP, CAST<safe_float>>::template convert_explicitly<T>(number);
    }

    // Access to internal representation
    FP get_stored_value() const { return number; }
    void set_stored_value(FP f) { number = f; }

    // unary arithmetic operators implementation
    safe_float<FP, CHECK, ERROR_HANDLING, CAST>& operator+=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
    {
        traits::report_pre_addition(policy(), number, rhs.number, handler()); // early error detection
        number += rhs.number;
        traits::report_post_addition(policy(), number, handler());
        return *this;
    }

    safe_float<FP, CHECK, ERROR_HANDLING, CAST>& operator-=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
    {
        traits::report_pre_subtraction(policy(), number, rhs.number, handler()); // early error detection
        number -= rhs.number;
        traits::report_post_subtraction(policy(), number, handler());
        return *this;
    }

    safe_float<FP, CHECK, ERROR_HANDLING, CAST>& operator*=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
    {
        traits::report_pre_multiplication(policy(), number, rhs.number, handler()); // early error detection
        number *= rhs.number;
        traits::report_post_multiplication(policy(), number, handler());
        return *this;
    }

    safe_float<FP, CHECK, ERROR_HANDLING, CAST>& operator/=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
    {
        traits::report_pre_division(policy(), number, rhs.number, handler()); // early error detection
        number /= rhs.number;
        traits::report_post_division(policy(), number, handler());
        return *this;
    }

    // unary negative operator
    safe_float<FP, CHECK, ERROR_HANDLING, CAST> operator-() const
    {
        return safe_float<FP, CHECK, ERROR_HANDLING, CAST>(-number);
    }
};

// binary arithmetic operators
template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline safe_float<FP, CHECK, ERROR_HANDLING, CAST> operator+(safe_float<FP, CHECK, ERROR_HANDLING, CAST> lhs,
                                                             const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    lhs += rhs;
    return lhs;
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline safe_float<FP, CHECK, ERROR_HANDLING, CAST> operator-(safe_float<FP, CHECK, ERROR_HANDLING, CAST> lhs,
                                                             const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    lhs -= rhs;
    return lhs;
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline safe_float<FP, CHECK, ERROR_HANDLING, CAST> operator*(safe_float<FP, CHECK, ERROR_HANDLING, CAST> lhs,
                                                             const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    lhs *= rhs;
    return lhs;
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline safe_float<FP, CHECK, ERROR_HANDLING, CAST> operator/(safe_float<FP, CHECK, ERROR_HANDLING, CAST> lhs,
                                                             const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    lhs /= rhs;
    return lhs;
}

// comparison operators
template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline bool operator<(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& lhs,
                      const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    return lhs.get_stored_value() < rhs.get_stored_value();
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline bool operator>(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& lhs,
                      const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    return rhs < lhs;
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline bool operator<=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& lhs,
                       const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    return !(lhs > rhs);
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline bool operator>=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& lhs,
                       const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    return !(lhs < rhs);
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline bool operator==(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& lhs,
                       const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    return lhs.get_stored_value() == rhs.get_stored_value();
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline bool operator!=(const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& lhs,
                       const safe_float<FP, CHECK, ERROR_HANDLING, CAST>& rhs)
{
    return !(lhs == rhs);
}

// iostream operators
template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline std::ostream& operator<<(std::ostream& out, safe_float<FP, CHECK, ERROR_HANDLING, CAST>& sf)
{
    out << sf.get_stored_value();
    return out;
}

template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
inline std::istream& operator>>(std::istream& in, safe_float<FP, CHECK, ERROR_HANDLING, CAST>& sf)
{
    FP number;
    in >> number;
    sf.set_stored_value(number);
    return in;
}

} // namespace safe_float
} // namespace boost

// Numeric limits specialization
namespace std
{
template<class FP, template<class T> class CHECK, class ERROR_HANDLING, template<class T> class CAST>
class numeric_limits<boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>>
{
    using number_type = boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>;

public:
    static constexpr bool is_specialized = true;
    static constexpr int digits = std::numeric_limits<FP>::digits;
    static constexpr int digits10 = std::numeric_limits<FP>::digits10;
    static constexpr int max_digits10 = std::numeric_limits<FP>::max_digits10;
    static constexpr bool is_signed = std::numeric_limits<FP>::is_signed;
    static constexpr bool is_integer = std::numeric_limits<FP>::is_integer;
    static constexpr int radix = std::numeric_limits<FP>::radix;
    static constexpr int min_exponent = std::numeric_limits<FP>::min_exponent;
    static constexpr int min_exponent10 = std::numeric_limits<FP>::min_exponent10;
    static constexpr int max_exponent = std::numeric_limits<FP>::max_exponent;
    static constexpr int max_exponent10 = std::numeric_limits<FP>::max_exponent10;
    static constexpr bool has_infinity = std::numeric_limits<FP>::has_infinity;
    static constexpr float_denorm_style has_denorm = std::numeric_limits<FP>::has_denorm;
    static constexpr bool has_denorm_loss = std::numeric_limits<FP>::has_denorm_loss;
    static constexpr bool is_iec559 = std::numeric_limits<FP>::is_iec559;
    static constexpr bool is_bounded = std::numeric_limits<FP>::is_bounded;
    static constexpr bool is_modulo = std::numeric_limits<FP>::is_modulo;
    static constexpr bool traps = std::numeric_limits<FP>::traps;
    static constexpr bool tinyness_before = std::numeric_limits<FP>::tinyness_before;

    static constexpr bool is_exact = std::numeric_limits<FP>::is_exact;           // TODO: check for inexact policies
    static constexpr bool has_quiet_NaN = std::numeric_limits<FP>::has_quiet_NaN; // TODO: check policies for nan
    static constexpr bool has_signaling_NaN = std::numeric_limits<FP>::has_signaling_NaN; // TODO: check policies for
                                                                                // nan
    static constexpr float_round_style round_style =
        std::numeric_limits<FP>::round_style; // TODO: check inexact policies

    static constexpr number_type min() noexcept(std::numeric_limits<FP>::min())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::min());
    }
    static constexpr number_type max() noexcept(std::numeric_limits<FP>::max())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::max());
    }
    static constexpr number_type lowest() noexcept(max) { return -(max)(); }
    static constexpr number_type epsilon() noexcept(std::numeric_limits<FP>::epsilon())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::epsilon());
    }
    static constexpr number_type round_error() noexcept(std::numeric_limits<FP>::round_error())
    {
        // TODO: check for inexact policies
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::round_error());
    }
    static constexpr number_type infinity() noexcept(std::numeric_limits<FP>::infinity())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::infinity());
    }
    static constexpr number_type quiet_NaN() noexcept(std::numeric_limits<FP>::quiet_NaN())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::quiet_NaN());
    }
    static constexpr number_type signaling_NaN() noexcept(std::numeric_limits<FP>::signaling_NaN())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::signaling_NaN());
    }
    static number_type denorm_min() noexcept(std::numeric_limits<FP>::denorm_min())
    {
        return boost::safe_float::safe_float<FP, CHECK, ERROR_HANDLING, CAST>(std::numeric_limits<FP>::denorm_min());
    }
};

} // namespace std

#endif // BOOST_SAFE_FLOAT_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_header.hpp

\details
Unit system for test purposes.

Output:
@verbatim
@endverbatim
**/

#ifndef BOOST_UNITS_TEST_HEADER_HPP
#define BOOST_UNITS_TEST_HEADER_HPP

#include <boost/test/minimal.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/derived_dimension.hpp>
#include <boost/units/static_constant.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/make_system.hpp>

#include <boost/units/physical_dimensions/length.hpp>
#include <boost/units/physical_dimensions/mass.hpp>
#include <boost/units/physical_dimensions/time.hpp>

#define BOOST_UNITS_CHECK_CLOSE(a, b) (BOOST_CHECK((std::abs((a) - (b)) < .0000001)))

namespace boost {

namespace units {

//struct length_base_dimension : boost::units::base_dimension<length_base_dimension,1> { };     ///> base dimension of length
//struct mass_base_dimension : boost::units::base_dimension<mass_base_dimension,2> { };         ///> base dimension of mass
//struct time_base_dimension : boost::units::base_dimension<time_base_dimension,3> { };         ///> base dimension of time

typedef length_base_dimension::dimension_type    length_dimension;
typedef mass_base_dimension::dimension_type      mass_dimension;
typedef time_base_dimension::dimension_type      time_dimension;

typedef derived_dimension<length_base_dimension,2>::type area_dimension;
typedef derived_dimension<mass_base_dimension,1,
                            length_base_dimension,2,
                            time_base_dimension,-2>::type  energy_dimension;
typedef derived_dimension<mass_base_dimension,-1,
                            length_base_dimension,-2,
                            time_base_dimension,2>::type   inverse_energy_dim;
typedef derived_dimension<length_base_dimension,1,
                            time_base_dimension,-1>::type  velocity_dimension;
typedef derived_dimension<length_base_dimension,3>::type volume_dimension;

/// placeholder class defining test unit system
struct length_unit : base_unit<length_unit, length_dimension, 4> {};
struct mass_unit : base_unit<mass_unit, mass_dimension, 5> {};
struct time_unit : base_unit<time_unit, time_dimension, 6> {};

typedef make_system<length_unit, mass_unit, time_unit>::type system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>            length;
typedef unit<mass_dimension,system>              mass;
typedef unit<time_dimension,system>              time;

typedef unit<area_dimension,system>              area;
typedef unit<energy_dimension,system>            energy;
typedef unit<inverse_energy_dim,system>    inverse_energy;
typedef unit<velocity_dimension,system>          velocity;
typedef unit<volume_dimension,system>            volume;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(meter,length);
BOOST_UNITS_STATIC_CONSTANT(meters,length);
BOOST_UNITS_STATIC_CONSTANT(kilogram,mass);
BOOST_UNITS_STATIC_CONSTANT(kilograms,mass);
BOOST_UNITS_STATIC_CONSTANT(second,time);
BOOST_UNITS_STATIC_CONSTANT(seconds,time);

BOOST_UNITS_STATIC_CONSTANT(square_meter,area);
BOOST_UNITS_STATIC_CONSTANT(square_meters,area);
BOOST_UNITS_STATIC_CONSTANT(joule,energy);
BOOST_UNITS_STATIC_CONSTANT(joules,energy);
BOOST_UNITS_STATIC_CONSTANT(meter_per_second,velocity);
BOOST_UNITS_STATIC_CONSTANT(meters_per_second,velocity);
BOOST_UNITS_STATIC_CONSTANT(cubic_meter,volume);
BOOST_UNITS_STATIC_CONSTANT(cubic_meters,volume);

template<> struct base_unit_info<length_unit>
{
    static std::string name()               { return "meter"; }
    static std::string symbol()             { return "m"; }
};
//]

template<> struct base_unit_info<mass_unit>
{
    static std::string name()               { return "kilogram"; }
    static std::string symbol()             { return "kg"; }
};

template<> struct base_unit_info<time_unit>
{
    static std::string name()               { return "second"; }
    static std::string symbol()             { return "s"; }
};

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_TEST_HEADER_HPP


#include <iostream>
#include <armadillo>
#include <mpi.h>
#include <omp.h>
#include <mkl.h>

#include "class_data.hpp"
#include "class_approximation.hpp"
#include "class_partition.hpp"
#include "evaluate_covariance.hpp"
#include "constants.hpp"
#include <string.h>

void Approximation::posterior_inference()
{
    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: calculating posterior quantities starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    //Set WORKING_REGION_FLAG to false for all regions before the second finest level
    memset(WORKING_REGION_FLAG,false,partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]-partition->nRegionsAtEachLevel[NUM_LEVELS_M-2]);

    //Allocate memory for KCholTimesCurrentw (vector size: partition->nKnots)
    KCholTimesCurrentw = new vec [partition->nRegionsInTotal];

    //Allocate memory for KCholTimesCurrentA (cube size: first dimension: partition->nKnots, second dimesnion: partition->nKnots, third dimension or the number of slices: (partition->nRegionsInTotal-1)*partition->nRegionsInTotal/2)
    KCholTimesCurrentA = new cube [partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];

    //Allocate memory for temporary variables
    int maxOpenMPThreads=omp_get_max_threads();
    vec* w = new vec [maxOpenMPThreads];
    mat* A = new mat [maxOpenMPThreads];
    double **tempMemory = new double* [maxOpenMPThreads];
    for(int i = 0; i < maxOpenMPThreads; i++)
    {
            w[i].set_size(partition->nKnots);
            A[i].set_size(partition->nKnots,partition->nKnots);
            tempMemory[i] =  new double [partition->nKnots*partition->nKnots];
    }

    //Temporary variables if load ATilde from disk
    int size=(NUM_LEVELS_M-1)*NUM_LEVELS_M/2;
    unsigned long offset = partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1];

    //From the second finest to the coarsest level
    for(int currentLevel = NUM_LEVELS_M-2; currentLevel > -1 ; currentLevel--)
    {
        double loglikelihoodThisLevel = 0;

        #pragma omp parallel for num_threads(maxOpenMPThreads) reduction(+:loglikelihoodThisLevel) schedule(dynamic,1)
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            //Skip regions that are not dealt with by this worker
            if( !WORKING_REGION_FLAG[iRegion] ) continue;

            int rankOpenMP = omp_get_thread_num();

            double loglikelihoodToAdd;
            bool supervisor = true;
            bool synchronizeFlag = false;

            unsigned long indexReginAtThisLevel = iRegion - REGION_START[currentLevel];

            //Deal with this region
            if(SAVE_TO_DISK_FLAG) load_ATilde_from_disk(iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, currentLevel == NUM_LEVELS_M-2, partition->nKnotsAtFinestLevel, offset, partition->nKnots, size);

            aggregate_w_and_A(w[rankOpenMP], A[rankOpenMP], tempMemory[rankOpenMP], currentLevel, (currentLevel+1)*(currentLevel+2)/2-1, iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, iRegion, supervisor, synchronizeFlag);

            if(supervisor)
            {
                if(CALCULATION_MODE != "prediction")
                    loglikelihoodToAdd = -2*sum(log(RChol[iRegion].diag()));

                //Compute the posterior conditional variance-covariance matrix
                RChol[iRegion] = chol( RChol[iRegion]*RChol[iRegion].t()+A[rankOpenMP],"lower");

                //Compute KCholTimesCurrentw that is the Cholesky factor of the inverse of the conditional posterior variance-covariance matrix times w
                KCholTimesCurrentw[iRegion].set_size(partition->nKnots);
                KCholTimesCurrentw[iRegion] = solve(trimatl(RChol[iRegion]),w[rankOpenMP]);

                //Compute KCholTimesCurrentA that is the Cholesky factor of the inverse of the conditional posterior variance-covariance matrix times A
                KCholTimesCurrentA[iRegion].set_size(partition->nKnots,partition->nKnots,currentLevel);
            }

            //Get wTilde and ATilde
            if(currentLevel != 0)
                get_wTilde_and_ATilde_in_posterior(w[rankOpenMP],A[rankOpenMP], tempMemory[rankOpenMP], partition->nKnots, KCholTimesCurrentA[iRegion], KCholTimesCurrentw[iRegion], iRegion, currentLevel, supervisor, synchronizeFlag);

            if(CALCULATION_MODE != "prediction")
                KCholTimesCurrentA[iRegion].reset();

            //Free wTilde and ATilde of the children to save memory
            free_wTilde_and_ATilde(iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J);
            if(!supervisor)
            {
                INDICES_REGIONS_AT_CURRENT_LEVEL.erase(iRegion);
                WORKING_REGION_FLAG[iRegion] = false;
            }

            if(supervisor && CALCULATION_MODE != "prediction")
                loglikelihoodToAdd += 2*sum(log(RChol[iRegion].diag()))-dot(KCholTimesCurrentw[iRegion],KCholTimesCurrentw[iRegion]);

            if(supervisor && CALCULATION_MODE != "prediction")
                loglikelihoodThisLevel += loglikelihoodToAdd;

            if(CALCULATION_MODE != "prediction")
                KCholTimesCurrentw[iRegion].reset();

            if(supervisor && SAVE_TO_DISK_FLAG)
            {
                //save ATilde of the current region to disk
                std::string fileName=TMP_DIRECTORY+"/"+std::to_string(iRegion)+".bin";
                ATilde[iRegion].save(fileName,arma_binary);
                ATilde[iRegion].reset();
            }

        }

        if(currentLevel > 0)
            synchronizeIndicesRegionsForEachWorker();

        loglikelihood += loglikelihoodThisLevel;

        timeval timeNow;
        gettimeofday(&timeNow, NULL);

        MPI_Barrier(WORLD);
        if(WORKER == 0) cout<<"Posterior: Level "<<currentLevel+1<<" is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";

    }

    MPI_Request request;

    if(CALCULATION_MODE!="prediction")
    {
        if(WORKER != 0)
            MPI_Isend(&loglikelihood,1,MPI_DOUBLE,0,MPI_TAG_LIKELIHOOD,WORLD, &request);
        else
        {
            double likelihoodComponent;
            for(int i = 1; i < MPI_SIZE; i++)
            {
                MPI_Recv(&likelihoodComponent,1,MPI_DOUBLE,i,MPI_TAG_LIKELIHOOD,WORLD,MPI_STATUS_IGNORE);
                loglikelihood += likelihoodComponent;
            }
            cout<<"The obtained loglikelihood is: "<<-0.5*(loglikelihood+NUM_OBSERVATIONS*log(2*3.14159265359))<<endl;
        }
    }

    //Deallocate temporary memory
    for(int i = 0; i < maxOpenMPThreads; i++)
    {
        w[i].reset();
        A[i].reset();
        delete[] tempMemory[i];
    }
    delete[] w;
    delete[] A;
    delete[] tempMemory;

    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================>  Processor 1: calculating posterior quantities is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
    }
}


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

#include <camera/camera_params.h>

#include <config_reader/config_reader.h>
#include <Eigen/Dense>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <fstream>
#include <iostream>

camera::CameraParameters::CameraParameters(Eigen::Vector2i const& image_size,
    Eigen::Vector2d const& focal_length,
    Eigen::Vector2d const& optical_center,
    Eigen::VectorXd const& distortion) {
  SetDistortedSize(image_size);
  SetUndistortedSize(image_size);
  focal_length_ = focal_length;
  optical_offset_ = optical_center;
  crop_offset_.setZero();
  SetDistortion(distortion);
}

camera::CameraParameters::CameraParameters(std::string const& calibration_file,
                                           std::string const & base_dir) {
  std::ifstream f(calibration_file);
  // it may be a relative path to the map file's directory
  if (!f.good() && base_dir != "") {
    f.open(base_dir + "/" + calibration_file);
  }
  if (!f.good()) {
    LOG(FATAL) <<  "Calibration file does not exist: " << calibration_file;
  }

  cv::FileStorage fs(calibration_file, cv::FileStorage::READ);

  cv::Mat cam_mat, dist_coeffs;
  Eigen::Vector2i size;

  // Read in distorted image size.
  fs["width"]  >> size[0];
  fs["height"] >> size[1];
  if (size[0] == 0 || size[1] == 0) {
    fs["image_width"]  >> size[0];
    fs["image_height"]  >> size[1];
  }
  SetDistortedSize(size);

  // Read in focal length and optical offset
  fs["intrinsic_matrix"] >> cam_mat;
  if (cam_mat.rows == 0 || cam_mat.cols == 0)
    fs["camera_matrix"] >> cam_mat;
  optical_offset_ << cam_mat.at<double>(0, 2), cam_mat.at<double>(1, 2);
  focal_length_ << cam_mat.at<double>(0, 0), cam_mat.at<double>(1, 1);

  // Read in the distortion coefficients
  fs["distortion"] >> dist_coeffs;
  if (dist_coeffs.rows == 0 || dist_coeffs.cols == 0)
    fs["distortion_coefficients"] >> dist_coeffs;
  Eigen::VectorXd buffer(dist_coeffs.cols * dist_coeffs.rows);
  for (int32_t i = 0; i < buffer.size(); i++) {
    buffer[i] = dist_coeffs.at<double>(i);
  }
  SetDistortion(buffer);

  // Load up the undistorted image size and handle if it was specified.
  fs["undistorted_width"] >> size[0];
  fs["undistorted_height"] >> size[1];
  if (!(size.array() > 0).all()) {
    size = distorted_image_size_;
  }
  SetUndistortedSize(size);

  // Read in crop offset
  fs["crop_x"] >> size[0];  // These default to zero if not specified
  fs["crop_y"] >> size[1];
  crop_offset_ = size;
}

camera::CameraParameters::CameraParameters(config_reader::ConfigReader* config, const char* name) {
  cv::Mat cam_mat(3, 3, CV_64F);
  Eigen::Vector2i size;

  config_reader::ConfigReader::Table camera(config, name);
  // Read in distorted image size.
  if (!camera.GetInt("width", &size[0]))
    fprintf(stderr, "Could not read camera width.");
  if (!camera.GetInt("height", &size[1]))
    fprintf(stderr, "Could not read camera height.");
  SetDistortedSize(size);

  config_reader::ConfigReader::Table vector(&camera, "intrinsic_matrix");
  for (int i = 0; i < 9; i++) {
    if (!vector.GetReal((i + 1), &cam_mat.at<double>(i / 3, i % 3))) {
      fprintf(stderr, "Failed to read vector intrinsic_matrix.");
      break;
    }
  }
  // Read in focal length and optical offset
  optical_offset_ << cam_mat.at<double>(0, 2), cam_mat.at<double>(1, 2);
  focal_length_ << cam_mat.at<double>(0, 0), cam_mat.at<double>(1, 1);

  // Read in the distortion coefficients (if not a real number, may be a list)
  Eigen::VectorXd buffer(1);
  if (!camera.GetReal("distortion_coeff", &buffer[0])) {
    config_reader::ConfigReader::Table dist(&camera, "distortion_coeff");
    buffer.resize(dist.GetSize());
    for (int i = 0; i < dist.GetSize(); i++) {
      if (!dist.GetReal(i + 1, &buffer[i]))
        fprintf(stderr, "Could not read camera distortion_coeff.");
    }
  }
  SetDistortion(buffer);

  if (!camera.GetInt("undistorted_width", &size[0]))
    fprintf(stderr, "Could not read camera undistorted_width.");
  if (!camera.GetInt("undistorted_height", &size[1]))
    fprintf(stderr, "Could not read camera undistorted_height.");
  SetUndistortedSize(size);

  // right now all our crops are zero
  // ff_common::ConfigReader::Table crop_table(&camera, "crop");
  // // Read in crop offset
  // if (!crop_table.GetInt("x", &size[0]))
  //   size[0] = 0;
  // if (!crop_table.GetInt("y", &size[1]))
  //   size[1] = 0;
  // crop_offset_ = size;
}

void camera::CameraParameters::SetDistortedSize(Eigen::Vector2i const& image_size) {
  distorted_image_size_ = image_size;
  distorted_half_size_ = image_size.cast<double>() / 2;
}

const Eigen::Vector2i& camera::CameraParameters::GetDistortedSize() const {
  return distorted_image_size_;
}

const Eigen::Vector2d& camera::CameraParameters::GetDistortedHalfSize() const {
  return distorted_half_size_;
}

void camera::CameraParameters::SetUndistortedSize(Eigen::Vector2i const& image_size) {
  undistorted_image_size_ = image_size;
  undistorted_half_size_ = image_size.cast<double>() / 2;
}

const Eigen::Vector2i& camera::CameraParameters::GetUndistortedSize() const {
  return undistorted_image_size_;
}

const Eigen::Vector2d& camera::CameraParameters::GetUndistortedHalfSize() const {
  return undistorted_half_size_;
}

void camera::CameraParameters::SetCropOffset(Eigen::Vector2i const& crop) {
  crop_offset_ = crop;
}

const Eigen::Vector2i& camera::CameraParameters::GetCropOffset() const {
  return crop_offset_;
}

void camera::CameraParameters::SetOpticalOffset(Eigen::Vector2d const& offset) {
  optical_offset_ = offset;
}

const Eigen::Vector2d& camera::CameraParameters::GetOpticalOffset() const {
  return optical_offset_;
}

void camera::CameraParameters::SetFocalLength(Eigen::Vector2d const& f) {
  focal_length_ = f;
}

double camera::CameraParameters::GetFocalLength() const {
  return focal_length_.mean();
}

const Eigen::Vector2d& camera::CameraParameters::GetFocalVector() const {
  return focal_length_;
}

void camera::CameraParameters::SetDistortion(Eigen::VectorXd const& distortion) {
  distortion_coeffs_ = distortion;

  // Ensure variables are initialized
  distortion_precalc1_ = 0;
  distortion_precalc2_ = 0;
  distortion_precalc3_ = 0;

  switch (distortion_coeffs_.size()) {
  case 0:
    // No lens distortion!
    break;
  case 1:
    // FOV model
    // inverse alpha
    distortion_precalc1_ = 1 / distortion[0];
    // Inside tangent function
    distortion_precalc2_ = 2 * tan(distortion[0] / 2);
    break;
  case 4:
    // Fall through intended.
  case 5:
    // Tsai model
    // There doesn't seem like there are any precalculations we can use.
    break;
  default:
    LOG(FATAL) << "Recieved irregular distortion vector size. Size = " << distortion_coeffs_.size();
  }
}

const Eigen::VectorXd& camera::CameraParameters::GetDistortion() const {
  return distortion_coeffs_;
}

void camera::CameraParameters::DistortCentered(Eigen::Vector2d const& undistorted_c,
                                               Eigen::Vector2d* distorted_c) const {
  // We assume that input x and y are pixel values that have
  // undistorted_len_x/2.0 and undistorted_len_y/2.0 subtracted from
  // them. The outputs will have distorted_len_x/2.0 and
  // distorted_len_y/2.0 subtracted from them.
  if (distortion_coeffs_.size() == 0) {
    // There is no distortion
    *distorted_c = undistorted_c + optical_offset_ - distorted_half_size_;
  } else if (distortion_coeffs_.size() == 1) {
    // This is the FOV model
    Eigen::Vector2d norm = undistorted_c.cwiseQuotient(focal_length_);
    double ru = norm.norm();
    double rd = atan(ru * distortion_precalc2_) * distortion_precalc1_;
    double conv;
    if (ru > 1e-5) {
      conv = rd / ru;
    } else {
      conv = 1;
    }
    *distorted_c = (optical_offset_ - distorted_half_size_) +
      conv * norm.cwiseProduct(focal_length_);
  } else if (distortion_coeffs_.size() == 4 ||
             distortion_coeffs_.size() == 5) {
    // Tsai lens distortion
    double k1 = distortion_coeffs_[0];
    double k2 = distortion_coeffs_[1];
    double p1 = distortion_coeffs_[2];
    double p2 = distortion_coeffs_[3];
    double k3 = 0;
    if (distortion_coeffs_.size() == 5)
      k3 = distortion_coeffs_[4];

    // To relative coordinates
    Eigen::Vector2d norm = undistorted_c.cwiseQuotient(focal_length_);
    double r2 = norm.squaredNorm();

    // Radial distortion
    double radial_dist = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2;
    *distorted_c = radial_dist * norm;

    // Tangential distortion
    *distorted_c +=
      Eigen::Vector2d(2 * p1 * norm[0] * norm[1] + p2 * (r2 + 2 * norm[0] * norm[0]),
                      p1 * (r2 + 2 * norm[1] * norm[1]) + 2 * p2 * norm[0] * norm[1]);

    // Back to absolute coordinates.
    *distorted_c = distorted_c->cwiseProduct(focal_length_) +
      (optical_offset_ - distorted_half_size_);
  } else {
    LOG(ERROR) << "Unknown distortion vector size!";
  }
}

void camera::CameraParameters::UndistortCentered(Eigen::Vector2d const& distorted_c,
                                                 Eigen::Vector2d *undistorted_c) const {
  // We assume that input x and y are pixel values that have
  // distorted_len_x and distorted_len_y subtracted from them. The
  // outputs will have undistorted_len_x and undistorted_len_y
  // subtracted from them.
  if (distortion_coeffs_.size() == 0) {
    // No lens distortion
    *undistorted_c = distorted_c - (optical_offset_ - distorted_half_size_);
  } else if (distortion_coeffs_.size() == 1) {
    // FOV lens distortion
    Eigen::Vector2d norm =
      (distorted_c - (optical_offset_ - distorted_half_size_)).cwiseQuotient(focal_length_);
    double rd = norm.norm();
    double ru = tan(rd * distortion_coeffs_[0]) / distortion_precalc2_;
    double conv = 1.0;
    if (rd > 1e-5)
      conv = ru / rd;
    *undistorted_c = conv * norm.cwiseProduct(focal_length_);
  } else if (distortion_coeffs_.size() == 4 ||
             distortion_coeffs_.size() == 5) {
    // Tsai lens distortion
    cv::Mat src(1, 1, CV_64FC2);
    cv::Mat dst(1, 1, CV_64FC2);
    Eigen::Map<Eigen::Vector2d> src_map(src.ptr<double>()), dst_map(dst.ptr<double>());
    cv::Mat dist_int_mat(3, 3, cv::DataType<double>::type),
      undist_int_mat(3, 3, cv::DataType<double>::type);
    cv::Mat cvdist;
    cv::eigen2cv(distortion_coeffs_, cvdist);
    cv::eigen2cv(GetIntrinsicMatrix<DISTORTED>(), dist_int_mat);
    cv::eigen2cv(GetIntrinsicMatrix<UNDISTORTED>(), undist_int_mat);
    src_map = distorted_c + distorted_half_size_;
    cv::undistortPoints(src, dst, dist_int_mat, cvdist, cv::Mat(), undist_int_mat);
    *undistorted_c = dst_map - undistorted_half_size_;
  } else {
    LOG(ERROR) << "Unknown distortion vector size!";
  }
}

void camera::CameraParameters::GenerateRemapMaps(cv::Mat* remap_map) {
  remap_map->create(undistorted_image_size_[1], undistorted_image_size_[0], CV_32FC2);
  Eigen::Vector2d undistorted, distorted;
  for (undistorted[1] = 0; undistorted[1] < undistorted_image_size_[1]; undistorted[1]++) {
    for (undistorted[0] = 0; undistorted[0] < undistorted_image_size_[0]; undistorted[0]++) {
      Convert<UNDISTORTED, DISTORTED>(undistorted, &distorted);
      remap_map->at<cv::Vec2f>(undistorted[1], undistorted[0])[0] = distorted[0];
      remap_map->at<cv::Vec2f>(undistorted[1], undistorted[0])[1] = distorted[1];
    }
  }
}

namespace camera {

  // Conversion function helpers
#define DEFINE_CONVERSION(TYPEA, TYPEB) \
  template <> \
  void camera::CameraParameters::Convert<TYPEA, TYPEB>(Eigen::Vector2d const& input, Eigen::Vector2d *output) const

  DEFINE_CONVERSION(RAW, DISTORTED) {
    *output = input - crop_offset_.cast<double>();
  }
  DEFINE_CONVERSION(DISTORTED, RAW) {
    *output = input + crop_offset_.cast<double>();
  }
  DEFINE_CONVERSION(UNDISTORTED_C, DISTORTED_C) {
    DistortCentered(input, output);
  }
  DEFINE_CONVERSION(DISTORTED_C, UNDISTORTED_C) {
    UndistortCentered(input, output);
  }
  DEFINE_CONVERSION(UNDISTORTED, UNDISTORTED_C) {
    *output = input - undistorted_half_size_;
  }
  DEFINE_CONVERSION(UNDISTORTED_C, UNDISTORTED) {
    *output = input + undistorted_half_size_;
  }
  DEFINE_CONVERSION(DISTORTED, UNDISTORTED) {
    Convert<DISTORTED_C, UNDISTORTED_C>(input - distorted_half_size_, output);
    *output += undistorted_half_size_;
  }
  DEFINE_CONVERSION(UNDISTORTED, DISTORTED) {
    Eigen::Vector2d centered_output;
    Convert<UNDISTORTED, UNDISTORTED_C>(input, output);
    Convert<UNDISTORTED_C, DISTORTED_C>(*output, &centered_output);
    *output = centered_output + distorted_half_size_;
  }
  DEFINE_CONVERSION(DISTORTED, UNDISTORTED_C) {
    Convert<DISTORTED_C, UNDISTORTED_C>(input - distorted_half_size_, output);
  }
  DEFINE_CONVERSION(UNDISTORTED_C, DISTORTED) {
    Convert<UNDISTORTED_C, DISTORTED_C>(input, output);
    *output += distorted_half_size_;
  }

#undef DEFINE_CONVERSION

  // Helper functions to give the intrinsic matrix
#define DEFINE_INTRINSIC(TYPE) \
  template <> \
  Eigen::Matrix3d camera::CameraParameters::GetIntrinsicMatrix<TYPE>() const

  DEFINE_INTRINSIC(RAW) {
    Eigen::Matrix3d k = focal_length_.homogeneous().asDiagonal();
    k.block<2, 1>(0, 2) = optical_offset_ + crop_offset_.cast<double>();
    return k;
  }
  DEFINE_INTRINSIC(DISTORTED) {
    Eigen::Matrix3d k = focal_length_.homogeneous().asDiagonal();
    k.block<2, 1>(0, 2) = optical_offset_;
    return k;
  }
  DEFINE_INTRINSIC(DISTORTED_C) {
    Eigen::Matrix3d k = focal_length_.homogeneous().asDiagonal();
    k.block<2, 1>(0, 2) = optical_offset_ - distorted_half_size_;
    return k;
  }
  DEFINE_INTRINSIC(UNDISTORTED) {
    Eigen::Matrix3d k = focal_length_.homogeneous().asDiagonal();
    k.block<2, 1>(0, 2) = undistorted_half_size_;
    return k;
  }
  DEFINE_INTRINSIC(UNDISTORTED_C) {
    Eigen::Matrix3d k = focal_length_.homogeneous().asDiagonal();
    return k;
  }

#undef DEFINE_INTRINSIC

}  // end namespace camera


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_PREV_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_PREV_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing prev capabilities

    Returns the greatest representable value  strictly less than the parameter

    @par Semantic:

    @code
    T r = prev(x);
    @endcode

    computes the greatest value strictly less than x in type T

    @see next, nextafter, successor, predecessor

  **/
  const boost::dispatch::functor<tag::prev_> prev = {};
} }
#endif

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

#endif


#include <iostream>
#include <chrono>
#include <thread>
#include <Eigen/Eigen>

#include <pangolin/pangolin.h>
#include <SceneGraph/SceneGraph.h>

using namespace std;

int main( int /*argc*/, char** /*argv[]*/ )
{
    // Create OpenGL window in single line thanks to GLUT
    pangolin::CreateWindowAndBind("Main",640,480);
    SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();

    // Scenegraph to hold GLObjects and relative transformations
    SceneGraph::GLSceneGraph glGraph;

    // Define grid object
    SceneGraph::GLGrid glGrid( 50, 2.0, true );
    glGraph.AddChild(&glGrid);

    // Define axis object, and set its pose
    SceneGraph::GLAxis glAxis;
    glAxis.SetPose( 0, 0, 0, 0, 0, 0);
    glAxis.SetScale(0.25);
    glGraph.AddChild(&glAxis);

    // Define 3D spiral using a GLCachedPrimitives object
    SceneGraph::GLCachedPrimitives glSpiral(GL_LINE_STRIP, SceneGraph::GLColor(1.0f,0.7f,0.2f) );
    for(double t=0; t < 10*M_PI; t+= M_PI/50) {
        glSpiral.AddVertex(Eigen::Vector3d(cos(t)+2, sin(t)+2, -0.1*t) );
    }
    glGraph.AddChild(&glSpiral);

    // Define 3D floating text object
    SceneGraph::GLText glText3d("3D Floating Text", -1, 1, -1);
    glGraph.AddChild(&glText3d);

#ifndef HAVE_GLES
    SceneGraph::GLMovableAxis glMovableAxis;
    glMovableAxis.SetPosition(-3,3,-1);
    glGraph.AddChild(&glMovableAxis);

    SceneGraph::GLAxisAlignedBox glBox;
    glBox.SetResizable();
    glMovableAxis.AddChild(&glBox);

    // Define movable waypoint object with velocity
    SceneGraph::GLWayPoint glWaypoint;
    glWaypoint.SetPose(0.5,0.5,-0.1,0,0,0);
    glGraph.AddChild(&glWaypoint);

    // Optionally clamp waypoint to specific plane
    glWaypoint.ClampToPlane(Eigen::Vector4d(0,0,1,0));
#endif

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState stacks3d(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
        pangolin::ModelViewLookAt(0,-2,-4, 0,1,0, pangolin::AxisNegZ)
    );

    // We define a new view which will reside within the container.
    pangolin::View view3d;

    // We set the views location on screen and add a handler which will
    // let user input update the model_view matrix (stacks3d) and feed through
    // to our scenegraph
    view3d.SetBounds(0.0, 1.0, 0.0, 1.0, 640.0f/480.0f)
          .SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d,pangolin::AxisNegZ))
          .SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));

    // Add our views as children to the base container.
    pangolin::DisplayBase().AddDisplay(view3d);

    // Default hooks for exiting (Esc) and fullscreen (tab).
    while( !pangolin::ShouldQuit() )
    {
        // Clear whole screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Swap frames and Process Events
        pangolin::FinishFrame();

        // Pause for 1/60th of a second.
        std::this_thread::sleep_for(std::chrono::milliseconds(1000/60));
    }

    return 0;
}


//////////////////////////////////////////////////////////////////////////
//
// pgworkload
//
// Copyright (C) 2020, EnterpriseDB Corporation. All rights reserved.
//
//////////////////////////////////////////////////////////////////////////

#include <exception>
#include <iostream>
#include <fstream>
#include <vector>
#include <array>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/interpolators/catmull_rom.hpp>

#include "include/Profile.h"

namespace bpt = boost::posix_time;
namespace bpg = boost::gregorian;

Profile::Profile(int threads) :
    m_threads(threads)
{
    // Get the time diff from now to 00:00:00 on the previous Monday
    bpg::date d = bpg::day_clock::local_day();

    while (d.day_of_week().as_number() != 1)
        d -= bpg::days(1);

    m_start_time = bpt::ptime(d, bpt::time_duration(0,0,0));

    bpg::date_facet *df = new bpg::date_facet{"%A, %d %B %Y"};
    std::cout.imbue(std::locale{std::cout.getloc(), df});
    std::cout << "Profile epoch: " << m_start_time << "\n";
}

Profile::~Profile()
{

}

bool Profile::load_profile(const std::string& profile_path)
{
    // If there's no profile path specified, just populate the
    // workload array with values of 100.
    if (profile_path == "")
    {
        for (int i=0; i < TOTAL_POINTS; i++)
            m_profile_data[i] = 100;

        std::cout << "Default workload profile of 100% constant activity loaded." << std::endl;
        return true;
    }

    // Vector to hold the input profile
    std::vector<std::array<double, 2>> points(MAX_CONFIG_POINTS);

    // Open the profile
    std::ifstream inp(profile_path, std::ios::in);
    if (!inp.is_open())
    {
        std::cout << "Error " << errno << " - " << strerror(errno) << ": " << profile_path << std::endl;
        return false;
    }

    // Loop through each line in the profile, and attempt to load the data
    int point_count = 0;
    int line_count = 0;
    std::string line;

    while (getline(inp, line))
    {
        // Ignore comments and blanks
        if (line[0] != '#' && line != "")
        {
            // Attempt to load a double. Ignore any errors, but dump a message
            try
            {
                points[point_count] = {std::stod(line)};
                point_count++;
            }
            catch (const std::exception& e)
            {
                std::cout << "Ignoring invalid value on line " << line_count << ": '" << line << "'" << std::endl;
            }
        }
        line_count++;

        // If we have more data than we want, throw a warning and quit loading
        if (point_count >= MAX_CONFIG_POINTS)
        {
            std::cout << "More than 168 values found. Ignoring additional ones." << std::endl;
            break;
        }
    }

    // We need at least 4 points for Catmull-Rom
    if (point_count < MIN_CONFIG_POINTS)
    {
        std::cout << "At least 4 values must be included in the profile definition. Exiting." << std::endl;
        return false;
    }

    // Trim the vector so it matches the size of what was loaded
    for (int i = MAX_CONFIG_POINTS-1; i >= point_count; i--)
        points.erase(points.begin() + i);

    // Do the Catmull-Rom thing
    boost::math::catmull_rom<std::array<double, 2>> cr(std::move(points));

    // Loop through the values and create an array of the final set of values,
    // interpolating them from the CR graph.
    auto max_s = cr.max_parameter();
    for (int i = 0; i < TOTAL_POINTS; i++)
    {
        double arg = max_s*i/double(TOTAL_POINTS);
        m_profile_data[i] = cr(arg)[0];
    }

    std::cout << "Workload profile loaded from: " << profile_path << std::endl;

    return true;
}

// Return the expected workload in threads at this point in time
double Profile::get_workload()
{
    bpt::ptime current_time = bpt::microsec_clock::local_time();
    bpt::time_duration time_diff = current_time - m_start_time;

    // Figure out what minute to access in the profile data
    int minute = ((time_diff.hours() * 60) + time_diff.minutes()) % TOTAL_POINTS;

    // Get the number of threads we expect
    int threads = m_threads * m_profile_data[minute] / 100;

    return threads;
}


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "wtoolkit.h"

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
namespace ba=boost::algorithm;

/*
 *
 * bottom_box:[batch_size,Nr,4](y,x,h,w)
 * bottom_pred:[batch_size,Nr,num_class]
 * output_box:[X,4]
 * output_classes:[X]
 * output_scores:[X]
 * output_batch_index:[X]
 */
REGISTER_OP("BoxesSelect")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
	.Attr("ignore_first:bool")
    .Input("bottom_box: T")
    .Input("bottom_pred: T")
	.Output("output_box:T")
	.Output("output_classes:T")
	.Output("output_scores:T")
	.Output("output_batch_index:T");

template <typename Device, typename T>
class BoxesSelectOp: public OpKernel {
	public:
		explicit BoxesSelectOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("ignore_first", &ignore_first));
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &bottom_box        = context->input(0);
			const Tensor &bottom_pred       = context->input(1);
			auto          bottom_box_flat   = bottom_box.flat<T>();
			auto          bottom_pred_flat  = bottom_pred.flat<T>();

			OP_REQUIRES(context, bottom_box.dims() == 3, errors::InvalidArgument("box data must be 3-dimensional"));
			OP_REQUIRES(context, bottom_pred.dims() == 3, errors::InvalidArgument("pred data must be 2-dimensional"));

			const auto     batch_nr  = bottom_box.dim_size(0);
			const auto     data_nr   = bottom_box.dim_size(1);
			const auto     class_nr  = bottom_pred.dim_size(2);
			using Outtype=tuple<const float*,int,float,int>; //box,class,scores,batch_index
			vector<Outtype>  tmp_outdata;
			auto type_func = ignore_first?type_without_first:type_with_first;
			auto shard = [this, &bottom_box_flat,&bottom_pred_flat,class_nr,data_nr,batch_nr,&tmp_outdata,type_func]
				(int64 start, int64 limit) {
					for (int64 b = start; b < limit; ++b) {
						int batch_ind = b;
						int data_ind  = batch_ind%data_nr;
						batch_ind /= data_nr;
						const auto   base_offset0 = batch_ind *data_nr *4+data_ind *4;
						const auto   base_offset1 = batch_ind *data_nr *class_nr+data_ind *class_nr;
						const float *box_data    = bottom_box_flat.data()+base_offset0;
						const float *pred        = bottom_pred_flat.data()+base_offset1;
						const auto   type        = type_func(pred,class_nr);
						const auto   scores      = pred[type];

						if((scores >= threshold) && (box_area(box_data)>1E-4))
							tmp_outdata.emplace_back(box_data,type,scores,batch_ind);
					}
				};

			const DeviceBase::CpuWorkerThreads& worker_threads =
			*(context->device()->tensorflow_cpu_worker_threads());
			const int64 total_cost= batch_nr*data_nr;
			//Shard(worker_threads.num_threads, worker_threads.workers,total_cost,total_cost, shard);
			shard(0,total_cost);
			int dims_2d[2] = {int(tmp_outdata.size()),4};
			int dims_1d[1] = {int(tmp_outdata.size())};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_scores      = NULL;
			Tensor      *output_batch_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_scores));
			OP_REQUIRES_OK(context, context->allocate_output(3, outshape1, &output_batch_index));

			auto obox         = output_box->template flat<T>();
			auto oclasses     = output_classes->template flat<T>();
			auto oscores      = output_scores->template flat<T>();
			auto obatch_index = output_batch_index->template flat<T>();

			for(int i=0; i<tmp_outdata.size(); ++i) {
				auto& data = tmp_outdata[i];
				auto box = get<0>(data);
				std::copy(box,box+4,obox.data()+4*i);
				oclasses(i) = get<1>(data);
				oscores(i) = get<2>(data);
				obatch_index(i) = get<3>(data);
			}
		}
		static int type_with_first(const float* data,size_t size) {
			auto it = max_element(data,data+size);
			return it-data;
		}
		static int type_without_first(const float* data,size_t size) {
			auto it = max_element(data+1,data+size);
			return it-data;
		}
	private:
		bool  ignore_first;
		float threshold;
};
REGISTER_KERNEL_BUILDER(Name("BoxesSelect").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesSelectOp<CPUDevice, float>);

/*
 * bottom_boxes:[Nr,4](ymin,xmin,ymax,xmax) proposal box
 * width:float
 * height:float
 * output:[Nr,4] 相对坐标(ymin,xmin,ymax,xmax)
 */
REGISTER_OP("BoxesRelativeToAbsolute")
    .Attr("T: {float, double}")
	.Attr("width: int")
	.Attr("height: int")
    .Input("bottom_boxes: T")
	.Output("output:T")
    .SetShapeFn(shape_inference::UnchangedShape);

template <typename Device, typename T>
class BoxesRelativeToAbsoluteOp: public OpKernel {
	public:
		explicit BoxesRelativeToAbsoluteOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("width", &width_));
			OP_REQUIRES_OK(context, context->GetAttr("height", &height_));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &bottom_boxes      = context->input(0);
			auto          bottom_boxes_flat = bottom_boxes.flat<T>();

			OP_REQUIRES(context, bottom_boxes.dims() == 2, errors::InvalidArgument("boxes data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_boxes.dim_size(1)==4, errors::InvalidArgument("Boxes second dim size must be 4."));
			const auto nr = bottom_boxes.dim_size(0);

			TensorShape output_shape = bottom_boxes.shape();
			// Create output tensors
			Tensor* output_tensor = NULL;
			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));

			auto output = output_tensor->template flat<T>();
			auto shard = [this, &bottom_boxes_flat,&output]
					 (int64 start, int64 limit) {
						 for (int64 b = start; b < limit; ++b) {
							 const auto base_offset = b *4;
							 const auto box_data    = bottom_boxes_flat.data()+base_offset;
							 const auto output_data = output.data()+base_offset;
							 output_data[0] = box_data[0]*(height_-1)+0.5f;
							 output_data[1] = box_data[1]*(width_-1)+0.5f;
							 output_data[2] = box_data[2]*(height_-1)+0.5f;
							 output_data[3] = box_data[3]*(width_-1)+0.5f;
						 }
					 };

			const DeviceBase::CpuWorkerThreads& worker_threads =
				*(context->device()->tensorflow_cpu_worker_threads());
			Shard(worker_threads.num_threads, worker_threads.workers,
					nr, 1000, shard);
		}
	private:
		int width_;
		int height_;
};
REGISTER_KERNEL_BUILDER(Name("BoxesRelativeToAbsolute").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesRelativeToAbsoluteOp<CPUDevice, float>);
/*
 * 在image中剪切出一个ref_box定义的区域，同时对原来在image中的boxes进行处理，如果boxes与剪切区域jaccard得分小于threshold的会被删除
 * ref_box:shape=[4],[ymin,xmin,ymax,xmax] 参考box,相对坐标
 * boxes:[Nr,4],(ymin,xmin,ymax,xmax),需要处理的box
 * threshold:阀值
 * output:[Y,4]
 */
REGISTER_OP("CropBoxes")
    .Attr("T: {float, double}")
	.Attr("threshold:float")
    .Input("ref_box: T")
    .Input("boxes: T")
	.Output("output:T")
	.Output("mask:bool")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(c->Dim(c->input(1), 0)));
			return Status::OK();
			});

template <typename Device, typename T>
class CropBoxesOp: public OpKernel {
	public:
		explicit CropBoxesOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context,
					context->GetAttr("threshold", &threshold));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &ref_box      = context->input(0);
			const Tensor &boxes        = context->input(1);
			auto          ref_box_flat = ref_box.flat<T>();
			auto          boxes_flat   = boxes.flat<T>();
			const auto    nr           = boxes.dim_size(0);
			vector<int>   good_index;
			vector<bool>  good_mask(nr,false);

			OP_REQUIRES(context, ref_box.dims() == 1, errors::InvalidArgument("ref box must be 1-dimensional"));
			OP_REQUIRES(context, boxes.dims() == 2, errors::InvalidArgument("boxes must be 2-dimensional"));

            good_index.reserve(nr);

            for(int i=0; i<nr; ++i) {
                auto cur_box = boxes_flat.data()+4*i;
                if(bboxes_jaccard_of_box0(cur_box,ref_box_flat.data()) < threshold) continue;
                good_index.push_back(i);
				good_mask[i] = true;
            }
            const int   out_nr       = good_index.size();
            const int   dims_2d[]    = {out_nr,4};
            const int   dims_1d[]    = {int(nr)};
            TensorShape output_shape;
            TensorShape output_shape1;

			TensorShapeUtils::MakeShape(dims_2d, 2, &output_shape);
			TensorShapeUtils::MakeShape(dims_1d, 1, &output_shape1);

			Tensor *output_tensor = NULL;
			Tensor *output_mask   = NULL;

			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));
			OP_REQUIRES_OK(context, context->allocate_output(1, output_shape1, &output_mask));

			auto output           = output_tensor->template flat<T>();
			auto output_mask_flat = output_mask->template flat<bool>();

			for(auto i=0; i<nr; ++i)
				output_mask_flat.data()[i] = good_mask[i];

            for(int i=0; i<out_nr; ++i) {
                cut_box(ref_box_flat.data(),boxes_flat.data()+good_index[i]*4,output.data()+i*4);
            }
		}
	private:
		float threshold=1.0;
};
REGISTER_KERNEL_BUILDER(Name("CropBoxes").Device(DEVICE_CPU).TypeConstraint<float>("T"), CropBoxesOp<CPUDevice, float>);

/*
 * 删除与边框的jaccard得分大于threshold的box
 * size:shape=[2],[h,w] 相对坐标
 * boxes:[Nr,4],(ymin,xmin,ymax,xmax),需要处理的box
 * threshold:阀值
 * output:[Y,4]
 */
REGISTER_OP("RemoveBoundaryBoxes")
    .Attr("T: {float, double}")
	.Attr("threshold:float")
    .Input("size: T")
    .Input("boxes: T")
	.Output("output:T")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(c->UnknownDim(), 4));
			c->set_output(1, c->Vector(c->UnknownDim()));
			return Status::OK();
			});

template <typename Device, typename T>
class RemoveBoundaryBoxesOp: public OpKernel {
	public:
		explicit RemoveBoundaryBoxesOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context,
					context->GetAttr("threshold", &threshold));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &size         = context->input(0);
			const Tensor &boxes        = context->input(1);
			auto          size_flat    = size.flat<T>();
			auto          boxes_flat   = boxes.flat<T>();
			vector<int>   good_index;
			T             ref_boxes0[] = {0.0f,0.0f,1.0f,size_flat.data()[1]};
			T             ref_boxes1[] = {0.0f,0.0f,size_flat.data()[0],1.0f};
			T             ref_boxes2[] = {0.0f,1.0f-size_flat.data()[1],1.0f,1.0f};
			T             ref_boxes3[] = {1.0f-size_flat.data()[0],0.0f,1.0f,1.0f};

			OP_REQUIRES(context, size.dims() == 1, errors::InvalidArgument("size must be 1-dimensional"));
			OP_REQUIRES(context, boxes.dims() == 2, errors::InvalidArgument("boxes must be 2-dimensional"));

			const auto nr = boxes.dim_size(0);
            good_index.reserve(nr);

            for(int i=0; i<nr; ++i) {
                auto cur_box = boxes_flat.data()+4*i;
                if(bboxes_jaccard_of_box0(cur_box,ref_boxes0) > threshold) continue;
                if(bboxes_jaccard_of_box0(cur_box,ref_boxes1) > threshold) continue;
                if(bboxes_jaccard_of_box0(cur_box,ref_boxes2) > threshold) continue;
                if(bboxes_jaccard_of_box0(cur_box,ref_boxes3) > threshold) continue;
                good_index.push_back(i);
            }
            const int   out_nr       = good_index.size();
            const int   dims_2d[]    = {out_nr,4};
            const int   dims_1d[]    = {out_nr};
            TensorShape output_shape;
            TensorShape output_shape1;

			TensorShapeUtils::MakeShape(dims_2d, 2, &output_shape);
			TensorShapeUtils::MakeShape(dims_1d, 1, &output_shape1);

			Tensor* output_tensor = NULL;
			Tensor* output_tensor_index = NULL;
			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));
			OP_REQUIRES_OK(context, context->allocate_output(1, output_shape1, &output_tensor_index));

			auto output       = output_tensor->template flat<T>();
			auto output_index = output_tensor_index->template flat<int32_t>();

            for(int i=0; i<out_nr; ++i) {
                copy_box(boxes_flat.data()+good_index[i]*4,output.data()+i*4);
				output_index.data()[i] = good_index[i];
            }
		}
	private:
		float threshold=1.0;
};
REGISTER_KERNEL_BUILDER(Name("RemoveBoundaryBoxes").Device(DEVICE_CPU).TypeConstraint<float>("T"), RemoveBoundaryBoxesOp<CPUDevice, float>);

Status distored_boxes_shape(shape_inference::InferenceContext* c)
{
    auto shape = c->input(0);

    if(!c->FullyDefined(shape)) {
        c->set_output(0,c->Matrix(c->UnknownDim(),4));
        return Status::OK();
    }

    auto          data_nr  = c->Value(c->Dim(shape,0));
    bool          keep_org;
    int           res_nr   = 0;
    vector<float> xoffset;
    vector<float> yoffset;
    vector<float> scale;

    c->GetAttr("keep_org",&keep_org);
    c->GetAttr("xoffset",&xoffset);
    c->GetAttr("yoffset",&yoffset);
    c->GetAttr("scale",&scale);
    if(keep_org)
        res_nr = data_nr;
    else
        res_nr = 0;
    res_nr += (xoffset.size()+yoffset.size()+scale.size())*data_nr;
    c->set_output(0,c->Matrix(res_nr,4));
    return Status::OK();
}
/*
 * 对Boxes:[Nr,4]进行多样化处理
 * scale:对box进行缩放处理
 * offset:对box进行上下左右的平移处理
 */
REGISTER_OP("DistoredBoxes")
    .Attr("T: {float, double}")
	.Attr("scale:list(float)")
	.Attr("xoffset:list(float)")
	.Attr("yoffset:list(float)")
	.Attr("keep_org:bool")
    .Input("boxes: T")
	.Output("output_boxes:T")
	.SetShapeFn(distored_boxes_shape);

template <typename Device, typename T>
class DistoredBoxesOp: public OpKernel {
	private:
	public:
		explicit DistoredBoxesOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("scale", &scale_));
			OP_REQUIRES_OK(context, context->GetAttr("xoffset", &xoffset_));
			OP_REQUIRES_OK(context, context->GetAttr("yoffset", &yoffset_));
			OP_REQUIRES_OK(context, context->GetAttr("keep_org", &keep_org_));
		}
        inline int get_output_nr(int nr)const {
            auto    output_nr  = 0;
            if(keep_org_)
                output_nr += nr;
            output_nr += (xoffset_.size()+yoffset_.size())*nr;
            output_nr += scale_.size()*nr;
            return output_nr;
        }

		void Compute(OpKernelContext* context) override
        {
            const Tensor &boxes      = context->input(0);
            auto          boxes_flat = boxes.flat<T>();
            const auto    nr         = boxes.dim_size(0);
            const auto    output_nr  = get_output_nr(nr);

            OP_REQUIRES(context, boxes.dims() == 2, errors::InvalidArgument("boxes must be 2-dimensional"));

            const int   dims_2d[]     = {int(output_nr),4};
            TensorShape output_shape0;

            TensorShapeUtils::MakeShape(dims_2d, 2, &output_shape0);

            Tensor *output_boxes  = NULL;

            OP_REQUIRES_OK(context, context->allocate_output(0, output_shape0, &output_boxes));

            auto outputboxes  = output_boxes->template flat<T>();
            auto output_index = 0;

            if(keep_org_) {
                copy_boxes(boxes_flat.data(),outputboxes.data(),nr);
                output_index = nr;
            }
            processOffsetX(boxes_flat.data(),outputboxes.data(),xoffset_,nr,output_index);
            processOffsetY(boxes_flat.data(),outputboxes.data(),yoffset_,nr,output_index);

            for(auto i=0; i<scale_.size(); ++i) {
                processScale(boxes_flat.data(),outputboxes.data(),scale_[i],nr,output_index);
            }
        }
		void processOffsetX(const T* src_boxes,T* out_boxes,const vector<float>& xoffset,const int nr,int& output_index)
		{
			for(auto k=0; k<nr; ++k) {
				const auto src_box = src_boxes+4*k;
				for(auto i=0; i<xoffset.size(); ++i) {
					const auto os      = xoffset[i];
					const auto dx      = (src_box[3]-src_box[1]) *os;
					auto       cur_box = out_boxes+4 *output_index++;

					copy_box(src_box,cur_box);
					cur_box[1] += dx;
					cur_box[3] += dx;
				}
			}
		}
		void processOffsetY(const T* src_boxes,T* out_boxes,const vector<float>& yoffset,const int nr,int& output_index)
		{
			for(auto k=0; k<nr; ++k) {
				const auto src_box = src_boxes+4*k;
				for(auto i=0; i<yoffset.size(); ++i) {
					const auto os      = yoffset[i];
					const auto dy      = (src_box[2]-src_box[0]) *os;
					auto       cur_box = out_boxes+4 *output_index++;

					copy_box(src_box,cur_box);
					cur_box[0] -= dy;
					cur_box[2] -= dy;
				}
			}
		}
		void processScale(const T* src_boxes,T* out_boxes,const float scale,const int nr,int& output_index)
		{
			for(auto k=0; k<nr; ++k) {
				const auto src_box = src_boxes+4 *k;
				const auto dx      = (src_box[3]-src_box[1]) *(scale-1.0)/2.;
				const auto dy      = (src_box[2]-src_box[0]) *(scale-1.0)/2.;
				auto       cur_box = out_boxes+4 *output_index++;

				copy_box(src_box,cur_box);
				cur_box[0] -= dy;
				cur_box[1] -= dx;
				cur_box[2] += dy;
				cur_box[3] += dx;
			}
		}
	private:
		vector<float> scale_;
		vector<float> xoffset_;
		vector<float> yoffset_;
		bool          keep_org_;
};
REGISTER_KERNEL_BUILDER(Name("DistoredBoxes").Device(DEVICE_CPU).TypeConstraint<float>("T"), DistoredBoxesOp<CPUDevice, float>);

Status random_distored_boxes_shape(shape_inference::InferenceContext* c)
{
    auto shape = c->input(0);

    if(!c->FullyDefined(shape)) {
        c->set_output(0,c->Matrix(c->UnknownDim(),4));
        return Status::OK();
    }

    auto data_nr  = c->Value(c->Dim(shape,0));
    bool keep_org;
    int  res_nr   = 0;
    int  size;

    c->GetAttr("keep_org",&keep_org);
    c->GetAttr("size",&size);
    if(keep_org)
        res_nr = data_nr;
    else
        res_nr = 0;
    res_nr += size*data_nr;
    c->set_output(0,c->Matrix(res_nr,4));
    return Status::OK();
}
/*
 * 对Boxes:[Nr,4]进行多样化处理
 * limits:[xoffset,yoffset,scale]大小限制
 * size:[产生xoffset,yoffset,scale的数量]
 */
REGISTER_OP("RandomDistoredBoxes")
    .Attr("T: {float, double}")
	.Attr("limits:list(float)")
	.Attr("size:int")
	.Attr("keep_org:bool")
    .Input("boxes: T")
	.Output("output_boxes:T")
	.SetShapeFn(random_distored_boxes_shape);

template <typename Device, typename T>
class RandomDistoredBoxesOp: public OpKernel {
	private:
	public:
		explicit RandomDistoredBoxesOp(OpKernelConstruction* context) : OpKernel(context) {
		    vector<float> limits;
			OP_REQUIRES_OK(context, context->GetAttr("limits", &limits));
			OP_REQUIRES_OK(context, context->GetAttr("size", &size_));
			OP_REQUIRES_OK(context, context->GetAttr("keep_org", &keep_org_));
            for(auto x:limits)
                limits_.push_back(sqrt(x));
		}
        inline int get_output_nr(int nr)const {
            auto    output_nr  = 0;
            if(keep_org_)
                output_nr += nr;
            output_nr += size_*nr;
            return output_nr;
        }

		void Compute(OpKernelContext* context) override
		{
            TIME_THISV1("RandomDistoredBoxes");

			const Tensor &boxes      = context->input(0);
			auto          boxes_flat = boxes.flat<T>();
			const auto    nr         = boxes.dim_size(0);
			const auto    output_nr  = get_output_nr(nr);

			OP_REQUIRES(context, boxes.dims() == 2, errors::InvalidArgument("boxes must be 2-dimensional"));

			const int   dims_2d[]     = {int(output_nr),4};
			TensorShape output_shape0;

			TensorShapeUtils::MakeShape(dims_2d, 2, &output_shape0);

			Tensor *output_boxes  = NULL;

			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape0, &output_boxes));

			auto outputboxes  = output_boxes->template flat<T>();
			auto output_index = 0;

			if(keep_org_) {
				copy_boxes(boxes_flat.data(),outputboxes.data(),nr);
				output_index = nr;
			}
			std::random_device rd;
			std::mt19937 gen(rd());
			std::uniform_real_distribution<> xoffset(-limits_[0],limits_[0]);
			std::uniform_real_distribution<> yoffset(-limits_[1],limits_[1]);
			std::uniform_real_distribution<> scale(-limits_[2],limits_[2]);
			for(auto i=0; i<size_; ++i) {
				/*process(boxes_flat.data(),outputboxes.data(),
						nr,
						xoffset(gen),
						yoffset(gen),
						scale(gen),
						output_index);*/
				process(boxes_flat.data(),outputboxes.data(),
						nr,
						xoffset,
						yoffset,
						scale,
                        gen,
						output_index);
			}
		}
        template<typename dis_t,typename gen_t>
		void process(const T* src_boxes,T* out_boxes,int nr,dis_t& xoffset,dis_t& yoffset,dis_t& scale,gen_t& gen,int& output_index)
		{
            for(auto k=0; k<nr; ++k) {
                const auto src_box  = src_boxes+4 *k;
                const auto rxoffset = spow(xoffset(gen));
                const auto ryoffset = spow(yoffset(gen));
                const auto rscale   = spow(scale(gen))/2;
                const auto sdx      = (src_box[3]-src_box[1])*rscale;
                const auto sdy      = (src_box[2]-src_box[0])*rscale;
                const auto dx       = (src_box[3]-src_box[1]) *rxoffset;
                const auto dy       = (src_box[2]-src_box[0]) *ryoffset;
                auto       cur_box  = out_boxes+4 *output_index++;

                copy_box(src_box,cur_box);
				cur_box[0] += dy-sdy;
				cur_box[1] += dx-sdx;
				cur_box[2] += dy+sdy;
				cur_box[3] += dx+sdx;
                ba::clamp_range(cur_box,cur_box+4,cur_box,0.0f,1.0f);
            }
		}
        inline float spow(float x) {
            if(x<0)
                return -x*x;
            return x*x;
        }
	private:
		int           size_;
		vector<float> limits_;
		bool          keep_org_;
};
REGISTER_KERNEL_BUILDER(Name("RandomDistoredBoxes").Device(DEVICE_CPU).TypeConstraint<float>("T"), RandomDistoredBoxesOp<CPUDevice, float>);


/**
 * Copyright (c) 2016 Eugene Lazin <4lazin@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "blockstore.h"
#include "log_iface.h"
#include "util.h"
#include "status_util.h"
#include "crc32c.h"
#include "akumuli_version.h"

#include <cassert>

#include <boost/filesystem.hpp>

namespace Akumuli {
namespace StorageEngine {

static u64 hash32(u32 value, u32 bits, u64 seed) {
    // hashes x strongly universally into N bits
    // using the random seed.
    static const u64 a = (1ul << 32) - 1;
    return (a * value + seed) >> (64-bits);
}

static u64 hash(u64 value, u32 bits) {
    auto a = hash32(value & 0xFFFFFFFF, bits, 277);
    auto b = hash32(value >> 32, bits, 337);
    return a ^ b;
}

BlockCache::BlockCache(u32 Nbits)
    : block_cache_(1 << Nbits, PBlock())
    , bits_(Nbits)
    , gen_(dev_())
    , dist_(0, 1 << Nbits)
{
}

int BlockCache::probe(LogicAddr addr) {
    auto h = hash(addr, bits_);
    auto b = block_cache_.at(h);
    if (b) {
        return b->get_addr() == addr ? 2 : 1;
    }
    return 0;
}

void BlockCache::insert(PBlock block) {
    auto addr = block->get_addr();
    auto pr = probe(addr);
    if (pr == 2) {
        // No need to insert, addr already sits in the cache.
        return;
    }
    if (pr == 0) {
        // Eviction. Generate two random hashes. Evict least accessed.
        auto h1 = dist_(gen_);
        auto h2 = dist_(gen_);
        auto p1 = block_cache_.at(h1);
        auto p2 = block_cache_.at(h2);
        if (p1 && p2) {
            if (p1.use_count() > p2.use_count()) {
                block_cache_.at(h2).reset();
            } else if (p1.use_count() < p2.use_count()) {
                block_cache_.at(h1).reset();
            } else {
                if (p1->get_addr() < p2->get_addr()) {
                    block_cache_.at(h1).reset();
                } else {
                    block_cache_.at(h2).reset();
                }
            }
        }
    }
    auto h = hash(addr, bits_);
    block_cache_.at(h) = block;
}

BlockCache::PBlock BlockCache::loockup(LogicAddr addr) {
    auto it = hash(addr, bits_);
    auto p = block_cache_.at(it);
    if (p->get_addr() != addr) {
        p.reset();
    }
    return p;
}

Block::Block(LogicAddr addr, std::vector<u8>&& data)
    : data_(std::move(data))
    , addr_(addr)
    , zptr_(nullptr)
{
}

Block::Block(LogicAddr addr, const u8* ptr)
    : addr_(addr)
    , zptr_(ptr)
{
}

Block::Block()
    : data_(static_cast<size_t>(AKU_BLOCK_SIZE), 0)
    , addr_(EMPTY_ADDR)
    , zptr_(nullptr)
{
}

const u8* Block::get_data() const {
    return zptr_ ? zptr_ : data_.data();
}

const u8* Block::get_cdata() const {
    return zptr_ ? zptr_ : data_.data();
}

bool Block::is_readonly() const {
    return zptr_ != nullptr || addr_ != EMPTY_ADDR;
}

u8* Block::get_data() {
    assert(is_readonly() == false);
    return data_.data();
}

size_t Block::get_size() const {
    return zptr_ ? AKU_BLOCK_SIZE : data_.size();
}

LogicAddr Block::get_addr() const {
    return addr_;
}

void Block::set_addr(LogicAddr addr) {
    addr_ = addr;
}

FileStorage::FileStorage(std::shared_ptr<VolumeRegistry> meta)
    : meta_(MetaVolume::open_existing(meta))
    , current_volume_(0)
    , current_gen_(0)
    , total_size_(0)
{
    typedef VolumeRegistry::VolumeDesc TVol;
    auto volumes = meta->get_volumes();
    std::sort(volumes.begin(), volumes.end(), [](TVol const& a, TVol const& b) {
        return a.id < b.id;
    });
    for (auto const& volrec: volumes) {
        volume_names_.push_back(volrec.path);
    }
    for (u32 ix = 0ul; ix < volumes.size(); ix++) {
        auto volpath = volumes.at(ix).path;
        u32 nblocks = 0;
        aku_Status status = AKU_SUCCESS;
        std::tie(status, nblocks) = meta_->get_nblocks(ix);
        if (status != AKU_SUCCESS) {
            Logger::msg(AKU_LOG_ERROR, std::string("Can't open blockstore, volume " +
                                                   std::to_string(ix) + " failure: " +
                                                   StatusUtil::str(status)));
            AKU_PANIC("Can't open blockstore - " + StatusUtil::str(status));
        }
        auto uptr = Volume::open_existing(volpath.c_str(), nblocks);
        volumes_.push_back(std::move(uptr));
        dirty_.push_back(0);
    }

    for (const auto& vol: volumes_) {
        total_size_ += vol->get_size();
    }

    // set current volume, current volume is a first volume with free space available
    for (u32 i = 0u; i < volumes_.size(); i++) {
        u32 curr_gen, nblocks;
        aku_Status status;
        std::tie(status, curr_gen) = meta_->get_generation(i);
        if (status == AKU_SUCCESS) {
            std::tie(status, nblocks) = meta_->get_nblocks(i);
        } else {
            Logger::msg(AKU_LOG_ERROR, "Can't find current volume, meta-volume corrupted, error: "
                        + StatusUtil::str(status));
            AKU_PANIC("Meta-volume corrupted, " + StatusUtil::str(status));
        }
        if (volumes_[i]->get_size() > nblocks) {
            // Free space available
            current_volume_ = i;
            current_gen_ = curr_gen;
            break;
        }
    }
}

void FileStorage::create(std::vector<std::tuple<u32, std::string>> vols)
{
    std::vector<u32> caps;
    for (auto cp: vols) {
        std::string path;
        u32 capacity;
        std::tie(capacity, path) = cp;
        Volume::create_new(path.c_str(), capacity);
        caps.push_back(capacity);
    }
}

void FileStorage::handle_volume_transition() {
    Logger::msg(AKU_LOG_INFO, "Advance volume called, current gen:" + std::to_string(current_gen_));
    adjust_current_volume();
    aku_Status status;
    std::tie(status, current_gen_) = meta_->get_generation(current_volume_);
    if (status != AKU_SUCCESS) {
        Logger::msg(AKU_LOG_ERROR, "Can't read generation of next volume, " + StatusUtil::str(status));
        AKU_PANIC("Can't read generation of the next volume, " + StatusUtil::str(status));
    }
    // If volume is not empty - reset it and change generation
    u32 nblocks;
    std::tie(status, nblocks) = meta_->get_nblocks(current_volume_);
    if (status != AKU_SUCCESS) {
        Logger::msg(AKU_LOG_ERROR, "Can't read nblocks of next volume, " + StatusUtil::str(status));
        AKU_PANIC("Can't read nblocks of the next volume, " + StatusUtil::str(status));
    }
    if (nblocks != 0) {
        current_gen_ += volumes_.size();
        auto status = meta_->set_generation(current_volume_, current_gen_);
        if (status != AKU_SUCCESS) {
            Logger::msg(AKU_LOG_ERROR, "Can't set generation on volume, " + StatusUtil::str(status));
            AKU_PANIC("Invalid BlockStore state, can't reset volume's generation, " + StatusUtil::str(status));
        }
        // Rest selected volume
        status = meta_->set_nblocks(current_volume_, 0);
        if (status != AKU_SUCCESS) {
            Logger::msg(AKU_LOG_ERROR, "Can't reset nblocks on volume, " + StatusUtil::str(status));
            AKU_PANIC("Invalid BlockStore state, can't reset volume's nblocks, " + StatusUtil::str(status));
        }
        volumes_[current_volume_]->reset();
        dirty_[current_volume_]++;
    }
}

static u32 extract_gen(LogicAddr addr) {
    return addr >> 32;
}

static BlockAddr extract_vol(LogicAddr addr) {
    return addr & 0xFFFFFFFF;
}

static LogicAddr make_logic(u32 gen, BlockAddr addr) {
    return static_cast<u64>(gen) << 32 | addr;
}

std::tuple<aku_Status, LogicAddr> FileStorage::append_block(std::shared_ptr<Block> data) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    BlockAddr block_addr;
    aku_Status status;
    std::tie(status, block_addr) = volumes_[current_volume_]->append_block(data->get_data());
    if (status == AKU_EOVERFLOW) {
      // transition to new/next volume
      handle_volume_transition();
      std::tie(status, block_addr) = volumes_.at(current_volume_)->append_block(data->get_data());
      if (status != AKU_SUCCESS) {
        return std::make_tuple(status, 0ull);
      }
    }
    data->set_addr(block_addr);
    status = meta_->set_nblocks(current_volume_, block_addr + 1);
    if (status != AKU_SUCCESS) {
      AKU_PANIC("Invalid BlockStore state, " + StatusUtil::str(status));
    }
    dirty_[current_volume_]++;
    return std::make_tuple(status, make_logic(current_gen_, block_addr));
}

std::tuple<aku_Status, LogicAddr> FileStorage::append_block(std::shared_ptr<IOVecBlock> data) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    BlockAddr block_addr;
    aku_Status status;
    std::tie(status, block_addr) = volumes_[current_volume_]->append_block(data.get());
    if (status == AKU_EOVERFLOW) {
      // transition to new/next volume
      handle_volume_transition();
      std::tie(status, block_addr) = volumes_.at(current_volume_)->append_block(data.get());
      if (status != AKU_SUCCESS) {
        return std::make_tuple(status, 0ull);
      }
    }
    data->set_addr(block_addr);
    status = meta_->set_nblocks(current_volume_, block_addr + 1);
    if (status != AKU_SUCCESS) {
      AKU_PANIC("Invalid BlockStore state, " + StatusUtil::str(status));
    }
    dirty_[current_volume_]++;
    return std::make_tuple(status, make_logic(current_gen_, block_addr));
}

void FileStorage::flush() {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    /*
    for (size_t ix = 0; ix < dirty_.size(); ix++) {
        if (dirty_[ix]) {
            dirty_[ix] = 0;
            volumes_[ix]->flush();
        }
    }
    */
    for (size_t ix = 0; ix < volumes_.size(); ix++) {
        volumes_[ix]->flush();
    }
    meta_->flush();
}

BlockStoreStats FileStorage::get_stats() const {
    BlockStoreStats stats = {};
    stats.block_size = 4096;
    size_t nvol = meta_->get_nvolumes();
    for (u32 ix = 0; ix < nvol; ix++) {
        aku_Status stat;
        u32 res;
        std::tie(stat, res) = meta_->get_capacity(ix);
        if (stat == AKU_SUCCESS) {
            stats.capacity += res;
        }
        std::tie(stat, res) = meta_->get_nblocks(ix);
        if (stat == AKU_SUCCESS) {
            stats.nblocks += res;
        }
    }
    return stats;
}

PerVolumeStats FileStorage::get_volume_stats() const {
    PerVolumeStats result;
    size_t nvol = meta_->get_nvolumes();
    for (u32 ix = 0; ix < nvol; ix++) {
        BlockStoreStats stats = {};
        stats.block_size = 4096;
        aku_Status stat;
        u32 res;
        std::tie(stat, res) = meta_->get_capacity(ix);
        if (stat == AKU_SUCCESS) {
            stats.capacity += res;
        }
        std::tie(stat, res) = meta_->get_nblocks(ix);
        if (stat == AKU_SUCCESS) {
            stats.nblocks += res;
        }
        auto name = volume_names_.at(ix);
        result[name] = stats;
    }
    return result;
}

LogicAddr FileStorage::get_top_address() const {
    auto off = volumes_.at(current_volume_)->get_size();
    return make_logic(current_gen_, off);
}

static u32 crc32c(const u8* data, size_t size) {
    static crc32c_impl_t impl = chose_crc32c_implementation();
    return impl(0, data, size);
}

u32 FileStorage::checksum(u8 const* data, size_t size) const {
    return crc32c(data, size);
}

u32 FileStorage::checksum(const IOVecBlock& block, size_t offset, size_t size) const {
    static crc32c_impl_t impl = chose_crc32c_implementation();
    u32 crc32 = 0;
    for (int i = 0; i < IOVecBlock::NCOMPONENTS; i++) {
        if (block.get_size(i) < offset || block.get_size(i) == 0 || size == 0) {
            break;
        }
        size_t sz = std::min(block.get_size(i) - offset, size);
        crc32 = impl(crc32, block.get_cdata(i) + offset, sz);
        size -= sz;
        offset = 0;
    }
    return crc32;
}

// FixedSizeFileStorage

FixedSizeFileStorage::FixedSizeFileStorage(std::shared_ptr<VolumeRegistry> meta)
    : FileStorage::FileStorage(meta)
{
    // nothing specific needed except calling the parent constructor
}

std::shared_ptr<FixedSizeFileStorage> FixedSizeFileStorage::open(std::shared_ptr<VolumeRegistry> meta) {
    auto bs = new FixedSizeFileStorage(meta);
    return std::shared_ptr<FixedSizeFileStorage>(bs);
}

bool FixedSizeFileStorage::exists(LogicAddr addr) const {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    auto gen = extract_gen(addr);
    auto vol = extract_vol(addr);
    auto volix = gen % static_cast<u32>(volumes_.size());
    aku_Status status;
    u32 actual_gen;
    std::tie(status, actual_gen) = meta_->get_generation(volix);
    if (status != AKU_SUCCESS) {
      return false;
    }
    u32 nblocks;
    std::tie(status, nblocks) = meta_->get_nblocks(volix);
    if (status != AKU_SUCCESS) {
      return false;
    }
    return actual_gen == gen && vol < nblocks;
}

std::tuple<aku_Status, std::shared_ptr<Block>> FixedSizeFileStorage::read_block(LogicAddr addr) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    aku_Status status;
    auto gen = extract_gen(addr);
    auto vol = extract_vol(addr);
    auto volix = gen % static_cast<u32>(volumes_.size());
    u32 actual_gen;
    u32 nblocks;
    std::tie(status, actual_gen) = meta_->get_generation(volix);
    if (status != AKU_SUCCESS) {
        return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<Block>());
    }
    std::tie(status, nblocks) = meta_->get_nblocks(volix);
    if (status != AKU_SUCCESS) {
        return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<Block>());
    }
    if (actual_gen != gen || vol >= nblocks) {
        return std::make_tuple(AKU_EUNAVAILABLE, std::unique_ptr<Block>());
    }
    // Try to use zero-copy if possible
    const u8* mptr;
    std::tie(status, mptr) = volumes_[volix]->read_block_zero_copy(vol);
    if (status == AKU_SUCCESS) {
        std::shared_ptr<Block> zblock = std::make_shared<Block>(addr, mptr);
        return std::make_tuple(status, std::move(zblock));
    } else if (status == AKU_EUNAVAILABLE) {
        // Fallback to copying if not possible
        std::vector<u8> dest(AKU_BLOCK_SIZE, 0);
        status = volumes_[volix]->read_block(vol, dest.data());
        if (status != AKU_SUCCESS) {
            return std::make_tuple(status, std::unique_ptr<Block>());
        }
        auto block = std::make_shared<Block>(addr, std::move(dest));
        return std::make_tuple(status, std::move(block));
    }
    return std::make_tuple(status, std::unique_ptr<Block>());
}

std::tuple<aku_Status, std::shared_ptr<IOVecBlock>> FixedSizeFileStorage::read_iovec_block(LogicAddr addr) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    aku_Status status;
    auto gen = extract_gen(addr);
    auto vol = extract_vol(addr);
    auto volix = gen % static_cast<u32>(volumes_.size());
    u32 actual_gen;
    u32 nblocks;
    std::tie(status, actual_gen) = meta_->get_generation(volix);
    if (status != AKU_SUCCESS) {
        return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<IOVecBlock>());
    }
    std::tie(status, nblocks) = meta_->get_nblocks(volix);
    if (status != AKU_SUCCESS) {
        return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<IOVecBlock>());
    }
    if (actual_gen != gen || vol >= nblocks) {
        return std::make_tuple(AKU_EUNAVAILABLE, std::unique_ptr<IOVecBlock>());
    }
    // Read data from volume
    std::unique_ptr<IOVecBlock> block;
    std::tie(status, block) = volumes_[volix]->read_block(vol);
    if (status == AKU_SUCCESS) {
        return std::make_tuple(status, std::move(block));
    }
    return std::make_tuple(status, std::unique_ptr<IOVecBlock>());
}

void FixedSizeFileStorage::adjust_current_volume() {
    current_volume_ = (current_volume_ + 1) % volumes_.size();
}

// ExpandableFileStorage

ExpandableFileStorage::ExpandableFileStorage(std::shared_ptr<VolumeRegistry> meta)
    : FileStorage::FileStorage(meta)
    , db_name_(meta->get_dbname())
{
}

std::shared_ptr<ExpandableFileStorage> ExpandableFileStorage::open(std::shared_ptr<VolumeRegistry> meta)
{
    auto bs = new ExpandableFileStorage(meta);
    return std::shared_ptr<ExpandableFileStorage>(bs);
}

bool ExpandableFileStorage::exists(LogicAddr addr) const {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    auto gen = extract_gen(addr);
    auto vol = extract_vol(addr);
    aku_Status status;
    u32 actual_gen;
    std::tie(status, actual_gen) = meta_->get_generation(gen);
    if (status != AKU_SUCCESS) {
      return false;
    }
    u32 nblocks;
    std::tie(status, nblocks) = meta_->get_nblocks(gen);
    if (status != AKU_SUCCESS) {
      return false;
    }
    return actual_gen == gen && vol < nblocks;
}

std::tuple<aku_Status, std::shared_ptr<Block>> ExpandableFileStorage::read_block(LogicAddr addr) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    aku_Status status;
    auto gen = extract_gen(addr);
    auto vol = extract_vol(addr);
    u32 actual_gen;
    u32 nblocks;
    std::tie(status, actual_gen) = meta_->get_generation(gen);
    if (status != AKU_SUCCESS) {
      return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<Block>());
    }
    std::tie(status, nblocks) = meta_->get_nblocks(gen);
    if (status != AKU_SUCCESS) {
        return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<Block>());
    }
    if (actual_gen != gen || vol >= nblocks) {
      return std::make_tuple(AKU_EUNAVAILABLE, std::unique_ptr<Block>());
    }
    // Try to use zero-copy if possible
    const u8* mptr;
    std::tie(status, mptr) = volumes_[gen]->read_block_zero_copy(vol);
    if (status == AKU_SUCCESS) {
        std::shared_ptr<Block> zblock = std::make_shared<Block>(addr, mptr);
        return std::make_tuple(status, std::move(zblock));
    } else if (status == AKU_EUNAVAILABLE) {
        // Fallback to copying if not possible
        std::vector<u8> dest(AKU_BLOCK_SIZE, 0);
        status = volumes_[gen]->read_block(vol, dest.data());
        if (status != AKU_SUCCESS) {
            return std::make_tuple(status, std::unique_ptr<Block>());
        }
        auto block = std::make_shared<Block>(addr, std::move(dest));
        return std::make_tuple(status, std::move(block));
    }
    return std::make_tuple(status, std::unique_ptr<Block>());
}

std::tuple<aku_Status, std::shared_ptr<IOVecBlock>> ExpandableFileStorage::read_iovec_block(LogicAddr addr) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    aku_Status status;
    auto gen = extract_gen(addr);
    auto vol = extract_vol(addr);
    u32 actual_gen;
    u32 nblocks;
    std::tie(status, actual_gen) = meta_->get_generation(gen);
    if (status != AKU_SUCCESS) {
      return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<IOVecBlock>());
    }
    std::tie(status, nblocks) = meta_->get_nblocks(gen);
    if (status != AKU_SUCCESS) {
        return std::make_tuple(AKU_EBAD_ARG, std::unique_ptr<IOVecBlock>());
    }
    if (actual_gen != gen || vol >= nblocks) {
      return std::make_tuple(AKU_EUNAVAILABLE, std::unique_ptr<IOVecBlock>());
    }
    // Read the volume
    std::unique_ptr<IOVecBlock> block;
    std::tie(status, block) = volumes_[vol]->read_block(vol);
    if (status == AKU_SUCCESS) {
        return std::make_tuple(status, std::move(block));
    }
    return std::make_tuple(status, std::unique_ptr<IOVecBlock>());
}

std::unique_ptr<Volume> ExpandableFileStorage::create_new_volume(u32 id) {
    u32 prev_id = current_volume_ - 1;
    boost::filesystem::path prev_path(volumes_[prev_id]->get_path());
    auto pp = prev_path.parent_path();
    std::string basename = std::string(db_name_) + "_" + std::to_string(id) + ".vol";
    boost::filesystem::path new_path = pp / basename;
    Volume::create_new(new_path.c_str(), volumes_[prev_id]->get_size());
    return Volume::open_existing(new_path.c_str(), 0);
}

void ExpandableFileStorage::adjust_current_volume() {
    current_volume_ = current_volume_ + 1;
    if (current_volume_ >= volumes_.size()) {
        // add new volume
        auto vol = create_new_volume(current_volume_);

        // update internal state of this class to be consistent
        dirty_.push_back(0);
        volume_names_.push_back(vol->get_path());
        total_size_ += vol->get_size();

        // update metadata
        meta_->add_volume(current_volume_, vol->get_size(), vol->get_path());

        // finally add new volume to our internal list of volumes
        volumes_.push_back(std::move(vol));
    }
}

//! Address space should be started from this address (otherwise some tests will pass no matter what).
static const LogicAddr MEMSTORE_BASE = 619;

// MemStore
MemStore::MemStore()
    : write_pos_(0)
    , removed_pos_(0)
{
}

MemStore::MemStore(std::function<void(LogicAddr)> append_cb)
    : append_callback_(append_cb)
    , write_pos_(0)
    , removed_pos_(0)
{
}

MemStore::MemStore(std::function<void(LogicAddr)> append_cb,
                   std::function<void(LogicAddr)> read_cb)
    : append_callback_(append_cb)
    , read_callback_(read_cb)
    , write_pos_(0)
    , removed_pos_(0)
{
}

LogicAddr MemStore::remove(size_t n) {
    removed_pos_ = n;
    if (removed_pos_ > buffer_.size()) {
        buffer_.resize(removed_pos_*AKU_BLOCK_SIZE);
        write_pos_ = n;
    }
    return n + MEMSTORE_BASE;
}

u32 MemStore::checksum(u8 const* data, size_t size) const {
    return crc32c(data, size);
}

u32 MemStore::checksum(const IOVecBlock& block, size_t offset , size_t size) const {
    static crc32c_impl_t impl = chose_crc32c_implementation();
    u32 crc32 = 0;
    for (int i = 0; i < IOVecBlock::NCOMPONENTS; i++) {
        if (block.get_size(i) < offset || block.get_size(i) == 0 || size == 0) {
            break;
        }
        size_t sz = std::min(block.get_size(i) - offset, size);
        crc32 = impl(crc32, block.get_cdata(i) + offset, sz);
        size -= sz;
        offset = 0;
    }
    return crc32;
}

std::tuple<aku_Status, std::shared_ptr<Block>> MemStore::read_block(LogicAddr addr) {
    addr -= MEMSTORE_BASE;
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    std::shared_ptr<Block> block;
    u32 offset = static_cast<u32>(AKU_BLOCK_SIZE * addr);
    if (addr < removed_pos_) {
        return std::make_tuple(AKU_EUNAVAILABLE, block);
    }
    if (buffer_.size() < (offset + AKU_BLOCK_SIZE)) {
        return std::make_tuple(AKU_EBAD_ARG, block);
    }
    std::vector<u8> data;
    data.reserve(AKU_BLOCK_SIZE);
    auto begin = buffer_.begin() + offset;
    auto end = begin + AKU_BLOCK_SIZE;
    std::copy(begin, end, std::back_inserter(data));
    block.reset(new Block(addr + MEMSTORE_BASE, std::move(data)));
    if (read_callback_) {
        read_callback_(addr);
    }
    return std::make_tuple(AKU_SUCCESS, block);
}

std::tuple<aku_Status, std::shared_ptr<IOVecBlock>> MemStore::read_iovec_block(LogicAddr addr) {
    addr -= MEMSTORE_BASE;
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    u32 offset = static_cast<u32>(AKU_BLOCK_SIZE * addr);
    std::unique_ptr<IOVecBlock> block;
    if (buffer_.size() < (offset + AKU_BLOCK_SIZE)) {
        return std::make_tuple(AKU_EBAD_ARG, std::move(block));
    }
    if (addr < removed_pos_) {
        return std::make_tuple(AKU_EUNAVAILABLE, std::move(block));
    }
    auto begin = buffer_.begin() + offset;
    auto end = begin + AKU_BLOCK_SIZE;
    block.reset(new IOVecBlock(true));
    u8* dest = block->get_data(0);
    assert(block->get_size(0) == AKU_BLOCK_SIZE);
    std::copy(begin, end, dest);
    if (read_callback_) {
        read_callback_(addr);
    }
    return std::make_tuple(AKU_SUCCESS, std::move(block));
}

std::tuple<aku_Status, LogicAddr> MemStore::append_block(std::shared_ptr<Block> data) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    assert(data->get_size() == AKU_BLOCK_SIZE);
    std::copy(data->get_data(), data->get_data() + AKU_BLOCK_SIZE, std::back_inserter(buffer_));
    if (append_callback_) {
        append_callback_(write_pos_ + MEMSTORE_BASE);
    }
    auto addr = write_pos_++;
    addr += MEMSTORE_BASE;
    data->set_addr(addr);
    return std::make_tuple(AKU_SUCCESS, addr);
}

std::tuple<aku_Status, LogicAddr> MemStore::append_block(std::shared_ptr<IOVecBlock> data) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    for (int i = 0; i < IOVecBlock::NCOMPONENTS; i++) {
        if (data->get_size(i) != 0) {
            const u8* p = data->get_cdata(i);
            std::copy(p, p + IOVecBlock::COMPONENT_SIZE, std::back_inserter(buffer_));
        } else {
            std::fill_n(std::back_inserter(buffer_), IOVecBlock::COMPONENT_SIZE, 0);
        }
    }
    if (append_callback_) {
        append_callback_(write_pos_ + MEMSTORE_BASE);
    }
    auto addr = write_pos_++;
    addr += MEMSTORE_BASE;
    data->set_addr(addr);
    return std::make_tuple(AKU_SUCCESS, addr);
}

void MemStore::flush() {
    // no-op
}

BlockStoreStats MemStore::get_stats() const {
    BlockStoreStats s;
    s.block_size = 4096;
    s.capacity = 1024*4096;
    s.nblocks = write_pos_;
    return s;
}

PerVolumeStats MemStore::get_volume_stats() const {
    PerVolumeStats result;
    BlockStoreStats s;
    s.block_size = 4096;
    s.capacity = 1024*4096;
    s.nblocks = write_pos_;
    result["mem"] = s;
    return result;
}

bool MemStore::exists(LogicAddr addr) const {
    addr -= MEMSTORE_BASE;
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    return addr >= removed_pos_ && addr < write_pos_;
}

u32 MemStore::get_write_pos() {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    return write_pos_;
}

u32 MemStore::reset_write_pos(u32 pos) {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    auto tmp = write_pos_;
    write_pos_ = pos;
    return tmp;
}

LogicAddr MemStore::get_top_address() const {
    std::lock_guard<std::mutex> guard(lock_); AKU_UNUSED(guard);
    return MEMSTORE_BASE + write_pos_;
}

std::shared_ptr<MemStore> BlockStoreBuilder::create_memstore() {
    return std::make_shared<MemStore>();
}

std::shared_ptr<MemStore> BlockStoreBuilder::create_memstore(std::function<void(LogicAddr)> append_cb) {
    return std::make_shared<MemStore>(append_cb);
}

std::shared_ptr<MemStore> BlockStoreBuilder::create_memstore(std::function<void(LogicAddr)> append_cb,
                                                               std::function<void(LogicAddr)> read_cb) {
    return std::make_shared<MemStore>(append_cb, read_cb);
}

}}  // namespace