/**
 * @file upsc.cpp
 * @brief implementation of hdm upscaling
 * @author Alina Yapparova
 * @version
 * @date 2012-20-02
 */

#include <iomanip>
#include <iostream>

#include <list>
#include <map>

#include "bs_kernel.h"
#include "bs_misc.h"

#include "upsc.h"
#include "conf.h"

using namespace boost;

#ifdef BSPY_EXPORTING_PLUGIN

#include <boost/python.hpp>
using namespace boost::python;

#endif //BSPY_EXPORTING_PLUGIN

namespace blue_sky
{

  upsc::upsc (bs_type_ctor_param)
    {

      sp_prop = BS_KERNEL.create_object ("prop");
      if (!sp_prop)
        {
          bs_throw_exception ("Type (prop) not refractered");
        }

    }
  upsc::upsc (const upsc& rhs)
        : bs_refcounter ()
    {
      *this = rhs;
    }
  void
  upsc::init_prop ()
    {

    }

spv_float upsc::upscale_grid_zcolumn ( t_long Nx, t_long Ny, t_long Nz, spv_float zcorn_, spv_uint layers_ )
{
    t_int k1, k2, new_k;
    t_long new_Nz, new_zcorn_size, layer_size;
    v_uint& layers = *layers_;
    v_uint::iterator lit, lit_next;
    v_float& zcorn = *zcorn_;

    new_Nz = layers.size();
    new_zcorn_size = 8*Nx*Ny*new_Nz;
    layer_size = 4*Nx*Ny;

    spv_float new_zcorn = BS_KERNEL.create_object(v_float::bs_type());
    new_zcorn->resize(new_zcorn_size);

    new_k = 0;

    for (lit=layers.begin();lit!=layers.end();lit++)
        {
            k1 = (*lit);
            lit_next = lit + 1;
            if (lit_next!=layers.end())
                k2 = (*lit_next);
            else
                k2 = Nz;
            std::copy ( &zcorn[2*k1*layer_size], &zcorn[(2*k1+1)*layer_size], &(*new_zcorn)[(new_k++)*layer_size] );
            std::copy ( &zcorn[(2*k2-1)*layer_size], &zcorn[2*k2*layer_size], &(*new_zcorn)[(new_k++)*layer_size] );
        }

   return new_zcorn;
}

bp::tuple upsc::upscale_grid ( t_long Nx, t_long Ny, t_long Nz, t_long ux, t_long uy,
                               spv_float coord_, spv_float zcorn_, spv_uint layers_ )
{
    t_int i, j, n, k1, k2, k, ind;
    t_long new_Nx, new_Ny, new_Nz, new_coord_size, new_zcorn_size, layer_size;
    v_uint& layers = *layers_;
    v_uint::iterator lit, lit_next;
    v_float& coord = *coord_;
    v_float& zcorn = *zcorn_;

    new_Nx = ceil(double(Nx)/double(ux));
    new_Ny = ceil(double(Ny)/double(uy));
    new_Nz = layers.size();
    new_coord_size = 6*(new_Nx+1)*(new_Ny+1);
    new_zcorn_size = 8*new_Nx*new_Ny*new_Nz;
    layer_size = 4*Nx*Ny;

    spv_float new_coord = BS_KERNEL.create_object(v_float::bs_type());
    new_coord->resize(new_coord_size);
    spv_float new_zcorn = BS_KERNEL.create_object(v_float::bs_type());
    new_zcorn->resize(new_zcorn_size);

    n = 0;
    for (j=0; j<new_Ny; j++)
        {
            for (i=0; i<new_Nx; i++)
                {
                    std::copy ( &coord[6*(uy*j*(Nx+1)+ux*i)], &coord[6*(uy*j*(Nx+1)+ux*i+1)], &(*new_coord)[n] );
                    n += 6;
                }
            std::copy ( &coord[6*(uy*j*(Nx+1)+Nx)], &coord[6*(uy*j*(Nx+1)+Nx+1)], &(*new_coord)[n] );
            n += 6;
        }
    for (i=0; i<new_Nx; i++)
        {
            std::copy ( &coord[6*(Ny*(Nx+1)+ux*i)], &coord[6*(Ny*(Nx+1)+ux*i+1)], &(*new_coord)[n] );
            n += 6;
        }
    std::copy ( &coord[6*(Ny*(Nx+1)+Nx)], &coord[6*(Ny*(Nx+1)+Nx+1)], &(*new_coord)[n] );

    k = 0;
    for (lit=layers.begin();lit!=layers.end();lit++)
        {
            k1 = (*lit);
            lit_next = lit + 1;
            if (lit_next!=layers.end())
                k2 = (*lit_next);
            else
                k2 = Nz;

            // tops
            for (j=0; j<new_Ny-1; j++)
                {
                    for (i=0; i<new_Nx-1; i++)
                        {
                            ind = 2*k1*layer_size + 4*uy*j*Nx;
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                        }
                    ind = 2*k1*layer_size + 4*uy*j*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx-1)];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];

                    for (i=0; i<new_Nx-1; i++)
                        {
                            ind = 2*k1*layer_size + (2*uy*(j+1)-1)*2*Nx;
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                        }
                    ind = 2*k1*layer_size + (2*uy*(j+1)-1)*2*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx-1)];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];
                }

            for (i=0; i<new_Nx-1; i++)
                {
                    ind = 2*k1*layer_size + 4*uy*(new_Ny-1)*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                }
            ind = 2*k1*layer_size + 4*uy*(new_Ny-1)*Nx;
            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx-1)];
            (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];

            for (i=0; i<new_Nx-1; i++)
                {
                    ind = 2*k1*layer_size + (2*Ny-1)*2*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                }
            ind = 2*k1*layer_size + (2*Ny-1)*2*Nx;
            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx - 1)];
            (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];

            // bottoms
            for (j=0; j<new_Ny-1; j++)
                {
                    for (i=0; i<new_Nx-1; i++)
                        {
                            ind = (2*k2-1)*layer_size + 4*uy*j*Nx;
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                        }
                    ind = (2*k2-1)*layer_size + 4*uy*j*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx-1)];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];

                    for (i=0; i<new_Nx-1; i++)
                        {
                            ind = (2*k2-1)*layer_size + (2*uy*(j+1)-1)*2*Nx;
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                        }
                    ind = (2*k2-1)*layer_size + (2*uy*(j+1)-1)*2*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx-1)];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];
                }

            for (i=0; i<new_Nx-1; i++)
                {
                    ind = (2*k2-1)*layer_size + 4*uy*(new_Ny-1)*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                }
            ind = (2*k2-1)*layer_size + 4*uy*(new_Ny-1)*Nx;
            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx-1)];
            (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];

            for (i=0; i<new_Nx-1; i++)
                {
                    ind = (2*k2-1)*layer_size + (2*Ny-1)*2*Nx;
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*i];
                    (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(i+1) - 1];
                }
            ind = (2*k2-1)*layer_size + (2*Ny-1)*2*Nx;
            (*new_zcorn)[(k++)] = zcorn[ind + 2*ux*(new_Nx - 1)];
            (*new_zcorn)[(k++)] = zcorn[ind + 2*Nx - 1];
        }
   return bp::make_tuple (new_coord, new_zcorn);
}

t_double upsc::calc_sum_dW ( t_long k1, t_long k2, t_long Nx, t_long Ny,
                                          spv_float vol_, spv_float ntg_, spv_float poro_, spv_float permx_ )
{
    v_float& vol = *vol_;
    v_float& ntg = *ntg_;
    v_float& poro = *poro_;
    v_float& permx = *permx_;

    t_long i, j, n, z1, z2, layer_size;
    t_long index[2];
    t_double p[2];
    t_double dW, sum_dW = 0, sum_vol;

    layer_size = Nx*Ny;
    z1 = k1*layer_size;
    z2 = k2*layer_size;

    for (j = 0; j < Ny; j++)
      {
        for (i = 0; i < Nx; i++)
          {
            index[0] = i + j * Nx + z1;
            index[1] = i + j * Nx + z2;

            for (n = 0; n < 2; n++)
                {
                    if (poro[index[n]  != 0])
                        p[n] = ntg[index[n]] * permx[index[n]] / poro[index[n]];
                    else
                        p[n] = 0;
                }

            sum_vol = (vol[index[0]] + vol[index[1]]);
            if (sum_vol != 0)
                dW = (p[0] - p[1]) * (p[0] - p[1]) * vol[index[0]] * vol[index[1]] / sum_vol;
            else
                dW = 0;
            sum_dW += dW;
          }
      }
    return sum_dW;
}

spv_float upsc::upscale_saturation_cube (t_long Nx, t_long Ny, t_long Nz, t_long Nz_upsc,
                                         spv_uint layers_, spv_float vol_, spv_float ntg_, spv_float poro_, spv_float sat_)
{
    t_int i, j, k, n, k1, k2, ind, z;
    t_long layer_size, new_cube_size;
    t_double sum_sat_poro_ntg_vol, sum_poro_ntg_vol;

    v_uint& layers = *layers_;
    v_float& vol = *vol_;
    v_float& ntg = *ntg_;
    v_float& poro = *poro_;
    v_float& sat = *sat_;

    spv_float new_sat = BS_KERNEL.create_object(v_float::bs_type());

    // important: XYZ order
    // index <-- (i, j, k)
    // index = i + j*Nx + k*Nx*Ny;

    layer_size = Nx * Ny;

    for (n = 0; n < Nz_upsc; ++n)
        {
            k1 = layers[n];

            if (n == Nz_upsc-1)
                k2 = Nz;
            else
                k2 = layers[n+1];

            if (k1 != k2)
            for (j = 0; j < Ny; ++j)
                for (i = 0; i < Nx; ++i)
                    {
                        sum_sat_poro_ntg_vol = 0;
                        sum_poro_ntg_vol = 0;

                        for (k = k1; k < k2; ++k)
                            {
                                z = k*layer_size;
                                ind = i + j * Nx + z;
                                sum_sat_poro_ntg_vol += sat[ind]*poro[ind]*ntg[ind]*vol[ind];
                                sum_poro_ntg_vol += poro[ind]*ntg[ind]*vol[ind];
                            }
                        ind = i + j * Nx + k1 * layer_size;
                        if (sum_sat_poro_ntg_vol != 0)
                            sat[ind] = sum_sat_poro_ntg_vol/sum_poro_ntg_vol;
                        else
                            sat[ind] = 0;
                    }

        }

    new_cube_size = Nz_upsc*layer_size;
    new_sat->resize(new_cube_size);

    i = 0;
    for ( k = 0; k < Nz_upsc; ++k)
        {
            std::copy ( &sat[k*layer_size], &sat[(k+1)*layer_size], &(*new_sat)[i*layer_size] );
            i++;
        }

    return new_sat;
}

