/*
 * Copyright (C) 2015 INRA
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *    1. Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *    2. 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.
 *
 * THIS SOFTWARE IS PROVIDED "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 OR CONTRIBUTORS 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 <algorithm>
#include <iterator>
#include <boost/progress.hpp>
#include <vle/dsde/qss1.hpp>
#include <vle/vle.hpp>

using state_type = std::array <double, 5>;

enum FirePosition {
    NORTH = 1, SOUTH, EAST, WEST
};

struct NoStorage {
    using input_type = vle::dsde::qss1::inputport <5>;
    using output_type = vle::dsde::qss1::doubleport;

    inline void push(std::size_t, double, const input_type&) {}
    inline void push(std::size_t, double, const output_type&) {}
    inline void resize(std::size_t) {}
};

struct Storage {
    using input_type = vle::dsde::qss1::inputport <5>;
    using output_type = vle::dsde::qss1::doubleport;

    using input_data = std::vector <input_type>;
    using output_data = std::vector <output_type>;

    struct Event {
        Event(const input_type &input)
        {
            in.emplace_back(input);
        }

        Event(const output_type &output)
        {
            out.emplace_back(output);
        }

        input_data in;
        output_data out;
    };

    ~Storage()
    {
        try {
            std::cout << "Storage in progress:\n" << std::flush;

            for (auto i = 0ul, e = vector.size(); i != e; ++i) {
                std::string filename(vle::stringf("model-%lu.dat", i));
                std::ofstream ofs(filename);
                std::cout << "\rWrite: " << filename << std::flush;

                for (const auto &event : vector[i]) {
                    ofs << event.first << ' ';

                    if (event.second.in.empty())
                        ofs << "nan nan nan nan nan ";
                    else
                        for (const auto &in : event.second.in)
                            ofs << in[0] << ' ' << in[1] << ' ' << in[2] << ' '
                                << in[3] << ' ' << in[4] << ' ';

                    if (event.second.out.empty())
                        ofs << "nan";
                    else
                        for (const auto &out : event.second.out)
                            ofs << out[0];

                    ofs << '\n';
                }

                ofs << '\n';
            }
        } catch (...) {
            std::cerr << "failure\n";
        }
    }

    void push(std::size_t id, double time, const input_type &data)
    {
        auto ret = vector.at(id).emplace(time, data);

        if (ret.second == false)
            ret.first->second.in.emplace_back(data);
    }

    void push(std::size_t id, double time, const output_type &data)
    {
        auto ret = vector.at(id).emplace(time, data);

        if (ret.second == false)
            ret.first->second.out.emplace_back(data);
    }

    void resize(std::size_t size)
    {
        vector.resize(size);
    }

    std::vector <std::map <double, Event>> vector;
};

class Fire
{
public:
    Fire(double alpha, double K, double k, double enthalpy,
         double mass, double surrounding, double dx, double dy)
        : coeff_n(K / (dy * dy))
        , coeff_s(K / (dy * dy))
        , coeff_e(K / (dx * dx))
        , coeff_w(K / (dx * dx))
        , coeff_c(- k - 2.0 * K * (1.0 / (dx * dx) + 1.0 / (dy * dy)))
        , coeff_cste(k * surrounding)
        , alpha(alpha)
        , f_t0(enthalpy * mass * alpha)
        , t0(0.0)
        , fire(false)
    {}

    double operator()(const state_type &variables, const double t)
    {
        if (!fire && variables[0] > 400.0) {
            fire = true;
            t0 = t;
        }

        if (fire && variables[0] < 333.0) {
            fire = false;
        }

        double ret = coeff_n * ((variables[NORTH] > 0.) ?
                                variables[NORTH] : variables[0]) +
                     coeff_s * ((variables[SOUTH] > 0.) ?
                                variables[SOUTH] : variables[0]) +
                     coeff_e * ((variables[EAST] > 0.) ?
                                variables[EAST] : variables[0]) +
                     coeff_w * ((variables[WEST] > 0.) ?
                                variables[WEST] : variables[0]) +
                     coeff_c * variables[0] +
                     coeff_cste;

        if (fire) {
            double f_t = f_t0 * std::exp(alpha * (t0 - t));
            f_t0 = f_t;
            t0 = t;
            ret += f_t;
        }

        return ret;
    }

private:
    const double coeff_n, coeff_s, coeff_e, coeff_w, coeff_c, coeff_cste;
    const double alpha;
    double f_t0, t0;
    bool fire;
};

template <typename StoragePolicy>
class FireForrest : public vle::dsde::CoupledModel <
    vle::DoubleTime, vle::PortList <double>, vle::PortList <double>,
    vle::dsde::qss1::inputport <5>,
    vle::dsde::qss1::doubleport,
    vle::dsde::TransitionPolicyDefault <vle::DoubleTime >>
{
public:
    using parent_type = vle::dsde::CoupledModel <
                        vle::DoubleTime, vle::PortList <double>, vle::PortList <double>,
                        vle::dsde::qss1::inputport <5>,
                        vle::dsde::qss1::doubleport,
                        vle::dsde::TransitionPolicyDefault <vle::DoubleTime >>;
    using children_t = parent_type::children_t;
    using child_type = parent_type::child_type;

    FireForrest(const vle::Context &ctx,
                std::size_t width, std::size_t height,
                double alpha, double K, double k, double enthalpy,
                double mass, double surrounding, double dx, double dy,
                double dq, double epsilon)
        : parent_type(ctx)
        , m_width(width)
        , m_height(height)
    {
        m_storage.resize(m_width * m_height);
        m_models.reserve(width * height);

        for (auto y = 0ul; y != height; ++y) {
            for (auto x = 0ul; x != width; ++x) {
                double init = (x == width / 4 && y == height / 4) ? 600 : 300;
                double localmass = (x > width / 2 && y > height / 2) ? mass /
                                   10 : mass;
                m_models.emplace_back(
                    ctx, dq, epsilon, init, 0u,
                    Fire(alpha, K, k, enthalpy, localmass, surrounding, dx, dy));
            }
        }
    }

    FireForrest(const vle::Context &ctx,
                std::ifstream &ifs,
                double alpha, double K, double k, double enthalpy,
                double mass, double surrounding, double dx, double dy,
                double dq, double epsilon)
        : parent_type(ctx)
        , m_time(0)
    {
        ifs.exceptions(std::ios_base::failbit | std::ios_base::badbit);
        ifs >> m_width >> m_height;

        if (m_width == 0 or m_height == 0)
            return;

        m_storage.resize(m_width * m_height);
        m_models.reserve(m_width * m_height);
        std::vector <int> type(m_width * m_height, 0);
        std::vector <double> init(m_width * m_height, 0);
        std::copy_n(std::istream_iterator <int>(ifs),
                    m_width * m_height,
                    type.begin());
        std::copy_n(std::istream_iterator <double>(ifs),
                    m_width * m_height,
                    init.begin());
        std::cout << m_width << "x" << m_height << "→"
                  << m_height *m_width << '\n';

        for (auto i = 0ul; i != (m_height * m_width); ++i) {
            switch (type[i]) {
            case 2:
                std::cout << ((init[i] <= 300) ? ". " : "⚡ ");
                m_models.emplace_back(
                    ctx, dq, epsilon, init[i], 0u,
                    Fire(alpha, K, k, enthalpy, mass / 4.0, surrounding, dx, dy));
                break;
            default:
                std::cout << ((init[i] <= 300) ? ": " : "⚡ ");
                m_models.emplace_back(
                    ctx, dq, epsilon, init[i], 0u,
                    Fire(alpha, K, k, enthalpy, mass, surrounding, dx, dy));
                break;
            }

            if (((1 + i) % m_width) == 0)
                std::cout << '\n';
        }
    }

    virtual children_t children(const vle::Common &) override final
    {
        children_t ret(m_models.size(), nullptr);

        for (auto i = 0ul, e = m_models.size(); i != e; ++i)
            ret[i] = &m_models[i];

        return ret;
    }

    virtual void post(const UpdatedPort &out,
                      UpdatedPort &in) const override final
    {
        for (auto *mdl : out) {
            auto tmp = static_cast <const decltype(&m_models[0])>(mdl) - &m_models[0];
            std::size_t position = boost::numeric_cast<std::size_t>(tmp);
            auto x = position % m_width;
            auto y = position / m_width;
            m_storage.push(position, CoupledModel::tn, mdl->y);

            if (x > 0) {
                m_models[y * m_width + (x - 1)].x[EAST] = mdl->y[0];
                in.emplace(&m_models[y * m_width + (x - 1)]);
            }

            if (x + 1 < m_width) {
                m_models[y * m_width + (x + 1)].x[WEST] = mdl->y[0];
                in.emplace(&m_models[y * m_width + (x + 1)]);
            }

            if (y > 0) {
                m_models[(y - 1) * m_width + x].x[NORTH] = mdl->y[0];
                in.emplace(&m_models[(y - 1) * m_width + x]);
            }

            if (y + 1 < m_height) {
                m_models[(y + 1) * m_width + x].x[SOUTH] = mdl->y[0];
                in.emplace(&m_models[(y + 1) * m_width + x]);
            }
        }

        for (auto *mdl : in)
            m_storage.push(
                boost::numeric_cast<std::size_t>(
                    static_cast <const decltype(&m_models[0])>(mdl) - &m_models[0]),
                CoupledModel::tn,
                mdl->x);
    }

    void value(std::ostream &os)
    {
        for (auto i = 0ul, e = m_models.size(); i != e; ++i) {
            os << m_models[i].value();

            if ((i + 1) % m_width == 0)
                os << "\n";
            else
                os << " ";
        }

        os << "\n";
    }

    std::vector <vle::dsde::qss1::Equation<vle::DoubleTime, 5>> m_models;
    std::size_t m_width;
    std::size_t m_height;
    double m_time;
    mutable StoragePolicy m_storage;
};

template <typename StoragePolicy>
void run(FireForrest <StoragePolicy> &model, vle::Context &ctx)
{
    constexpr const double finish = 200.0;
    constexpr const unsigned int tostore = 40u;
    vle::dsde::Engine <vle::DoubleTime> dsde_engine;
    vle::SimulationStep <vle::dsde::Engine <vle::DoubleTime>> sim(ctx, dsde_engine);
    double current = sim.init(model, 0.0);
    double before = current;
    auto step = 0u;
    {
        boost::progress_timer timer;
        boost::progress_display progress(tostore);

        while (sim.step(model, current, finish)) {
            if ((current - before) >= (finish / tostore)) {
                std::ofstream ofs(vle::stringf("fire-%u.dat", step));
                model.value(ofs);
                before = current;
                ++step;
                ++progress;
            }
        }

        progress += (progress.expected_count() - progress.count());
    }
    std::ofstream ofs("fireforrest.gnuplot");
    ofs << "set terminal png nocrop enhanced size 4000,3000 font \"arial,9\"\n"
        << "set output 'fireforrest.png'\n"
        << "set multiplot layout "
        << 4u + (((step % 4u) > 0) ? 1u : 0u) << ','
        << step / 4u << " rowsfirst\n"
        << "set size ratio 1\n"
        << "set palette model XYZ rgbformulae 7,5,15\n"
        << "set xrange[-0.5:" << model.m_width << ".5]\n"
        << "set yrange[-0.5:" << model.m_height << ".5]\n"
        << "set cbrange[150.0:800.0]\n"
        << "set xtics 0,2," << model.m_width << "\n"
        << "set ytics 0,2," << model.m_height << "\n"
        << "set xtics offset -0.5,0\n";

    for (auto i = 0ul; i < step; ++i)
        ofs << "set title \"fire-" << i << ".dat" << "\"\n"
            << "plot \"fire-" << i << ".dat\" matrix w image noti\n";

    ofs << "unset multiplot\n";
}

int main(int argc, char **argv)
{
    vle::Context ctx = std::make_shared <vle::ContextImpl>();
    constexpr const std::size_t width = 30u;
    constexpr const std::size_t height = 30u;
    constexpr const double dq = 0.5;
    constexpr const double epsilon = 0.5;
    constexpr const double alpha = 0.19;
    constexpr const double K = 0.000031;
    constexpr const double k = 0.071;
    constexpr const double enthalpy = 3605;
    constexpr const double mass = 2;
    constexpr const double surrounding = 300;
    constexpr const double dx = 1. / 20.;
    constexpr const double dy = 1. / 20.;
    bool withstorage = false;
    std::vector <char *> files;
    files.reserve(4);

    for (int i = 1; i < argc; ++i)
        if (std::strcmp(argv[i], "-s") == 0)
            withstorage = true;
        else
            files.push_back(argv[i]);

    if (files.empty()) {
        std::cout << "Default simulation\n";
        FireForrest <NoStorage> model(ctx, width, height,
                                      alpha, K, k, enthalpy, mass,
                                      surrounding,  dx,  dy, dq, epsilon);
        run(model, ctx);
    } else {
        for (auto *str : files) {
            std::cout << str << " simulation\n";
            std::ifstream ifs(str);

            if (ifs.is_open()) {
                if (withstorage) {
                    FireForrest <Storage> model(ctx, ifs, alpha, K, k,
                                                enthalpy, mass, surrounding,
                                                dx,  dy, dq, epsilon);
                    run(model, ctx);
                } else {
                    FireForrest <NoStorage> model(ctx, ifs, alpha, K, k,
                                                  enthalpy, mass, surrounding,
                                                  dx,  dy, dq, epsilon);
                    run(model, ctx);
                }
            }
        }
    }

    return 0;
}


#include <boost/config.hpp>
#include <iostream>
#include <fstream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

#define LINEMAX 1024
//get a line from file, return -1 if failed
// lineptr is the line string, it is allocated inside but need to be free outside
// n is the number of chars in string(not includ the last "\0")
int mygetline(char** lineptr, size_t* n, FILE* stream)
{
    char* buffer = *lineptr;
    char* relocate_buffer = buffer;

    buffer = static_cast<char *>(malloc(LINEMAX + 1));

    if (buffer == NULL)
    {
        printf("allocate memory failed in getline!\n");
        return -1;
    }
    size_t linemax = LINEMAX + 1;
    size_t numread = 0;
    char c;
    while (numread< linemax)
    {
        c = (char)fgetc(stdin);
        if (c == EOF)break;// end of file
        buffer[numread] = c;
        numread++;
        if (c == '\n')// end of line
            break;
        if (numread == linemax-1) {
            //try to extend memory to 2 times
            relocate_buffer = static_cast<char *>(realloc(buffer, 2 * linemax));
            if (relocate_buffer == NULL) {
                free(buffer);
                printf("Error when extend memory in getline!\n");
                return -1;
            }else {
                buffer = relocate_buffer;
                linemax *= 2;
            }
        }

    }
    buffer[numread] = '\0';
    *lineptr = buffer;
    *n = numread;
    return 0;
}

//parse adjacency matrix,read a line string, get the a row of matrix, return the number of column
// return -1 if failed. if the char is 'x' the value in matrix will be -1
// the matrx_row_ptr must be already allocated;
int ParseWeight(char* lineptr, int* matrx_row) {
    size_t offset = 0;// line str parse offset
    size_t conlumn = 0; //conlumn index of this row
    while (lineptr[offset] != '\n')
    {
        if (lineptr[offset] == ' ')
        {
            offset++;
            continue;
        }

        if (lineptr[offset] == 'x')
        {
            offset++; // update str offset
            //do something
            // /*Debug*/printf("%c\n", 'x');
            matrx_row[conlumn] = -1;
            conlumn++;// update conlumn index
            continue;
        }
        char* nextchar;
        int weight = (int)strtol(&lineptr[offset], &nextchar, 10);// try to get the number
        if (weight == 0) {
            perror("ParseWeight failed!\n");
            return -1; }// farse failed
        offset = nextchar - lineptr;// update offset

        ///*Debug*/printf("%d\n", weight);
        matrx_row[conlumn] = weight;
        conlumn++;// update conlumn index
    }
    return conlumn;
}

