/// HEADER
#include <cslibs_vectormaps/maps/oriented_visibility_grid_vector_map.h>

/// COMPONENT
#include <cslibs_boost_geometry/algorithms.hpp>
#include <cslibs_vectormaps/utility/tools.hpp>

/// SYSTEM
#include <boost/geometry/algorithms/convex_hull.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/geometry.hpp>

using namespace cslibs_vectormaps;
using namespace cslibs_boost_geometry;

OrientedVisibilityGridVectorMap::OrientedVisibilityGridVectorMap(const BoundingBox &bounding,
                                                                 const double       range,
                                                                 const double       resolution,
                                                                 const double       angular_resolution,
                                                                 const bool         debug)
    : GridVectorMap(bounding, range, resolution, debug),
      angular_resolution_(angular_resolution)
{
    theta_bins_      = std::ceil(2 * M_PI / angular_resolution);
    theta_bins_inv_  = 1.0 / theta_bins_;
    grid_dimensions_ = {rows_, cols_, theta_bins_};
    grid_.resize(grid_dimensions_.globalSize());
}

OrientedVisibilityGridVectorMap::OrientedVisibilityGridVectorMap() :
    GridVectorMap()
{
}

namespace {
inline bool operator == (const VectorMap::Point& lhs, const VectorMap::Point& rhs)
{
    return lhs.x() == rhs.x() && lhs.y() == rhs.y();
}

inline bool operator != (const VectorMap::Point& lhs, const VectorMap::Point& rhs)
{
    return !(lhs == rhs);
}

inline bool operator == (const VectorMap::Vector& lhs, const VectorMap::Vector& rhs)
{
    return lhs.first == rhs.first && lhs.second == rhs.second;
}
inline bool operator != (const VectorMap::Vector& lhs, const VectorMap::Vector& rhs)
{
    return !(lhs == rhs);
}

bool isShadowedByOneOf(const VectorMap::Point& point, std::vector<OrientedVisibilityGridVectorMap::ShadowBorder>& areas)
{
    for(const OrientedVisibilityGridVectorMap::ShadowBorder& shadow : areas) {
        if(boost::geometry::within(point, shadow.area)) {
            return true;
        }
    }
    return false;
}

bool isShadowedByOneOf(const VectorMap::Vector& line, std::vector<OrientedVisibilityGridVectorMap::ShadowBorder>& areas)
{
    for(const OrientedVisibilityGridVectorMap::ShadowBorder& shadow : areas) {
        if(boost::geometry::within(line.first, shadow.area) && boost::geometry::within(line.second, shadow.area)) {
            return true;
        }
    }
    return false;
}

void extend(VectorMap::Vector& vec)
{
    VectorMap::Point delta = vec.second;
    boost::geometry::subtract_point(delta, vec.first);
    boost::geometry::multiply_value(delta, 1000. / (0.001 + hypot(delta.y(), delta.x())));
    boost::geometry::add_point(vec.second, delta);
}

bool checkPair(OrientedVisibilityGridVectorMap::Line& line,
               OrientedVisibilityGridVectorMap::Line& dual_line,
               const VectorMap::Polygon& area,
               OrientedVisibilityGridVectorMap::ShadowBorder& shadow)
{
    bool change = false;

    std::vector<VectorMap::Point> intersection;
    if(boost::geometry::intersection(line.line, shadow.outline.baseline, intersection)) {
        if(!intersection.empty()) {
            line.line.second = intersection.front();
            line.hit = true;
            if(!dual_line.hit) {
                extend(dual_line.line);
            }

            if(boost::geometry::within(shadow.outline.baseline.first, area)) {
                if(shadow.outline.baseline.first != intersection.front()) {
                    shadow.outline.first.valid = false;
                    shadow.outline.baseline.first = intersection.front();
                    change = true;
                }
            }

            if(boost::geometry::within(shadow.outline.baseline.second, area)) {
                if(shadow.outline.baseline.second != intersection.front()) {
                    shadow.outline.second.valid = false;
                    shadow.outline.baseline.second = intersection.front();
                    change = true;
                }
            }

            if(!change) {
                // split in two...
            }
        }
    }

    return change;
}

bool check(OrientedVisibilityGridVectorMap::ShadowBorder& border,
           OrientedVisibilityGridVectorMap::ShadowBorder& shadow)
{
    bool changed = false;
    changed |= checkPair(border.outline.first, border.outline.second, border.area, shadow);
    changed |= checkPair(border.outline.second, border.outline.first, border.area, shadow);

    return changed;
}

bool removeOneIntersection(VectorMap::Vectors& cs)
{
    for(std::size_t i = 0, total_i = cs.size(); i < total_i; ++i) {
        const VectorMap::Vector& a = cs[i];
        for(std::size_t j = i+1, total_j = cs.size(); j < total_j; ++j) {
            const VectorMap::Vector& b = cs[j];

            if(b == a) {
                cs.erase(cs.begin() + i);
                std::cerr << "erase\n";
                return true;
            }

            std::vector<VectorMap::Point> intersection;
            boost::geometry::intersection(a, b, intersection);
            if(!intersection.empty()){
                if(intersection.front() == a.first ||
                        intersection.front() == a.second ||
                        intersection.front() == b.first ||
                        intersection.front() == b.second) {
                    continue;
                }

                VectorMap::Vector a1, a2, b1, b2;
                a1.first = a.first;
                a1.second = intersection.front();
                a2.first = intersection.front();
                a2.second = a.second;
                b1.first = b.first;
                b1.second = intersection.front();
                b2.first = intersection.front();
                b2.second = b.second;
                cs.erase(cs.begin() + std::max(i, j));
                cs.erase(cs.begin() + std::max(j, i));
                cs.push_back(a1);
                cs.push_back(a2);
                cs.push_back(b1);
                cs.push_back(b2);
                std::cerr << "split" << "\n";
                return true;
            }
        }
    }

    return false;
}
}

OrientedVisibilityGridVectorMap::Shadow OrientedVisibilityGridVectorMap::castShadows(const VectorMap::Vectors& casters, const VectorMap::Vector& l)
{
    // remove intersections
    VectorMap::Vectors cs = casters;

    {
    bool change = true;
    while(change) {
        change = removeOneIntersection(cs);
    }
    }

    // generate raw shadows
    std::vector<ShadowBorder> shadows_init;
    for(const Vector& s : cs) {
        if(!boost::geometry::intersects(s, l)) {
            VectorMap::Vector line_s_first;
            VectorMap::Vector line_s_second;

            findBorderLines(s, l, line_s_first, line_s_second);

            ShadowBorder border;
            border.outline.baseline = s;

            border.outline.first.line = line_s_first;
            border.outline.second.line = line_s_second;

            border.area = calculateShadowPolygon(s, line_s_second, line_s_first);
            shadows_init.push_back(border);
        }
    }

    std::vector<ShadowBorder> shadows = shadows_init;

    bool change = true;
    //    while(change) {
    //        change = false;
    {
        // filter contained shadows
        std::vector<ShadowBorder> shadows_raw = shadows;
        shadows.clear();
        for(const ShadowBorder& border_to_test : shadows_raw) {
            bool contained = false;

            for(const ShadowBorder& border : shadows_raw) {
                if(boost::geometry::within(border_to_test.outline.baseline.first, border.area) &&
                        boost::geometry::within(border_to_test.outline.baseline.second, border.area)) {
                    contained = true;
                }
            }

            if(!contained) {
                shadows.push_back(border_to_test);
            }
        }

        // check borders and trunk them against lines
        for(std::size_t i = 0, total_i = shadows.size(); i < total_i; ++i) {
            ShadowBorder& border = shadows[i];

            for(std::size_t j = 0, total_j = shadows.size(); j < total_j; ++j) {
                if(i == j) {
                    continue;
                }

                bool c = check(border, shadows[j]);
                if(c) {
                    if(isShadowedByOneOf(shadows[j].outline.baseline, shadows)) {
                        shadows[j].outline.valid = false;
                    }
                }

                change |= c;
            }
        }
    }

    Shadow shadow;
    for(const ShadowBorder& s : shadows) {
        if(s.outline.valid) {
            shadow.lines.push_back(s.outline.baseline);
        }
    }

    for(const ShadowBorder& s : shadows) {
        if(!s.outline.valid) {
            continue;
        }
        if(s.outline.first.valid) {
            shadow.border_lines.push_back(s.outline.first.line);
        }
        if(s.outline.second.valid) {
            shadow.border_lines.push_back(s.outline.second.line);
        }
    }

    return shadow;
}

OrientedVisibilityGridVectorMap::Shadow OrientedVisibilityGridVectorMap::castShadow(const VectorMap::Vector& c, const VectorMap::Vector& l)
{
    Shadow shadow;
    if(boost::geometry::intersects(c, l)) {
        return shadow;
    }
    shadow.lines.push_back(c);

    VectorMap::Vector line_to_caster;
    VectorMap::Vector line_from_caster;

    findBorderLines(c, l, line_to_caster, line_from_caster);

    shadow.border_lines.push_back(line_from_caster);
    shadow.border_lines.push_back(line_to_caster);

    shadow.polygon = calculateShadowPolygon(c, line_from_caster, line_to_caster);

    return shadow;
}

VectorMap::Polygon OrientedVisibilityGridVectorMap::calculateShadowPolygon(const VectorMap::Vector& c, VectorMap::Vector line_from_caster, VectorMap::Vector line_to_caster)
{
    VectorMap::Polygon shadow_polygon;
    boost::geometry::append(shadow_polygon.outer(), c.first);
    boost::geometry::append(shadow_polygon.outer(), c.second);

    if(line_to_caster.second == line_from_caster.second) {
        boost::geometry::append(shadow_polygon.outer(), line_to_caster.second);
    } else {
        boost::geometry::append(shadow_polygon.outer(), line_to_caster.second);
        boost::geometry::append(shadow_polygon.outer(), line_from_caster.second);
    }

    VectorMap::Polygon result;
    boost::geometry::convex_hull(shadow_polygon, result);
    return result;
}