spv_float upsc::upscale_sat_cube_xy (t_long Nx, t_long Ny, t_long ux, t_long uy, t_long Nz,
                                     spv_uint layers_, spv_float vol_, spv_float ntg_, spv_float poro_, spv_float sat_)
{
    t_int i, j, ii, jj, k, new_k, n, k1, k2, index, ind, new_ind;
    t_long new_Nx, new_Ny, new_Nz;
    t_long layer_size, new_layer_size, new_cube_size;
    t_double sum_sat_poro_ntg_vol, sum_poro_ntg_vol;

    v_uint& layers = *layers_;
    v_float& vol = *vol_;
    v_float& ntg = *ntg_;
    v_float& poro = *poro_;
    v_float& sat = *sat_;

    t_int rx, ry;
    rx = Nx % ux;
    ry = Ny % uy;
    new_Nx = ceil(double(Nx)/double(ux));
    new_Ny = ceil(double(Ny)/double(uy));
    new_Nz = layers.size();

    spv_float new_sat = BS_KERNEL.create_object(v_float::bs_type());
    new_cube_size = new_Nx*new_Ny*new_Nz;
    new_sat->resize(new_cube_size);

    // important: XYZ order
    // index <-- (i, j, k)
    // index = i + j*Nx + k*Nx*Ny;

    layer_size = Nx * Ny;
    new_layer_size = new_Nx * new_Ny;

    new_k = 0;
    for (n = 0; n < new_Nz; ++n)
      {
        k1 = layers[n];

        if (n == new_Nz-1)
            k2 = Nz;
        else
            k2 = layers[n+1];

        for (j = 0; j < new_Ny-1; ++j)
          {
            for (i = 0; i < new_Nx-1; ++i)
              {
                index  = ux*i + uy*j*Nx + k1*layer_size;
                new_ind = i + j*new_Nx + new_k*new_layer_size;

                sum_sat_poro_ntg_vol = 0;
                sum_poro_ntg_vol = 0;

                for (k = 0; k < (k2-k1); ++k)
                  {
                    for (jj = 0; jj < uy; jj++)
                      {
                        for (ii = 0; ii < ux; ii++)
                          {
                            ind = index + ii + jj*Nx + k*layer_size;
                            sum_sat_poro_ntg_vol += sat[ind]*poro[ind]*ntg[ind]*vol[ind];
                            sum_poro_ntg_vol += poro[ind]*ntg[ind]*vol[ind];
                          }
                      }
                  }

                if (sum_sat_poro_ntg_vol != 0)
                    (*new_sat)[new_ind] = sum_sat_poro_ntg_vol/sum_poro_ntg_vol;
                else
                    (*new_sat)[new_ind] = 0;
              }

            index  = ux*(new_Nx-1) + uy*j*Nx + k1*layer_size;
            new_ind = new_Nx-1 + j*new_Nx + new_k*new_layer_size;

            sum_sat_poro_ntg_vol = 0;
            sum_poro_ntg_vol = 0;

            for (k = 0; k < (k2-k1); ++k)
              {
                for (jj = 0; jj < uy; jj++)
                  {
                    for (ii = 0; ii < rx; ii++)
                      {
                        ind = index + ii + jj*Nx + k*layer_size;
                        sum_sat_poro_ntg_vol += sat[ind]*poro[ind]*ntg[ind]*vol[ind];
                        sum_poro_ntg_vol += poro[ind]*ntg[ind]*vol[ind];
                      }
                  }
              }

            if (sum_sat_poro_ntg_vol != 0)
                (*new_sat)[new_ind] = sum_sat_poro_ntg_vol/sum_poro_ntg_vol;
            else
                (*new_sat)[new_ind] = 0;
          }

        for (i = 0; i < new_Nx-1; ++i)
          {
            index  = ux*i + uy*(new_Ny-1)*Nx + k1*layer_size;
            new_ind = i + (new_Ny-1)*new_Nx + new_k*new_layer_size;

            sum_sat_poro_ntg_vol = 0;
            sum_poro_ntg_vol = 0;

            for (k = 0; k < (k2-k1); ++k)
              {
                for (jj = 0; jj < ry; jj++)
                  {
                    for (ii = 0; ii < ux; ii++)
                      {
                        ind = index + ii + jj*Nx + k*layer_size;
                        sum_sat_poro_ntg_vol += sat[ind]*poro[ind]*ntg[ind]*vol[ind];
                        sum_poro_ntg_vol += poro[ind]*ntg[ind]*vol[ind];
                      }
                  }
              }
            if (sum_sat_poro_ntg_vol != 0)
                (*new_sat)[new_ind] = sum_sat_poro_ntg_vol/sum_poro_ntg_vol;
            else
                (*new_sat)[new_ind] = 0;
          }

        index  = ux*(new_Nx-1) + uy*(new_Ny-1)*Nx + k1*layer_size;
        new_ind = new_Nx-1 + (new_Ny-1)*new_Nx + new_k*new_layer_size;

        sum_sat_poro_ntg_vol = 0;
        sum_poro_ntg_vol = 0;

        for (k = 0; k < (k2-k1); ++k)
          {
            for (jj = 0; jj < ry; jj++)
              {
                for (ii = 0; ii < rx; ii++)
                  {
                    ind = index + ii + jj*Nx + k*layer_size;
                    sum_sat_poro_ntg_vol += sat[ind]*poro[ind]*ntg[ind]*vol[ind];
                    sum_poro_ntg_vol += poro[ind]*ntg[ind]*vol[ind];
                  }
              }
          }
        if (sum_sat_poro_ntg_vol != 0)
            (*new_sat)[new_ind] = sum_sat_poro_ntg_vol/sum_poro_ntg_vol;
        else
            (*new_sat)[new_ind] = 0;

        new_k ++;
      }

    return new_sat;
}

int upsc::upscale_cubes ( t_long k1, t_long k2, t_long Nx, t_long Ny,
                          spv_float vol_, spv_float ntg_, spv_float poro_, spv_float permx_ )
{
    v_float& vol = *vol_;
    v_float& ntg = *ntg_;
    v_float& poro = *poro_;
    v_float& permx = *permx_;

    t_long i, j, index[2], z1, z2, layer_size;
    t_double vol_sum, ntg_vol_sum, poro_ntg_vol_sum, permx_ntg_vol_sum;

    layer_size = Nx*Ny;
    z1 = k1*layer_size;
    z2 = k2*layer_size;

    for (j = 0;j < Ny; j++)
      {
        for (i = 0;i < Nx; i++)
          {
            index[0] = i + j * Nx + z1;
            index[1] = i + j * Nx + z2;

            vol_sum = vol[index[0]] + vol[index[1]];
            ntg_vol_sum = ntg[index[0]]*vol[index[0]] +  ntg[index[1]]*vol[index[1]];
            poro_ntg_vol_sum = poro[index[0]]*ntg[index[0]]*vol[index[0]] + poro[index[1]]*ntg[index[1]]*vol[index[1]];
            permx_ntg_vol_sum = permx[index[0]]*ntg[index[0]]*vol[index[0]] + permx[index[1]]*ntg[index[1]]*vol[index[1]];

            vol[index[0]] = vol_sum;
            if (vol_sum != 0)
                ntg[index[0]] = ntg_vol_sum/vol_sum;
            else
                ntg[index[0]] = 0;

            if (ntg_vol_sum != 0)
                {
                    poro[index[0]] = poro_ntg_vol_sum/ntg_vol_sum;
                    permx[index[0]] = permx_ntg_vol_sum/ntg_vol_sum;
                }
            else
                {
                    poro[index[0]] = 0;
                    permx[index[0]] = 0;
                }

          }
      }

    return 0;
}

bp::tuple upsc::upscale_cubes_xy ( t_long Nx, t_long Ny, t_long Nz, t_long ux, t_long uy,
                                   spv_float vol_, spv_float ntg_, spv_float poro_)
{
    v_float& vol = *vol_;
    v_float& ntg = *ntg_;
    v_float& poro = *poro_;

    spv_float new_vol = BS_KERNEL.create_object(v_float::bs_type());
    spv_float new_ntg = BS_KERNEL.create_object(v_float::bs_type());
    spv_float new_poro = BS_KERNEL.create_object(v_float::bs_type());

    t_long new_cube_size, new_Nx, new_Ny;
    t_int rx, ry;
    new_Nx = ceil(double(Nx)/double(ux));
    new_Ny = ceil(double(Ny)/double(uy));
    rx = Nx % ux;
    ry = Ny % uy;
    new_cube_size = new_Nx*new_Ny*Nz;

    new_vol->resize(new_cube_size);
    new_ntg->resize(new_cube_size);
    new_poro->resize(new_cube_size);

    t_long i, j, ii, jj, k, layer_size, new_layer_size;
    t_long ind, index, new_ind;
    t_double vol_sum, ntg_vol_sum, poro_ntg_vol_sum;
    layer_size = Nx*Ny;
    new_layer_size = new_Nx*new_Ny;

    for (k = 0; k < Nz; k++)
      {
        for (j = 0; j < new_Ny-1; j++)
          {
            for (i = 0; i < new_Nx-1; i++)
              {
                ind  = ux*i + uy*j*Nx + k*layer_size;
                new_ind = i + j*new_Nx + k*new_layer_size;

                vol_sum = 0;
                ntg_vol_sum = 0;
                poro_ntg_vol_sum = 0;

                for (jj = 0; jj < uy; jj++)
                  {
                    for (ii = 0; ii < ux; ii++)
                      {
                        index = ind + ii;
                        vol_sum += vol[index];
                        ntg_vol_sum += ntg[index]*vol[index];
                        poro_ntg_vol_sum += poro[index]*ntg[index]*vol[index];
                      }
                    ind  += Nx;
                  }
                (*new_vol)[new_ind] = vol_sum;

                if (vol_sum != 0)
                    (*new_ntg)[new_ind] = ntg_vol_sum/vol_sum;
                else
                    (*new_ntg)[new_ind] = 0;

                if (ntg_vol_sum != 0)
                  {
                    (*new_poro)[new_ind] = poro_ntg_vol_sum/ntg_vol_sum;
                  }
                else
                  {
                    (*new_poro)[new_ind] = 0;
                  }
              }

            ind  = ux*(new_Nx-1) + uy*j*Nx + k*layer_size;
            new_ind = new_Nx-1 + j*new_Nx + k*new_layer_size;

            vol_sum = 0;
            ntg_vol_sum = 0;
            poro_ntg_vol_sum = 0;

            for (jj = 0; jj < uy; jj++)
              {
                for (ii = 0; ii < rx; ii++)
                  {
                    index = ind + ii;
                    vol_sum += vol[index];
                    ntg_vol_sum += ntg[index]*vol[index];
                    poro_ntg_vol_sum += poro[index]*ntg[index]*vol[index];
                  }
                ind  += Nx;
              }
            (*new_vol)[new_ind] = vol_sum;

            if (vol_sum != 0)
                (*new_ntg)[new_ind] = ntg_vol_sum/vol_sum;
            else
                (*new_ntg)[new_ind] = 0;

            if (ntg_vol_sum != 0)
              {
                (*new_poro)[new_ind] = poro_ntg_vol_sum/ntg_vol_sum;
              }
            else
              {
                (*new_poro)[new_ind] = 0;
              }
          }

        for (i = 0; i < new_Nx-1; i++)
          {
            ind  = ux*i + uy*(new_Ny-1)*Nx + k*layer_size;
            new_ind = i + (new_Ny-1)*new_Nx + k*new_layer_size;

            vol_sum = 0;
            ntg_vol_sum = 0;
            poro_ntg_vol_sum = 0;

            for (jj = 0; jj < ry; jj++)
              {
                for (ii = 0; ii < ux; ii++)
                  {
                    index = ind + ii;
                    vol_sum += vol[index];
                    ntg_vol_sum += ntg[index]*vol[index];
                    poro_ntg_vol_sum += poro[index]*ntg[index]*vol[index];
                  }
                ind  += Nx;
              }
            (*new_vol)[new_ind] = vol_sum;

            if (vol_sum != 0)
                (*new_ntg)[new_ind] = ntg_vol_sum/vol_sum;
            else
                (*new_ntg)[new_ind] = 0;

            if (ntg_vol_sum != 0)
              {
                (*new_poro)[new_ind] = poro_ntg_vol_sum/ntg_vol_sum;
              }
            else
              {
                (*new_poro)[new_ind] = 0;
              }
          }

        ind  = ux*(new_Nx-1) + uy*(new_Ny-1)*Nx + k*layer_size;
        new_ind = new_Nx-1 + (new_Ny-1)*new_Nx + k*new_layer_size;

        vol_sum = 0;
        ntg_vol_sum = 0;
        poro_ntg_vol_sum = 0;

        for (jj = 0; jj < ry; jj++)
          {
            for (ii = 0; ii < rx; ii++)
              {
                index = ind + ii;
                vol_sum += vol[index];
                ntg_vol_sum += ntg[index]*vol[index];
                poro_ntg_vol_sum += poro[index]*ntg[index]*vol[index];
              }
            ind  += Nx;
          }
        (*new_vol)[new_ind] = vol_sum;

        if (vol_sum != 0)
            (*new_ntg)[new_ind] = ntg_vol_sum/vol_sum;
        else
            (*new_ntg)[new_ind] = 0;

        if (ntg_vol_sum != 0)
          {
            (*new_poro)[new_ind] = poro_ntg_vol_sum/ntg_vol_sum;
          }
        else
          {
            (*new_poro)[new_ind] = 0;
          }
      }

    return bp::make_tuple(new_ntg, new_poro);
}