int main(int, char*[])
{

    char* lineptr = NULL;
    size_t linelength = 0;
    //read number of cities
    int rt = -1;
    rt=mygetline(&lineptr, &linelength, stdin);
    if (rt == -1)
    {
        printf("Read number of city failed!");
        return -1;
    }
    char* nextchar;
    int ncity = (int)strtol(lineptr, &nextchar, 10);// try to get the number
    free(lineptr);
    if( ncity < 1) {
        printf("INPUT ERROR, please input city number N first\n");
        return -1;
    }

    // now read the adjacency matrix(should be n-1 lines)

    //adjacency matrix
    int** adj_matrix = static_cast<int **>(malloc(sizeof(int *) * (ncity - 1)));
    if (adj_matrix == NULL) {
        perror("Adjacency matrix allocation failed!\n");
        return -1;
    }

    for (int i = 0; i < ncity-1; i++)
    {
        //printf("int i: %d ",i);
        rt=mygetline(&lineptr, &linelength, stdin);
        if (rt == -1)
        {
            printf("Read adjacency lines failed!");
            return -1;
        }
        // allocate memory for adj_matrix[i]
        adj_matrix[i] = static_cast<int *>(malloc(sizeof(int) * (i + 1)));
        if (adj_matrix[i] == NULL) {
            printf("Adjacency matrix row %d allocation failed!\n", i);
            return -1;
        }

        rt=ParseWeight(lineptr, adj_matrix[i]);
        if (rt==-1)
        {
            printf("Adjacency matrix row %d ParseWeight failed!\n", i);
            return -1;
        }
        free(lineptr);//lineptr allocated inside getline
    }

    typedef adjacency_list< listS, vecS, undirectedS, no_property,
            property< edge_weight_t, int > >
            graph_t;
    typedef graph_traits< graph_t >::vertex_descriptor vertex_descriptor;
    typedef std::pair< int, int > Edge;

    std::vector<Edge> edge_array;
    std::vector<int> weights;
    // add edges
    for (int irow = 0; irow < ncity-1; irow++)
    {
        for (int jcol = 0; jcol <= irow; jcol++)
        {
            if(adj_matrix[irow][jcol]==-1)continue;
            edge_array.emplace_back(irow + 1, jcol);
            weights.push_back(adj_matrix[irow][jcol]);

            /*// add w->v too, as we can reach cities from both directions
            edge_array.emplace_back(jcol, irow+1);
            weights.push_back(adj_matrix[irow][jcol]);*/

            ///*debug*/printf("%d--%d-->%d......%d--%d-->%d\n", irow+1, adj_matrix[irow][jcol], jcol, jcol,
            //               adj_matrix[irow][jcol], irow+1);
        }
    }

    int num_arcs = edge_array.size();
    graph_t g(edge_array.data(), edge_array.data()+num_arcs, weights.data(), ncity);
    property_map< graph_t, edge_weight_t >::type weightmap
            = get(edge_weight, g);
    std::vector< vertex_descriptor > p(num_vertices(g));
    std::vector< int > d(num_vertices(g));
    vertex_descriptor s = vertex(0, g);

    /*dijkstra_shortest_paths(g, s,
                            predecessor_map(boost::make_iterator_property_map(
                                    p.begin(), get(boost::vertex_index, g)))
                                    .distance_map(boost::make_iterator_property_map(
                                            d.begin(), get(boost::vertex_index, g))));*/

    dijkstra_shortest_paths(g, s,
                            distance_map(boost::make_iterator_property_map(
                                            d.begin(), get(boost::vertex_index, g))));

    //std::cout << "distances and parents:" << std::endl;
    graph_traits< graph_t >::vertex_iterator vi, vend;
    std::vector<int> dist[100];
    /*for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi)
    {
        std::cout << "distance(" << *vi << ") = " << d[*vi] << ", ";
        std::cout << "parent(" << *vi << ") = " << p[*vi]
                  << std::endl;
    }*/
    auto max=*std::max_element(d.begin(),d.end());
    std::cout << max << std::endl;

    return EXIT_SUCCESS;
}


#include "itkTractsToVectorImageFilter.h"

// VTK
#include <vtkPolyLine.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>

// ITK
#include <itkTimeProbe.h>
#include <itkImageRegionIterator.h>

// misc
#define _USE_MATH_DEFINES
#include <cmath>
#include <boost/progress.hpp>
#include <mitkDiffusionFunctionCollection.h>