void OrientedVisibilityGridVectorMap::findBorderLines(const VectorMap::Vector &c, const VectorMap::Vector &l, VectorMap::Vector &first_line, VectorMap::Vector &second_line)
{
    VectorMap::Polygon poly;
    boost::geometry::append(poly.outer(), c.first);
    boost::geometry::append(poly.outer(), c.second);
    boost::geometry::append(poly.outer(), l.first);
    boost::geometry::append(poly.outer(), l.second);

    VectorMap::Polygon hull;
    boost::geometry::convex_hull(poly, hull);

    VectorMap::Vector first;
    VectorMap::Vector second;

    const VectorMap::Polygon::ring_type &ring = hull.outer();
    for(VectorMap::Polygon::ring_type::const_iterator begin = ring.begin(), end = ring.end(), it = begin; it != end; ++it) {
        const VectorMap::Point& pt = *it;
        const VectorMap::Point& next = (it + 1 == end) ? *begin : *(it+1);

        bool pt_is_line = pt == l.first || pt == l.second;
        bool next_is_line = next == l.first || next == l.second;

        if(pt_is_line && ! next_is_line) {
            VectorMap::Point projection;
            VectorMap::Point delta = next;
            boost::geometry::subtract_point(delta, pt);
            boost::geometry::multiply_value(delta, 1000. / (0.001 + hypot(delta.y(), delta.x())));
            projection.x(next.x() + (delta.x()));
            projection.y(next.y() + (delta.y()));

            first.first = next;
            first.second = projection;

        } else if(!pt_is_line && next_is_line) {
            VectorMap::Point projection;
            VectorMap::Point delta = pt;
            boost::geometry::subtract_point(delta, next);
            boost::geometry::multiply_value(delta, 1000. / (0.001 + hypot(delta.y(), delta.x())));
            projection.x(next.x() + (delta.x()));
            projection.y(next.y() + (delta.y()));

            second.first = pt;
            second.second = projection;
        }
    }

    std::vector<VectorMap::Point> intersections;
    boost::geometry::intersection(second, first, intersections);
    if(!intersections.empty()) {
        second.second = intersections.front();
        first.second = intersections.front();
    }

    if(c.first == first.first || c.first == first.second) {
        first_line = first;
        second_line = second;
    } else {
        first_line = second;
        second_line = first;
    }
}

unsigned int OrientedVisibilityGridVectorMap::handleInsertion()
{
    unsigned int assigned = 0;

    std::size_t rows = grid_dimensions_.size<0>();
    std::size_t cols = grid_dimensions_.size<1>();

    std::cerr << "generate ground state\n";

    for(Vector& line : data_) { // TODO: make const!
        Point la = line.first;
        Point lb = line.second;
        boost::geometry::divide_value(la, resolution_);
        boost::geometry::divide_value(lb, resolution_);
        boost::geometry::subtract_point(la, min_corner_);
        boost::geometry::subtract_point(lb, min_corner_);

        Point min, max;
        min.x(std::min(la.x(), lb.x()) - padding_);
        max.x(std::max(la.x(), lb.x()) + padding_);
        min.y(std::min(la.y(), lb.y()) - padding_);
        max.y(std::max(la.y(), lb.y()) + padding_);

        std::size_t col_start = std::max(0, static_cast<int>(std::floor(min.x())));
        std::size_t col_to = std::min(cols-1, static_cast<std::size_t>(std::ceil(max.x())));
        std::size_t row_start = std::max(0, static_cast<int>(std::floor(min.y())));
        std::size_t row_to = std::min(rows-1, static_cast<std::size_t>(std::ceil(max.y())));

        for(std::size_t i = row_start; i < row_to; ++i) {
            for(std::size_t j = col_start; j < col_to; ++j) {
                for(unsigned int t = 0 ; t < theta_bins_ ; ++t) {
                    // TODO: check in view
                    VectorPtrs& entry = grid_[grid_dimensions_.index(i,j,t)];
                    entry.push_back(&line);
                }
            }
        }
    }

    int line_count = 0;
    std::cerr << "optimize using visibility\n";
    for(Vector& caster : data_) { // TODO: make const!
        std::cerr << "analyzing line " << line_count << " / " << data_.size() << "\n";
        ++line_count;

        for(Vector& line : data_) { // TODO: make const!
            if(&line == &caster) {
                // no self checking
                continue;
            }

            if(boost::geometry::intersects(line, caster)) {
                // skip intersecting lines
                continue;
            }

            if(boost::geometry::distance(line.first, caster) > padding_ &&
                    boost::geometry::distance(line.second, caster) > padding_) {
                // skip intersecting lines
                continue;
            }

            OrientedVisibilityGridVectorMap::Shadow shadow = castShadow(caster, line);

            Point la = line.first;
            Point lb = line.second;
            boost::geometry::divide_value(la, resolution_);
            boost::geometry::divide_value(lb, resolution_);
            boost::geometry::subtract_point(la, min_corner_);
            boost::geometry::subtract_point(lb, min_corner_);

            Point min, max;
            min.x(std::min(la.x(), lb.x()) - padding_);
            max.x(std::max(la.x(), lb.x()) + padding_);
            min.y(std::min(la.y(), lb.y()) - padding_);
            max.y(std::max(la.y(), lb.y()) + padding_);

            std::size_t col_start = std::max(0, static_cast<int>(std::floor(min.x())));
            std::size_t col_to = std::min(cols-1, static_cast<std::size_t>(std::ceil(max.x()))); // TODO: should this be cols and not cols-1?
            std::size_t row_start = std::max(0, static_cast<int>(std::floor(min.y())));
            std::size_t row_to = std::min(rows-1, static_cast<std::size_t>(std::ceil(max.y()))); // TODO: should this be rows and not rows-1?

            for(std::size_t i = row_start; i < row_to; ++i) {
                for(std::size_t j = col_start; j < col_to; ++j) {
                    // test if cell is in shadow by testing the four corners
                    double x = min_corner_.x() + j * resolution_;
                    double y = min_corner_.y() + i * resolution_;

                    VectorMap::Point tl, tr, bl, br;
                    tl.x(x); tl.y(y);
                    tr.x(x+resolution_); tr.y(y);
                    bl.x(x); bl.y(y+resolution_);
                    br.x(x+resolution_); br.y(y+resolution_);

                    if(boost::geometry::within(tl, shadow.polygon) &&
                            boost::geometry::within(tr, shadow.polygon) &&
                            boost::geometry::within(bl, shadow.polygon) &&
                            boost::geometry::within(br, shadow.polygon)) {

                        for(unsigned int t = 0 ; t < theta_bins_ ; ++t) {
                            VectorPtrs& entry = grid_[grid_dimensions_.index(i,j,t)];
                            VectorPtrs::iterator pos = std::find(entry.begin(), entry.end(), &line);
                            if(pos != entry.end()) {
                                entry.erase(pos);
                            }
                        }
                    }
                }
            }
        }
    }

    return assigned;
}

double OrientedVisibilityGridVectorMap::angularResolution() const
{
    return angular_resolution_;
}

const void* OrientedVisibilityGridVectorMap::cell(const Point& pos) const
{
    return &grid_[grid_dimensions_.index(row(pos), col(pos), 0)];
}

double OrientedVisibilityGridVectorMap::minSquaredDistanceNearbyStructure(const Point& pos,
                                                                          const void* cell_ptr,
                                                                          double angle) const
{
    unsigned int theta = angle2index(angle);
    double min_dist = std::numeric_limits<double>::max();
    double dist = 0.0;
    const VectorPtrs& cell = static_cast<const VectorPtrs*>(cell_ptr)[theta];

    for (const Vector* line : cell) {
        dist = algorithms::distance<double, Point>(pos, *line);
        if (dist < min_dist)
            min_dist = dist;
    }

    return min_dist;
}

double OrientedVisibilityGridVectorMap::minDistanceNearbyStructure(const Point &pos,
                                                                   const unsigned int row,
                                                                   const unsigned int col,
                                                                   const double angle) const
{
    unsigned int theta = angle2index(angle);
    double min_dist = std::numeric_limits<double>::max();
    double dist(0.0);
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
    for(const Vector* line : cell) {
        dist = algorithms::distance<double,Point>(pos, *line);
        if(dist < min_dist)
            min_dist = dist;
    }

    return min_dist;
}

double OrientedVisibilityGridVectorMap::minSquaredDistanceNearbyStructure(const Point &pos,
                                                                          const unsigned int row,
                                                                          const unsigned int col,
                                                                          const double angle) const
{
    unsigned int theta = angle2index(angle);
    double min_squared_dist = std::numeric_limits<double>::max();
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        double squared_dist = boost::geometry::comparable_distance(pos, *line);
        if(squared_dist < min_squared_dist)
            min_squared_dist = squared_dist;
    }

    return min_squared_dist;
}

unsigned int OrientedVisibilityGridVectorMap::thetaBins() const
{
    return theta_bins_;
}

double OrientedVisibilityGridVectorMap::minDistanceNearbyStructure(const Point &pos) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    double min_dist = std::numeric_limits<double>::max();
    double dist(0.0);
    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
        for(const Vector* line : cell) {
            dist = algorithms::distance<double,Point>(pos, *line);
            if(dist < min_dist)
                min_dist = dist;
        }
    }

    if(min_dist == std::numeric_limits<double>::max())
        return -1.0;
    else
        return min_dist;
}

double OrientedVisibilityGridVectorMap::minSquaredDistanceNearbyStructure(const Point &pos) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);
    double min_squared_dist = std::numeric_limits<double>::max();

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
        for(const Vector* line : cell) {
            double squared_dist = boost::geometry::comparable_distance(pos, *line);
            if(squared_dist < min_squared_dist)
                min_squared_dist = squared_dist;
        }
    }

    if(min_squared_dist == std::numeric_limits<double>::max())
        return -1.0;
    else
        return min_squared_dist;
}

bool OrientedVisibilityGridVectorMap::structureNearby(const Point &pos,
                                                      const double thresh) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    double dist(0.0);
    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
        for(const Vector* line : cell) {
            dist = algorithms::distance<double,Point>(pos, *line);
            if(dist > 0.0 && dist < thresh)
                return true;
        }
    }

    return false;
}