t_double upsc::solve_pressure_zcolumn (t_long Ny, t_long Nz, t_long i, t_long j, t_long k1, t_long k2, BS_SP(rs_mesh_iface) sp_mesh_iface)
{
    t_long size, k, n, index, ext_ind[2];
    plane_t plane[2];
    mesh_element3d element[2];
    fpoint3d_t center[2];

    t_double tz;
    t_double p_left = 1, p_right = 0;
    t_double dl, dL, dp, dP, K;

    spv_double p = BS_KERNEL.create_object(v_double::bs_type());
    spv_double rhs = BS_KERNEL.create_object(v_double::bs_type());
    spv_float tran_vals = BS_KERNEL.create_object(v_float::bs_type());
    sp_dens_mtx_t tran = BS_KERNEL.create_object("dens_matrix");
    sp_blu_solver_t solver = BS_KERNEL.create_object("blu_solver");

    size = k2 - k1 +1;

    p->resize(size);
    rhs->resize(size);

    tran->init(size, size, 60);
    tran_vals = tran->get_values();

    v_float& A = *tran_vals;
    v_double& b = *rhs;

    smart_ptr<bs_mesh_grdecl> sp_mesh(sp_mesh_iface, bs_static_cast());
    mesh_grdecl mesh = sp_mesh->get_wrapped();

    // important: ZYX order
    // direction dir along_dim1->X (along_dim2->Y, along_dim3->Z)

    // k = k1 //////////////////////////////////////////////////////////////////////////////////////////
    ext_ind[0] = k1 + j * Nz + i * Ny * Nz;
    mesh.calc_element (i, j, k1, element[0]);
    center[0] = element[0].get_center();
    element[0].get_plane (z_axis_minus, plane[0]);
    tz = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim3);

    // FIXME
    b[0] = -tz*p_left;
    A[0] -= tz;

    // k1 < k < k2 /////////////////////////////////////////////////////////////////////////////////////
    for (k = k1; k < k2; ++k)
        {
            ext_ind[0] = k + j * Nz + i * Ny * Nz;
            mesh.calc_element (i, j, k, element[0]);
            center[0] = element[0].get_center();

            ext_ind[1] = ext_ind[0] + 1;
            mesh.calc_element (i, j, k + 1, element[1]);
            element[0].get_plane (z_axis_plus, plane[0]);

            //element[1].get_plane (z_axis_minus, plane[1]);

            center[1] = element[1].get_center();
            // don't pass plane[1] to calc_tran
            // because zcolumn cells are always fully adjacent
            //tz = mesh.calc_tran(ext_ind[0], ext_ind[1], plane[0], center[0], center[1], along_dim3, &plane[1]);
            tz = mesh.calc_tran(ext_ind[0], ext_ind[1], plane[0], center[0], center[1], along_dim3);

            n = k - k1;
            // Z-
            index = n + (n+1)*size;
            A[index] = tz;
            index += 1;
            A[index] -= tz;

            // Z+
            index = (n+1) + n*size;
            A[index] = tz;
            index -= 1;
            A[index] -= tz;
        }

    // k = k2 //////////////////////////////////////////////////////////////////////////////////////////
    ext_ind[0] = k2 + j * Nz + i * Ny * Nz;
    mesh.calc_element (i, j, k2, element[0]);
    center[0] = element[0].get_center();
    element[0].get_plane (z_axis_plus, plane[0]);
    tz = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim3);

    b[size-1] = -tz*p_right;
    index  =  size*size - 1;
    A[index] -= tz;

    // solve system, find pressure
    solver->setup (tran);
    solver->solve (tran, rhs, p);

    dp = (p_left - (*p)[0]);
    dP = ( (p_left + (*p)[0])/2 - ((*p)[size-1] + p_right)/2 );

    mesh.calc_element (i, j, k1, element[0]);
    element[0].get_plane (z_axis_minus, plane[0]);
    center[1] = element[0].get_center ();
    get_plane_center (plane[0],  center[0]);

    dl = get_len(center[0], center[1])*2;

    mesh.calc_element (i, j, k2, element[1]);
    element[1].get_plane (z_axis_plus, plane[1]);
    get_plane_center (plane[1],  center[1]);

    dL = get_len(center[0], center[1]);

    if (dl < 10*EPSILON)
        {
            dl = 2*EPSILON;
            dL += EPSILON;
        }

    K = (dp/dl)/(dP/dL);

    return K;
}

t_double upsc::solve_pressure_block (t_int direction, t_long Ny, t_long Nz, t_long i1, t_long i2, t_long j1, t_long j2, t_long k1, t_long k2, spv_float ntg_, BS_SP(rs_mesh_iface) sp_mesh_iface)
{
    const t_double cdarcy = 0.008640;
    t_long size, i, j, k, ind[2], ext_ind[2], index, ny, nz;
    plane_t plane[2];
    mesh_element3d element[2];
    fpoint3d_t center[2];

    t_double tz, ty, tx;
    t_double p_left = 1, p_right = 0;
    t_double dL, dP, S, K, Q;

    spv_double p = BS_KERNEL.create_object(v_double::bs_type());
    spv_double rhs = BS_KERNEL.create_object(v_double::bs_type());
    spv_float tran_vals = BS_KERNEL.create_object(v_float::bs_type());
    sp_dens_mtx_t tran = BS_KERNEL.create_object("dens_matrix");
    sp_blu_solver_t solver = BS_KERNEL.create_object("blu_solver");

    size = (i2-i1)*(j2-j1)*(k2-k1);
    ny = j2 - j1;
    nz = k2 - k1;

    p->resize(size);
    rhs->resize(size);

    tran->init(size, size, 60);
    tran_vals = tran->get_values();

    v_float& A = *tran_vals;
    v_double& b = *rhs;
    v_float& ntg = *ntg_;

    smart_ptr<bs_mesh_grdecl> sp_mesh(sp_mesh_iface, bs_static_cast());
    mesh_grdecl mesh = sp_mesh->get_wrapped();

    // important: ZYX order

    // direction dir along_dim1->X (2->Y, 3->Z)

    for (i = i1; i < i2; ++i)
        for (j = j1; j < j2; ++j)
            for (k = k1; k < k2; ++k)
              {
                ext_ind[0] = k + j * Nz + i * Ny * Nz;
                ind[0] = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
                mesh.calc_element (i, j, k, element[0]);
                center[0] = element[0].get_center();

                // TRANZ /////////////////////////////////////////////////////////////////////////////////////////////
                if (k!=k2-1)
                    {
                        ext_ind[1] = ext_ind[0] + 1;
                        ind[1] = ind[0] + 1;
                        mesh.calc_element (i, j, k + 1, element[1]);
                        element[0].get_plane (z_axis_plus, plane[0]);
                        element[1].get_plane (z_axis_minus, plane[1]);
                        center[1] = element[1].get_center();
                        tz = mesh.calc_tran(ext_ind[0], ext_ind[1], plane[0], center[0], center[1], along_dim3, &plane[1]);

                        // Z-
                        index = ind[0] + ind[1]*size;
                        A[index] = tz;
                        index += 1;
                        A[index] -= tz;

                        // Z+
                        index = ind[1] + ind[0]*size;
                        A[index] = tz;
                        index -= 1;
                        A[index] -= tz;
                    }

                // TRANY /////////////////////////////////////////////////////////////////////////////////////////////
                if (j != j2-1)
                    {
                        ext_ind[1] = ext_ind[0] + Nz;
                        ind[1] = ind[0] + nz;
                        mesh.calc_element (i, j + 1, k, element[1]);
                        element[0].get_plane (y_axis_plus, plane[0]);
                        element[1].get_plane (y_axis_minus, plane[1]);
                        center[1] = element[1].get_center();
                        ty = mesh.calc_tran(ext_ind[0], ext_ind[1], plane[0], center[0], center[1], along_dim2, &plane[1]);

                        // Y-
                        index = ind[0] + ind[1]*size;
                        A[index] = ty;
                        index += nz;
                        A[index] -= ty;

                        // Y+
                        index = ind[1] + ind[0]*size;
                        A[index] = ty;
                        index -= nz;
                        A[index] -= ty;
                    }

                // TRANX /////////////////////////////////////////////////////////////////////////////////////////////
                if (i != i2-1)
                    {
                        ext_ind[1] = ext_ind[0] + Ny * Nz;
                        ind[1] = ind[0] + ny*nz;
                        mesh.calc_element (i + 1, j, k, element[1]);
                        element[0].get_plane (x_axis_plus, plane[0]);
                        element[1].get_plane (x_axis_minus, plane[1]);
                        center[1] = element[1].get_center();
                        tx = mesh.calc_tran(ext_ind[0], ext_ind[1], plane[0], center[0], center[1], along_dim1, &plane[1]);

                        // X-
                        index = ind[0] + ind[1]*size;
                        A[index] = tx;
                        index += ny*nz;
                        A[index] -= tx;

                        // X+
                        index = ind[1] + ind[0]*size;
                        A[index] = tx;
                        index -= ny*nz;
                        A[index] -= tx;

                    }
               // Boundary Conditions /////////////////////////////////////////////////////////////////////////////////
               if (direction == 1)
                 {
                    if (i == i1)
                        {
                           element[0].get_plane (x_axis_minus, plane[0]);
                           tx = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim1);
                           b[ind[0]] = -tx*p_left;
                           index  =  ind[0] + ind[0]*size;
                           A[index] -= tx;
                        }
                    if (i == i2-1)
                        {
                           element[0].get_plane (x_axis_plus, plane[0]);
                           tx = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim1);
                           b[ind[0]] = -tx*p_right;
                           index  =  ind[0] + ind[0]*size;
                           A[index] -= tx;
                        }
                 }
               else if (direction == 2)
                 {
                    if (j == j1)
                        {
                           element[0].get_plane (y_axis_minus, plane[0]);
                           ty = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim2);
                           b[ind[0]] = -ty*p_left;
                           index  =  ind[0] + ind[0]*size;
                           A[index] -= ty;
                        }
                    if (j == j2-1)
                        {
                           element[0].get_plane (y_axis_plus, plane[0]);
                           ty = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim2);
                           b[ind[0]] = -ty*p_right;
                           index  =  ind[0] + ind[0]*size;
                           A[index] -= ty;
                        }
                 }
               else if (direction == 3)
                 {
                    if (k == k1)
                        {
                           element[0].get_plane (z_axis_minus, plane[0]);
                           tz = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim3);
                           b[ind[0]] = -tz*p_left;
                           index  =  ind[0] + ind[0]*size;
                           A[index] -= tz;
                        }
                    if (k == k2-1)
                        {
                           element[0].get_plane (z_axis_plus, plane[0]);
                           tz = mesh.calc_tran_boundary (ext_ind[0], plane[0], center[0], along_dim3);
                           b[ind[0]] = -tz*p_right;
                           index  =  ind[0] + ind[0]*size;
                           A[index] -= tz;
                        }
                 }

              }

    solver->setup (tran);
    solver->solve (tran, rhs, p);

    // Q=sum(q) q=T*dp
    // Q=k*S*dP/dL
    // for permx and permy don't forget NTG!

    Q = 0;
    S = 0;

    if (direction == 1)
      {
        i = i1;
        for (j = j1; j < j2; ++j)
            for (k = k1; k < k2; ++k)
              {
                mesh.calc_element (i, j, k, element[0]);
                element[0].get_plane (x_axis_minus, plane[0]);
                center[0] = element[0].get_center ();
                get_plane_center (plane[0],  center[1]);

                index = k + j * Nz + i * Ny * Nz;
                S += ntg[index]*find_area_of_side (plane[0][0], plane[0][1], plane[0][2], plane[0][3]);
                tx = mesh.calc_tran_boundary (index, plane[0], center[0], along_dim1);

                index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
                dP = (p_left - (*p)[index]);
                Q += tx*dP;
              }
      }
    else if (direction == 2)
      {
        j = j1;
        for (i = i1; i < i2; ++i)
            for (k = k1; k < k2; ++k)
              {
                mesh.calc_element (i, j, k, element[0]);
                element[0].get_plane (y_axis_minus, plane[0]);
                center[0] = element[0].get_center ();
                get_plane_center (plane[0],  center[1]);

                index = k + j * Nz + i * Ny * Nz;
                S += ntg[index]*find_area_of_side (plane[0][0], plane[0][1], plane[0][2], plane[0][3]);
                ty = mesh.calc_tran_boundary (index, plane[0], center[0], along_dim2);

                index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
                dP = (p_left - (*p)[index]);
                Q += ty*dP;
              }
      }
    else if (direction == 3)
      {
        k = k1;
        for (i = i1; i < i2; ++i)
            for (j = j1; j < j2; ++j)
              {
                mesh.calc_element (i, j, k, element[0]);
                element[0].get_plane (z_axis_minus, plane[0]);
                center[0] = element[0].get_center ();
                get_plane_center (plane[0],  center[1]);

                index = k + j * Nz + i * Ny * Nz;
                S += find_area_of_side (plane[0][0], plane[0][1], plane[0][2], plane[0][3]);
                tz = mesh.calc_tran_boundary (index, plane[0], center[0], along_dim3);

                index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
                dP = (p_left - (*p)[index]);
                Q += tz*dP;
              }
      }

    i = i1; j = j1; k = k1;
    if (direction == 1)
      {
        mesh.calc_element (i, j, k, element[0]);
        element[0].get_plane (x_axis_minus, plane[0]);
        get_plane_center (plane[0],  center[0]);

        index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
        dP = (p_left + (*p)[index])/2;

        i = i2-1;
        mesh.calc_element (i, j, k, element[1]);
        element[1].get_plane (x_axis_plus, plane[1]);
        get_plane_center (plane[1],  center[1]);

        index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
        dP -= ((*p)[index] + p_right)/2;
      }
    else if (direction == 2)
      {
        mesh.calc_element (i, j, k, element[0]);
        element[0].get_plane (y_axis_minus, plane[0]);
        get_plane_center (plane[0],  center[0]);

        index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
        dP = (p_left + (*p)[index])/2;

        j = j2-1;
        mesh.calc_element (i, j, k, element[1]);
        element[1].get_plane (y_axis_plus, plane[1]);
        get_plane_center (plane[1],  center[1]);

        index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
        dP -= ((*p)[index] + p_right)/2;
      }
    else if (direction == 3)
      {
        mesh.calc_element (i, j, k, element[0]);
        element[0].get_plane (z_axis_minus, plane[0]);
        get_plane_center (plane[0],  center[0]);

        index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
        dP = (p_left + (*p)[index])/2;

        k = k2-1;
        mesh.calc_element (i, j, k, element[1]);
        element[1].get_plane (z_axis_plus, plane[1]);
        get_plane_center (plane[1],  center[1]);

        index = (k-k1) + (j-j1)*nz + (i-i1)*ny*nz;
        dP -= ((*p)[index] + p_right)/2;
      }

    dL = get_len(center[0], center[1]);
    if (S < EPSILON || dP < EPSILON)
        K = 0;
    else
        K = Q*dL/(cdarcy*S*dP);

    return K;

}