namespace itk{

static inline bool CompareVectorLengths(const vnl_vector_fixed< double, 3 >& v1, const vnl_vector_fixed< double, 3 >& v2)
{
  return (v1.magnitude()>v2.magnitude());
}

template< class PixelType >
TractsToVectorImageFilter< PixelType >::TractsToVectorImageFilter():
  m_NormalizationMethod(GLOBAL_MAX),
  m_AngularThreshold(0.7),
  m_Epsilon(0.999),
  m_MaxNumDirections(3),
  m_SizeThreshold(0.3),
  m_OnlyUseMaskGeometry(false)
{
  this->SetNumberOfRequiredOutputs(1);
}

template< class PixelType >
TractsToVectorImageFilter< PixelType >::~TractsToVectorImageFilter()
{
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToVectorImageFilter< PixelType >::GetVnlVector(double point[])
{
  vnl_vector_fixed<double, 3> vnlVector;
  vnlVector[0] = point[0];
  vnlVector[1] = point[1];
  vnlVector[2] = point[2];
  return vnlVector;
}

template< class PixelType >
void TractsToVectorImageFilter< PixelType >::GenerateData()
{
  mitk::BaseGeometry::Pointer geometry = m_FiberBundle->GetGeometry();

  // calculate new image parameters
  itk::Vector<double> spacing3;
  itk::Point<double> origin3;
  itk::Matrix<double, 3, 3> direction3;
  ImageRegion<3> imageRegion3;
  if (!m_MaskImage.IsNull())
  {
    spacing3 = m_MaskImage->GetSpacing();
    imageRegion3 = m_MaskImage->GetLargestPossibleRegion();
    origin3 = m_MaskImage->GetOrigin();
    direction3 = m_MaskImage->GetDirection();
  }
  else
  {
    spacing3 = geometry->GetSpacing();
    origin3 = geometry->GetOrigin();
    mitk::BaseGeometry::BoundsArrayType bounds = geometry->GetBounds();
    origin3[0] += bounds.GetElement(0);
    origin3[1] += bounds.GetElement(2);
    origin3[2] += bounds.GetElement(4);

    for (int i=0; i<3; i++)
      for (int j=0; j<3; j++)
        direction3[j][i] = geometry->GetMatrixColumn(i)[j];
    imageRegion3.SetSize(0, geometry->GetExtent(0)+1);
    imageRegion3.SetSize(1, geometry->GetExtent(1)+1);
    imageRegion3.SetSize(2, geometry->GetExtent(2)+1);

    m_MaskImage = ItkUcharImgType::New();
    m_MaskImage->SetSpacing( spacing3 );
    m_MaskImage->SetOrigin( origin3 );
    m_MaskImage->SetDirection( direction3 );
    m_MaskImage->SetRegions( imageRegion3 );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }
  OutputImageType::RegionType::SizeType outImageSize = imageRegion3.GetSize();
  m_OutImageSpacing = m_MaskImage->GetSpacing();
  m_ClusteredDirectionsContainer = ContainerType::New();

  // initialize num directions image
  m_NumDirectionsImage = ItkUcharImgType::New();
  m_NumDirectionsImage->SetSpacing( spacing3 );
  m_NumDirectionsImage->SetOrigin( origin3 );
  m_NumDirectionsImage->SetDirection( direction3 );
  m_NumDirectionsImage->SetRegions( imageRegion3 );
  m_NumDirectionsImage->Allocate();
  m_NumDirectionsImage->FillBuffer(0);

  itk::Vector<double, 4> spacing4;
  itk::Point<float, 4> origin4;
  itk::Matrix<double, 4, 4> direction4;
  itk::ImageRegion<4> imageRegion4;

  spacing4[0] = spacing3[0]; spacing4[1] = spacing3[1]; spacing4[2] = spacing3[2]; spacing4[3] = 1;
  origin4[0] = origin3[0]; origin4[1] = origin3[1]; origin4[2] = origin3[2]; origin3[3] = 0;
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction4[r][c] = direction3[r][c];
  direction4[3][3] = 1;
  imageRegion4.SetSize(0, imageRegion3.GetSize()[0]);
  imageRegion4.SetSize(1, imageRegion3.GetSize()[1]);
  imageRegion4.SetSize(2, imageRegion3.GetSize()[2]);
  imageRegion4.SetSize(3, m_MaxNumDirections*3);

  m_DirectionImage = ItkDirectionImageType::New();
  m_DirectionImage->SetSpacing( spacing4 );
  m_DirectionImage->SetOrigin( origin4 );
  m_DirectionImage->SetDirection( direction4 );
  m_DirectionImage->SetRegions( imageRegion4 );
  m_DirectionImage->Allocate();
  m_DirectionImage->FillBuffer(0.0);

  // iterate over all fibers
  vtkSmartPointer<vtkPolyData> fiberPolyData = m_FiberBundle->GetFiberPolyData();
  int numFibers = m_FiberBundle->GetNumFibers();
  m_DirectionsContainer = ContainerType::New();

  VectorContainer< unsigned int, std::vector< double > >::Pointer peakLengths = VectorContainer< unsigned int, std::vector< double > >::New();

  MITK_INFO << "Generating directions from tractogram";
  boost::progress_display disp(numFibers);
  for( int i=0; i<numFibers; i++ )
  {
    ++disp;
    vtkCell* cell = fiberPolyData->GetCell(i);
    int numPoints = cell->GetNumberOfPoints();
    vtkPoints* points = cell->GetPoints();
    if (numPoints<2)
      continue;

    vnl_vector_fixed<double, 3> dir;
    vnl_vector<double> v;

    float fiberWeight = m_FiberBundle->GetFiberWeight(i);

    for( int j=0; j<numPoints-1; j++)
    {
      itk::Point<float, 3> startVertex = mitk::imv::GetItkPoint(points->GetPoint(j));
      itk::Index<3> startIndex;
      itk::ContinuousIndex<float, 3> startIndexCont;
      m_MaskImage->TransformPhysicalPointToIndex(startVertex, startIndex);
      m_MaskImage->TransformPhysicalPointToContinuousIndex(startVertex, startIndexCont);

      itk::Point<float, 3> endVertex = mitk::imv::GetItkPoint(points->GetPoint(j + 1));
      itk::Index<3> endIndex;
      itk::ContinuousIndex<float, 3> endIndexCont;
      m_MaskImage->TransformPhysicalPointToIndex(endVertex, endIndex);
      m_MaskImage->TransformPhysicalPointToContinuousIndex(endVertex, endIndexCont);

      dir[0] = endVertex[0]-startVertex[0];
      dir[1] = endVertex[1]-startVertex[1];
      dir[2] = endVertex[2]-startVertex[2];
      if (dir.is_zero())
        continue;
      dir.normalize();

      std::vector< std::pair< itk::Index<3>, double > > segments = mitk::imv::IntersectImage(spacing3, startIndex, endIndex, startIndexCont, endIndexCont);
      for (std::pair< itk::Index<3>, double > segment : segments)
      {
        if (!m_MaskImage->GetLargestPossibleRegion().IsInside(segment.first) || (!m_OnlyUseMaskGeometry && m_MaskImage->GetPixel(segment.first)==0))
          continue;

        // add direction to container
        unsigned int idx = segment.first[0] + outImageSize[0]*(segment.first[1] + outImageSize[1]*segment.first[2]);
        DirectionContainerType::Pointer dirCont;
        if (m_DirectionsContainer->IndexExists(idx))
        {
          peakLengths->ElementAt(idx).push_back(fiberWeight*segment.second);

          dirCont = m_DirectionsContainer->GetElement(idx);
          if (dirCont.IsNull())
          {
            dirCont = DirectionContainerType::New();
            dirCont->push_back(dir);
            m_DirectionsContainer->InsertElement(idx, dirCont);
          }
          else
            dirCont->push_back(dir);
        }
        else
        {
          dirCont = DirectionContainerType::New();
          dirCont->push_back(dir);
          m_DirectionsContainer->InsertElement(idx, dirCont);

          std::vector< double > lengths; lengths.push_back(fiberWeight*segment.second);
          peakLengths->InsertElement(idx, lengths);
        }
      }

    }
  }

  itk::ImageRegionIterator<ItkUcharImgType> dirIt(m_NumDirectionsImage, m_NumDirectionsImage->GetLargestPossibleRegion());

  MITK_INFO << "Clustering directions";
  float max_dir_mag = 0;
  boost::progress_display disp2(outImageSize[0]*outImageSize[1]*outImageSize[2]);
  while(!dirIt.IsAtEnd())
  {
    ++disp2;
    OutputImageType::IndexType idx3 = dirIt.GetIndex();
    int idx_lin = idx3[0]+(idx3[1]+idx3[2]*outImageSize[1])*outImageSize[0];

    itk::Index<4> idx4; idx4[0] = idx3[0]; idx4[1] = idx3[1]; idx4[2] = idx3[2];

    if (!m_DirectionsContainer->IndexExists(idx_lin))
    {
      ++dirIt;
      continue;
    }
    DirectionContainerType::Pointer dirCont = m_DirectionsContainer->GetElement(idx_lin);
    if (dirCont.IsNull() || dirCont->empty())
    {
      ++dirIt;
      continue;
    }

    DirectionContainerType::Pointer directions;
    if (m_MaxNumDirections>0)
    {
      directions = FastClustering(dirCont, peakLengths->GetElement(idx_lin));
      std::sort( directions->begin(), directions->end(), CompareVectorLengths );
    }
    else
      directions = dirCont;

    unsigned int numDir = directions->size();
    if (m_MaxNumDirections>0 && numDir>m_MaxNumDirections)
      numDir = m_MaxNumDirections;

    float voxel_max_mag = 0;
    for (unsigned int i=0; i<numDir; i++)
    {
      DirectionType dir = directions->at(i);
      float mag = dir.magnitude();

      if (mag>voxel_max_mag)
        voxel_max_mag = mag;
      if (mag>max_dir_mag)
        max_dir_mag = mag;
    }

    int count = 0;
    for (unsigned int i=0; i<numDir; i++)
    {
      DirectionType dir = directions->at(i);
      count++;

      float mag = dir.magnitude();
      if (m_NormalizationMethod==MAX_VEC_NORM && voxel_max_mag>mitk::eps)
        dir /= voxel_max_mag;
      else if (m_NormalizationMethod==SINGLE_VEC_NORM && mag>mitk::eps)
        dir.normalize();

      for (unsigned int j = 0; j<3; j++)
      {
        idx4[3] = i*3 + j;
        m_DirectionImage->SetPixel(idx4, dir[j]);
      }
    }
    dirIt.Set(count);
    ++dirIt;
  }

  if (m_NormalizationMethod==GLOBAL_MAX && max_dir_mag>0)
  {
    itk::ImageRegionIterator<ItkDirectionImageType> dirImgIt(m_DirectionImage, m_DirectionImage->GetLargestPossibleRegion());
    while(!dirImgIt.IsAtEnd())
    {
      dirImgIt.Set(dirImgIt.Get()/max_dir_mag);
      ++dirImgIt;
    }
  }
}

template< class PixelType >
TractsToVectorImageFilter< PixelType >::DirectionContainerType::Pointer TractsToVectorImageFilter< PixelType >::FastClustering(DirectionContainerType::Pointer inDirs, std::vector< double > lengths)
{
  DirectionContainerType::Pointer outDirs = DirectionContainerType::New();
  if (inDirs->size()<2)
  {
    if (inDirs->size()==1)
      inDirs->SetElement(0, inDirs->at(0)*lengths.at(0));
    return inDirs;
  }

  DirectionType oldMean, currentMean;
  std::vector< int > touched;

  // initialize
  touched.resize(inDirs->size(), 0);
  bool free = true;
  currentMean = inDirs->at(0);  // initialize first seed
  currentMean.normalize();
  double length = lengths.at(0);
  touched[0] = 1;
  std::vector< double > newLengths;
  bool meanChanged = false;

  double max = 0;
  while (free)
  {
    oldMean.fill(0.0);

    // start mean-shift clustering
    double angle = 0;

    while (fabs(dot_product(currentMean, oldMean))<0.99)
    {
      oldMean = currentMean;
      currentMean.fill(0.0);
      for (unsigned int i=0; i<inDirs->size(); i++)
      {
        angle = dot_product(oldMean, inDirs->at(i));
        if (angle>=m_AngularThreshold)
        {
          currentMean += inDirs->at(i);
          if (meanChanged)
            length += lengths.at(i);
          touched[i] = 1;
          meanChanged = true;
        }
        else if (-angle>=m_AngularThreshold)
        {
          currentMean -= inDirs->at(i);
          if (meanChanged)
            length += lengths.at(i);
          touched[i] = 1;
          meanChanged = true;
        }
      }
      if(!meanChanged)
        currentMean = oldMean;
      else
        currentMean.normalize();
    }

    // found stable mean
    outDirs->push_back(currentMean);
    newLengths.push_back(length);
    if (length>max)
      max = length;

    // find next unused seed
    free = false;
    for (unsigned int i=0; i<touched.size(); i++)
      if (touched[i]==0)
      {
        currentMean = inDirs->at(i);
        free = true;
        meanChanged = false;
        length = lengths.at(i);
        touched[i] = 1;
        break;
      }
  }

  if (inDirs->size()==outDirs->size())
  {
    if (max>0)
    {
      for (unsigned int i=0; i<outDirs->size(); i++)
        outDirs->SetElement(i, outDirs->at(i)*newLengths.at(i));
    }
    return outDirs;
  }
  else
    return FastClustering(outDirs, newLengths);
}

}


#include "factor_graph_initializer.h"
#include "srrg_solver/variables_and_factors/types_2d/all_types.h"
#include "srrg_solver/variables_and_factors/types_3d/all_types.h"
#include "srrg_solver/variables_and_factors/types_projective/all_types.h"
#include <Eigen/Eigenvalues>

namespace srrg2_solver {