bool OrientedVisibilityGridVectorMap::retrieveFiltered(const Point &pos,
                                                       const double orientation,
                                                       Vectors &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);
    unsigned int theta = angle2index(orientation);

    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    // compute the exact bound box for pos
    //  (if the resolution is large, there might be many unnecessary lines)
    Point min(pos.x() - range_, pos.y() - range_);
    Point max(pos.x() + range_, pos.y() + range_);
    BoundingBox bound(min, max);

    for(const Vector* line : cell) {
        // filter out unnecessary lines
        if(algorithms::touches<Point>(*line, bound)) {
            lines.push_back(*line);
        }
    }

    return lines.size() > 0;
}

bool OrientedVisibilityGridVectorMap::retrieve(const Point &pos,
                                               const double orientation,
                                               Vectors     &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);
    unsigned int theta = angle2index(orientation);

    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

bool OrientedVisibilityGridVectorMap::retrieve(const double x,
                                               const double y,
                                               const double orientation,
                                               Vectors &lines) const
{
    if(tools::coordinatesOutsideMap(x, y, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(y);
    unsigned int col = GridVectorMap::col(x);
    unsigned int theta = angle2index(orientation);

    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

bool OrientedVisibilityGridVectorMap::retrieve(const unsigned int row,
                                               const unsigned int col,
                                               const double angle,
                                               Vectors &lines) const
{
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col,  angle2index(angle)));

    for(const Vector* line : cell) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

double OrientedVisibilityGridVectorMap::intersectScanRay(const Vector &ray,
                                                         const void* cell_ptr,
                                                         const double angle,
                                                         const double max_range) const
{
    const VectorPtrs& cell = static_cast<const VectorPtrs*>(cell_ptr)[angle2index(angle)];
    return algorithms::nearestIntersectionDistance<float, types::Point2d>(ray, cell, max_range);
}

double OrientedVisibilityGridVectorMap::intersectScanRay(const Vector &ray,
                                                         const unsigned int row,
                                                         const unsigned int col,
                                                         const double angle,
                                                         const double max_range) const
{
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, angle2index(angle)));
    return algorithms::nearestIntersectionDistance<float, types::Point2d>(ray, cell, max_range);
}

bool OrientedVisibilityGridVectorMap::retrieveFiltered(const Point &pos,
                                                       Vectors &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);
    Point min, max;
    BoundingBox bound;

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        // compute the exact bound box for pos
        //  (if the resolution is large, there might be many unnecessary lines)
        min.x(pos.x() - range_);
        min.y(pos.y() - range_);
        max.x(pos.x() + range_);
        max.y(pos.y() + range_);
        bound.min_corner() = min;
        bound.max_corner() = max;

        for(const Vector* line : cell) {
            // filter out unnecessary lines
            if(algorithms::touches<Point>(*line, bound)) {
                lines.push_back(*line);
            }
        }
    }

    return lines.size() > 0;
}

bool OrientedVisibilityGridVectorMap::retrieve(const Point &pos,
                                               Vectors     &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        for(const Vector* line : cell) {
            lines.push_back(*line);
        }
    }

    return lines.size() > 0;
}

int OrientedVisibilityGridVectorMap::intersectScanPattern (
        const Point& pos,
        const Vectors &pattern,
        IntersectionSet &intersections) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position "
                         "(" << pos.x() << "|" << pos.y() << ")"
                         " to test not within grid structured area!\n";
        }
        return -1;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    int intersection_count = 0;
    ValidPoints result;

    for(const Vector& line : pattern) {
        double dx = line.second.x() - line.first.x();
        double dy = line.second.y() - line.first.y();
        double angle = atan2(dy, dx);
        unsigned int theta = angle2index(angle);
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        result.result.clear();
        result.valid = algorithms::nearestIntersection<Point>(line,
                                                              cell,
                                                              result.result);
        intersection_count += cell.size();
        intersections.push_back(result);
    }

    return intersection_count;
}

int OrientedVisibilityGridVectorMap::intersectScanPattern (
        const Point& pos,
        const Vectors &pattern,
        std::vector<double> &angles,
        IntersectionSet &intersections) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position "
                         "(" << pos.x() << "|" << pos.y() << ")"
                         " to test not within grid structured area!\n";
        }
        return -1;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    int intersection_count = 0;
    ValidPoints result;
    Vectors::const_iterator it                   = pattern.begin();
    std::vector<double>::const_iterator it_angle = angles.begin();
    for(; it != pattern.end(); ++it, ++it_angle) {
        const Vector& line = *it;
        unsigned int theta = angle2index(*it_angle);
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        result.result.clear();
        result.valid = algorithms::nearestIntersection<Point>(line,
                                                              cell,
                                                              result.result);
        intersection_count += cell.size();
        intersections.push_back(result);
    }

    return intersection_count;
}

void OrientedVisibilityGridVectorMap::intersectScanPattern(const Point   &pos,
                                                           const Vectors &pattern,
                                                           std::vector<float> &ranges,
                                                           const float default_measurement) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedVisibilityGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    ranges.resize(pattern.size());
    for(unsigned int i = 0 ; i < pattern.size() ; ++i) {
        const Vector& line = pattern[i];
        double dx = line.second.x() - line.first.x();
        double dy = line.second.y() - line.first.y();
        double angle = atan2(dy, dx);
        unsigned int theta = angle2index(angle);
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        ranges[i] = algorithms::nearestIntersectionDistance<float, types::Point2d>(line, cell, default_measurement);
    }
}

void OrientedVisibilityGridVectorMap::doLoad(const YAML::Node &node)
{
    GridVectorMap::doLoad(node);
    angular_resolution_ = node["angular_resolution"].as<double>();
    theta_bins_         = node["theta_bins"].as<std::size_t>();
    theta_bins_inv_     = node["theta_bins_inv"].as<double>();

    grid_dimensions_ = {rows_, cols_, theta_bins_};
}