spv_float upsc::upscale_permz_zcolumn (t_long Nx, t_long Ny, t_long Nz, spv_uint layers_, spv_float permz_, spv_uint actnum_, BS_SP(rs_mesh_iface) sp_mesh_iface)
  {
    t_int i, j, k, n, Nz_upsc, k1, k2, z1, ind, index;
    t_long layer_size, new_cube_size;

    t_double upsc_factor;

    spv_float new_permz = BS_KERNEL.create_object(v_float::bs_type());
    v_float& permz = *permz_;
    v_uint& actnum = *actnum_;
    v_uint& layers = *layers_;

    smart_ptr<bs_mesh_grdecl> sp_mesh(sp_mesh_iface, bs_static_cast());
    mesh_grdecl mesh = sp_mesh->get_wrapped();

    Nz_upsc = layers.size();
    layer_size = Nx*Ny;

    // important: XYZ order
    // index <-- (i, j, k)
    // index = i + j*Nx + k*Nx*Ny;

    for (n = 0; n < Nz_upsc; ++n)
        {
            k1 = layers[n];

            if (n == Nz_upsc-1)
                k2 = Nz - 1;
            else
                k2 = layers[n+1] - 1;

            if (k1 != k2)
                {
                    z1 = k1*layer_size;
                    for (j = 0; j < Ny; ++j)
                        for (i = 0; i < Nx; ++i)
                            {
                                index = i + j * Nx + z1;

                                // in general case call function that finds isolated bodies
                                for (k = k1; k <= k2; k++)
                                    {
                                        ind = i + j * Nx + k * Nx * Ny;
                                        if (!actnum [ind])
                                            {
                                                permz[index] = 0;
                                                break;
                                            }
                                    }
                                if (permz[index])
                                    {
                                        upsc_factor = solve_pressure_zcolumn (Ny, Nz, i, j, k1, k2, sp_mesh_iface);
                                        permz[index] *= upsc_factor;
                                    }

                            }
                }
        }

    new_cube_size = Nz_upsc*layer_size;
    new_permz->resize(new_cube_size);

    i = 0;
    for ( n = 0; n < Nz_upsc; n++)
        {
            k = layers[n];
            std::copy ( &permz[k*layer_size], &permz[(k+1)*layer_size], &(*new_permz)[i*layer_size] );
            i++;
        }

    return new_permz;
  }

spv_float upsc::upscale_perm_block (t_int dir, t_long Nx, t_long Ny, t_long ux, t_long uy, t_long Nz, spv_uint layers_, spv_float ntg_, spv_uint actnum_, BS_SP(rs_mesh_iface) sp_mesh_iface)
  {
    t_int i, j, n, i1, i2, j1, j2, k1, k2, new_k, new_ind;
    t_long new_Nx, new_Ny, new_Nz;
    t_long new_layer_size, new_cube_size;

    spv_float new_perm = BS_KERNEL.create_object(v_float::bs_type());
    v_uint& actnum = *actnum_;
    v_uint& layers = *layers_;

    smart_ptr<bs_mesh_grdecl> sp_mesh(sp_mesh_iface, bs_static_cast());
    mesh_grdecl mesh = sp_mesh->get_wrapped();

    t_int rx, ry;
    rx = Nx % ux;
    ry = Ny % uy;
    new_Nx = ceil(double(Nx)/double(ux));
    new_Ny = ceil(double(Ny)/double(uy));
    new_Nz = layers.size();
    new_layer_size = new_Nx * new_Ny;
    new_cube_size = new_Nx*new_Ny*new_Nz;
    new_perm->resize(new_cube_size);

    // important: XYZ order
    // index <-- (i, j, k)
    // index = i + j*Nx + k*Nx*Ny;

    // FIXME: call function that finds isolated bodies

    new_k = 0;
    for (n = 0; n < new_Nz; ++n)
      {
        k1 = layers[n];

        if (n == new_Nz-1)
            k2 = Nz;
        else
            k2 = layers[n+1] - 1;

        for (j = 0; j < new_Ny-1; ++j)
          {
            for (i = 0; i < new_Nx-1; ++i)
              {
                i1 = ux*i;
                i2 = i1 + ux;
                j1 = uy*j;
                j2 = j1 + uy;
                new_ind = i + j*new_Nx + new_k*new_layer_size;
                (*new_perm)[new_ind] = solve_pressure_block (dir, Ny, Nz, i1, i2, j1, j2, k1, k2, ntg_, sp_mesh_iface);
              }

            if (rx)
              {
                i1 = ux*(new_Nx-1);
                i2 = i1 + rx;
                j1 = uy*j;
                j2 = j1 + uy;
                new_ind = new_Nx-1 + j*new_Nx + new_k*new_layer_size;
                (*new_perm)[new_ind] = solve_pressure_block (dir, Ny, Nz, i1, i2, j1, j2, k1, k2, ntg_, sp_mesh_iface);
              }
          }

        if (ry)
          {
            for (i = 0; i < new_Nx-1; ++i)
              {
                i1 = ux*i;
                i2 = i1 + ux;
                j1 = uy*(new_Ny-1);
                j2 = j1 + ry;
                new_ind = i + (new_Ny-1)*new_Nx + new_k*new_layer_size;
                (*new_perm)[new_ind] = solve_pressure_block (dir, Ny, Nz, i1, i2, j1, j2, k1, k2, ntg_, sp_mesh_iface);

              }

            if (rx)
              {
                i1 = ux*(new_Nx-1);
                i2 = i1 + rx;
                j1 = uy*(new_Ny-1);
                j2 = j1 + ry;
                new_ind = new_Nx-1 + (new_Ny-1)*new_Nx + new_k*new_layer_size;
                (*new_perm)[new_ind] = solve_pressure_block (dir, Ny, Nz, i1, i2, j1, j2, k1, k2, ntg_, sp_mesh_iface);
              }
          }

        new_k ++;
      }

    return new_perm;
  }

  bp::tuple
  upsc::king_method (t_long Nx, t_long Ny, t_long Nz, t_long Nz_upsc,
                            spv_float vol_, spv_float ntg_, spv_float poro_, spv_float permx_)
  {
    t_int i, k, n, k0, k1, k2, k3;
    t_long layer_size, new_cube_size;
    t_float sum_dW;

    v_float& vol = *vol_;
    v_float& ntg = *ntg_;
    v_float& poro = *poro_;
    v_float& permx = *permx_;

    std::list <t_int> layers;
    std::list <t_int>::iterator lit, tmp_it;

    layer_mmap_t sum_dW2layer;
    layer_mmap_it_t mit;
    layer_map_t layer2mmap_it;
    layer_map_it_t lmit;

    spv_float new_vol = BS_KERNEL.create_object(v_float::bs_type());
    spv_float new_ntg = BS_KERNEL.create_object(v_float::bs_type());
    spv_float new_poro = BS_KERNEL.create_object(v_float::bs_type());
    spv_float new_permx = BS_KERNEL.create_object(v_float::bs_type());
    spv_uint layers_v = BS_KERNEL.create_object(v_uint::bs_type());

    layer_size = Nx*Ny;

    for (i = 0;i < Nz; i++)
        layers.push_back(i);

    // important: XYZ order
    // index <-- (i, j, k)
    // index = i + j*Nx + k*Nx*Ny;

    // calculate sum_dW for every pair of adjacent layers
    // and store it in sumdw2layer multimap (sum_dW -> k) sorted in increasing order
    // and in layer2mmapit map (k -> mmap_it)
    for (k = 0; k < Nz-1; k++)
    {
        sum_dW = calc_sum_dW ( k, k+1,  Nx, Ny, vol_, ntg_, poro_, permx_ );
        mit = sum_dW2layer.insert(std::make_pair(sum_dW, k));
        layer2mmap_it[k] = mit;
    }

    k0 = 0;
    k3 = Nz-1;

    for (n=Nz;n>Nz_upsc;n--)
    {
        // [k0, k1, k2, k3] - indices of 4 consequentive layers
        // [k1, k2]  - indices of 2 layers to be united
        k1 = (*sum_dW2layer.begin()).second;
        lit = find(layers.begin(), layers.end(), k1);
        tmp_it = lit;
        tmp_it ++;
        k2 = (*tmp_it);

        // upscaling of cubes
        upscale_cubes (k1, k2, Nx, Ny, vol_, ntg_, poro_, permx_ );

        // erase sum_dW for (k1; k2) pair of layers
        mit = layer2mmap_it[k1];
        sum_dW2layer.erase(mit);
        layer2mmap_it.erase(k1);

        // if k2 isn't the last layer
        if (k2 != layers.back())
            {
                // erase sum_dW for (k2; k3) pair of layers
                mit = layer2mmap_it[k2];
                sum_dW2layer.erase(mit);
                layer2mmap_it.erase(k2);

                tmp_it ++;
                k3 = (*tmp_it);
                tmp_it --;

                // add sum_dW for (k1; k3) pair of layers
                sum_dW = calc_sum_dW ( k1, k3, Nx, Ny, vol_, ntg_, poro_, permx_ );
                mit = sum_dW2layer.insert ( std::make_pair(sum_dW, k1) );
                layer2mmap_it[k1] = mit;
            }

        // delete k2 layer
        layers.erase(tmp_it);

        // if k1 isn't the first layer
        if (k1 != 0)
            {
                tmp_it = lit;
                tmp_it --;
                k0 = (*tmp_it);

                // erase sum_dW for (k0; k1) pair of layers
                mit = layer2mmap_it[k0];
                sum_dW2layer.erase(mit);
                layer2mmap_it.erase(k0);

                // add sum_dW for (k0; k1) pair of layers
                sum_dW = calc_sum_dW ( k0, k1, Nx, Ny, vol_, ntg_, poro_, permx_ );
                mit = sum_dW2layer.insert ( std::make_pair(sum_dW, k0) );
                layer2mmap_it[k0] = mit;
            }

    }

    new_cube_size = Nz_upsc*layer_size;

    new_vol->resize(new_cube_size);
    new_ntg->resize(new_cube_size);
    new_poro->resize(new_cube_size);
    new_permx->resize(new_cube_size);
    layers_v->resize(Nz_upsc);

    i = 0;
    for (lit=layers.begin();lit!=layers.end();lit++)
        {
            k = (*lit);
            std::copy ( &vol[k*layer_size], &vol[(k+1)*layer_size], &(*new_vol)[i*layer_size] );
            std::copy ( &ntg[k*layer_size], &ntg[(k+1)*layer_size], &(*new_ntg)[i*layer_size] );
            std::copy ( &poro[k*layer_size], &poro[(k+1)*layer_size], &(*new_poro)[i*layer_size] );
            std::copy ( &permx[k*layer_size], &permx[(k+1)*layer_size], &(*new_permx)[i*layer_size] );
            i++;
        }

    std::copy ( layers.begin(), layers.end(), &(*layers_v)[0] );

    return bp::make_tuple (layers_v, new_vol, new_ntg, new_poro, new_permx) ;
  }

#ifdef BSPY_EXPORTING_PLUGIN
  std::string
  upsc::py_str () const
    {
      std::stringstream s;
      s << wstr2str (sp_prop->py_str ()) << "\n";
      return s.str ();
    }
#endif //BSPY_EXPORTING_PLUGIN
/////////////////////////////////BS Register
/////////////////////////////////Stuff//////////////////////////

  BLUE_SKY_TYPE_STD_CREATE (upsc);
  BLUE_SKY_TYPE_STD_COPY (upsc);

  BLUE_SKY_TYPE_IMPL(upsc,  upsc_iface, "upsc", "upscaling", "hdm upscaling");

}  // blue_sky namespace