  void FactorGraphInitializer::setGraph(FactorGraphInterface& graph_) {
    _graph = &graph_;
    _entries.clear();
    _initialized_variables.clear();
    _queue = FactorGraphVisitEntryQueue();
    // populate the entry structure to hold the visit
    // populate the queue with all fixed variables
    for (auto it = _graph->variables().begin(); it != _graph->variables().end(); ++it) {
      VariableBase* variable    = it.value();
      if (variable->status() == VariableBase::NonActive)
        continue;
      VariableVisitEntry* entry = _entries.add(VariableVisitEntry(variable));
      if (variable->status() == VariableBase::Fixed) {
        entry->cost = 0;
        if (! _parameter_ids.count(variable->graphId()))
          _queue.push(*entry);
        _initialized_variables.insert(variable->graphId());
      } else {
        entry->cost = -1;
      }
    }
    if (verbose)
      std::cerr << "queue.size()" << _queue.size() << std::endl;
  }

  float FactorGraphInitializer::getCost(VariableBase* v) {
    VariableVisitEntry* ne = _entries.at(v->graphId());
    if (! ne)
      throw std::runtime_error("getCost on non visited variable");
    std::set<VariableBase*> neighbors;
    for (auto f_it: _graph->factors(ne->variable)) {
      FactorBase* f=f_it.second;
      if (! f->enabled())
        continue;
      if (param_level.value()!=-1 && f->level()!=param_level.value())
        continue;
      for (int i=0; i<f->numVariables(); ++i) {
        int v_id=f->variableId(i);
        if (_parameter_ids.count(v_id))
          continue;
        VariableBase* v_other=_graph->variable(v_id);
        neighbors.insert(v_other);
      }
    }
    float min_cost=std::numeric_limits<float>::max();
    for (auto v_other: neighbors) {
      VariableVisitEntry* e_other=_entries.at(v_other->graphId());
      if (! e_other)
        continue;
      if (e_other->cost<0)
        continue;
      min_cost=std::min(e_other->cost, min_cost);
    }
    return min_cost;
  }

  void FactorGraphInitializer::updateGraph() {
    // populate the queue with all variables that are fixed
    // and all the ones that were not in the graph before
    _queue = FactorGraphVisitEntryQueue();
    for (auto it = _graph->variables().begin(); it != _graph->variables().end(); ++it) {
      VariableBase* variable    = it.value();
      if (variable->status() == VariableBase::NonActive)
        continue;
      VariableVisitEntry* entry = _entries.at(variable->graphId());
      if (! entry) {
        entry = _entries.add(VariableVisitEntry(variable));
        if (variable->status() == VariableBase::Fixed) {
          entry->cost = 0;
          _initialized_variables.insert(variable->graphId());
          if (! _parameter_ids.count(variable->graphId()))
              _queue.push(*entry);
        } else {
          entry->cost=-1;
        }
      } else {
        _queue.push(*entry);
      }

    }
    //std::cerr << "queue.size()" << _queue.size() << std::endl;
  }
  using namespace std;

  void FactorGraphInitializer::compute() {
    if (verbose)
      cerr << "init [ ";
    while (!_queue.empty()) {
      VariableVisitEntry e = _queue.top();
      VariableVisitEntry* e_other=_entries.at(e.variable->graphId());
      if (verbose ) std::cerr << "pop: " << e.variable->graphId() << " " << e.cost << std::endl;
      _queue.pop();
      if (e_other->cost<e.cost)
        continue;
      if (param_max_cost.value()>-1 && e.cost>param_max_cost.value())
        return;
      VariableBase* v = e.variable;
      if (! _graph->variable(v->graphId()))
          throw std::runtime_error("FactorGraphInitializer, bookkeping error");
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        int v_pos          = -1;
        FactorBase* f = f_it.second;
        if (! f->enabled()) {
          continue;
        }
        for (int i = 0; i < f->numVariables(); ++i) {
          if (f->variable(i) == v) {
            e.var_pos = i;
            v_pos      = i;
            break;
          }
        }

        // scan all variables
        for (int nv_pos = 0; nv_pos < f->numVariables(); ++nv_pos) {
          if (nv_pos == v_pos)
            continue;
          VariableBase* nv       = f->variable(nv_pos);
          if (_parameter_ids.count(nv->graphId()))
            continue;

          VariableVisitEntry* ne = _entries.at(nv->graphId());
          //std::cerr << "\t nv:" << nv->graphId() << std::endl;
          //std::cerr << "ne: " << ne << std::endl;
          if (! ne)
            continue;

          //assert(ne && "bookkeeping error 2");
          // if variable already initialized
          if (ne->cost >= 0)
            continue;
          if (initVariable(nv)) {
            if (verbose) {
              cerr << "(" << v->graphId() << "-" << nv->graphId() << ") " ;
            }
            _initialized_variables.insert(nv->graphId());
            //ne->cost = e.cost + 1;
            ne->cost = getCost(nv) + 1;
            _queue.push(*ne);
            //std::cerr << "push: " << ne->variable->graphId() << " " << ne->cost << std::endl;
          }
        }
      }
    }
    if (verbose)
      cerr << "]";
  }

  bool FactorGraphInitializer::initVariable(VariableBase* variable) {
    for (auto it = _rules.begin(); it != _rules.end(); ++it) {
      if ((*it)->init(variable))
        return true;
    }
    return false;
  }

  bool FactorGraphInitializer::isInit(VariableBase* v) {
    VariableVisitEntry* ne = _entries.at(v->graphId());
    if (! ne)
      return false;
    return ne->cost >= 0;
  }

  float FactorGraphInitializer::maxCost() {
    float max_cost=-1;
    for (auto v_id: _initialized_variables) {
      auto entry=_entries.at(v_id);
      if (! entry)
        continue;
      max_cost=std::max(entry->cost, max_cost);
    }
    return max_cost;
  }

  struct SE2toSE2InitializerRule : public FactorGraphInitializerRule_<VariableSE2Base> {
    SE2toSE2InitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariableSE2Base>(initializer_) {
    }