void OrientedVisibilityGridVectorMap::doSave(YAML::Node &node) const
{
    GridVectorMap::doSave(node);
    node["map_type"]           = "oriented_grid";
    node["angular_resolution"] = angular_resolution_;
    node["theta_bins"]         = theta_bins_;
    node["theta_bins_inv"]     = theta_bins_inv_;

    assert(node.IsMap());
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/try_c.hpp>
#include <mpllibs/metamonad/exception.hpp>
#include <mpllibs/metamonad/data.hpp>
#include <mpllibs/metamonad/value_to_stream.hpp>

#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/divides.hpp>

#include <iostream>

using boost::mpl::int_;
using boost::mpl::eval_if;
using boost::mpl::equal_to;
using boost::mpl::divides;

using mpllibs::metamonad::try_c;
using mpllibs::metamonad::exception;

typedef int_<0> int0;
typedef int_<13> int13;

MPLLIBS_DATA(division_by_zero, 0, ((division_by_zero, 0)));

template <class A, class B>
struct safe_divides :
  try_c<
    eval_if<
      typename equal_to<B, int0>::type,
      exception<division_by_zero>,
      divides<A, B>
    >
  >
{};

int main()
{
  mpllibs::metamonad::value_to_stream<
    safe_divides<int13, int0>
  >::run(std::cout);
  std::cout << std::endl;
}


/*
 * Copyright (c) 2021 The Foundation for Research on Information Technologies in Society (IT'IS).
 *
 * This file is part of iSEG
 * (see https://github.com/ITISFoundation/osparc-iseg).
 *
 * This software is released under the MIT License.
 *  https://opensource.org/licenses/MIT
 */
#include "Precompiled.h"

#include "ImageWriter.h"
#include "VtkGlue/itkImageToVTKImageFilter.h"

#include "Data/Logger.h"
#include "Data/SlicesHandlerITKInterface.h"
#include "Data/Transform.h"

#include <itkCastImageFilter.h>
#include <itkImage.h>
#include <itkImageFileWriter.h>
#include <itkImageSeriesWriter.h>
#include <itkNumericSeriesFileNames.h>
#include <itkRescaleIntensityImageFilter.h>

#include <vtkNew.h>
#include <vtkStructuredPointsWriter.h>
#include <vtkXMLImageDataWriter.h>

#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>

namespace iseg {

template<typename T>
bool ImageWriter::WriteVolume(const std::string& filename, const std::vector<T*>& all_slices, eSliceSelection selection, const SlicesHandlerInterface* handler)
{
	unsigned dims[3] = {handler->Width(), handler->Height(), handler->NumSlices()};
	unsigned start = 0, end = handler->NumSlices();
	switch (selection)
	{
	case eSliceSelection::kActiveSlices:
		start = handler->StartSlice();
		end = handler->EndSlice();
		break;
	case eSliceSelection::kSlice:
		start = handler->ActiveSlice();
		end = handler->ActiveSlice() + 1;
		break;
	default:
		break;
	}

	auto image = wrapToITK(all_slices, dims, start, end, handler->Spacing(), handler->ImageTransform());
	if (image)
	{
		boost::filesystem::path path(filename);
		std::string ext = boost::algorithm::to_lower_copy(path.has_extension() ? path.extension().string() : "");
		if (ext == ".vti" || ext == ".vtk")
		{
			using image_type = itk::SliceContiguousImage<T>;
			using contiguous_image_type = itk::Image<T, 3>;
			using caster_type = itk::CastImageFilter<image_type, contiguous_image_type>;
			using connector_type = itk::ImageToVTKImageFilter<contiguous_image_type>;

			auto caster = caster_type::New();
			caster->SetInput(image);

			auto connector = connector_type::New();
			connector->SetInput(caster->GetOutput());
			connector->Update();

			if (ext == ".vti")
			{
				vtkNew<vtkXMLImageDataWriter> writer;
				writer->SetFileName(filename.c_str());
				writer->SetInputData(connector->GetOutput());
				writer->SetDataModeToAppended();
				return writer->Write() != 0;
			}
			else
			{
				vtkNew<vtkStructuredPointsWriter> writer;
				writer->SetFileName(filename.c_str());
				writer->SetInputData(connector->GetOutput());
				writer->SetFileType(m_Binary ? VTK_BINARY : VTK_ASCII);
				return writer->Write() != 0;
			}
		}
		else if (ext == ".bmp" || ext == ".png" || ext == ".jpg" || ext == ".jpeg")
		{
			using t_slices_type = itk::SliceContiguousImage<T>;
			using t_image_type = itk::Image<T, 3>;
			using o_image_type = itk::Image<unsigned char, 3>;
			using o_image2_type = itk::Image<unsigned char, 2>;

			std::string format = (path.parent_path() / (path.stem().string() + "%04d" + ext)).string();
			auto names_generator = itk::NumericSeriesFileNames::New();
			names_generator->SetSeriesFormat(format.c_str());
			names_generator->SetStartIndex(start);
			names_generator->SetEndIndex(end - 1);
			names_generator->SetIncrementIndex(1);

			using rescale_type = itk::RescaleIntensityImageFilter<t_slices_type, t_image_type>;
			auto rescale = rescale_type::New();
			rescale->SetInput(image);
			rescale->SetOutputMinimum(0);
			rescale->SetOutputMaximum(itk::NumericTraits<o_image_type::PixelType>::max());

			using filter_type = itk::CastImageFilter<t_image_type, o_image_type>;
			auto filter = filter_type::New();
			filter->SetInput(rescale->GetOutput());

			using writer_type = itk::ImageSeriesWriter<o_image_type, o_image2_type>;
			auto writer = writer_type::New();
			writer->SetFileNames(names_generator->GetFileNames());
			writer->SetInput(filter->GetOutput());

			try
			{
				writer->Update();
				return true;
			}
			catch (const itk::ExceptionObject& e)
			{
				ISEG_ERROR(e.GetDescription());
			}
		}
		else
		{
			using image_type = itk::SliceContiguousImage<T>;
			using contiguous_image_type = itk::Image<T, 3>;
			using caster_type = itk::CastImageFilter<image_type, contiguous_image_type>;
			using writer_type = itk::ImageFileWriter<contiguous_image_type>;

			auto caster = caster_type::New();
			caster->SetInput(image);

			auto writer = writer_type::New();
			writer->SetInput(caster->GetOutput());
			writer->SetFileName(filename);
			try
			{
				writer->Update();
				return true;
			}
			catch (const itk::ExceptionObject& e)
			{
				ISEG_ERROR(e.GetDescription());
			}
		}
	}
	return false;
}

bool ImageWriter::WriteVolume(const std::string& file_path, SlicesHandlerInterface* handler, eImageSelection img_selection, eSliceSelection slice_selection)
{
	switch (img_selection)
	{
	case eImageSelection::kSource:
		return WriteVolume<float>(file_path, handler->SourceSlices(), slice_selection, handler);
	case eImageSelection::kTarget:
		return WriteVolume<float>(file_path, handler->TargetSlices(), slice_selection, handler);
	case eImageSelection::kTissue:
		return WriteVolume<tissues_size_t>(file_path, handler->TissueSlices(handler->ActiveTissuelayer()), slice_selection, handler);
	}
	return false;
}

} // namespace iseg


#include <iostream>
#include <iomanip>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <vector>
#include <cmath>
#include <cstring>

#include <unistd.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "NRGOpMatRules.hpp"
#include "NRG_main.hpp"
#include "TwoChQS.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

#ifndef _CHIN_
#define _CHIN_

double chiN(int Nsites, double Lambda)
{

  double daux[3];
  daux[0]=1.0-pow( Lambda,-((double)(Nsites)+1.0) );
  daux[1]=sqrt( 1.0-pow(Lambda,-(2.0*(double)(Nsites)+1.0)) );
  daux[2]=sqrt( 1.0-pow(Lambda,-(2.0*(double)(Nsites)+3.0)) );

  return(daux[0]/(daux[1]*daux[2]));

}

#endif

int main (int argc, char* argv[]){

// Parameters for command-line passing (GetOpt)

  CNRGCodeHandler ThisCode;

//   char ModelOption[]="Anderson";
//   int ModelNo=5;

  // Read Model, etc. (not now)

#include"ModelOptMain.cpp"

  strcpy(ThisCode.Symmetry,ModelSymmetry);
  strcpy(ThisCode.ModelOption,ModelOption);
  ThisCode.ModelNo=ModelNo;
  ThisCode.SymNo=SymNo;
  ThisCode.SaveData=false;

  // NRG objects

  CNRGarray Aeig;
  CNRGbasisarray AeigCut;
  CNRGbasisarray Abasis;
  CNRGbasisarray SingleSite;

  // STL vector

  CNRGmatrix HN;
  CNRGmatrix Qm1fNQ;

  //CNRGmatrix MQQp1;

  CNRGmatrix* MatArray;
  int NumNRGarrays=4;
  // Jul 09: Will this work???
  vector<CNRGmatrix> STLMatArray;
  CNRGmatrix auxNRGMat;
  // MatArray 0 is f_ch1
  // MatArray 1 is f_ch2
  // MatArray 2 is Sz
  // MatArray 3 is Sz2

  // STL vectors
  vector <double> Params;
  vector <double> ParamsHN;
  vector <double> ParamsBetabar;
  vector<int> CommonQNs;
  vector<int> totSpos;

  // Integers
  //int Nsites;

  // Thermodynamics

  CNRGthermo Suscep;
  CNRGthermo Entropy;

  CNRGthermo *ThermoArray;
  int NumThermoArrays=2;

  double TM=0.0;
  //double DN=0.0;
  //double betabar=0.727;
  ThisCode.Nsites=0;
  ThisCode.betabar=0.727;

  // instream
  ifstream InFile;
  // outstream
  ofstream OutFile;

  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////
  ///                                              ///
  ///                Main code                     ///
  ///                                              ///
  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////

  // Read stuff (separate routine)

// Allocate MatArray (hope this works!)
// Yes, but, for some reason, needs to be BEFORE everything!

   MatArray = new CNRGmatrix [NumNRGarrays];
   ThermoArray = new CNRGthermo [NumThermoArrays];

   for (int imat=0;imat<NumNRGarrays;imat++){STLMatArray.push_back(auxNRGMat);}

   ThisCode.Nsites0=0;
   ThisCode.NumChannels=1;
   ThisCode.NumNRGmats=NumNRGarrays;
   ThisCode.NumThermoMats=NumThermoArrays;
   ThisCode.InitialSetUp();
   //InFile.open("input_nrg.dat"); // Needs this here is I
                                      // new MatArray after!! Why????

   ThisCode.SetSingleSite(&SingleSite);

   // Copy pointers for saving/reading.
   ThisCode.pAbasis=&Abasis;
   //ThisCode.MatArray=MatArray;
   ThisCode.MatArray=&STLMatArray[0];
   ThisCode.pAcut=&AeigCut;

  // Set H0
  // HN.SetH0(Params,&SingleSite,&Aeig,&Abasis,MatArray);
  //(vector<double> Params,CNRGbasisarray* pSingleSite,CNRGarray* pAeig,CNRGbasisarray* pAbasisH0,CNRGmatrix* NRGMats)

  // Set initial Aeig and matrices (such as Qm1fNQ[])/Operators
  //  - Model dependent functions (hardest part)
  //  - Set quantum numbers, etc,etc,
  //  - HN. will

  // Thermobasics

   strcpy(ThermoArray[0].ArqName,"SuscepImp_Main.dat");
   strcpy(ThermoArray[0].ChainArqName,"SuscepChain2Ch.dat");
   ThermoArray[0].Calc=CalcSuscep;

   strcpy(ThermoArray[1].ArqName,"EntropyImp_Main.dat");
   strcpy(ThermoArray[1].ChainArqName,"EntropyChain2Ch.dat");
   ThermoArray[1].Calc=CalcEntropy;

   for (int ithermo=0;ithermo<NumThermoArrays;ithermo++){
     if (ThisCode.calcdens==3)
       ThermoArray[ithermo].CalcChain=true;
     else
       ThermoArray[ithermo].CalcChain=false;
   }
   // HN basics
   HN.NeedOld=false;
   HN.UpperTriangular=true;
   HN.CheckForMatEl=Diag_check;

   /// For all codes
   double Gamma=0.0282691;
   double Lambda=2.5;
   if (ThisCode.dInitParams.size()>1)
     Gamma=ThisCode.dInitParams[1];
   if (ThisCode.Lambda>0.0)
     Lambda=ThisCode.Lambda;
   double HalfLambdaFactor=0.5*(1.0+(1.0/Lambda));
   double chi_m1=sqrt(2.0*Gamma/pi)/(sqrt(Lambda)*HalfLambdaFactor);

   // Jul 09: NRG chain
   CNRGchain chain1(ThisCode.Lambda,ThisCode.Nsitesmax);

   // Sep 08: 1ChQ chain calculations diverted to Anderson (ModelNo=0)
   // Aug 09: Why?? Is this correct?

   if ( (ThisCode.ModelNo==3)&&(ThisCode.SymNo==3) ){
     ThisCode.ZeroParams();
     ThisCode.ModelNo=0;
   }

///////////////////////////////////
////   Model specific hamiltonians ///
///////////////////////////////////

   switch(ThisCode.ModelNo){
   case 0: // Anderson
     switch(ThisCode.SymNo){
     case 2: // Anderson model: OneChQSz

       // Initialize matrices (OneChQ routines work here!)
       // Jul 09: using STMatArray

       STLMatArray[0].NeedOld=false;
       STLMatArray[0].CheckForMatEl=OneChQ_cd_check;
       STLMatArray[0].CalcMatEl=OneChQ_fNup_MatEl;
       STLMatArray[0].SaveMatYN=false;

       STLMatArray[1].NeedOld=false;
       STLMatArray[1].CheckForMatEl=OneChQ_cd_check;
       STLMatArray[1].CalcMatEl=OneChQ_fNdn_MatEl;
       STLMatArray[1].SaveMatYN=false;

       // Param for H0
       Params.push_back(ThisCode.dInitParams[0]); // U
       Params.push_back(ThisCode.dInitParams[2]); // ed
       Params.push_back(Lambda);
       Params.push_back(HalfLambdaFactor);
       Params.push_back(ThisCode.dInitParams[3]);        // Mag Field

       //OneChQSz_SetAnderson_Hm1(Params,&Aeig,MatArray);
       OneChQSz_SetAnderson_Hm1(Params,&Aeig,STLMatArray);

       // cd_up and cd_dn (OneChQSz_SetAnderson equals
       // MatArray[2,3] to MatArray[0,1]
       STLMatArray[2].NeedOld=true;
       STLMatArray[2].CheckForMatEl=OneChQSz_cdup_check;
       STLMatArray[2].CalcMatEl=OneChQSz_cdup_MatEl;
       STLMatArray[2].SaveMatYN=true;

       STLMatArray[3].NeedOld=true;
       STLMatArray[3].CheckForMatEl=OneChQSz_cddn_check;
       STLMatArray[3].CalcMatEl=OneChQSz_cddn_MatEl;
       STLMatArray[3].SaveMatYN=true;

       cout << " N = -1" << endl;
       // 	cout << " fN_up : " << endl;
       // 	MatArray[0].PrintAllBlocks();
       // 	cout << " fN_dn : " << endl;
       // 	MatArray[1].PrintAllBlocks();
       cout << " cdN_up : " << endl;
       STLMatArray[2].PrintAllBlocks();
       cout << " cdN_dn : " << endl;
       STLMatArray[3].PrintAllBlocks();

       ////////////////////////

       // BuildBasis params
       CommonQNs.push_back(2); // No of common QNs
       CommonQNs.push_back(0); // pos of QN 1 in old
       CommonQNs.push_back(1); // pos of QN 2 in old

       CommonQNs.push_back(0); // pos of QN 1 in SingleSite
       CommonQNs.push_back(1); // pos of QN 1 in SingleSite

       // No total S variables. Leave totSpos empty
       HN.CalcHNMatEl=OneChQSz_HN_MatEl;

       ThisCode.NumChannels=1;
       ThisCode.Nsites0=0;
       //ThisCode.NumNRGmats=4;//2
       ThisCode.NumNRGmats=STLMatArray.size();
       ThisCode.MatArray=&STLMatArray[0]; // Need to do this after
                                          // changes in STLMatArray!

       ThisCode.SaveData=true;

       break;

     case 3: // Anderson model: OneChQ

       // OLD!!

//        MatArray[0].NeedOld=false;
//        MatArray[0].CheckForMatEl=OneChQ_cd_check;
//        MatArray[0].CalcMatEl=OneChQ_fNup_MatEl;

//        MatArray[1].NeedOld=false;
//        MatArray[1].CheckForMatEl=OneChQ_cd_check;
//        MatArray[1].CalcMatEl=OneChQ_fNdn_MatEl;

//        // Sz and Sz2

//        MatArray[2].NeedOld=true;
//        MatArray[2].UpperTriangular=false;
//        MatArray[2].CheckForMatEl=Diag_check;
//        MatArray[2].CalcMatEl=ImpOnly_MatEl;

//        MatArray[3].NeedOld=true;
//        MatArray[3].UpperTriangular=false;
//        MatArray[3].CheckForMatEl=Diag_check;
//        MatArray[3].CalcMatEl=ImpOnly_MatEl;

       // Initialize matrices (uncomment later)

       STLMatArray[0].NeedOld=false;
       STLMatArray[0].CheckForMatEl=OneChQ_cd_check;
       STLMatArray[0].CalcMatEl=OneChQ_fNup_MatEl;

       STLMatArray[1].NeedOld=false;
       STLMatArray[1].CheckForMatEl=OneChQ_cd_check;
       STLMatArray[1].CalcMatEl=OneChQ_fNdn_MatEl;

       // Sz and Sz2
       STLMatArray[2].NeedOld=true;
       STLMatArray[2].UpperTriangular=false;
       STLMatArray[2].CalcAvg=true;  // CalcAvg
       STLMatArray[2].CheckForMatEl=Diag_check;
       STLMatArray[2].CalcMatEl=ImpOnly_MatEl;
       strcpy(STLMatArray[2].MatName,"Sz");

       STLMatArray[3].NeedOld=true;
       STLMatArray[3].UpperTriangular=false;
       STLMatArray[3].CalcAvg=true;  // CalcAvg
       STLMatArray[3].CheckForMatEl=Diag_check;
       STLMatArray[3].CalcMatEl=ImpOnly_MatEl;
       strcpy(STLMatArray[3].MatName,"Sz2");

       if (ThisCode.calcdens==1){
	 // All operators have the same Checks/MatEls in |Q> basis
	 // what changes is the initial set-up
	 auxNRGMat.NeedOld=true;
	 auxNRGMat.UpperTriangular=false;
	 auxNRGMat.CheckForMatEl=OneChQ_cd_check;
	 auxNRGMat.CalcMatEl=OneChQ_cd_MatEl;
	 auxNRGMat.SaveMatYN=true;

	 STLMatArray.push_back(auxNRGMat); // 4 - cd1_up
	 STLMatArray.push_back(auxNRGMat); // 5 - cd1_dn
	 NumNRGarrays=STLMatArray.size();
	 ThisCode.NumNRGmats=NumNRGarrays;
	 ThisCode.MatArray=&STLMatArray[0]; //
	 // changes in STLMatArray!
	 ThisCode.SaveData=true;
	 strcpy(ThisCode.SaveArraysFileName,"1chQAnd");
       }
       else{
	 // Set Thermodynamics
	 // Entropy calculation only
	 strcpy(ThermoArray[0].ArqName,"EntropyImp1Ch_Anderson_Q.dat");
	 strcpy(ThermoArray[0].ChainArqName,"EntropyChain1Ch_Q.dat");
	 ThermoArray[0].Calc=CalcEntropy;
	 ThermoArray[0].dImpValue=2.0*log(2.0);
	 //ThermoArray[0].CalcChain=false;
	 ThisCode.NumThermoMats=1;
       }
       // calculate spec function or thermo?

       // Param for H0
       Params.push_back(ThisCode.dInitParams[0]); // U
       Params.push_back(ThisCode.dInitParams[2]); // ed
       Params.push_back(Lambda);
       Params.push_back(HalfLambdaFactor);
       OneChQ_SetAnderson_Hm1(Params,&Aeig,STLMatArray);
       //OneChQ_SetAnderson_Hm1_old(Params,&Aeig,MatArray);

       ////////////////////////

       // BuildBasis params
       CommonQNs.push_back(1); // No of common QNs
       CommonQNs.push_back(0); // pos of QN 1 in old
       CommonQNs.push_back(0); // pos of QN 1 in SingleSite
       // No total S variables. Leave totSpos empty
       HN.CalcHNMatEl=OneChQ_HN_MatEl;

       ThisCode.NumChannels=1;
       ThisCode.Nsites0=0;

       break;
     default:
       cout << " Symmetry " << ThisCode.Symmetry
	    << " not implemented for " << ThisCode.ModelOption << endl;
       exit(0);
     }
     break;
   case 3: // Chain only
     ThisCode.ZeroParams();
     ThisCode.Nsites0=1;
     ThisCode.NumThermoMats=2;
     switch(ThisCode.SymNo){
     case 0: // Chain model: OneChQS
       // fN (reduced)
       STLMatArray[0].NeedOld=false;
       STLMatArray[0].UpperTriangular=false;
       STLMatArray[0].CheckForMatEl=OneChQS_cd_check;
       STLMatArray[0].CalcMatEl=OneChQS_fN_MatEl;
       STLMatArray.pop_back(); // Only ONE array
       STLMatArray.pop_back(); // Only ONE array
       STLMatArray.pop_back(); // Only ONE array
       // Set ChainH0: SingleSite is already set, just get Aeig and STLMatArray.
       OneChQS_SetChainH0(&Aeig,STLMatArray);
       ThisCode.NumNRGmats=STLMatArray.size();
       ThisCode.MatArray=&STLMatArray[0]; // Need to do this after
       ThisCode.SaveData=false;
       // BuildBasis params
       CommonQNs.push_back(2); // Q and S and commont QNs
       CommonQNs.push_back(0); // position of Q in old basis
       CommonQNs.push_back(1); // position of S in old basis
       CommonQNs.push_back(0); // position of Q in SingleSite
       CommonQNs.push_back(1); // position of S in SingleSite
       totSpos.push_back(1);   // SU(2) symmetry in position 1
       // HN params
       HN.CalcHNMatEl=OneChQS_HN_MatEl;
       ThisCode.NumChannels=1;
       chi_m1=chiN(0,Lambda); // is actually chi0
       strcpy(ThermoArray[0].ChainArqName,"SuscepChain1Ch_QS.dat");
       strcpy(ThermoArray[1].ChainArqName,"EntropyChain1Ch_QS.dat");
       break;
//      case 1: // Chain model: TwoChQS
//        TwoChQS_SetChainH0(&Aeig,STLMatArray);
//        break;
//      case 2: // Chain model: OneChQSz
//        OneChQSz_SetChainH0(&Aeig,STLMatArray);
//        break;
//      case 3: // Chain model: OneChQ
//        OneChQ_SetChainH0(&Aeig,STLMatArray);
//        break;
     default:
       cout << " Symmetry " << ThisCode.Symmetry
	    << " not implemented for " << ThisCode.ModelOption << endl;
       exit(0);
     }
     //cout << " Chain calculation still under implementation" << endl;
     //exit(0);
     break;
   case 5: // SMM model, symmetry is OneChQ
     // Params for H0
     Params.push_back(Lambda);  // Lambda
     Params.push_back(HalfLambdaFactor); // Lambda factor

     if (ThisCode.dInitParams.size()>10){
       Params.push_back(ThisCode.dInitParams[0]); // U1
       Params.push_back(ThisCode.dInitParams[2]); // ed1

       Params.push_back(ThisCode.dInitParams[3]); // U2
       Params.push_back(ThisCode.dInitParams[5]); // ed2

       Params.push_back(ThisCode.dInitParams[6]); // J12

       Params.push_back(ThisCode.dInitParams[7]); // BmagPar
       Params.push_back(ThisCode.dInitParams[8]); // BmagPerp

       Params.push_back(ThisCode.dInitParams[9]); // Dz
       Params.push_back(ThisCode.dInitParams[10]); // B2 anisot

       double chi2_m1=sqrt(2.0*ThisCode.dInitParams[4]/pi)/(sqrt(Lambda)*HalfLambdaFactor);
       Params.push_back(chi_m1); // gamma1
       Params.push_back(chi2_m1); // gamma2
     }
     else{
       double U=0.5;
       double ed=-0.5*U;
       Params.push_back(U); // U1
       Params.push_back(ed); // ed1

       Params.push_back(0.0); // U2
       Params.push_back(0.0); // ed2

       Params.push_back(0.0); // J12

       Params.push_back(0.0); // BmagPar
       Params.push_back(0.0); // BmagPerp

       Params.push_back(0.0); // Dz
       Params.push_back(0.0); // B2 anisot

       Params.push_back(chi_m1); // gamma1
       Params.push_back(0.0); // gamma2
     }

     // Set rules for MatArray
     // Jul 09: using STMatArray

     // fN_up
     STLMatArray[0].NeedOld=false;
     STLMatArray[0].CheckForMatEl=OneChQ_cd_check;
     STLMatArray[0].CalcMatEl=OneChQ_fNup_MatEl;

     // fN_dn
     STLMatArray[1].NeedOld=false;
     STLMatArray[1].CheckForMatEl=OneChQ_cd_check;
     STLMatArray[1].CalcMatEl=OneChQ_fNdn_MatEl;

     // Sz and Sz2

     STLMatArray[2].NeedOld=true;
     STLMatArray[2].UpperTriangular=false;
     STLMatArray[2].CalcAvg=true;  // CalcAvg
     STLMatArray[2].CheckForMatEl=Diag_check;
     STLMatArray[2].CalcMatEl=ImpOnly_MatEl;
     strcpy(STLMatArray[2].MatName,"Sz");

     STLMatArray[3].NeedOld=true;
     STLMatArray[3].UpperTriangular=false;
     STLMatArray[2].CalcAvg=true;
     STLMatArray[3].CheckForMatEl=Diag_check;
     STLMatArray[3].CalcMatEl=ImpOnly_MatEl;
     strcpy(STLMatArray[3].MatName,"Sz2");

     // cd1_up, cd1_dn, cd2_up, cd2_dn
     // MatArray is an STL vector!!!
     if (ThisCode.calcdens==1){
       // All operators have the same Checks/MatEls in |Q> basis
       // what changes is the initial set-up
       auxNRGMat.NeedOld=true;
       auxNRGMat.CheckForMatEl=OneChQ_cd_check;
       auxNRGMat.CalcMatEl=OneChQ_cd_MatEl;
       auxNRGMat.SaveMatYN=true;

       STLMatArray.push_back(auxNRGMat); // 4 - cd1_up
       STLMatArray.push_back(auxNRGMat); // 5 - cd1_dn
       STLMatArray.push_back(auxNRGMat); // 6 - cd2_up
       STLMatArray.push_back(auxNRGMat); // 7 - cd2_dn
       NumNRGarrays=STLMatArray.size();
       ThisCode.NumNRGmats=NumNRGarrays;
       ThisCode.MatArray=&STLMatArray[0]; // Need to do this after
                                          // changes in STLMatArray!
       ThisCode.SaveData=true;
       strcpy(ThisCode.SaveArraysFileName,"SMM");

     }
     else{
       // Set Thermodynamics

       // Entropy calculation only

       strcpy(ThermoArray[0].ArqName,"EntropyImp1Ch_Anderson_Q.dat");
       strcpy(ThermoArray[0].ChainArqName,"EntropyChain1Ch_Q.dat");
       ThermoArray[0].Calc=CalcEntropy;
       ThermoArray[0].dImpValue=4.0*log(2.0);
       ThermoArray[0].CalcChain=false;
       ThisCode.NumThermoMats=1;
     }
     // calculate spec function or thermo?

     // Set H0: output pAeig, MatArray

     OneChQ_SetSMM_H0(Params,&Aeig,&SingleSite,STLMatArray);

     // BuildBasis params
     CommonQNs.push_back(1); // No of common QNs
     CommonQNs.push_back(0); // pos of QN 1 in old
     CommonQNs.push_back(0); // pos of QN 1 in SingleSite
     // No total S variables. Leave totSpos empty
     HN.CalcHNMatEl=OneChQ_HN_MatEl;
     ThisCode.Nsites0=1;
     ThisCode.NumChannels=1;

     chi_m1=chiN(0,Lambda);

     break;

   case 4: // CM Phonons - 2channel
     switch(ThisCode.SymNo){
     case 4:{ // TwoChQSP
       Params.push_back(Lambda);  // Lambda
       Params.push_back(HalfLambdaFactor); // Lambda factor

       Params.push_back(ThisCode.dInitParams[0]); // U1
       Params.push_back(ThisCode.dInitParams[2]); // ed1

       Params.push_back(chi_m1); // sqrt(2gamma1/Pi)/sqrt(L)*HalfLambdaFactor

       Params.push_back(ThisCode.dInitParams[3]); // w0
       Params.push_back(ThisCode.dInitParams[4]); // lambda_ph
       Params.push_back(ThisCode.dInitParams[5]); // alpha
       Params.push_back(ThisCode.dInitParams[6]); // Nph

       // Update rules for f_ch1 and fch2

       // Set rules for MatArray

       // fN_ch1 (reduced)
       MatArray[0].NeedOld=false;
       MatArray[0].CheckForMatEl=TwoChQS_cd_check;
       MatArray[0].CalcMatEl=TwoChQS_cd_ich1_Phonon_MatEl;

       // fN_ch2 (reduced)
       MatArray[1].NeedOld=false;
       MatArray[1].CheckForMatEl=TwoChQSP_fA_check; // mixes parities
       MatArray[1].CalcMatEl=TwoChQS_cd_ich2_Phonon_MatEl;

       //CNRGarray MyTest1;

       // Set H0, get pAeig and Update MatArray
       TwoChQSP_SetH0CMphonon(Params, &Aeig,&SingleSite,MatArray);

       // No phonons needed from now on
       MatArray[0].CalcMatEl=TwoChQS_fNch1_MatEl;
       MatArray[1].CalcMatEl=TwoChQS_fNch2_MatEl;

       // BuildBasis params
       CommonQNs.push_back(3); // No of common QNs
       CommonQNs.push_back(0); // pos of QN 1 in old
       CommonQNs.push_back(1); // pos of QN 2 in old
       CommonQNs.push_back(2); // pos of QN 3 in old

       CommonQNs.push_back(0); // pos of QN 1 in SingleSite
       CommonQNs.push_back(1); // pos of QN 2 in SingleSite
       CommonQNs.push_back(3); // pos of QN 3 in SingleSite

       CommonQNs.push_back(2); // Pos of parity in old
       totSpos.push_back(1); // Pos of S in old

       // Hamiltonian/Code params
       HN.CalcHNMatEl=TwoChQS_HN_MatEl;
       ThisCode.Nsites0=1;
       chi_m1=chiN(0,Lambda);
       ThisCode.NumNRGmats=2;
       ThisCode.NumChannels=2;
       ThisCode.NumThermoMats=2;

       ThermoArray[0].dImpValue=0.0;
       ThermoArray[1].dImpValue=0.0; // Check this!!

       // Thermobasics

       strcpy(ThermoArray[0].ArqName,"SuscepImp2Ch_CMphonon_QSP.dat");
       strcpy(ThermoArray[0].ChainArqName,"SuscepChain2Ch.dat");
       ThermoArray[0].Calc=CalcSuscep;

       strcpy(ThermoArray[1].ArqName,"EntropyImp2Ch_CMphonon_QSP.dat");
       strcpy(ThermoArray[1].ChainArqName,"EntropyChain2Ch.dat");
       ThermoArray[1].Calc=CalcEntropy;

       cout << " Implementing this model NOW " << endl;

       cout << " fN_ch1 : " << endl;
       MatArray[0].PrintAllBlocks();
       cout << " fN_ch2 : " << endl;
       MatArray[1].PrintAllBlocks();
       break;
     }// Need curly braces in "case" is declaring anything in it
     default:
       cout << " Symmetry " << ThisCode.Symmetry
	    << " not implemented for " << ThisCode.ModelOption << endl;
       exit(0);
     }
     break;
   case 6: // Double Quantum dot
     switch(ThisCode.SymNo){
     case 0:{ // OneChQS
       cout << " Implementing DQD case... " << endl;

       double chi2_m1=sqrt(2.0*ThisCode.dInitParams[4]/pi)/(sqrt(Lambda)*HalfLambdaFactor);

       Params.push_back(Lambda);
       Params.push_back(HalfLambdaFactor);
       Params.push_back(ThisCode.dInitParams[0]); // U1
       Params.push_back(ThisCode.dInitParams[2]); // ed1
       Params.push_back(chi_m1); // gamma1
       Params.push_back(ThisCode.dInitParams[3]); // U2
       Params.push_back(ThisCode.dInitParams[5]); // ed2
       Params.push_back(chi2_m1); // gamma2

       // Question: Can these things be implemented within the subroutine??

       // fN (reduced)
       STLMatArray[0].NeedOld=false;
       STLMatArray[0].UpperTriangular=false;
       STLMatArray[0].CheckForMatEl=OneChQS_cd_check;
       STLMatArray[0].CalcMatEl=OneChQS_fN_MatEl;

       switch (ThisCode.calcdens){
       case 0: // Calculates Op averages only
	 auxNRGMat.NeedOld=true;
	 auxNRGMat.UpperTriangular=false; // update procedure only works for 'false'
	 auxNRGMat.CalcAvg=true;  // CalcAvg
	 auxNRGMat.CheckForMatEl=Diag_check;
	 auxNRGMat.CalcMatEl=ImpOnly_MatEl;
	 STLMatArray[1]=auxNRGMat; // 1 - ndot (there already)
	 STLMatArray[2]=auxNRGMat; // 2 - ndot^2 (there already)
	 STLMatArray[3]=auxNRGMat; // 3 - Sdot^2 (there already)
	 STLMatArray.push_back(auxNRGMat); // 4 - S1 dot S2
	 strcpy(STLMatArray[1].MatName,"Ndqd");
	 strcpy(STLMatArray[2].MatName,"NdqdSq");
	 strcpy(STLMatArray[3].MatName,"SdqdSq");
	 strcpy(STLMatArray[4].MatName,"S1dotS2");
	 NumNRGarrays=STLMatArray.size();
	 ThisCode.NumNRGmats=NumNRGarrays;
	 ThisCode.MatArray=&STLMatArray[0]; // Need to do this after
	 // changes in STLMatArray!
	 ThisCode.SaveData=false;
	 break;
       case 1: // Calc Spectral functions
	 // All operators have the same Checks/MatEls in |Q> basis
	 // what changes is the initial set-up
	 auxNRGMat.NeedOld=true;
	 auxNRGMat.CheckForMatEl=OneChQS_cd_check;
	 auxNRGMat.CalcMatEl=OneChQS_cd_MatEl;
	 auxNRGMat.SaveMatYN=true;
	 STLMatArray[1]=auxNRGMat; // 1 - cd1
	 STLMatArray[2]=auxNRGMat; // 2 - cd2
	 strcpy(STLMatArray[1].MatName,"cdot1");
	 strcpy(STLMatArray[2].MatName,"cdot2");
	 STLMatArray.pop_back(); // 3 elements only
	 NumNRGarrays=STLMatArray.size();
	 ThisCode.NumNRGmats=NumNRGarrays;
	 ThisCode.MatArray=&STLMatArray[0]; // Need to do this after
	 // changes in STLMatArray!
	 ThisCode.SaveData=true;
	 strcpy(ThisCode.SaveArraysFileName,"DQD");
	 break;
       case 2: // Thermodynamics
	 STLMatArray.pop_back(); // 1 Matrix only
 	 STLMatArray.pop_back(); // 1 Matrix only
 	 STLMatArray.pop_back(); // 1 Matrix only
      	 strcpy(ThermoArray[0].ArqName,"SuscepImp1ChQS_DQD.dat");
	 strcpy(ThermoArray[0].ChainArqName,"SuscepChain1Ch_QS.dat");
      	 strcpy(ThermoArray[1].ArqName,"EntropyImp1ChQS_DQD.dat");
	 strcpy(ThermoArray[1].ChainArqName,"EntropyChain1Ch_QS.dat");
	 ThermoArray[0].Calc=CalcSuscep;
	 ThermoArray[0].dImpValue=1.0/4.0; // check
	 ThermoArray[1].Calc=CalcEntropy;
	 ThermoArray[1].dImpValue=4.0*log(2.0);
	 ThisCode.NumThermoMats=2;
	 break;
       default:
	 cout << " Calculating levels only " << endl;

       }
       // end switch calcdens

       OneChQS_SetHm1DQD(Params,&Aeig,&SingleSite,STLMatArray);

       // BuildBasis params
       CommonQNs.push_back(2); // Q and S and commont QNs
       CommonQNs.push_back(0); // position of Q in old basis
       CommonQNs.push_back(1); // position of S in old basis
       CommonQNs.push_back(0); // position of Q in SingleSite
       CommonQNs.push_back(1); // position of S in SingleSite
       totSpos.push_back(1);   // SU(2) symmetry in position 1
       // HN params
       HN.CalcHNMatEl=OneChQS_HN_MatEl;
       ThisCode.Nsites0=1;
       ThisCode.NumChannels=1;
       chi_m1=chiN(0,Lambda);
       break;
     }
     default:
       cout << " Symmetry " << ThisCode.Symmetry
	    << " not implemented for " << ThisCode.ModelOption << endl;
       exit(0);
     }
     break;
   default:
     cout << " Model not implemented. Exiting... " << endl;
     exit(0);
   }
   // end switch ModelNo

   // Set Initial AeigCut
   //CNRGbasisarray AeigCut=CutStates(&Aeig, ThisCode.Ncutoff);
   // ThisCode.pAcut=&AeigCut;

   CutStates(Aeig, AeigCut, ThisCode.Ncutoff);

  //AeigCut=CutStates(&Aeig, ThisCode.Ncutoff);
   cout << "Got here... I" << endl;

   // watch out for the "Nsites0-2"
   ThisCode.DN=HalfLambdaFactor*pow(Lambda,(-(ThisCode.Nsites0-2)/2.0) );
   TM=ThisCode.DN/ThisCode.betabar;
   cout << "DN = " << ThisCode.DN << "  TM = " << TM << endl;

   //
   // The idea is to eliminate this and put into CalcStuff
   //
//    if (ThisCode.SymNo==3){
//      ParamsBetabar.clear();
//      ParamsBetabar.push_back(ThisCode.betabar);
//      //double dSz=CalcOpAvg(ParamsBetabar,&AeigCut,&MatArray[2],false,0);
//      //double dSz2=CalcOpAvg(ParamsBetabar,&AeigCut,&MatArray[3],false,0);
//      double dSz=CalcOpAvg(ParamsBetabar,&AeigCut,&STLMatArray[2],false,0);
//      double dSz2=CalcOpAvg(ParamsBetabar,&AeigCut,&STLMatArray[3],false,0);

//      cout << " T = " << TM << " Sz = " << dSz << " Sz2 = " << dSz2
// 	  << "  T_M chi = " << dSz2-dSz*dSz << endl;
//    }

   //
   for (int ithermo=0;ithermo<NumThermoArrays;ithermo++){
     ThisCode.ThermoSTLArray.push_back(ThermoArray[ithermo]);
   }

   if (ThisCode.Nsites0=1) // Calculates Thermo from N=0 on (not N=-1)
     ThisCode.CalcThermo(ThermoArray,&Aeig);

   ThisCode.CalcStuff(&Aeig,&AeigCut);

   // Save Params?
   if (ThisCode.SaveData){ThisCode.SaveGenPars();}

   // Ok, with that we can go into the main loop

   // Parameters for HN (always the same?)
   ParamsHN.push_back(Lambda);
   for (int ich=0;ich<ThisCode.NumChannels;ich++)
     ParamsHN.push_back(chi_m1);

   // Entering calculation of H_Nsites0
   ThisCode.Nsites=ThisCode.Nsites0;

   while (ThisCode.Nsites<ThisCode.Nsitesmax){
     ThisCode.DN=HalfLambdaFactor*pow(Lambda,(-(ThisCode.Nsites-1)/2.0) );
     TM=ThisCode.DN/ThisCode.betabar;
     cout << "DN = " << ThisCode.DN << "TM = " << TM << endl;
     // Debugging...
     //bool disp=(ThisCode.Nsites==3?true:false);
     bool disp=false;

     // Build Basis

     //SingleSite.PrintQNumbers();
     //Aeig.PrintQNumbers();

     BuildBasis(CommonQNs, totSpos,&AeigCut,&Abasis,
		&SingleSite,ThisCode.UpdateBefCut);

     //Abasis.PrintAll();

     // Diagonalize HN

     cout << "Diagonalizing HN... " << endl;
     cout << " chi_N = " << ParamsHN[1] << endl;

     // Need to adapt ALL models to use STLMatArray...
     // in the meantime, this will work.
//      if ( (ThisCode.ModelNo==5)||(ThisCode.ModelNo==0)||(ThisCode.ModelNo==6) ){
//        HN.DiagHN(ParamsHN,&Abasis,&SingleSite,&STLMatArray[0],&Aeig,disp);
//      }else{
//        HN.DiagHN(ParamsHN,&Abasis,&SingleSite,MatArray,&Aeig);
//      }
     if ( (ThisCode.ModelNo==4) ){
       HN.DiagHN(ParamsHN,&Abasis,&SingleSite,MatArray,&Aeig);
     }else{
       HN.DiagHN(ParamsHN,&Abasis,&SingleSite,&STLMatArray[0],&Aeig,disp);
     }

     cout << "... done diagonalizing HN. " << endl;

     Aeig.PrintEn();

     //       for (int ibl=0;ibl<Aeig.NumBlocks();ibl++)
     // 	Aeig.PrintBlock(ibl);

     // Calculate Susceptibility/Entropy (does not need update)

     ThisCode.CalcThermo(ThermoArray,&Aeig);

     // Update matrices (if UpdateBefCut)
     // Cut and Update AC (If it does not need updated matrices, this should be cut).
     // OLD
     //AeigCut.ClearAll();
     //AeigCut=CutStates(&Aeig, ThisCode.Ncutoff);

     // NEW (updates after cutting)
     CutStates(Aeig, AeigCut, ThisCode.Ncutoff);

     if (ThisCode.Nsites<ThisCode.Nsitesmax){
       // Need to adapt ALL models to use STLMatArray...
       // in the meantime, this will work.
       if ( (ThisCode.ModelNo==4) ){
	 UpdateMatrices(&SingleSite,&AeigCut,
			&Abasis,MatArray, ThisCode.NumNRGmats);

       }else{
//  	 UpdateMatrices(&SingleSite,&AeigCut,
//  			&Abasis,&STLMatArray[0],STLMatArray.size());
         // Debugging
	 UpdateMatrices(&SingleSite,&AeigCut,
			&Abasis,&STLMatArray[0],STLMatArray.size(),disp);
       }
     }
     // end update matrices
     // Q Sz tests (remove later)
     cout << " N = " << ThisCode.Nsites << endl;

     // Save stuff to files

     //// Read/Write test (remove later) ///////////

     if (ThisCode.SaveData){ThisCode.SaveArrays();}

     // Calculate Other things that need updated matrices
     //
     // The idea is to eliminate this and put into CalcStuff
     // (need to test with OneChQ!!
     if (ThisCode.SymNo==3){
       ParamsBetabar.clear();
       ParamsBetabar.push_back(ThisCode.betabar);
       double dSz=CalcOpAvg(ParamsBetabar,&AeigCut,&STLMatArray[2],false,0);
       double dSz2=CalcOpAvg(ParamsBetabar,&AeigCut,&STLMatArray[3],false,0);
       //double dSz=CalcOpAvg(ParamsBetabar,&AeigCut,MatArray,false,0);
       //double dSz2=CalcOpAvg(ParamsBetabar,&AeigCut,MatArray,false,0);

       cout << " T = " << TM << " Sz = " << dSz << " Sz2 = " << dSz2
	    << "  T_M chi = " << dSz2-dSz*dSz << endl;
     }

     ThisCode.CalcStuff(&Aeig,&AeigCut);

     // Update ParamsHN
     //ParamsHN[1]=chiN(ThisCode.Nsites,Lambda); // Old
     for (int ich=0;ich<ThisCode.NumChannels;ich++)
       //ParamsHN[ich+1]=chiN(ThisCode.Nsites,Lambda);
       // Try this...
       ParamsHN[ich+1]=chain1.GetChin(ThisCode.Nsites);

     // Update sites
     ThisCode.Nsites++;

   }
   // end Nsites loop

   // De-allocate MatArray
   delete[] ThermoArray;
   delete[] MatArray;

   ThisCode.WrapUp();

}
// end main


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <Eigen/Core>
#include "genfile/vcf/get_set_eigen.hpp"
#include "components/SampleSummaryComponent/SampleSummaryComputation.hpp"
#include "components/SampleSummaryComponent/IntensityDistributionComputation.hpp"
#include "metro/mean_and_variance.hpp"

// #define DEBUG_INTENSITY_DISTRIBUTION_COMPUTATION 1

namespace sample_stats {
	IntensityDistributionComputation::IntensityDistributionComputation():
		m_call_threshhold( 0.9 ),
		m_snp_index( 0 )
	{}

	void IntensityDistributionComputation::accumulate( genfile::VariantIdentifyingData const&, Genotypes const& genotypes, genfile::VariantDataReader& data_reader ) {
		if( !data_reader.supports( "XY" )) {
			return ;
		}

		int const N = genotypes.rows() ;

		{
			genfile::vcf::MatrixSetter< IntensityMatrix > intensity_setter( m_intensities, m_nonmissingness ) ;
			data_reader.get( "XY", intensity_setter ) ;
			assert( m_intensities.rows() == N ) ;
			assert( m_intensities.cols() == 2 ) ;
			assert( m_nonmissingness.rows() == N ) ;
			assert( m_nonmissingness.cols() == 2 ) ;

			// Explicit check for nan or inf.  These mess with our computations so we exclude 'em.
			// Can't really find a better way to do this than a loop right now.
			for( int i = 0; i < N; ++i ) {
				for( int j = 0; j < 2; ++j ) {
					if( ( m_intensities(i,j) - m_intensities(i,j) ) != ( m_intensities(i,j) - m_intensities(i,j) ) ) {
						m_intensities(i,j) = 0 ;
						m_nonmissingness(i,j) = 0 ;
					}
				}
			}
		}

		if( m_snp_index == 0 ) {
			m_number_of_samples = N ;
			m_accumulator_by_genotype.resize( 4 ) ;
		}

		// We have a column for X and a column for Y.
		// We use a linear transformation to compute means of X, Y, X+Y and X-Y.
		Eigen::Matrix< double, 2, 4 > transform ;
		transform <<
			1,  0,  1,  1,
			0,  1,  1, -1
		;
		m_intensities = m_intensities * transform ;
		m_nonmissingness = m_nonmissingness * transform ;

		// In third column, nonmissingness now equals #of non-missing values (zero, one or two).
		// Replace 3rd and fourth columns with an indicator.
		m_nonmissingness.col(2).array() = ( m_nonmissingness.col(2).array() == 2 ).cast< double >() ;
		m_nonmissingness.col(3) = m_nonmissingness.col(2) ;

#if DEBUG_INTENSITY_DISTRIBUTION_COMPUTATION
		std::cerr << "Intensities are:\n"
			<< m_intensities.block( 0, 0, std::min( N, 10 ), 4 )
			<< "with nonmissingness:\n"
			<< m_nonmissingness.block( 0, 0, std::min( N, 10 ), 4 )
			<< "\n" ;
#endif
		// This is an "on-line" algorithm for computing the mean and variance.
		// see http://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#On-line_algorithm
		//

		m_accumulator.accumulate( m_intensities, m_nonmissingness ) ;

#if DEBUG_INTENSITY_DISTRIBUTION_COMPUTATION
			std::cerr << "mean is " << m_accumulator.get_mean().block( 0, 0, std::min( N, 10 ), 2 ) << "\n" ;
#endif
		for( int g = 0; g < 4; ++g ) {
			m_intensities_by_genotype = m_intensities.block( 0, 0, N, 2 ) ;
			m_nonmissingness_by_genotype = m_nonmissingness.block( 0, 0, N, 2 ) ;

			for( int i = 0; i < N; ++i ) {
				if( g == 3 ) { // representing missing genotype
					if( genotypes.row( i ).maxCoeff() >= m_call_threshhold ) {
						m_nonmissingness_by_genotype.row( i ).setZero() ;
					}
				}
				else {
					if( genotypes( i, g ) < m_call_threshhold ) {
						m_nonmissingness_by_genotype.row( i ).setZero() ;
					}
				}
			}

#if DEBUG_INTENSITY_DISTRIBUTION_COMPUTATION
			std::cerr << "Accumulating genotypes for g=" << g << ":\n"
				<< m_intensities_by_genotype.block( 0, 0, std::min( N, 10 ), 2 )
				<< "\n nonmissingness = "
				<< m_nonmissingness_by_genotype.block( 0, 0, std::min( N, 10 ), 2 )
				<< "\n" ;
#endif
			m_accumulator_by_genotype[ g ].accumulate( m_intensities_by_genotype, m_nonmissingness_by_genotype ) ;
#if DEBUG_INTENSITY_DISTRIBUTION_COMPUTATION
			std::cerr << "mean is " << m_accumulator_by_genotype[ g ].get_mean().block( 0, 0, std::min( N, 10 ), 2 ) << "\n" ;
#endif
		}

		++m_snp_index ;
	}

	void IntensityDistributionComputation::compute( int sample, ResultCallback callback ) {
		if( m_snp_index > 0 ) {
			Eigen::MatrixXd const& mean = m_accumulator.get_mean() ;
			Eigen::MatrixXd const& variance = m_accumulator.get_variance() ;
			assert( std::size_t( mean.rows() ) == m_number_of_samples ) ;
			assert( std::size_t( mean.cols() ) == 4 ) ;

			callback( sample, "mean_X", mean( sample, 0 ) ) ;
			callback( sample, "mean_Y", mean( sample, 1 ) ) ;
			callback( sample, "mean_X+Y", mean( sample, 2 ) ) ;
			callback( sample, "mean_X-Y", mean( sample, 3 ) ) ;
			callback( sample, "variance_X", variance( sample, 0 ) ) ;
			callback( sample, "variance_Y", variance( sample, 1 ) ) ;
			callback( sample, "variance_X+Y", variance( sample, 2 ) ) ;
			callback( sample, "variance_X-Y", variance( sample, 3 ) ) ;

			for( int g = 0; g < 4; ++g ) {
				std::string const stub = "g=" + ( g == 3 ? std::string( "NA" ) : genfile::string_utils::to_string( g ) ) ;
				callback( sample, stub + ":count", m_accumulator_by_genotype[g].get_count( sample, 0 ) ) ;
				callback( sample, stub + ":mean_X", m_accumulator_by_genotype[g].get_mean( sample, 0 ) ) ;
				callback( sample, stub + ":mean_Y", m_accumulator_by_genotype[g].get_mean( sample, 1 ) ) ;
				callback( sample, stub + ":variance_X", m_accumulator_by_genotype[g].get_variance( sample, 0 ) ) ;
				callback( sample, stub + ":variance_Y", m_accumulator_by_genotype[g].get_variance( sample, 1 ) ) ;
			}
		}
	}

	std::string IntensityDistributionComputation::get_summary( std::string const& prefix, std::size_t column_width ) const {
		return prefix + "IntensityDistributionComputation" ;
	}
}


//Machine-generated by miind.py. Edit at your own risk.

#include <boost/timer/timer.hpp>
#include <GeomLib.hpp>
#include <TwoDLib.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/RateAlgorithmCode.hpp>
#include <MPILib/include/SimulationRunParameter.hpp>
#include <MPILib/include/report/handler/RootReportHandler.hpp>
#include <MPILib/include/WilsonCowanAlgorithm.hpp>
#include <MPILib/include/PersistantAlgorithm.hpp>
#include <MPILib/include/DelayAlgorithmCode.hpp>
#include <MPILib/include/RateFunctorCode.hpp>
typedef MPILib::MPINetwork<MPILib::DelayedConnection, MPILib::utilities::CircularDistribution> Network;
	// defining variables
int main(int argc, char *argv[]){
	Network network;
	boost::timer::auto_cpu_timer t;

#ifdef ENABLE_MPI
	// initialise the mpi environment this cannot be forwarded to a class
	boost::mpi::environment env(argc, argv);
#endif

	try {	// generating algorithms
	std::vector<std::string> vec_mat_0{"condee2a5ff4-0087-4d69-bae3-c0a223d03693_0_0.05_0_0_.mat"};
	TwoDLib::MeshAlgorithm<DelayedConnection> alg_mesh_0("condee2a5ff4-0087-4d69-bae3-c0a223d03693.model",vec_mat_0,0.00001, 0.0);
	MPILib::Rate RateFunction_1(MPILib::Time);
	MPILib::RateFunctor<DelayedConnection> rate_functor_1(RateFunction_1);
	// generating nodes
	MPILib::NodeId id_0 = network.addNode(alg_mesh_0,MPILib::EXCITATORY_DIRECT);
	MPILib::NodeId id_1 = network.addNode(rate_functor_1,MPILib::NEUTRAL);
	// generating connections
	DelayedConnection con_1_0_0(1,0.05,0);
	network.makeFirstInputOfSecond(id_1,id_0,con_1_0_0);
	// generation simulation parameter
	MPILib::report::handler::RootReportHandler handler("cond.dat",false);

	SimulationRunParameter par_run( handler,1000000,0,0.1,0.001,0.0001,"twopop.log",0.001);
	network.configureSimulation(par_run);
	network.evolve();
	} catch(std::exception& exc){
		std::cout << exc.what() << std::endl;
#ifdef ENABLE_MPI
	//Abort the MPI environment in the correct way :
	env.abort(1);
#endif
	}

	MPILib::utilities::MPIProxy().barrier();
	t.stop();
	if (MPILib::utilities::MPIProxy().getRank() == 0) {

		std::cout << "Overall time spend\n";
		t.report();
	}
	return 0;
}
MPILib::Rate RateFunction_1(MPILib::Time t){
	return 1000.;
}