/* Copyright 2019 The Spin-Scenario Authors. All Rights Reserved.
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 "spin_system.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>

namespace ssl {
namespace spinsys {
spin_system::spin_system(const std::string &filename) : inter_(comp_) {

  sol::state lua;
  lua.script_file(filename);
  sol::table t = lua["sys"];
  set_sys(t);
}
spin_system::spin_system(const sol::table &t) : inter_(comp_) {
  set_sys(t);
}
spin_system::~spin_system() {
}
void spin_system::set_sys(const sol::table &t) {
  if (!t.valid() || t.empty())
    throw std::runtime_error("invalid 'spin_system' table parameters (nil or empty).");

  //try {
//  std::string str = retrieve_table("B0", t).as<std::string>();
//  boost::to_lower(str);
//  std::vector<std::string> str_vec;
//  boost::split(str_vec, str, boost::is_any_of(", "), boost::token_compress_on);
//  double val_B0 = boost::lexical_cast<double>(str_vec[0]);
//  if (str_vec[1] == "tesla" || str_vec[1] == "t")
//    set_magnet_field(val_B0);
//  if (str_vec[1] == "mhz")
//    set_proton_freq(val_B0);

  if (g_B0_ < 0) {
    throw std::runtime_error("static magnetic field not set yet!");
  }

  comp_.B0_ = g_B0_;

  set_isotopes(retrieve_table("spin", t).as<std::string>());
  //setBasis();
  inter_.alloc();

  if (is_retrievable("zeeman", t))
    inter_.set_zeeman(retrieve_table("zeeman", t).as<std::string>());

  if (is_retrievable("jcoupling", t))
    inter_.set_Jcoupling(retrieve_table("jcoupling", t).as<std::string>());

  if (is_retrievable("coord", t))
    inter_.set_Jcoupling_coords(retrieve_table("coord", t).as<sol::table>());

  if (is_retrievable("relaxation", t))
    inter_.set_relaxation(retrieve_table("relaxation", t).as<std::string>());

  inter_.init();
  inter_.set_assumption("nmr");
  /*} catch (const std::runtime_error& e) {
      std::string s = str(boost::format("%s\n") % std::string(e.what()));
      ssl_color_text("err", s);
  }*/
}

void spin_system::set_isotopes(const std::string list) {
  std::vector<std::string> symbol_vec;
  boost::split(symbol_vec, list, boost::is_any_of("\t, "), boost::token_compress_on);
  for (size_t i = 0; i < symbol_vec.size(); i++)
    comp_.add_spin(isotope(symbol_vec[i]));
#ifdef SSL_OUTPUT_ENABLE
  std::string s = str(boost::format("%s %s.\n") % "isotopes set to be" % list);
  ssl_color_text("info", s);
#endif
  comp_.init();
}

void spin_system::set_magnet_field(double tesla) {
  comp_.B0_ = tesla;
#ifdef SSL_OUTPUT_ENABLE
  std::string s = str(boost::format("%s %.3f Tesla.\n") % "magnet field set to be" % tesla);
  ssl_color_text("info", s);
#endif
}

void spin_system::set_proton_freq(double MHz) {
  isotope proton("1H");
  double tesla = MHz * 2 * _pi / proton.gamma() * 1e6;
#ifdef SSL_OUTPUT_ENABLE
  std::string s = str(boost::format("%s %.3f MHz.\n") % "proton resonance frequency set to be" % MHz);
  ssl_color_text("info", s);
#endif
  set_magnet_field(tesla);
}
double spin_system::get_proton_freq() const {
  return comp_.get_proton_freq();
}
sp_mat spin_system::op(const std::string &list) const {
  return op(list, kComm);
}
sp_mat spin_system::op(const sol::table &t) const {
  return op(t, kComm);
}
sp_mat spin_system::op(const std::string &list, op_side type) const {
  return comp_.op(list, type);
}
sp_mat spin_system::op(const sol::table &t, op_side type) const {
  if (!t.valid() || t.empty())
    throw std::runtime_error("invalid 'op' table parameters (nil or empty).");

  std::vector<std::string> s;
  for (size_t i = 0; i < t.size(); i++) {
    sol::object val = t[i + 1];
    std::string item;
    switch (val.get_type()) {
      case sol::type::number:item = boost::lexical_cast<std::string>(val.as<int>());
        break;
      case sol::type::string:item = val.as<std::string>();
        break;
      default:break;
    }
    s.push_back(item);
  }
  std::string list = boost::algorithm::join(s, " ");
  return comp_.op(list, type);
}

sp_cx_vec spin_system::equilibrium_state() const {
  return inter_.equilibrium_state();
  }

sp_cx_mat spin_system::smart_op(const std::string expr) const {
  state_par s = state_evaluate(expr);
  int num = s.coeff.size();
  sp_cx_mat result = s.coeff[0] * op(s.expr[0]);
  for (int i = 1; i < num; i++)
    result += s.coeff[i] * op(s.expr[i]);
  return result;
}

sp_cx_vec spin_system::smart_state(const std::string expr) const {
  state_par s = state_evaluate(expr);
  int num = s.coeff.size();
  sp_cx_vec result = s.coeff[0] * state(s.expr[0]);
  for (int i = 1; i < num; i++)
    result += s.coeff[i] * state(s.expr[i]);
  return result;
}

std::map<std::string, sp_cx_vec> spin_system::cartesian_basis_states() const {
  std::string base_expr = "(1+Ix+Iy+Iz)";
  int nspins = comp_.nspins();
  std::string expr_in;
  boost::regex reg("I(\\w)");
  std::vector<std::string> exprs(nspins);
  for (int i = 0; i < nspins; i++) {
    exprs[i] = base_expr;
    std::string id = boost::lexical_cast<std::string>(i + 1);
    std::string rep = "I" + id + "$1";
    std::string s = boost::regex_replace(exprs[i], reg, rep);
    exprs[i] = s;
    //std::cout << rep<<" "<<exprs[i] << "\n";
  }

  expr_in = exprs[0];
  for (int i = 1; i < nspins; i++)
    expr_in += "*" + exprs[i];

  expr_in = "Simplify(" + expr_in + ")";
  //std::cout << expr_in<<"\n";
  std::string expr_out = yacas_evaluate(expr_in);
  boost::erase_all(expr_out, ";");
  //std::cout << expr_out << "\n";
  //boost::replace_all(expr_out, "J", "I");
  //std::vector<std::string> expr_vec;
  std::vector<std::string> expr;
  boost::algorithm::split(expr, expr_out, boost::is_any_of("+"));
  expr.pop_back(); // !!! remove 1.
  std::map<std::string, sp_cx_vec> basis;
  for (size_t i = 0; i < expr.size(); i++) {
    boost::erase_all(expr[i], "*");
    //std::cout << i + 1 << " " << expr[i] << "\n";
    basis.insert(std::pair<std::string, sp_cx_vec>(expr[i], smart_state(expr[i])));
  }
  /*std::map<std::string, sp_cx_vec>::iterator iter;
  for (iter = basis.begin(); iter != basis.end(); iter++) {
      std::string label = iter->first;
      sp_cx_vec val = iter->second;
      std::cout << label << "\n" << val << "\n";
  }*/
  return basis;
}

sp_cx_vec spin_system::state(const std::string &list) const {
  return comp_.state(list);
}
sp_cx_vec spin_system::state(const sol::table &t) const {
  if (!t.valid() || t.empty())
    throw std::runtime_error("invalid 'op' table parameters (nil or empty).");
  std::vector<std::string> s;
  for (size_t i = 0; i < t.size(); i++) {
    sol::object val = t[i + 1];
    std::string item;
    switch (val.get_type()) {
      case sol::type::number:item = boost::lexical_cast<std::string>(val.as<int>());
        break;
      case sol::type::string:item = val.as<std::string>();
        break;
      default:break;
    }
    s.push_back(item);
  }
  std::string list = boost::algorithm::join(s, " ");
  return comp_.state(list);
}

ham_op spin_system::hamiltonian(const op_side type, bool build_aniso) const {
  return inter_.hamiltonian(type, build_aniso);
}

sp_cx_mat spin_system::free_hamiltonian() const {
  return hamiltonian(kComm, 0).isotropic;
}

std::vector<sp_cx_mat> spin_system::free_hamiltonians() {
  std::vector<sp_cx_mat> L0s;
  std::vector<std::vector<cs_par>> result_cs = inter_.parsing_zeeman_broadband();
  std::vector<std::vector<jcoup_par>> result_jcoup = inter_.parsing_Jcoupling_broadband();
  if (result_cs.size())
    std::cout << "chemical shifts num: " << result_cs.size() << "\n";
  if (result_jcoup.size())
    std::cout << "j-coupling num: " << result_jcoup.size() << "\n";
  // zeeman case.
  std::vector<double> scalars = inter_.zeeman_.scalars; // backup.
  mat scalar = inter_.coupling_.scalar;
  if (result_cs.size()&&result_jcoup.size()==0)
  for (size_t i = 0; i < result_cs.size(); i++) {
    for (size_t j = 0; j < result_cs[i].size(); j++) {
      const cs_par &cur = result_cs[i][j];
      //std::cout << (cur.id + 1) << " " << inter_.zeeman_.scalars[cur.id] << " ";
      inter_.zeeman_.scalars[cur.id] += cur.val;
      //std::cout << (cur.id + 1) << " " << inter_.zeeman_.scalars[cur.id] << " ";
    }
    //std::cout << "\n";
    inter_.init_broadband();
    L0s.push_back(free_hamiltonian());
    inter_.zeeman_.scalars = scalars; // recover to original pars for the next L0 calculation.
  }
  // J-coupling case.
    if (result_cs.size()==0&&result_jcoup.size())
  for (size_t i = 0; i < result_jcoup.size(); i++) {
    for (size_t j = 0; j < result_jcoup[i].size(); j++) {
      const jcoup_par &cur = result_jcoup[i][j];
      int id1 = cur.id1;
      int id2 = cur.id2;
      inter_.coupling_.scalar(id1, id2) += cur.val;
      if (id1 != id2)
        inter_.coupling_.scalar(id2, id1) += cur.val;
      //std::cout << (cur.id1 + 1) << " " << (cur.id2 + 1) << " " << inter_.coupling_.scalar(id1, id2) << " ";
    }
    //std::cout << "\n";
    inter_.init_broadband();
    L0s.push_back(free_hamiltonian());
    inter_.coupling_.scalar = scalar;
  }
	//omp_set_num_threads(omp_core_num);
	if(result_cs.size()&&result_jcoup.size()) {
    //#pragma omp parallel for
    for (size_t i = 0; i < result_cs.size(); i++) {
      if (i % 100 == 0) std::cout << i << "==>\n";

      for (size_t j = 0; j < result_cs[i].size(); j++) {
        const cs_par &cur = result_cs[i][j];
        inter_.zeeman_.scalars[cur.id] += cur.val;
      }

      for (size_t ii = 0; ii < result_jcoup.size(); ii++) {

        for (size_t jj = 0; jj < result_jcoup[ii].size(); jj++) {
          const jcoup_par &cur = result_jcoup[ii][jj];
          int id1 = cur.id1;
          int id2 = cur.id2;
          inter_.coupling_.scalar(id1, id2) += cur.val;
          if (id1 != id2) inter_.coupling_.scalar(id2, id1) += cur.val;
        }

        inter_.init_broadband();
        L0s.push_back(free_hamiltonian());

        inter_.coupling_.scalar = scalar;
      }
      inter_.zeeman_.scalars = scalars;  // recover to original pars for the next L0 calculation.
    }
	}
  return L0s;
}

sol::object spin_system::free_hamiltonians(sol::this_state s) {
  sol::state_view lua(s);
  sol::table t = lua.create_table();
  std::vector<sp_cx_mat> L0s = free_hamiltonians();
  for (size_t i = 0; i < L0s.size(); i++)
    t.add(L0s[i]);
  return t;
}
sp_cx_mat spin_system::relaxation() const {
  return inter_.relaxation();
}