    bool doInit(VariableSE2Base* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;

      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;

        SE2PosePoseGeodesicErrorFactor* this_factor =
          dynamic_cast<SE2PosePoseGeodesicErrorFactor*>(f);
        if (!this_factor)
          continue;
        bool direct             = true;
        VariableSE2Base* root_v = this_factor->variables().at<0>();
        if (root_v == v) {
          root_v = this_factor->variables().at<1>();
          direct = false;
        }
        if (!_initializer->isInit(root_v))
          continue;

        if (direct) {
          v->setEstimate(root_v->estimate() * this_factor->measurement());
        } else {
          v->setEstimate(root_v->estimate() * this_factor->measurement().inverse());
        }
        //std::cerr << "se2 pose_pose init" << root_v->graphId() << " " << v->graphId() << std::endl;
        return true;
      }
      return false;
    }
  };

  struct SE2toPoint2InitializerRule : public FactorGraphInitializerRule_<VariablePoint2> {
    SE2toPoint2InitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariablePoint2>(initializer_) {
    }

    bool doInit(VariablePoint2* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        SE2PosePointErrorFactor* this_factor =
          dynamic_cast<SE2PosePointErrorFactor*>(f);
        if (!this_factor) {
          continue;
        }
        VariableSE2Base* root_v = this_factor->variables().at<0>();
        if (!_initializer->isInit(root_v))
          continue;
        v->setEstimate(root_v->estimate() * this_factor->measurement());
        //std::cerr << "se2 pose_point init" << root_v->graphId() << " " << v->graphId() << std::endl;
        return true;
      }
      return false;
    }
  };

  struct SE2toPoint2BearingInitializerRule : public FactorGraphInitializerRule_<VariablePoint2> {
    SE2toPoint2BearingInitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariablePoint2>(initializer_) {
    }

    bool doInit(VariablePoint2* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
      std::list<SE2PosePointBearingErrorFactor*> active_factors;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        SE2PosePointBearingErrorFactor* this_factor =
          dynamic_cast<SE2PosePointBearingErrorFactor*>(f);
        if (!this_factor) {
          continue;
        }
        VariableSE2Base* root_v = this_factor->variables().at<0>();
        if (!_initializer->isInit(root_v))
          continue;
        active_factors.push_back(this_factor);
      }
      if (active_factors.size() < 2)
        return false;
      // do a triangulation with the factors
      Matrix2f H = Matrix2f::Zero();
      Vector2f b = Vector2f::Zero();
      Vector2f x = v->estimate();
      for (auto it = active_factors.begin(); it != active_factors.end(); ++it) {
        SE2PosePointBearingErrorFactor* f = *it;
        VariableSE2Base* pose                    = f->variables().at<0>();
        float theta                              = f->measurement()(0);
        Vector2f n        = pose->estimate().linear() * Vector2f(cos(theta), sin(theta));
        n                 = Vector2f(n.y(), -n.x());
        const Vector2f& p = pose->estimate().translation();
        float e           = n.dot(x - p);
        H += n * n.transpose();
        b += n * e;
      }
      x -= (H).ldlt().solve(b);
      v->setEstimate(x);
      // todo: check for consistency of solution, otherwise disable variable
      //std::cerr << "se2 pose point bearinf :" << x.transpose() << std::endl;
      return true;
    }
  };

  struct SE3toSE3InitializerRule : public FactorGraphInitializerRule_<VariableSE3Base> {
    SE3toSE3InitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariableSE3Base>(initializer_) {
    }

    bool doInit(VariableSE3Base* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        SE3PosePoseGeodesicErrorFactor* this_factor =
          dynamic_cast<SE3PosePoseGeodesicErrorFactor*>(f);
        if (!this_factor)
          continue;
        bool direct             = true;
        VariableSE3Base* root_v = this_factor->variables().at<0>();
        if (root_v == v) {
          root_v = this_factor->variables().at<1>();
          direct = false;
        }
        if (!_initializer->isInit(root_v))
          continue;

        if (direct) {
          v->setEstimate(root_v->estimate() * this_factor->measurement());
        } else {
          v->setEstimate(root_v->estimate() * this_factor->measurement().inverse());
        }
        //std::cerr << "se3 pose_pose init" << root_v->graphId() << " " << v->graphId() << std::endl;
        return true;
      }
      return false;
    }
  };

  struct Sim3toSim3InitializerRule : public FactorGraphInitializerRule_<VariableSim3Base> {
    Sim3toSim3InitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariableSim3Base>(initializer_) {
    }

    bool doInit(VariableSim3Base* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        Sim3PosePoseErrorFactorAD* this_factor =
          dynamic_cast<Sim3PosePoseErrorFactorAD*>(f);
        if (!this_factor)
          continue;
        bool direct             = true;
        VariableSim3Base* root_v = this_factor->variables().at<0>();
        if (root_v == v) {
          root_v = this_factor->variables().at<1>();
          direct = false;
        }
        if (!_initializer->isInit(root_v))
          continue;

        if (direct) {
          v->setEstimate(root_v->estimate() * this_factor->measurement());
        } else {
          v->setEstimate(root_v->estimate() * this_factor->measurement().inverse());
        }
        //std::cerr << "se3 pose_pose init" << root_v->graphId() << " " << v->graphId() << std::endl;
        return true;
      }
      return false;
    }
  };

  struct SE3toPoint3InitializerRule : public FactorGraphInitializerRule_<VariablePoint3> {
    SE3toPoint3InitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariablePoint3>(initializer_) {
    }

    bool doInit(VariablePoint3* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
        for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;

        if (level!=-1 && f->level()!=level)
          continue;
        SE3PosePointErrorFactor* this_factor =
          dynamic_cast<SE3PosePointErrorFactor*>(f);
        if (!this_factor)
          continue;
        VariableSE3Base* root_v = this_factor->variables().at<0>();
        if (!_initializer->isInit(root_v))
          continue;
        v->setEstimate(root_v->estimate() * this_factor->measurement());
        //std::cerr << "se3 pose_point init" << root_v->graphId() << " " << v->graphId() << std::endl;
        return true;
      }
      return false;
    }
  };

  struct SE3toPoint3OffsetInitializerRule : public FactorGraphInitializerRule_<VariablePoint3> {
    SE3toPoint3OffsetInitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariablePoint3>(initializer_) {
    }

    bool doInit(VariablePoint3* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        SE3PosePointOffsetErrorFactor* this_factor =
          dynamic_cast<SE3PosePointOffsetErrorFactor*>(f);
        if (!this_factor)
          continue;
        VariableSE3Base* offset_v = this_factor->variables().at<2>();
        if (!_initializer->isInit(offset_v))
          continue;
        VariableSE3Base* root_v = this_factor->variables().at<0>();
        if (!_initializer->isInit(root_v))
          continue;

        v->setEstimate(root_v->estimate() * offset_v->estimate() * this_factor->measurement());
        //std::cerr << "se3 pose_point_offset init" << root_v->graphId() << " " << v->graphId() << std::endl;
        return true;
      }
      return false;
    }
  };

  struct SE3toPoint3OmniBAInitializerRule : public FactorGraphInitializerRule_<VariablePoint3> {
    SE3toPoint3OmniBAInitializerRule(FactorGraphInitializer* initializer_) :
      FactorGraphInitializerRule_<VariablePoint3>(initializer_) {
    }

    bool doInit(VariablePoint3* v) override {
      std::list<SE3PosePointOmniBAErrorFactor*> active_factors;
      FactorGraphInterface* _graph = _initializer->_graph;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        SE3PosePointOmniBAErrorFactor* this_factor =
          dynamic_cast<SE3PosePointOmniBAErrorFactor*>(f);
        if (!this_factor) {
          continue;
        }
        VariableSE3Base* root_v = this_factor->variables().at<0>();
        if (!_initializer->isInit(root_v))
          continue;
        active_factors.push_back(this_factor);
      }
      if (active_factors.size() < 3)
        return false;
      // do a triangulation with the factors
      Matrix3f H = Matrix3f::Zero();
      Vector3f b = Vector3f::Zero();
      Vector3f x = v->estimate();
      for (auto it = active_factors.begin(); it != active_factors.end(); ++it) {
        SE3PosePointOmniBAErrorFactor* f = *it;
        VariableSE3Base* pose                    = f->variables().at<0>();
        VariableSE3Base* offset                  = f->variables().at<2>();
        Vector3f d        = pose->estimate().linear()*offset->estimate().linear()*f->measurement();
        Matrix3f J=geometry3d::skew(d);
        const Vector3f& p = pose->estimate()*offset->estimate().translation();
        Vector3f e        = J*(x - p);
        H += J.transpose() * J;
        b += J.transpose() * e;
      }
      x -= (H).ldlt().solve(b);
      v->setEstimate(x);
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f>  es;
      es.compute(H);
      float lambda_min=es.eigenvalues()(0);
      float lambda_max=es.eigenvalues()(2);
      if (lambda_min/lambda_max<1e-1)
        return false;
      // todo: check for consistency of solution, otherwise disable variable
      //std::cerr << "se3 pose point omni ba:" << x.transpose() << std::endl;
      return true;
    }
  };

    struct Point3toSE3InitializerRule : public FactorGraphInitializerRule_<VariableSE3Base> {

      Point3toSE3InitializerRule(FactorGraphInitializer* initializer_)
        : FactorGraphInitializerRule_<VariableSE3Base>(initializer_) {
      }

    bool doInit(VariableSE3Base* v) override {
      FactorGraphInterface* _graph = _initializer->_graph;
      std::list<SE3PosePointErrorFactor*> active_factors;
      for (auto f_it: _graph->FactorGraphInterface::factors(v)) {
        FactorBase* f = f_it.second;
        if (level!=-1 && f->level()!=level)
          continue;
        SE3PosePointErrorFactor* this_factor =
          dynamic_cast<SE3PosePointErrorFactor*>(f);
        if (!this_factor) {
          continue;
        }
        VariablePoint3* root_v = this_factor->variables().at<1>();
        if (!_initializer->isInit(root_v))
          continue;
        active_factors.push_back(this_factor);
      }
      if (active_factors.size() < 4)
        return false;
      const int iterations=10;
      //tiny ICP;
      Matrix6f H;
      Vector6f b;
      Vector6f dx;
      Isometry3f X=v->estimate().inverse();
      float last_chi =0;
      //cerr << "init : [";
      for (int i=0; i<iterations; ++i) {
        last_chi=0;
        H.setZero();
        b.setZero();
        dx.setZero();
        Matrix3_6f J;
        J.setZero();
        J.block<3,3>(0,0).setIdentity();
        // do a triangulation with the factors
        for (auto it = active_factors.begin(); it != active_factors.end(); ++it) {
          auto f = *it;
          auto p = f->variables().at<1>()->estimate();
          auto z = f->measurement();
          auto z_hat=X*p;
          J.block<3,3>(0,3)=-geometry3d::skew(z_hat);
          auto e = z_hat-z;
          H += J.transpose() * f->informationMatrix() * J;
          b += J.transpose() * f->informationMatrix() * e;
          last_chi+=e.transpose()*f->informationMatrix()*e;
        }
        //cerr << last_chi/active_factors.size() << " ";
        dx -= (H).ldlt().solve(b);
        X=geometry3d::ta2t(dx)*X;
      }
      //cerr << "] " << endl;

      Eigen::SelfAdjointEigenSolver<Matrix6f>  es;
      es.compute(H);
      float lambda_min=es.eigenvalues()(0);
      //float lambda_max=es.eigenvalues()(5);
      if (lambda_min<1e-1) { //gg: sucks big times. more times. can't stop sucking
        return false;
      }
      v->setEstimate(X.inverse());
      // todo: check for consistency of solution, otherwise disable variable
      //std::cerr << "se3 pose point omni ba:" << x.transpose() << std::endl;
      //cerr << "init!";
      return true;
    }
  };

  FactorGraphInitializer::FactorGraphInitializer() {
    _rules.push_back(FactorGraphInitializerRulePtr(new SE2toSE2InitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new SE3toSE3InitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new Sim3toSim3InitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new SE2toPoint2InitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new SE3toPoint3InitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new SE3toPoint3OffsetInitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new SE2toPoint2BearingInitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new SE3toPoint3OmniBAInitializerRule(this)));
    _rules.push_back(FactorGraphInitializerRulePtr(new Point3toSE3InitializerRule(this)));
 }

} // namespace srrg2_solver


#include <boost/asio.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <HElib/FHE.h>
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <iostream>
#include <fstream>

void run_client() {
    FHEcontext context(1024, 1031, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);
    std::ofstream of("pk.pk", std::ios::binary);
    FHEPubKey pk(sk);
    // pk.makeSymmetric();
    of << pk;
    of.close();

    Ctxt ctx(sk);
    sk.Encrypt(ctx, NTL::to_ZZX(3));

    using boost::asio::ip::tcp;
    tcp::iostream connect("127.0.0.1", "12345");
    if (!connect) {
        std::cerr << "Can not connect:" <<
             connect.error().message() << std::endl;
        return;
    }
    writeContextBase(connect, context);
    connect << context;
    connect << ctx;
    connect.flush();
    Ctxt ctx_2(sk);
    connect >> ctx_2;
    NTL::ZZX dec_2;
    sk.Decrypt(dec_2, ctx_2);
    NTL::ZZX dec;
    sk.Decrypt(dec, ctx);
    if (dec[0] != dec_2[0])
        std::cout << "IO ERRR" << std::endl;
    connect.close();
}

void run_server() {
    using boost::asio::ip::tcp;
    boost::asio::io_service ios;
    tcp::endpoint endpoint(tcp::v4(), 12345);
    tcp::acceptor acceptor(ios, endpoint);

    for (;;) {
        tcp::iostream client;
        boost::system::error_code err;
        acceptor.accept(*client.rdbuf(), err);
        if (!err) {
            std::cout << "connected" << std::endl;
            unsigned long m, p, r;
            std::vector<long> gens, ords;
            readContextBase(client, m, p, r, gens, ords);
            FHEcontext context(m, p, r, gens, ords);
            client >> context;
            std::ifstream ifs("pk.pk", std::ios::binary);
            FHEPubKey pk(context);
            ifs >> pk;
            ifs.close();

            Ctxt ctx(pk);
            client >> ctx;
            //ctx.multiplyBy(ctx);
            //ctx.modDownToLevel(1);
            client << ctx;
            client.close();
            break;
        } else {
            std::cerr << "Error " << err << std::endl;
        }
    }
}

int main(int argc, char *argv[]) {
    if (argc > 1) {
        run_client();
    } else {
        run_server();
    }
}


//   Copyright 2019 <Huawei Technologies Co., Ltd>
//
//   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.

#ifndef SWAPPING_HPP
#define SWAPPING_HPP

#include <glog/logging.h>

#include <boost/format.hpp>
#include <cstdint>
#include <vector>

#include "funcs.hpp"
#include "simulator-mpi/SwapArrays.hpp"

namespace swapping
{
template <size_t n_bits>
struct SwappingBase
{
     template <class T, class Iterator>
     static size_t doCalc(const Iterator state_vector, uint64_t n,
                          size_t start_free_idx, T* svalues, uint64_t* indices,
                          const std::vector<uint64_t>& swap_bits)
     {
          size_t free_idx_end = start_free_idx + n;
          for (size_t free_idx = start_free_idx; free_idx < free_idx_end;
               ++free_idx) {
               size_t res_free_idx = free_idx;
               for (size_t bit_idx = 0; bit_idx < n_bits; ++bit_idx) {
                    uint64_t sw_bit = swap_bits[bit_idx];
                    uint64_t l = res_free_idx & (sw_bit - 1);
                    uint64_t r = res_free_idx - l;

                    res_free_idx = 2 * r + l;
               }

               for (size_t swap_idx = 0; swap_idx < (1ul << n_bits);
                    ++swap_idx) {
                    size_t res_idx = res_free_idx;
                    size_t res_swap_idx = 0;
                    for (size_t bi = 0; bi < n_bits; ++bi) {
                         bool bv = swap_idx & (1ul << bi);
                         res_swap_idx
                             |= bv * (swap_bits[swap_bits[n_bits + bi]]);
                    }

                    res_idx += res_swap_idx;
                    size_t idx = n * swap_idx + free_idx - start_free_idx;
                    indices[idx] = res_idx;
                    svalues[idx] = state_vector[res_idx];
               }
          }

          return free_idx_end;
     }
};

template <size_t n_bits_max>
struct Swapping
{
     template <class T, class Iterator>
     static size_t calcSwap(uint64_t n_bits, const Iterator state_vector,
                            uint64_t n, size_t start_free_idx, T* svalues,
                            uint64_t* indices,
                            const std::vector<uint64_t>& swap_bits)
     {
          if (n_bits == n_bits_max) {
               return SwappingBase<n_bits_max>::doCalc(state_vector, n,
                                                       start_free_idx, svalues,
                                                       indices, swap_bits);
          }
          else {
               return Swapping<n_bits_max - 1>::calcSwap(
                   n_bits, state_vector, n, start_free_idx, svalues, indices,
                   swap_bits);
          }
     }
};

template <>
struct Swapping<0>
{
     template <class T, class Iterator>
     static size_t calcSwap(uint64_t, const Iterator, uint64_t, size_t, T*,
                            uint64_t*, const std::vector<uint64_t>&)
     {
          return 0;
     }
};

}  // namespace swapping

#endif  // SWAPPING_HPP


#include <config.h>

#include "error_calc.hh"

#include <assert.h>
#include <boost/filesystem/fstream.hpp>
#include <iostream>
#include <map>
#include <unordered_map>
#include <memory>
#include <string>
#include <utility>

#include <dune/gdt/assembler/system.hh>
#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/products/l2.hh>
#include <dune/gdt/products/h1.hh>
#include <dune/gdt/operators/prolongations.hh>

#include <dune/multiscale/common/traits.hh>
#include <dune/multiscale/common/grid_creation.hh>
#include <dune/multiscale/problems/base.hh>
#include <dune/multiscale/msfem/localsolution_proxy.hh>
#include <dune/multiscale/msfem/proxygridview.hh>
#include <dune/multiscale/msfem/fem_solver.hh>
#include <dune/multiscale/problems/selector.hh>
#include <dune/multiscale/tools/misc/outputparameter.hh>
#include <dune/xt/common/parallel/partitioner.hh>
#include <dune/grid/utility/partitioning/seedlist.hh>
#include <dune/stuff/grid/information.hh>
#include <dune/xt/common/filesystem.hh>
#include <dune/xt/common/configuration.hh>
#include <dune/multiscale/common/heterogenous.hh>
#include <dune/multiscale/msfem/fem_solver.hh>

using namespace Dune::Multiscale;
namespace DGP = Dune::GDT::Products;
typedef DSFu::Difference<Problem::ExactSolutionType, CommonTraits::ConstDiscreteFunctionType> DifferenceType;
typedef DSFu::Difference<CommonTraits::ConstDiscreteFunctionType, CommonTraits::ConstDiscreteFunctionType>
    DiscreteDifferenceType;
typedef DGP::L2Localizable<CommonTraits::InteriorGridViewType, DifferenceType> L2ErrorAnalytical;
typedef DGP::L2Localizable<CommonTraits::InteriorGridViewType, DiscreteDifferenceType> L2ErrorDiscrete;
typedef DGP::H1SemiLocalizable<CommonTraits::InteriorGridViewType, DifferenceType> H1sErrorAnalytical;
typedef DGP::H1SemiLocalizable<CommonTraits::InteriorGridViewType, DiscreteDifferenceType> H1sErrorDiscrete;
typedef DGP::L2Localizable<CommonTraits::InteriorGridViewType, CommonTraits::ConstDiscreteFunctionType> DiscreteL2;

double surface_flow_gdt(const CommonTraits::GridType& grid,
                        const CommonTraits::ConstDiscreteFunctionType& solution,
                        const DMP::ProblemContainer& problem);

void solution_output(const DMP::ProblemContainer& problem,
                     const CommonTraits::ConstDiscreteFunctionType& solution,
                     std::string name = "msfem_solution_")
{
  using namespace Dune;

  Dune::Multiscale::OutputParameters outputparam(problem.config().get("global.datadir", "data"));
  outputparam.set_prefix(name);
  solution.visualize(outputparam.fullpath(solution.name()));
}
template <typename L, typename R>
void solution_output(const DMP::ProblemContainer& problem,
                     const DSFu::Difference<L, R>& solution,
                     const CommonTraits::GridViewType& view,
                     std::string name)
{
  using namespace Dune;

  Dune::Multiscale::OutputParameters outputparam(problem.config().get("global.datadir", "data"));
  outputparam.set_prefix(name);
  solution.visualize(view, outputparam.fullpath(solution.name()));
}
void data_output(const DMP::ProblemContainer& problem, const CommonTraits::GridViewType& gridPart)
{
  using namespace Dune;
  Dune::Multiscale::OutputParameters outputparam(problem.config().get("global.datadir", "data"));

  if (problem.getModelData().hasExactSolution()) {
    const auto& u = problem.getExactSolution();
    outputparam.set_prefix("exact_solution");
    u.visualize(gridPart, outputparam.fullpath(u.name()));
  }
}

Dune::Multiscale::ErrorCalculator::ErrorCalculator(const DMP::ProblemContainer& problem,
                                                   const std::unique_ptr<LocalsolutionProxy>& msfem_solution,
                                                   CommonTraits::ConstDiscreteFunctionType* fem_solution)
  : problem_(problem)
  , msfem_solution_(msfem_solution)
  , fem_solution_(fem_solution)
  , timing_("error.fem+msfem")
{
  assert(fem_solution_);
}

ErrorCalculator::ErrorCalculator(const DMP::ProblemContainer& problem,
                                 const std::unique_ptr<LocalsolutionProxy>& msfem_solution)
  : problem_(problem)
  , msfem_solution_(msfem_solution)
  , fem_solution_(nullptr)
  , timing_("error.msfem")
{
  assert(msfem_solution_);
  if (problem.config().get("msfem.fem_comparison", false)) {
    fem_solver_ = Dune::XT::Common::make_unique<Elliptic_FEM_Solver>(problem);
    try {
      fem_solution_ = &fem_solver_->solve();
    } catch (Dune::Exception& e) {
      MS_LOG_ERROR << "fine CGFEM solution failed: " << e.what();
      fem_solution_ = nullptr;
    }
  }
}

void match_check(const CommonTraits::GridType& coarse_grid, const CommonTraits::GridType& fine_grid)
{
  const auto fine_view = fine_grid.leafGridView<CommonTraits::InteriorBorderPartition>();
  const auto coarse_view = coarse_grid.leafGridView<CommonTraits::InteriorBorderPartition>();
  const auto coarse_dimensions = DSG::dimensions(coarse_view);
  const auto fine_dimensions = DSG::dimensions(fine_view);
  for (const auto i : Dune::XT::Common::value_range(CommonTraits::world_dim)) {
    const bool match =
        Dune::XT::Common::FloatCmp::eq(coarse_dimensions.coord_limits[i].min(), fine_dimensions.coord_limits[i].min())
        && Dune::XT::Common::FloatCmp::eq(coarse_dimensions.coord_limits[i].max(),
                                          fine_dimensions.coord_limits[i].max());
    if (!match) {
      DUNE_THROW(Dune::InvalidStateException,
                 "Coarse and fine mesh do not match after load balancing, do \
                 you use different refinements in different spatial dimensions?\n"
                     << coarse_dimensions.coord_limits[i]
                     << " | "
                     << fine_dimensions.coord_limits[i]);
    }
  }
}