sp_cx_mat spin_system::total_hamiltonian() const {
  return free_hamiltonian() + ci * relaxation();
}
rf_ham spin_system::rf_hamiltonian() const {
  rf_ham rf_ctrl;
  rf_ctrl.init(channels());
  for (size_t i = 0; i < rf_ctrl.channels; i++) {
    // Get the control operators
    std::string list_plus = rf_ctrl.chs[i] + " I+";
    std::string list_minus = rf_ctrl.chs[i] + " I-";
    sp_mat Lp = op(list_plus);
    sp_mat Lm = op(list_minus);
    rf_ctrl.Lx[i] = 0.5 * (Lp + Lm).cast<cd>();  // cx
    rf_ctrl.Ly[i] = -0.5 * ci * (Lp - Lm).cast<cd>();  // cy
  }
  return rf_ctrl;
}
vec spin_system::nominal_broadband() {
  if (inter_.zeeman_.bb_scalars.size())
    return inter_.zeeman_.bb_scalars[0].nominal_offset;
  if (inter_.coupling_.bb_scalars.size())
    return inter_.coupling_.bb_scalars[0].nominal_offset;
  return vec(1);
}

}
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_PIO2_2_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_PIO2_2_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio2_2 generic tag

     Represents the Pio2_2 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    //6.07710050630396597660e-11
    BOOST_SIMD_CONSTANT_REGISTER( Pio2_2, double
                                , 0, 0x37354400
                                , 0x3DD0B4611A600000ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio2_2, Site> dispatching_Pio2_2(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio2_2, Site>();
   }
   template<class... Args>
   struct impl_Pio2_2;
  }
  /*!
    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T0:

    @code
    T0 r = Pio2_2<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio2_2, Pio2_2);
}

#endif


#include <boost/geometry/algorithms/area.hpp>


#include "myslam/dataset.h"
#include "myslam/frame.h"
#include "myslam/config.h"

#include <boost/format.hpp>
#include <fstream>
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

namespace myslam {

#if 1
    void imshowAndWait(string title, Mat img) {
        imshow("imshowAndWait: " + title, img);
        if (Config::Get<int>("isw_block") != 0) {
            while (1) {
                if (waitKey(50) != -1) {
                    break;
                }
            }
        } else {
            waitKey(1);
        }
    }
#else
    void imshowAndWait(Mat img) {}
#endif

Dataset::Dataset(const std::string& dataset_path)
    : dataset_path_(dataset_path) {

}

bool Dataset::Init() {

    video.open(Config::Get<int>("video_id"));

    cameraMatrix = (
        Mat_<double>(3, 3) <<
        421.288596, 0.000000, 335.509012,
        0.000000, 421.079122, 261.189746,
        0.000000, 0.000000, 1.000000
    );

    distortionCoefficient = (
        Mat_<double>(5, 1) <<
        -0.347294, 0.090865, 0.001867, 0.001221, 0.000000
    );

    initUndistortRectifyMap(
        cameraMatrix, distortionCoefficient, Mat(), Mat(),
        Size(640, 480), CV_16SC2, map1, map2
    );

    Mat33 K;
    K << cameraMatrix.at<double>(0, 0), cameraMatrix.at<double>(0, 1), cameraMatrix.at<double>(0, 2),
        cameraMatrix.at<double>(1, 0), cameraMatrix.at<double>(1, 1), cameraMatrix.at<double>(1, 2),
        cameraMatrix.at<double>(2, 0), cameraMatrix.at<double>(2, 1), cameraMatrix.at<double>(2, 2);
    // The left camera
    {
        Vec3 t;
        t << 0, 0, 0;
        Camera::Ptr new_camera(new Camera(K(0, 0), K(1, 1), K(0, 2), K(1, 2),
                                          t.norm(), SE3(SO3(), t)));
        cameras_.push_back(new_camera);
        LOG(INFO) << "Camera " << 0 << " extrinsics: " << t.transpose();
    }
    // The right camera
    {
        Vec3 t;
        t << -0.061, 0, 0;
        Camera::Ptr new_camera(new Camera(K(0, 0), K(1, 1), K(0, 2), K(1, 2),
                                          t.norm(), SE3(SO3(), t)));
        cameras_.push_back(new_camera);
        LOG(INFO) << "Camera " << 1 << " extrinsics: " << t.transpose();
    }

    video.set(CAP_PROP_FRAME_WIDTH, 1280);
    video.set(CAP_PROP_FRAME_HEIGHT, 480);

    current_image_index_ = 0;
    return true;
}

Frame::Ptr Dataset::NextFrame() {
    cv::Mat image, img_left, img_right;

    // read images
    if (!first_frame_) {
        video >> image;
    } else {
        for (int i = 0; i < 30; i++) {
            video >> image;
        }
        first_frame_ = false;
    }

    if (image.data == nullptr) {
        LOG(WARNING) << "cannot find images at index " << current_image_index_;
        return nullptr;
    }

#define OUT(what) {cout << (what) << endl;};

    {
        Mat tmp;
        cvtColor(image, tmp, COLOR_BGR2GRAY);
        image = tmp;
    }

    img_left = image(Rect(0, 0, image.cols / 2, image.rows)).clone();
    img_right = image(Rect(image.cols / 2, 0, image.cols / 2, image.rows)).clone();

    {
        Mat tmp(img_left.rows, img_left.cols, img_left.type());
        remap(img_left, tmp, map1, map2, INTER_LINEAR);
        img_left = tmp;
    }

    {
        Mat tmp(img_right.rows, img_right.cols, img_right.type());
        remap(img_right, tmp, map1, map2, INTER_LINEAR);
        img_right = tmp;
    }

    cout << "Frame: " << current_image_index_ << endl;

//     imwrite("out/orig.png", image);
//     imwrite("out/lf.png", img_left);
//     imwrite("out/rt.png", img_right);

    if (Config::Get<int>("frame_wait") != 0) {
        imshowAndWait("src", image);
    }

    auto new_frame = Frame::CreateFrame();
    new_frame->left_img_ = img_left;
    new_frame->right_img_ = img_right;
    current_image_index_++;
    return new_frame;
}

}  // namespace myslam


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SCALAR_COMPARATOR_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SCALAR_COMPARATOR_HPP_INCLUDED
#include <boost/simd/toolbox/swar/functions/comparator.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/include/functions/scalar/any.hpp>
/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::comparator_, tag::cpu_,
                            (A0)(A2),
			     ((scalar_<arithmetic_<A0> >))
			     ((scalar_<arithmetic_<A0> >))
			     ((scalar_<arithmetic_<A2> >))
                           )
  {
    typedef boost::fusion::tuple<A0,A0,bool>     result_type;
    inline result_type operator()(const A0& a0, const A0& a1, const A2& a2)
    {
      result_type res;
      eval( a0, a1, a2
           , boost::fusion::at_c<0>(res)
           , boost::fusion::at_c<1>(res)
           , boost::fusion::at_c<2>(res)
           );
      return res;
    }
  private:
    template<class AA0,class AA2> inline void
    eval(AA0 const& a0, AA0 const& a1, AA0 const& a2, AA0& r0, AA0& r1, bool& modified)const
    {
      r0 = a0;
      r1 = a1;
      if (a2)
      {
        if ( (modified = (a0 > a1)) ) std::swap(r0, r1);
      }
      else
      {
        if ( (modified = (a1 > a0)) ) std::swap(r0, r1);
      }
    }
  };
} } }
#endif


#include <treeDAG/decomposer.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>
#include <boost/timer/timer.hpp>

typedef treeDAG::SeparatorConfig::Graph Graph;

Graph create_cyle(std::size_t size)
{
    Graph g(size);
    for(std::size_t i = 0; i < size; ++i)
    {
        std::size_t src = i;
        std::size_t dst = (i+1)%size;
        boost::add_edge(src, dst, g);
    }

    return g;
}

Graph create_path(std::size_t size)
{
    Graph g(size);
    for(std::size_t i = 1; i < size; ++i)
        boost::add_edge(i-1, i, g);

    return g;
}

Graph create_test_graph()
{
    Graph g(7);
    boost::add_edge(0, 1, g);
    boost::add_edge(2, 1, g);
    boost::add_edge(5, 1, g);
    boost::add_edge(2, 3, g);
    boost::add_edge(5, 3, g);
    boost::add_edge(3, 4, g);
    boost::add_edge(3, 6, g);
    boost::add_edge(0, 4, g);
    boost::add_edge(0, 6, g);

    return g;
}

Graph create_test_graph2()
{
    Graph g(7);
    boost::add_edge(0, 1, g);
    boost::add_edge(1, 2, g);
    boost::add_edge(2, 3, g);
    boost::add_edge(3, 0, g);
    boost::add_edge(3, 4, g);
    boost::add_edge(4, 5, g);
    boost::add_edge(5, 6, g);
    boost::add_edge(6, 4, g);

    return g;
}

Graph create_test_graph3()
{
    Graph g(8);
    boost::add_edge(0, 1, g);
    boost::add_edge(1, 2, g);
    boost::add_edge(2, 3, g);
    boost::add_edge(3, 0, g);
    boost::add_edge(2, 4, g);
    boost::add_edge(4, 5, g);
    boost::add_edge(5, 6, g);
    boost::add_edge(6, 7, g);
    boost::add_edge(7, 5, g);

    return g;
}

int main(int argc, char ** argv)
{
    if(argc != 2)
    {
        std::cerr << "usage: " << argv[0] << " cycle_size" << std::endl;
        return -1;
    }

    std::size_t sz = boost::lexical_cast<std::size_t>(argv[1]);
//    Graph g = create_cyle(sz);
//    Graph g = create_test_graph3();
    Graph g = create_path(sz);

    std::stringstream times;

    for(std::size_t i = 0; i < 1; ++i)
    {
        boost::timer::cpu_timer t;
        t.start();

        treeDAG::Decomposer decomposer(&g, 2);
        decomposer.initialize();

        std::vector<unsigned char> roots;
        roots.push_back(0);
        roots.push_back(1);

        decomposer.process(roots.begin(), roots.end());
        t.stop();

        std::cout << sz << ";" << decomposer.decompositionDAG().numberOfNodes() << ";" << decomposer.decompositionDAG().numberOfBranches() << ";" << t.elapsed().wall << std::endl;

        std::stringstream filename;
        filename << "test_" << sz << ".dot";

        std::ofstream ofs;
        ofs.open(filename.str().c_str());

        decomposer.writeDot(ofs);
    }
}


#ifndef SKELLYSIM_HPP
#define SKELLYSIM_HPP

#include <toml.hpp>

#include <msgpack.hpp>
#define EIGEN_MATRIX_PLUGIN "eigen_matrix_plugin.h"
#define EIGEN_QUATERNION_PLUGIN "eigen_quaternion_plugin.h"
#define EIGEN_USE_MKL_ALL
#define MKL_DIRECT_CALL

#include <Eigen/Core>

typedef Eigen::Map<Eigen::VectorXd> VectorMap;
typedef Eigen::Map<const Eigen::VectorXd> CVectorMap;
typedef Eigen::Map<Eigen::ArrayXd> ArrayMap;
typedef Eigen::Map<const Eigen::ArrayXd> CArrayMap;
typedef Eigen::Map<Eigen::MatrixXd> MatrixMap;
typedef Eigen::Map<const Eigen::MatrixXd> CMatrixMap;
typedef const Eigen::Ref<const Eigen::ArrayXd> ArrayRef;
typedef const Eigen::Ref<const Eigen::VectorXd> VectorRef;
typedef const Eigen::Ref<const Eigen::MatrixXd> MatrixRef;

#endif


#ifndef MJOLNIR_TEST_UTIL_CHECK_FORCE_ENERGY_VIRIAL_HPP
#define MJOLNIR_TEST_UTIL_CHECK_FORCE_ENERGY_VIRIAL_HPP
#include <mjolnir/math/Vector.hpp>
#include <mjolnir/core/System.hpp>

#include <mjolnir/core/LocalInteractionBase.hpp>
#include <mjolnir/core/GlobalInteractionBase.hpp>
#include <type_traits>

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

namespace mjolnir
{
namespace test
{

// This checks the consistency between `calc_force` and `calc_force_and_energy`.

template<typename traitsT, typename Interaction>
void check_force_energy_virial(System<traitsT> ref,
                               const Interaction& interaction,
                               const typename traitsT::real_type tol)
{
    using coordinate_type = typename traitsT::coordinate_type;
    using matrix33_type   = typename traitsT::matrix33_type;

    for(std::size_t i=0; i<ref.size(); ++i)
    {
        ref.force(i) = math::make_coordinate<coordinate_type>(0,0,0);
    }
    ref.virial() = matrix33_type(0,0,0, 0,0,0, 0,0,0);

    System<traitsT> sys(ref);

    ref.preprocess_forces();
    interaction.calc_force(ref);
    ref.postprocess_forces();
    const auto ref_ene = interaction.calc_energy(ref);

    sys.preprocess_forces();
    const auto ene = interaction.calc_force_virial_energy(sys);
    sys.postprocess_forces();

    BOOST_TEST(ref_ene == ene, boost::test_tools::tolerance(tol));

    for(std::size_t idx=0; idx<sys.size(); ++idx)
    {
        BOOST_TEST(math::X(sys.force(idx)) == math::X(ref.force(idx)), boost::test_tools::tolerance(tol));
        BOOST_TEST(math::Y(sys.force(idx)) == math::Y(ref.force(idx)), boost::test_tools::tolerance(tol));
        BOOST_TEST(math::Z(sys.force(idx)) == math::Z(ref.force(idx)), boost::test_tools::tolerance(tol));
    }

    // external forcefield does not support virial because they depends absolute
    // coordinate
    if(std::is_base_of< LocalInteractionBase<traitsT>, Interaction>::value ||
       std::is_base_of<GlobalInteractionBase<traitsT>, Interaction>::value)
    {
        for(std::size_t i=0; i<ref.size(); ++i)
        {
            ref.force(i) = math::make_coordinate<coordinate_type>(0,0,0);
        }
        ref.virial() = matrix33_type(0,0,0, 0,0,0, 0,0,0);

        // calc virial
        interaction.calc_force_and_virial(ref);

        // compare ref virial with calc_force_energy_virial
        for(std::size_t i=0; i<9; ++i)
        {
            BOOST_TEST(sys.virial()[i] == ref.virial()[i], boost::test_tools::tolerance(tol));
        }
    }

    return;
}

} // test
} // mjolnir
#endif// MJOLNIR_TEST_UTIL_CHECK_FORCE_HPP


//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#include <hector_quadrotor_model/quadrotor_propulsion.h>
#include <hector_quadrotor_model/helpers.h>

#include <ros/node_handle.h>
#include <ros/callback_queue.h>

#include <boost/array.hpp>

#include "matlab_helpers.h"

//extern "C" {
//  #include "quadrotorPropulsion/quadrotorPropulsion.h"
//  #include "quadrotorPropulsion/quadrotorPropulsion_initialize.h"
//}

namespace hector_quadrotor_model {

struct PropulsionParameters
{
    real_T k_m;
    real_T k_t;
    real_T CT2s;
    real_T CT1s;
    real_T CT0s;
    real_T Psi;
    real_T J_M;
    real_T R_A;
    real_T alpha_m;
    real_T beta_m;
    real_T l_m;

    PropulsionParameters()
      : k_m(0.0)
      , k_t(0.0)
      , CT2s(0.0)
      , CT1s(0.0)
      , CT0s(0.0)
      , Psi(0.0)
      , J_M(std::numeric_limits<real_T>::infinity())
      , R_A(std::numeric_limits<real_T>::infinity())
      , alpha_m(0.0)
      , beta_m(0.0)
      , l_m(0.0)
    {}
};

struct QuadrotorPropulsion::PropulsionModel {
  PropulsionParameters parameters_;
  boost::array<real_T,4>  x;
  boost::array<real_T,4>  x_pred;
  boost::array<real_T,10> u;
  boost::array<real_T,14> y;
};

QuadrotorPropulsion::QuadrotorPropulsion()
{
  // initialize propulsion model
  // quadrotorPropulsion_initialize();
  propulsion_model_ = new PropulsionModel;
}

QuadrotorPropulsion::~QuadrotorPropulsion()
{
  delete propulsion_model_;
}

/*
 * quadrotorPropulsion.c
 *
 * Code generation for function 'quadrotorPropulsion'
 *
 * C source code generated on: Sun Nov  3 13:34:35 2013
 *
 */