std::map<std::string, double> Dune::Multiscale::ErrorCalculator::print(std::ostream& out)
{
  using namespace std;
  using namespace DSC;
  out << std::endl << "The L2 errors:" << std::endl << std::endl;

  const size_t over_integrate = problem_.config().get("global.error.over_integrate", 0u);

  auto grids = make_grids(problem_);
  const auto coarse_grid = grids.first;
  const auto fine_grid = grids.second;
  if (!fem_solution_)
    assert(fine_grid);
  const auto fine_space =
      fem_solution_ ? fem_solution_->space() : CommonTraits::SpaceChooserType::make_space(*fine_grid);
  const auto fine_interior_view =
      fine_space.grid_view().grid().template leafGridView<CommonTraits::InteriorBorderPartition>();
  Dune::XT::Common::IndexSetPartitioner<CommonTraits::InteriorGridViewType> ip(fine_interior_view.indexSet());
  SeedListPartitioning<typename CommonTraits::InteriorGridViewType::Grid, 0> partitioning(fine_interior_view, ip);
  GDT::SystemAssembler<CommonTraits::SpaceType, CommonTraits::InteriorGridViewType> system_assembler(
      fine_space, fine_interior_view);
  const auto& grid_view = fine_space.grid_view();

  CommonTraits::DiscreteFunctionType projected_coarse_fem_solution(fine_space);
  Elliptic_FEM_Solver coarse_fem_solver(problem_, coarse_grid);
  try {
    auto& coarse_fem_solution = coarse_fem_solver.solve();
    const Dune::GDT::Operators::LagrangeProlongation<CommonTraits::GridViewType> prolongation_operator(
        fine_space.grid_view());
    prolongation_operator.apply(coarse_fem_solution, projected_coarse_fem_solution);
    if (problem_.config().get("global.vtk_output", false))
      solution_output(problem_, coarse_fem_solution, "coarse-cg-fem_solution_");
  } catch (Dune::Exception& e) {
    MS_LOG_ERROR << "coarse CGFEM solution failed: " << e.what();
    fem_solution_ = nullptr;
  }

  CommonTraits::DiscreteFunctionType fine_msfem_solution(fine_space, "MsFEM_Solution");
  if (msfem_solution_) {
    match_check(*coarse_grid, *fine_grid);
    MsFEMProjection::project(*msfem_solution_, fine_msfem_solution);
    if (problem_.config().get("global.vtk_output", false)) {
      MS_LOG_INFO_0 << "Solution output for MsFEM Solution." << std::endl;
      data_output(problem_, fine_space.grid_view());
      solution_output(problem_, fine_msfem_solution);
    }
    const auto space = CommonTraits::SpaceChooserType::make_space(*coarse_grid);
    CommonTraits::DiscreteFunctionType coarse_fun(space, "MsFEM_Solution_coarse");
    const auto flow = surface_flow_gdt(*coarse_grid, coarse_fun, problem_);
    MS_LOG_ERROR_0 << "FLOW " << flow << std::endl;
  }

  const string msfem_exact = "msfem_exact", fem_exact = "fem_exact", coarse_fem_exact = "coarse_fem_exact",
               msfem_fem = "msfem_fem", msfem_coarse_fem = "msfem_coarse_fem";
  unordered_map<string, DifferenceType> differences;
  unordered_map<string, DiscreteDifferenceType> discrete_differences;
  unordered_map<string, L2ErrorAnalytical> l2_analytical_errors;
  unordered_map<string, H1sErrorAnalytical> h1s_analytical_errors;
  unordered_map<string, L2ErrorDiscrete> l2_discrete_errors;
  unordered_map<string, H1sErrorDiscrete> h1s_discrete_errors;
  std::unique_ptr<DiscreteL2> l2_msfem;
  constexpr auto pcw = std::piecewise_construct_t();

  //! ----------------- compute L2- and H1- errors -------------------
  if (problem_.getModelData().hasExactSolution()) {
    const auto& u = problem_.getExactSolution();

    if (msfem_solution_) {
      const auto name = forward_as_tuple(msfem_exact);
      const auto& difference =
          map_emplace(differences, pcw, name, forward_as_tuple(u, fine_msfem_solution)).first->second;
      const auto product_args = forward_as_tuple(fine_interior_view, difference, over_integrate);
      system_assembler.add(map_emplace(l2_analytical_errors, pcw, name, product_args).first->second);
      system_assembler.add(map_emplace(h1s_analytical_errors, pcw, name, product_args).first->second);
    }

    if (fem_solution_) {
      const auto name = forward_as_tuple(fem_exact);
      const auto& difference = map_emplace(differences, pcw, name, forward_as_tuple(u, *fem_solution_)).first->second;
      const auto product_args = forward_as_tuple(fine_interior_view, difference, over_integrate);
      system_assembler.add(map_emplace(l2_analytical_errors, pcw, name, product_args).first->second);
      system_assembler.add(map_emplace(h1s_analytical_errors, pcw, name, product_args).first->second);
    }
    const auto name = forward_as_tuple(coarse_fem_exact);
    const auto& difference =
        map_emplace(differences, pcw, name, forward_as_tuple(u, projected_coarse_fem_solution)).first->second;
    const auto product_args = forward_as_tuple(fine_interior_view, difference, over_integrate);
    system_assembler.add(map_emplace(l2_analytical_errors, pcw, name, product_args).first->second);
    system_assembler.add(map_emplace(h1s_analytical_errors, pcw, name, product_args).first->second);
  }

  if (msfem_solution_) {
    l2_msfem = Dune::XT::Common::make_unique<DiscreteL2>(fine_interior_view, fine_msfem_solution, over_integrate);
    system_assembler.add(*l2_msfem);
    {
      const auto name = forward_as_tuple(msfem_coarse_fem);
      const auto& difference =
          map_emplace(
              discrete_differences, pcw, name, forward_as_tuple(fine_msfem_solution, projected_coarse_fem_solution))
              .first->second;
      const auto product_args = forward_as_tuple(fine_interior_view, difference, over_integrate);
      system_assembler.add(map_emplace(l2_discrete_errors, pcw, name, product_args).first->second);
      system_assembler.add(map_emplace(h1s_discrete_errors, pcw, name, product_args).first->second);
    }
    if (fem_solution_) {
      const auto name = forward_as_tuple(msfem_fem);
      const auto& difference =
          map_emplace(discrete_differences, pcw, name, forward_as_tuple(fine_msfem_solution, *fem_solution_))
              .first->second;
      const auto product_args = forward_as_tuple(fine_interior_view, difference, over_integrate);
      system_assembler.add(map_emplace(l2_discrete_errors, pcw, name, product_args).first->second);
      system_assembler.add(map_emplace(h1s_discrete_errors, pcw, name, product_args).first->second);
    }
  }

  system_assembler.assemble(partitioning);

  std::map<std::string, double> csv;
  if (problem_.getModelData().hasExactSolution()) {
    if (msfem_solution_) {
      const auto msfem_error = std::sqrt(l2_analytical_errors.at(msfem_exact).apply2());
      out << "|| u_msfem - u_exact ||_L2 =  " << msfem_error << std::endl;
      const auto h1_msfem_error = std::sqrt(h1s_analytical_errors.at(msfem_exact).apply2());
      out << "|| u_msfem - u_exact ||_H1s =  " << h1_msfem_error << std::endl << std::endl;

      csv[msfem_exact + "_L2"] = msfem_error;
      csv[msfem_exact + "_H1s"] = h1_msfem_error;
    }

    if (fem_solution_) {
      const auto fem_error = std::sqrt(l2_analytical_errors.at(fem_exact).apply2());
      out << "|| u_fem_h - u_exact ||_L2 =  " << fem_error << std::endl;
      const auto h1_fem_error = std::sqrt(h1s_analytical_errors.at(fem_exact).apply2());
      out << "|| u_fem_h - u_exact ||_H1s =  " << h1_fem_error << std::endl << std::endl;

      csv[fem_exact + "_L2"] = fem_error;
      csv[fem_exact + "_H1s"] = h1_fem_error;
    }
    const auto fem_error = std::sqrt(l2_analytical_errors.at(coarse_fem_exact).apply2());
    out << "|| u_fem_H - u_exact ||_L2 =  " << fem_error << std::endl;
    const auto h1_fem_error = std::sqrt(h1s_analytical_errors.at(coarse_fem_exact).apply2());
    out << "|| u_fem_H - u_exact ||_H1s =  " << h1_fem_error << std::endl << std::endl;

    csv[coarse_fem_exact + "_L2"] = fem_error;
    csv[coarse_fem_exact + "_H1s"] = h1_fem_error;
  }

  if (msfem_solution_) {
    const auto norm = std::sqrt(l2_msfem->apply2());
    out << "|| u_msfem ||_L2 =  " << norm << std::endl;
    csv["msfem_L2"] = norm;
    const auto fem_error = std::sqrt(l2_discrete_errors.at(msfem_coarse_fem).apply2());
    out << "|| u_fem_H - u_msfem ||_L2 =  " << fem_error << std::endl;
    out << "|| u_fem_H - u_msfem ||_L2 / || u_msfem ||_L2 =  " << fem_error / norm << std::endl;
    const auto h1_fem_error = std::sqrt(h1s_discrete_errors.at(msfem_coarse_fem).apply2());
    out << "|| u_fem_H - u_msfem ||_H1s =  " << h1_fem_error << std::endl << std::endl;

    csv[msfem_coarse_fem + "_L2"] = fem_error;
    csv[msfem_coarse_fem + "_H1s"] = h1_fem_error;

    if (fem_solution_) {
      const auto approx_msfem_error = std::sqrt(l2_discrete_errors.at(msfem_fem).apply2());
      if (std::abs(norm) > 1e-12)
        out << "|| u_msfem - u_fem ||_L2 / || u_msfem ||_L2 =  " << approx_msfem_error / norm << std::endl;
      else
        out << "|| u_msfem - u_fem ||_L2 =  " << approx_msfem_error << std::endl;

      const auto h1_approx_msfem_error = std::sqrt(h1s_discrete_errors.at(msfem_fem).apply2());
      out << "|| u_msfem - u_fem ||_H1s =  " << h1_approx_msfem_error << std::endl << std::endl;

      csv[msfem_fem + "_L2"] = approx_msfem_error;
      csv[msfem_fem + "_H1s"] = h1_approx_msfem_error;
    }
  }

  if (problem_.config().get("global.vtk_output", false)) {
    MS_LOG_INFO_0 << "Differences output for MsFEM Solution." << std::endl;
    for (const auto& mpair : differences)
      solution_output(problem_, mpair.second, grid_view, mpair.first);
    for (const auto& mpair : discrete_differences)
      solution_output(problem_, mpair.second, grid_view, mpair.first);
    if (fem_solution_)
      solution_output(problem_, *fem_solution_, "fine-cg-fem_solution_");
  }

  std::unique_ptr<boost::filesystem::ofstream> csvfile(Dune::XT::Common::make_ofstream(
      std::string(problem_.config().get("global.datadir", "data/")) + std::string("/errors.csv")));
  const std::string sep(",");
  for (const auto& key_val : csv) {
    *csvfile << key_val.first << sep;
  }
  *csvfile << std::endl;
  for (const auto& key_val : csv) {
    *csvfile << key_val.second << sep;
  }
  *csvfile << std::endl;
  return csv;
}

double surface_flow_gdt(const Dune::Multiscale::CommonTraits::GridType& grid,
                        const Dune::Multiscale::CommonTraits::ConstDiscreteFunctionType& solution,
                        const DMP::ProblemContainer& problem)
{
  using namespace Dune::Multiscale;
  const auto gv = grid.leafGridView();

  // Constants and types
  constexpr auto dim = CommonTraits::world_dim;
  typedef double REAL; // TODO read from input
  typedef typename Dune::FieldVector<REAL, dim> FV; // point on cell
  typedef typename Dune::FieldMatrix<REAL, dim, dim> FM; // point on cell
  typedef typename Dune::FieldMatrix<REAL, 1, dim> Grad; // point on cell
  typedef typename Dune::QuadratureRule<REAL, dim - 1> QR;
  typedef typename Dune::QuadratureRules<REAL, dim - 1> QRS;

  const auto& diffusion = problem.getDiffusion();

  // Quadrature rule
  auto iCell = gv.template begin<0, Dune::Interior_Partition>();
  auto iFace = gv.ibegin(*iCell);
  const QR& rule = QRS::rule(iFace->geometry().type(), 2); // TODO order as para

  // Loop over cells
  REAL localFlux(0);
  for (iCell = gv.template begin<0, Dune::Interior_Partition>();
       iCell != gv.template end<0, Dune::Interior_Partition>();
       ++iCell) {
    // Loop over interfaces
    const auto local_solution = solution.local_function(*iCell);
    for (iFace = gv.ibegin(*iCell); iFace != gv.iend(*iCell); ++iFace) {
      if (iFace->boundary() && iFace->geometry().center()[0] == 0) {
        double area = iFace->geometry().volume();
        // Loop over gauss points
        for (auto iGauss = rule.begin(); iGauss != rule.end(); ++iGauss) {
          FV pos = iFace->geometry().global(iGauss->position());
          Grad grad;
          FM diff;
          diffusion.evaluate(pos, diff);
          local_solution->jacobian(pos, grad);
          localFlux -= iGauss->weight() * area * diff[0][0] * grad[0][0];
        }
      }
    }
  }
  localFlux = grid.comm().sum(localFlux);
  return localFlux;
}


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * 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 Politecnico di Milano 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 OR CONTRIBUTORS 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.
 */

#ifndef AMA_TENSOR_IEXP_IEXP_COPY_HPP
#define AMA_TENSOR_IEXP_IEXP_COPY_HPP 1

#include <ama/tensor/detail/copy.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/inserter.hpp>
#include <boost/mpl/map/map0.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/transform.hpp>