/* Include files */
//#include "rt_nonfinite.h"
//#include "motorspeed.h"
//#include "quadrotorPropulsion.h"
//#include "quadrotorPropulsion_rtwutil.h"

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */

/* Function Definitions */
void quadrotorPropulsion(const real_T xin[4], const real_T uin[10], const
  PropulsionParameters parameter, real_T dt, real_T y[14], real_T xpred[4])
{
  real_T v_1[4];
  int32_T i;
  real_T F_m[4];
  real_T U[4];
  real_T M_e[4];
  real_T I[4];
  real_T F[3];
  real_T b_F_m;
  real_T temp;
  real_T d0;

  /*  initialize vectors */
  for (i = 0; i < 4; i++) {
    xpred[i] = xin[i];

    /*  motorspeed */
    v_1[i] = 0.0;
  }

  memset(&y[0], 0, 14U * sizeof(real_T));
  for (i = 0; i < 4; i++) {
    F_m[i] = 0.0;
    U[i] = 0.0;
    M_e[i] = 0.0;
    I[i] = 0.0;
  }

  for (i = 0; i < 3; i++) {
    F[i] = 0.0;
  }

  /*  Input variables */
  U[0] = uin[6];
  U[1] = uin[7];
  U[2] = uin[8];
  U[3] = uin[9];

  /*  Constants */
  v_1[0] = -uin[2] + parameter.l_m * uin[4];
  v_1[1] = -uin[2] - parameter.l_m * uin[3];
  v_1[2] = -uin[2] - parameter.l_m * uin[4];
  v_1[3] = -uin[2] + parameter.l_m * uin[3];

  /*  calculate thrust for all 4 rotors */
  for (i = 0; i < 4; i++) {
    /*  if the flow speed at infinity is negative */
    if (v_1[i] < 0.0) {
      b_F_m = (parameter.CT2s * rt_powd_snf(v_1[i], 2.0) + parameter.CT1s *
               v_1[i] * xin[i]) + parameter.CT0s * rt_powd_snf(xin[i], 2.0);

      /*  if the flow speed at infinity is positive */
    } else {
      b_F_m = (-parameter.CT2s * rt_powd_snf(v_1[i], 2.0) + parameter.CT1s *
               v_1[i] * xin[i]) + parameter.CT0s * rt_powd_snf(xin[i], 2.0);
    }

    /*  sum up all rotor forces */
    /*  Identification of Roxxy2827-34 motor with 10x4.5 propeller */
    /*  temporarily used Expressions */
    temp = (U[i] * parameter.beta_m - parameter.Psi * xin[i]) / (2.0 *
      parameter.R_A);
    temp += sqrt(rt_powd_snf(temp, 2.0) + U[i] * parameter.alpha_m /
                 parameter.R_A);
    d0 = parameter.Psi * temp;

    /*  electrical torque motor 1-4 */
    /*  new version */
    /*  old version */
    /*  fx   = (Psi/R_A*(U-Psi*omega_m) - M_m)/J_M; */
    /*  A    = -(Psi^2/R_A)/J_M; */
    /*  B(1) =  Psi/(J_M*R_A); */
    /*  B(2) = -1/J_M; */
    /*  system outputs. Use euler solver to predict next time step */
    /*  predicted motor speed */
    /*  electric torque */
    /* y = [M_e I]; */
    /*  system jacobian */
    /*  A       = 1 + dt*A; */
    /*  input jacobian */
    /*  B       = A*B*dt; */
    M_e[i] = d0;
    I[i] = temp;
    xpred[i] = xin[i] + dt * (1.0 / parameter.J_M * (d0 - (parameter.k_t * b_F_m
      + parameter.k_m * xin[i])));
    F_m[i] = b_F_m;
    F[2] += b_F_m;
  }

  /*  System output, i.e. force and torque of quadrotor */
  y[0] = 0.0;
  y[1] = 0.0;
  y[2] = F[2];

  /*  torque for rotating quadrocopter around x-axis is the mechanical torque */
  y[3] = (F_m[3] - F_m[1]) * parameter.l_m;

  /*  torque for rotating quadrocopter around y-axis is the mechanical torque */
  y[4] = (F_m[0] - F_m[2]) * parameter.l_m;

  /*  torque for rotating quadrocopter around z-axis is the electrical torque */
  y[5] = ((-M_e[0] - M_e[2]) + M_e[1]) + M_e[3];

  /*  motor speeds (rad/s) */
  for (i = 0; i < 4; i++) {
    y[i + 6] = xpred[i];
  }

  /*  motor current (A) */
  for (i = 0; i < 4; i++) {
    y[i + 10] = I[i];
  }

  /*  M_e(1:4) / Psi; */
}

/* End of code generation (quadrotorPropulsion.c) */

inline void QuadrotorPropulsion::f(const double xin[4], const double uin[10], double dt, double y[14], double xpred[4]) const
{
  quadrotorPropulsion(xin, uin, propulsion_model_->parameters_, dt, y, xpred);
}

bool QuadrotorPropulsion::configure(const ros::NodeHandle &param)
{
  // get model parameters
  if (!param.getParam("k_m",     propulsion_model_->parameters_.k_m)) return false;
  if (!param.getParam("k_t",     propulsion_model_->parameters_.k_t)) return false;
  if (!param.getParam("CT0s",    propulsion_model_->parameters_.CT0s)) return false;
  if (!param.getParam("CT1s",    propulsion_model_->parameters_.CT1s)) return false;
  if (!param.getParam("CT2s",    propulsion_model_->parameters_.CT2s)) return false;
  if (!param.getParam("J_M",     propulsion_model_->parameters_.J_M)) return false;
  if (!param.getParam("l_m",     propulsion_model_->parameters_.l_m)) return false;
  if (!param.getParam("Psi",     propulsion_model_->parameters_.Psi)) return false;
  if (!param.getParam("R_A",     propulsion_model_->parameters_.R_A)) return false;
  if (!param.getParam("alpha_m", propulsion_model_->parameters_.alpha_m)) return false;
  if (!param.getParam("beta_m",  propulsion_model_->parameters_.beta_m)) return false;

#ifndef NDEBUG
  std::cout << "Loaded the following quadrotor propulsion model parameters from namespace " << param.getNamespace() << ":" << std::endl;
  std::cout << "k_m     = " << propulsion_model_->parameters_.k_m << std::endl;
  std::cout << "k_t     = " << propulsion_model_->parameters_.k_t << std::endl;
  std::cout << "CT2s    = " << propulsion_model_->parameters_.CT2s << std::endl;
  std::cout << "CT1s    = " << propulsion_model_->parameters_.CT1s << std::endl;
  std::cout << "CT0s    = " << propulsion_model_->parameters_.CT0s << std::endl;
  std::cout << "Psi     = " << propulsion_model_->parameters_.Psi << std::endl;
  std::cout << "J_M     = " << propulsion_model_->parameters_.J_M << std::endl;
  std::cout << "R_A     = " << propulsion_model_->parameters_.R_A << std::endl;
  std::cout << "l_m     = " << propulsion_model_->parameters_.l_m << std::endl;
  std::cout << "alpha_m = " << propulsion_model_->parameters_.alpha_m << std::endl;
  std::cout << "beta_m  = " << propulsion_model_->parameters_.beta_m << std::endl;
#endif

  initial_voltage_ = 14.8;
  param.getParam("supply_voltage", initial_voltage_);
  reset();

  return true;
}

void QuadrotorPropulsion::reset()
{
  boost::mutex::scoped_lock lock(mutex_);

  propulsion_model_->x.assign(0.0);
  propulsion_model_->x_pred.assign(0.0);
  propulsion_model_->u.assign(0.0);
  propulsion_model_->y.assign(0.0);

  wrench_ = geometry_msgs::Wrench();

  motor_status_ = hector_uav_msgs::MotorStatus();
  motor_status_.voltage.resize(4);
  motor_status_.frequency.resize(4);
  motor_status_.current.resize(4);

  supply_ = hector_uav_msgs::Supply();
  supply_.voltage.resize(1);
  supply_.voltage[0] = initial_voltage_;

  last_command_time_ = ros::Time();

  command_queue_ = std::queue<hector_uav_msgs::MotorPWMConstPtr>(); // .clear();
}

void QuadrotorPropulsion::setVoltage(const hector_uav_msgs::MotorPWM& voltage)
{
  boost::mutex::scoped_lock lock(mutex_);
  last_command_time_ = voltage.header.stamp;

  if (motor_status_.on && voltage.pwm.size() >= 4) {
    propulsion_model_->u[6] = voltage.pwm[0] * supply_.voltage[0] / 255.0;
    propulsion_model_->u[7] = voltage.pwm[1] * supply_.voltage[0] / 255.0;
    propulsion_model_->u[8] = voltage.pwm[2] * supply_.voltage[0] / 255.0;
    propulsion_model_->u[9] = voltage.pwm[3] * supply_.voltage[0] / 255.0;
  } else {
    propulsion_model_->u[6] = 0.0;
    propulsion_model_->u[7] = 0.0;
    propulsion_model_->u[8] = 0.0;
    propulsion_model_->u[9] = 0.0;
  }
}

void QuadrotorPropulsion::setTwist(const geometry_msgs::Twist &twist)
{
  boost::mutex::scoped_lock lock(mutex_);
  propulsion_model_->u[0] = twist.linear.x;
  propulsion_model_->u[1] = -twist.linear.y;
  propulsion_model_->u[2] = -twist.linear.z;
  propulsion_model_->u[3] = twist.angular.x;
  propulsion_model_->u[4] = -twist.angular.y;
  propulsion_model_->u[5] = -twist.angular.z;

  // We limit the input velocities to +-100. Required for numeric stability in case of collisions,
  // where velocities returned by Gazebo can be very high.
  limit(boost::iterator_range<boost::array<real_T,10>::iterator>(&(propulsion_model_->u[0]), &(propulsion_model_->u[6])), -100.0, 100.0);
}