namespace ama
{
  namespace tensor_
  {

    namespace mpl = ::boost::mpl;

    /* from two list create a map*/
    template <
          typename I1 /* index name */
        , typename I2 /* index value */
        >
    struct make_imap:
        mpl::transform<
              I1
            , I2
            , mpl::pair<mpl::_1, mpl::_2>
            , mpl::inserter<
                    mpl::map0<>
                  , mpl::insert<mpl::_1, mpl::_2>
                  >
            > { };

    /* this struct implement the copy */
     template <
           typename D               /* the dimensione of tensor */
         , typename O               /* the order of tensor */
         , typename I = first<O>    /* the first multi-index */
         , typename R = mpl::false_ /* this type is true for the last multi-index */
         >
     struct iexp_copy
     {
       template <typename SRC, typename DST>
       static
       void apply(SRC const & src, DST & dst)
       {
         typedef typename make_imap<typename DST::index_list, I>::type imap;
         dst.template at<imap>() = src.template at<imap>();

         /* increment the multi-index */
         typedef typename increment<D,I>::type increment_type;

         /* the first is the multi-index incremented */
         typedef typename mpl::first<increment_type>::type i;
         /* the second is a boolean flag toidentify the last multi-index */
         typedef typename mpl::second<increment_type>::type r;

         /* iterative call */
         iexp_copy<D,O,i,r>::apply(src,dst);
       }
     };

     /* partial specialization, end the iterative call */
     template <typename D, typename O, typename I>
     struct iexp_copy<D,O,I,mpl::true_>
     {
       template <typename SRC, typename DST>
       static void apply(SRC const &, DST &) { }
     };

  }
}

#endif /* AMA_TENSOR_IEXP_IEXP_COPY_HPP */


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/to-string.hpp"
#include "crocoddyl/core/utils/math.hpp"

namespace crocoddyl {

/**
 * @brief Abstract class for differential action model
 *
 * A differential action model is the time-continuous version of an action model, i.e.
 * \f[
 * \begin{aligned}
 * &\dot{\mathbf{v}} = \mathbf{f}(\mathbf{q}, \mathbf{v}, \mathbf{u}), &\textrm{(dynamics)}\\
 * &l(\mathbf{x},\mathbf{u}) = \int_0^{\delta t} a(\mathbf{r}(\mathbf{x},\mathbf{u}))\,dt, &\textrm{(cost)}
 * \end{aligned}
 * \f]
 * where the configuration \f$\mathbf{q}\in\mathcal{Q}\f$ lies in the configuration manifold described with a
 * `nq`-tuple, the velocity \f$\mathbf{v}\in T_{\mathbf{q}}\mathcal{Q}\f$ its a tangent vector to this manifold with
 * `nv` dimension, \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ is the input commands, \f$\mathbf{r}\f$ and \f$a\f$ is a residual
 * and activation functions (see `ActivationModelAbstractTpl`). Both configuration and velocity describe the system
 * space \f$\mathbf{x}\in\mathbf{X}\f$ which lies in the state manifold. Note that the acceleration
 * \f$\dot{\mathbf{v}}\in T_{\mathbf{q}}\mathcal{Q}\f$ lies also in the tangent space of the configuration manifold.
 *
 * The main computations are carrying out in `calc` and `calcDiff`. `calc` computes the acceleration and cost and
 * `calcDiff` computes the derivatives of the dynamics and cost function. Concretely speaking, `calcDiff` builds a
 * linear-quadratic approximation of the differential action, where the dynamics and cost functions have linear and
 * quadratic forms, respectively. \f$\mathbf{f_x}\in\mathbb{R}^{nv\times ndx}\f$,
 * \f$\mathbf{f_u}\in\mathbb{R}^{nv\times nu}\f$ are the Jacobians of the dynamics;
 * \f$\mathbf{l_x}\in\mathbb{R}^{ndx}\f$, \f$\mathbf{l_u}\in\mathbb{R}^{nu}\f$,
 * \f$\mathbf{l_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$, \f$\mathbf{l_{xu}}\in\mathbb{R}^{ndx\times nu}\f$,
 * \f$\mathbf{l_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Jacobians and Hessians of the cost function, respectively.
 * Additionally, it is important remark that `calcDiff()` computes the derivates using the latest stored values by
 * `calc()`. Thus, we need to run first `calc()`.
 *
 * \sa `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class DifferentialActionModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the differential action model
   *
   * @param[in] state  State description
   * @param[in] nu     Dimension of control vector
   * @param[in] nr     Dimension of cost-residual vector
   */
  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
                                     const std::size_t& nr = 0);
  virtual ~DifferentialActionModelAbstractTpl();

  /**
   * @brief Compute the system acceleration and cost value
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   * @param[in] u     Control input
   */
  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Compute the derivatives of the dynamics and cost functions
   *
   * It computes the partial derivatives of the dynamical system and the cost function. It assumes that `calc()` has
   * been run first. This function builds a quadratic approximation of the time-continuous action model (i.e. dynamical
   * system and cost function).
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   * @param[in] u     Control input
   */
  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Create the differential action data
   *
   * @return the differential action data
   */
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  /**
   * @brief Checks that a specific data belongs to this model
   */
  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);

  /**
   * @copybrief calc()
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   */
  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @copybrief calcDiff()
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   */
  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Computes the quasic static commands
   *
   * The quasic static commands are the ones produced for a the reference posture as an equilibrium point, i.e.
   * for \f$\mathbf{f}(\mathbf{q},\mathbf{v}=\mathbf{0},\mathbf{u})=\mathbf{0}\f$
   *
   * @param[in] data    Differential action data
   * @param[out] u      Quasic static commands
   * @param[in] x       State point (velocity has to be zero)
   * @param[in] maxiter Maximum allowed number of iterations
   * @param[in] tol     Tolerance
   */
  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
                           const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
                           const Scalar& tol = Scalar(1e-9));

  /**
   * @copybrief quasicStatic()
   *
   * @copydetails quasicStatic()
   *
   * @param[in] data    Differential action data
   * @param[in] x       State point (velocity has to be zero)
   * @param[in] maxiter Maximum allowed number of iterations
   * @param[in] tol     Tolerance
   * @return Quasic static commands
   */
  VectorXs quasiStatic_x(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const VectorXs& x,
                         const std::size_t& maxiter = 100, const Scalar& tol = Scalar(1e-9));

  /**
   * @brief Return the dimension of the control input
   */
  const std::size_t& get_nu() const;

  /**
   * @brief Return the dimension of the cost-residual vector
   */
  const std::size_t& get_nr() const;

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Return the control lower bound
   */
  const VectorXs& get_u_lb() const;

  /**
   * @brief Return the control upper bound
   */
  const VectorXs& get_u_ub() const;

  /**
   * @brief Indicates if there are defined control limits
   */
  bool const& get_has_control_limits() const;

  /**
   * @brief Modify the control lower bounds
   */
  void set_u_lb(const VectorXs& u_lb);

  /**
   * @brief Modify the control upper bounds
   */
  void set_u_ub(const VectorXs& u_ub);

 protected:
  std::size_t nu_;                          //!< Control dimension
  std::size_t nr_;                          //!< Dimension of the cost residual
  boost::shared_ptr<StateAbstract> state_;  //!< Model of the state
  VectorXs unone_;                          //!< Neutral state
  VectorXs u_lb_;                           //!< Lower control limits
  VectorXs u_ub_;                           //!< Upper control limits
  bool has_control_limits_;                 //!< Indicates whether any of the control limits is finite

  void update_has_control_limits();
};

template <typename _Scalar>
struct DifferentialActionDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
      : cost(0.),
        xout(model->get_state()->get_nv()),
        Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        Fu(model->get_state()->get_nv(), model->get_nu()),
        r(model->get_nr()),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()) {
    xout.setZero();
    r.setZero();
    Fx.setZero();
    Fu.setZero();
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }
  virtual ~DifferentialActionDataAbstractTpl() {}

  Scalar cost;    //!< cost value
  VectorXs xout;  //!< evolution state
  MatrixXs Fx;    //!< Jacobian of the dynamics
  MatrixXs Fu;    //!< Jacobian of the dynamics
  VectorXs r;     //!< Cost residual
  VectorXs Lx;    //!< Jacobian of the cost function
  VectorXs Lu;    //!< Jacobian of the cost function
  MatrixXs Lxx;   //!< Hessian of the cost function
  MatrixXs Lxu;   //!< Hessian of the cost function
  MatrixXs Luu;   //!< Hessian of the cost function
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/diff-action-base.hxx"

#endif  // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_


/***
第三节类模板中的友元
(1)友元类：
让某个类B称为另外一个类A的友元类，这样的话，类B就可以在其成员函数中访问类A的所有成员，
而不管这些成员在类A中是用什么（public,protected,private）来修饰的。
如果现在类A和类B都变成了类模板，那么能否让类模板B成为类模板A的友元类模板呢？
1.1：让类模板的某个实例成为友元类
1.2：让类模板成为友元类模板
1.3：让类型模板参数成为友元类:C++11新标准中引入：如果传递进来的类型模板参数是一个类类型，则这个类类型可以成为当前类模板的友元类。
a)代码行A2<CF> aobj1; 的效果是让CF类成为了A2<CF>类的友元类；
b)于是，在CF类的成员函数中，可以直接访问aobj1这个A2<CF>类对象的data私有成员变量。
如果传递给类模板A2的类型模板参数不是一个类类型，那么代码行friend T;就会被忽略。
***/

#include <iostream>

//#include <boost/type_index.hpp>
using namespace std;
//#pragma warning(disable : 4996)

namespace _nmsp1
{
	//template <typename U> class B;       //类模板B的声明

	template <typename T>
	class A
	{
		//friend class B<long>;            //不需要任何public,private等修饰符。
		template<typename> friend class B; //类模板B作为类模板A的友元
	private:
		int data;
	};

	template <typename U>
	class B
	{
	public:
		void callBAF()                     //在类模板B中访问类模板A的私有成员
		{
			A<int> atmpobj;
			atmpobj.data = 5;
			cout << atmpobj.data << endl;
		}
	};
}

namespace _nmsp2
{
	template <typename T>
	class A2
	{
		friend T;                       //类T成为类模板A2的友元
		friend  class CF;

	private:
		int data;
	};

	class CF
	{
	public:
		void callCFAF()
		{
			A2<CF> aobj1;                //让CF类成为了A2<CF>类的友元类
			aobj1.data = 12;
			cout << aobj1.data << endl;

			A2<int> aobj2;               //因为CF类并不是A2<int>的友元类，自然不能在这里访问aobj2这个A2<int>类对象的data私有成员变量。
			aobj2.data = 15;
			cout << aobj2.data << endl;
		}
	};
}

int main()
{
	//测试nmsp1
	//_nmsp1::B<long> bobj;
	_nmsp1::B<int> bobj;
	bobj.callBAF();

	//测试nmsp2
	_nmsp2::CF mycfobj;
	mycfobj.callCFAF();

	/*_nmsp2::A2<_nmsp2::CF> aobj1;
	aobj1.data = 12;*/

	return 0;
}