void QuadrotorPropulsion::addCommandToQueue(const hector_uav_msgs::MotorCommandConstPtr& command)
{
  hector_uav_msgs::MotorPWMPtr pwm(new hector_uav_msgs::MotorPWM);
  pwm->header = command->header;
  pwm->pwm.resize(command->voltage.size());
  for(std::size_t i = 0; i < command->voltage.size(); ++i) {
    int temp = round(command->voltage[i] / supply_.voltage[0] * 255.0);
    if (temp < 0)
      pwm->pwm[i] = 0;
    else if (temp > 255)
      pwm->pwm[i] = 255;
    else
      pwm->pwm[i] = temp;
  }
  addPWMToQueue(pwm);
}

void QuadrotorPropulsion::addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr& pwm)
{
  boost::mutex::scoped_lock lock(command_queue_mutex_);

  if (!motor_status_.on) {
    ROS_WARN_NAMED("quadrotor_propulsion", "Received new motor command. Enabled motors.");
    engage();
  }

  ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Received motor command valid at " << pwm->header.stamp);
  command_queue_.push(pwm);
  command_condition_.notify_all();
}

bool QuadrotorPropulsion::processQueue(const ros::Time &timestamp, const ros::Duration &tolerance, const ros::Duration &delay, const ros::WallDuration &wait, ros::CallbackQueue *callback_queue) {
  boost::mutex::scoped_lock lock(command_queue_mutex_);
  bool new_command = false;

  ros::Time min(timestamp), max(timestamp);
  try {
    min = timestamp - delay - tolerance /* - ros::Duration(dt) */;
  } catch (std::runtime_error &e) {}

  try {
    max = timestamp - delay + tolerance;
  } catch (std::runtime_error &e) {}

  do {

    while(!command_queue_.empty()) {
      hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = command_queue_.front();
      ros::Time new_time = new_motor_voltage->header.stamp;

      if (new_time.isZero() || (new_time >= min && new_time <= max)) {
        setVoltage(*new_motor_voltage);
        command_queue_.pop();
        new_command = true;

      // new motor command is too old:
      } else if (new_time < min) {
        ROS_DEBUG_NAMED("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).toSec());
        command_queue_.pop();

      // new motor command is too new:
      } else {
        break;
      }
    }

    if (command_queue_.empty() && !new_command) {
      if (!motor_status_.on || wait.isZero()) break;

      ROS_DEBUG_NAMED("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.toSec(), (timestamp - last_command_time_).toSec());
      if (!callback_queue) {
        if (command_condition_.timed_wait(lock, wait.toBoost())) continue;
      } else {
        lock.unlock();
        callback_queue->callAvailable(wait);
        lock.lock();
        if (!command_queue_.empty()) continue;
      }

      ROS_ERROR_NAMED("quadrotor_propulsion", "Command timed out. Disabled motors.");
      shutdown();
    }

  } while(false);

  if (new_command) {
      ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)");
  }

  return new_command;
}

void QuadrotorPropulsion::update(double dt)
{
  if (dt <= 0.0) return;

  ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.twist:   " << PrintVector<double>(propulsion_model_->u.begin(), propulsion_model_->u.begin() + 6));
  ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.voltage: " << PrintVector<double>(propulsion_model_->u.begin() + 6, propulsion_model_->u.begin() + 10));
  ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_prior: " << PrintVector<double>(propulsion_model_->x.begin(), propulsion_model_->x.end()));

  checknan(propulsion_model_->u, "propulsion model input");
  checknan(propulsion_model_->x, "propulsion model state");

  // update propulsion model
  f(propulsion_model_->x.data(), propulsion_model_->u.data(), dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data());
  propulsion_model_->x = propulsion_model_->x_pred;

  ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_post:  " << PrintVector<double>(propulsion_model_->x.begin(), propulsion_model_->x.end()));
  ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.force:   " << PrintVector<double>(propulsion_model_->y.begin() + 0, propulsion_model_->y.begin() + 3) << " " <<
                                                 "propulsion.torque:  " << PrintVector<double>(propulsion_model_->y.begin() + 3, propulsion_model_->y.begin() + 6));

  checknan(propulsion_model_->y, "propulsion model output");

  wrench_.force.x  =  propulsion_model_->y[0];
  wrench_.force.y  = -propulsion_model_->y[1];
  wrench_.force.z  =  propulsion_model_->y[2];
  wrench_.torque.x =  propulsion_model_->y[3];
  wrench_.torque.y = -propulsion_model_->y[4];
  wrench_.torque.z = -propulsion_model_->y[5];

  motor_status_.voltage[0] = propulsion_model_->u[6];
  motor_status_.voltage[1] = propulsion_model_->u[7];
  motor_status_.voltage[2] = propulsion_model_->u[8];
  motor_status_.voltage[3] = propulsion_model_->u[9];

  motor_status_.frequency[0] = propulsion_model_->y[6];
  motor_status_.frequency[1] = propulsion_model_->y[7];
  motor_status_.frequency[2] = propulsion_model_->y[8];
  motor_status_.frequency[3] = propulsion_model_->y[9];
  motor_status_.running = motor_status_.frequency[0] > 1.0 && motor_status_.frequency[1] > 1.0 && motor_status_.frequency[2] > 1.0 && motor_status_.frequency[3] > 1.0;

  motor_status_.current[0] = propulsion_model_->y[10];
  motor_status_.current[1] = propulsion_model_->y[11];
  motor_status_.current[2] = propulsion_model_->y[12];
  motor_status_.current[3] = propulsion_model_->y[13];
}

void QuadrotorPropulsion::engage()
{
  motor_status_.on = true;
}

void QuadrotorPropulsion::shutdown()
{
  motor_status_.on = false;
}

} // namespace hector_quadrotor_model


#pragma once
#include <Eigen/Dense>
#include <vector>
#include <array>
#include "plane3d.hpp"
#include "shape2d.hpp"
#include "line_interval.hpp"
#include "found_path.hpp"

using namespace Eigen;
using namespace std;

#ifndef PLANE2D_HPP
#define PLANE2D_HPP

class Shape2d;
class LineInterval;

class Plane2d{
    vector<Shape2d> shapes;
    vector<LineInterval> lineIntervals;
    vector<LineInterval> candidateIntervals;
    double minUpperBound;
  public:
    Plane2d(vector<Shape3d>& shapes, Plane3d plane3d);
    vector<Shape2d> Shapes();
    void CalcLineIntervalsInit();
    vector<FoundPath> FindPaths(double distBound);
    bool DivideCandidateIntervals(double distBound, int time);
    void PruneCandidateIntervals();
    vector<array<double, 2>> LineIntervalBounds();
    vector<unsigned long int> IntervalShapeIds(unsigned long int intervalId);
    double MinUpperBound();
    vector<LineInterval> CandidateIntervals();
};

#endif


#pragma once
#ifndef CANNON_GRAPHICS_CAMERA_H
#define CANNON_GRAPHICS_CAMERA_H

/*!
 * \file cannon/graphics/camera.hpp
 * \brief File containing Camera class definition.
 */

/*!
 * \namespace cannon::graphics
 * \brief Namespace containing graphics classes and functions. Many of these are
 * wrappers around OpenGL functionality.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace graphics {

    /*!
     * \brief Class representing a simple 3D camera able to translate and rotate.
     *
     * This class implements a simple, fly-style camera. It is adapted from
     * https://learnopengl.com/Getting-started/Camera. The camera is
     * responsible for generating appropriate perspective matrices for OpenGL
     * rendering and is held by graphics classes that provide 3D rendering capabilities.
     *
     * \sa cannon::graphics::Viewer3D, cannon::graphics::DeferredRenderer
     */
    class Camera {
      public:
        Camera() = delete;

        /*!
         * Constructor creating a Camera from an initial pose, a target
         * location for the camera to point at, an up vector that is
         * non-colinear with the viewing direction, and optionally a movement
         * speed for the camera.
         *
         * \param pos The initial camera position.
         * \param target The initial location for the camera to point at.
         * \param up A vector representing the camera's local "up" direction.
         * \param speed The initial camera movement speed.
         *
         */
        Camera(const Vector3f& pos, const Vector3f& target, const Vector3f& up, float speed=0.05) : pos_(pos), direction_(pos - target), up_(up), speed_(speed) {
          direction_.normalize();
          up_.normalize();

          right_ = up_.cross(direction_);
          right_.normalize();
        }

        /*!
         * Method returning the view matrix that this camera represents. For
         * more information on computing this matrix, see
         * https://learnopengl.com/Getting-started/Camera
         *
         * \returns The view matrix.
         */
        Matrix4f get_view_mat() const;

        /*!
         * Method returning the camera position.
         *
         * \returns The camera position vector.
         */
        Vector3f get_pos() const;

        /*!
         * Method returning the camera facing direction.
         *
         * \returns The camera facing direction as a vector.
         */
        Vector3f get_direction() const;

        /*!
         * Method returning the camera-local "right" vector, which is
         * orthogonal to the camera facing direction and "up" direction.
         *
         * \returns The "right" vector.
         */
        Vector3f get_right() const;

        /*!
         * Method to set the camera movement speed.
         *
         * \param speed The new camera speed.
         */
        void set_speed(float speed);

        /*!
         * Method to set the camera base speed. This is the camera speed when
         * un-modified by "sprinting".
         *
         * \param base_speed The new base camera speed.
         */
        void set_base_speed(float base_speed);

        // Controls
        /*!
         * Method to cause the camera to move forward. "Forward" is defined by
         * the camera moving direction, and the amount that the camera position
         * is modified is defined by the camera speed.
         *
         * \sa cannon::graphics::Camera::move_forward
         */
        void move_forward();

        /*!
         * Method to cause the camera to move backward. "Backward" is defined
         * by negation of the camera facing direction.
         *
         * \sa cannon::graphics::Camera::move_forward
         */
        void move_backward();

        /*!
         * Method to cause the camera to strafe left. "Left" is defined by
         * the negation of the camera's right vector.
         *
         * \sa cannon::graphics::Camera::move_forward
         */
        void strafe_left();

        /*!
         * Method to cause the camera to strafe right. "Right" is defined by
         * the camera's right vector.
         *
         * \sa cannon::graphics::Camera::move_forward
         */
        void strafe_right();

        /*!
         * Method to cause the camera to continuously move forward, or to cause
         * the camera to stop continuously moving forward. This method is
         * useful for moving the camera in response to user input, as it
         * reduces over-registration of keyboard input and can lead to more
         * consistent movement speed.
         *
         * \param on Whether the camera should move forward or cease moving forward.
         */
        void set_move_forward(bool on);

        /*!
         * Method to cause the camera to continuously move backward (or stop moving backward).
         *
         * \param on Whether the camera should move backward or cease moving backward.
         *
         * \sa cannon::graphics::Camera::set_move_forward
         */
        void set_move_backward(bool on);

        /*!
         * Method to cause the camera to continuously move left (or stop moving left).
         *
         * \param on Whether the camera should move left or cease moving left.
         *
         * \sa cannon::graphics::Camera::set_move_forward
         */
        void set_strafe_left(bool on);

        /*!
         * Method to cause the camera to continuously move right (or stop moving right).
         *
         * \param on Whether the camera should move right or cease moving right.
         *
         * \sa cannon::graphics::Camera::set_move_forward
         */
        void set_strafe_right(bool on);

        /*!
         * Method to cause the camera to update its position. This is primarily
         * used in concert with the set_move_* and set_strafe_* methods.
         */
        void update_pos();

        /*!
         * Method to set the camera position.
         *
         * \param pos The new camera position.
         */
        void set_pos(const Vector3f& pos);

        /*!
         * Method to set the camera facing direction.
         *
         * \param direction The new camera direction, which should be of unit length.
         */
        void set_direction(const Vector3f& direction);

      private:
        Vector3f pos_; //!< The camera position
        Vector3f direction_; //!< The camera facing direction
        Vector3f up_; //!< The camera-local up vector
        Vector3f right_; //!< The camera-local right vector

        bool move_forward_ = false; //!< Whether the camera is moving continuously forward
        bool move_backward_ = false; //!< Whether the camera is moving continuously backward
        bool strafe_right_ = false; //!< Whether the camera is moving continuously right
        bool strafe_left_ = false; //!< Whether the camera is moving continuously left

        float speed_ = 1.0; //!< The current camera movement speed
        float base_speed_ = 0.5; //!< The base camera movement speed
    };

  } // namespace graphics
} // namespace cannon

#endif /* ifndef CANNON_GRAPHICS_CAMERA_H */