//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to 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)
#ifndef BOOST_MATH_TOOLS_ULP_PLOT_HPP
#define BOOST_MATH_TOOLS_ULP_PLOT_HPP
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <cassert>
#include <vector>
#include <utility>
#include <fstream>
#include <string>
#include <list>
#include <random>
#include <stdexcept>
#include <boost/math/tools/condition_numbers.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/algorithm/string/predicate.hpp>

// Design of this function comes from:
// https://blogs.mathworks.com/cleve/2017/01/23/ulps-plots-reveal-math-function-accurary/

// The envelope is the maximum of 1/2 and half the condition number of function evaluation.

namespace boost::math::tools {

namespace detail {
template<class F1, class F2, class CoarseReal, class PreciseReal>
void write_gridlines(std::ostream& fs, int horizontal_lines, int vertical_lines,
                     F1 x_scale, F2 y_scale, CoarseReal min_x, CoarseReal max_x, PreciseReal min_y, PreciseReal max_y,
                     int graph_width, int graph_height, int margin_left, std::string const & font_color)
{
  // Make a grid:
  for (int i = 1; i <= horizontal_lines; ++i) {
      PreciseReal y_cord_dataspace = min_y +  ((max_y - min_y)*i)/horizontal_lines;
      auto y = y_scale(y_cord_dataspace);
      fs << "<line x1='0' y1='" << y << "' x2='" << graph_width
         << "' y2='" << y
         << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

      fs << "<text x='" <<  -margin_left/4 + 5 << "' y='" << y - 3
         << "' font-family='times' font-size='10' fill='" << font_color << "' transform='rotate(-90 "
         << -margin_left/4 + 8 << " " << y + 5 << ")'>"
         << std::setprecision(4) << y_cord_dataspace << "</text>\n";
   }

    for (int i = 1; i <= vertical_lines; ++i) {
        CoarseReal x_cord_dataspace = min_x +  ((max_x - min_x)*i)/vertical_lines;
        CoarseReal x = x_scale(x_cord_dataspace);
        fs << "<line x1='" << x << "' y1='0' x2='" << x
           << "' y2='" << graph_height
           << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

        fs << "<text x='" <<  x - 10  << "' y='" << graph_height + 10
           << "' font-family='times' font-size='10' fill='" << font_color << "'>"
           << std::setprecision(4) << x_cord_dataspace << "</text>\n";
    }
}
}

template<class F, typename PreciseReal, typename CoarseReal>
class ulps_plot {
public:
    ulps_plot(F hi_acc_impl, CoarseReal a, CoarseReal b,
             size_t samples = 1000, bool perturb_abscissas = false, int random_seed = -1);

    ulps_plot& clip(PreciseReal clip);

    ulps_plot& width(int width);

    ulps_plot& envelope_color(std::string const & color);

    ulps_plot& title(std::string const & title);

    ulps_plot& background_color(std::string const & background_color);

    ulps_plot& font_color(std::string const & font_color);

    ulps_plot& crop_color(std::string const & color);

    ulps_plot& nan_color(std::string const & color);

    ulps_plot& ulp_envelope(bool write_ulp);

    template<class G>
    ulps_plot& add_fn(G g, std::string const & color = "steelblue");

    ulps_plot& horizontal_lines(int horizontal_lines);

    ulps_plot& vertical_lines(int vertical_lines);

    void write(std::string const & filename) const;

    friend std::ostream& operator<<(std::ostream& fs, ulps_plot const & plot)
    {
        using std::abs;
        using std::floor;
        using std::isnan;
        if (plot.ulp_list_.size() == 0)
        {
            throw std::domain_error("No functions added for comparison.");
        }
        if (plot.width_ <= 1)
        {
            throw std::domain_error("Width = " + std::to_string(plot.width_) + ", which is too small.");
        }

        PreciseReal worst_ulp_distance = 0;
        PreciseReal min_y = std::numeric_limits<PreciseReal>::max();
        PreciseReal max_y = std::numeric_limits<PreciseReal>::lowest();
        for (auto const & ulp_vec : plot.ulp_list_)
        {
            for (auto const & ulp : ulp_vec)
            {
                if (static_cast<PreciseReal>(abs(ulp)) > worst_ulp_distance)
                {
                    worst_ulp_distance = static_cast<PreciseReal>(abs(ulp));
                }
                if (static_cast<PreciseReal>(ulp) < min_y)
                {
                    min_y = static_cast<PreciseReal>(ulp);
                }
                if (static_cast<PreciseReal>(ulp) > max_y)
                {
                    max_y = static_cast<PreciseReal>(ulp);
                }
            }
        }

        // half-ulp accuracy is the best that can be expected; sometimes we can get less, but barely less.
        // then the axes don't show up; painful!
        if (max_y < 0.5) {
            max_y = 0.5;
        }
        if (min_y > -0.5) {
            min_y = -0.5;
        }

        if (plot.clip_ > 0)
        {
            if (max_y > plot.clip_)
            {
                max_y = plot.clip_;
            }
            if (min_y < -plot.clip_)
            {
                min_y = -plot.clip_;
            }
        }

        int height = static_cast<int>(floor(double(plot.width_)/1.61803));
        int margin_top = 40;
        int margin_left = 25;
        if (plot.title_.size() == 0)
        {
            margin_top = 10;
            margin_left = 15;
        }
        int margin_bottom = 20;
        int margin_right = 20;
        int graph_height = height - margin_bottom - margin_top;
        int graph_width = plot.width_ - margin_left - margin_right;

        // Maps [a,b] to [0, graph_width]
        auto x_scale = [&](CoarseReal x)->CoarseReal
        {
            return ((x-plot.a_)/(plot.b_ - plot.a_))*static_cast<CoarseReal>(graph_width);
        };

        auto y_scale = [&](PreciseReal y)->PreciseReal
        {
            return ((max_y - y)/(max_y - min_y) )*static_cast<PreciseReal>(graph_height);
        };

        fs << "<?xml version=\"1.0\" encoding='UTF-8' ?>\n"
           << "<svg xmlns='http://www.w3.org/2000/svg' width='"
           << plot.width_ << "' height='"
           << height << "'>\n"
           << "<style>\nsvg { background-color:" << plot.background_color_ << "; }\n"
           << "</style>\n";
        if (plot.title_.size() > 0)
        {
            fs << "<text x='" << floor(plot.width_/2)
               << "' y='" << floor(margin_top/2)
               << "' font-family='Palatino' font-size='25' fill='"
               << plot.font_color_  << "'  alignment-baseline='middle' text-anchor='middle'>"
               << plot.title_
               << "</text>\n";
        }

        // Construct SVG group to simplify the calculations slightly:
        fs << "<g transform='translate(" << margin_left << ", " << margin_top << ")'>\n";
            // y-axis:
        fs  << "<line x1='0' y1='0' x2='0' y2='" << graph_height
            << "' stroke='gray' stroke-width='1'/>\n";
        PreciseReal x_axis_loc = y_scale(static_cast<PreciseReal>(0));
        fs << "<line x1='0' y1='" << x_axis_loc
            << "' x2='" << graph_width << "' y2='" << x_axis_loc
            << "' stroke='gray' stroke-width='1'/>\n";

        if (worst_ulp_distance > 3)
        {
            detail::write_gridlines(fs, plot.horizontal_lines_, plot.vertical_lines_, x_scale, y_scale, plot.a_, plot.b_,
                                    min_y, max_y, graph_width, graph_height, margin_left, plot.font_color_);
        }
        else
        {
            std::vector<double> ys{-3.0, -2.5, -2.0, -1.5, -1.0, -0.5, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0};
            for (size_t i = 0; i < ys.size(); ++i)
            {
                if (min_y <= ys[i] && ys[i] <= max_y)
                {
                    PreciseReal y_cord_dataspace = ys[i];
                    PreciseReal y = y_scale(y_cord_dataspace);
                    fs << "<line x1='0' y1='" << y << "' x2='" << graph_width
                       << "' y2='" << y
                       << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

                    fs << "<text x='" <<  -margin_left/2 << "' y='" << y - 3
                       << "' font-family='times' font-size='10' fill='" << plot.font_color_ << "' transform='rotate(-90 "
                       << -margin_left/2 + 7 << " " << y << ")'>"
                       <<  std::setprecision(4) << y_cord_dataspace << "</text>\n";
                }
            }
            for (int i = 1; i <= plot.vertical_lines_; ++i)
            {
                CoarseReal x_cord_dataspace = plot.a_ +  ((plot.b_ - plot.a_)*i)/plot.vertical_lines_;
                CoarseReal x = x_scale(x_cord_dataspace);
                fs << "<line x1='" << x << "' y1='0' x2='" << x
                   << "' y2='" << graph_height
                   << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

                fs << "<text x='" <<  x - 10  << "' y='" << graph_height + 10
                   << "' font-family='times' font-size='10' fill='" << plot.font_color_ << "'>"
                   << std::setprecision(4) << x_cord_dataspace << "</text>\n";
            }
        }

        int color_idx = 0;
        for (auto const & ulp : plot.ulp_list_)
        {
            std::string color = plot.colors_[color_idx++];
            for (size_t j = 0; j < ulp.size(); ++j)
            {
                if (isnan(ulp[j]))
                {
                    if(plot.nan_color_ == "")
                        continue;
                    CoarseReal x = x_scale(plot.coarse_abscissas_[j]);
                    PreciseReal y = y_scale(static_cast<PreciseReal>(plot.clip_));
                    fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << plot.nan_color_ << "'/>\n";
                    y = y_scale(static_cast<PreciseReal>(-plot.clip_));
                    fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << plot.nan_color_ << "'/>\n";
                }
                if (plot.clip_ > 0 && static_cast<PreciseReal>(abs(ulp[j])) > plot.clip_)
                {
                   if (plot.crop_color_ == "")
                      continue;
                   CoarseReal x = x_scale(plot.coarse_abscissas_[j]);
                   PreciseReal y = y_scale(static_cast<PreciseReal>(ulp[j] < 0 ? -plot.clip_ : plot.clip_));
                   fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << plot.crop_color_ << "'/>\n";
                }
                else
                {
                   CoarseReal x = x_scale(plot.coarse_abscissas_[j]);
                   PreciseReal y = y_scale(static_cast<PreciseReal>(ulp[j]));
                   fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << color << "'/>\n";
                }
            }
        }

        if (plot.ulp_envelope_)
        {
            std::string close_path = "' stroke='"  + plot.envelope_color_ + "' stroke-width='1' fill='none'></path>\n";
            size_t jstart = 0;
            while (plot.cond_[jstart] > max_y)
            {
                ++jstart;
                if (jstart >= plot.cond_.size())
                {
                    goto done;
                }
            }

            size_t jmin = jstart;
        new_top_path:
            if (jmin >= plot.cond_.size())
            {
                goto start_bottom_paths;
            }
            fs << "<path d='M" << x_scale(plot.coarse_abscissas_[jmin]) << " " << y_scale(plot.cond_[jmin]);

            for (size_t j = jmin + 1; j < plot.coarse_abscissas_.size(); ++j)
            {
                bool bad = isnan(plot.cond_[j]) || (plot.cond_[j] > max_y);
                if (bad)
                {
                    ++j;
                    while ( (j < plot.coarse_abscissas_.size() - 2) && bad)
                    {
                        bad = isnan(plot.cond_[j]) || (plot.cond_[j] > max_y);
                        ++j;
                    }
                    jmin = j;
                    fs << close_path;
                    goto new_top_path;
                }

                CoarseReal t = x_scale(plot.coarse_abscissas_[j]);
                PreciseReal y = y_scale(plot.cond_[j]);
                fs << " L" << t << " " << y;
            }
            fs << close_path;
        start_bottom_paths:
            jmin = jstart;
        new_bottom_path:
            if (jmin >= plot.cond_.size())
            {
                goto done;
            }
            fs << "<path d='M" << x_scale(plot.coarse_abscissas_[jmin]) << " " << y_scale(-plot.cond_[jmin]);

            for (size_t j = jmin + 1; j < plot.coarse_abscissas_.size(); ++j)
            {
                bool bad = isnan(plot.cond_[j]) || (-plot.cond_[j] < min_y);
                if (bad)
                {
                    ++j;
                    while ( (j < plot.coarse_abscissas_.size() - 2) && bad)
                    {
                        bad = isnan(plot.cond_[j]) || (-plot.cond_[j] < min_y);
                        ++j;
                    }
                    jmin = j;
                    fs << close_path;
                    goto new_bottom_path;
                }
                CoarseReal t = x_scale(plot.coarse_abscissas_[j]);
                PreciseReal y = y_scale(-plot.cond_[j]);
                fs << " L" << t << " " << y;
            }
            fs << close_path;
        }
    done:
        fs << "</g>\n"
           << "</svg>\n";
        return fs;
    }

private:
    std::vector<PreciseReal> precise_abscissas_;
    std::vector<CoarseReal> coarse_abscissas_;
    std::vector<PreciseReal> precise_ordinates_;
    std::vector<PreciseReal> cond_;
    std::list<std::vector<CoarseReal>> ulp_list_;
    std::vector<std::string> colors_;
    CoarseReal a_;
    CoarseReal b_;
    PreciseReal clip_;
    int width_;
    std::string envelope_color_;
    bool ulp_envelope_;
    int horizontal_lines_;
    int vertical_lines_;
    std::string title_;
    std::string background_color_;
    std::string font_color_;
    std::string crop_color_;
    std::string nan_color_;
};

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::envelope_color(std::string const & color)
{
    envelope_color_ = color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::clip(PreciseReal clip)
{
    clip_ = clip;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::width(int width)
{
    width_ = width;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::horizontal_lines(int horizontal_lines)
{
    horizontal_lines_ = horizontal_lines;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::vertical_lines(int vertical_lines)
{
    vertical_lines_ = vertical_lines;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::title(std::string const & title)
{
    title_ = title;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::background_color(std::string const & background_color)
{
    background_color_ = background_color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::font_color(std::string const & font_color)
{
    font_color_ = font_color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::crop_color(std::string const & color)
{
    crop_color_ = color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::nan_color(std::string const & color)
{
    nan_color_ = color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::ulp_envelope(bool write_ulp_envelope)
{
    ulp_envelope_ = write_ulp_envelope;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
void ulps_plot<F, PreciseReal, CoarseReal>::write(std::string const & filename) const
{
    if (!boost::algorithm::ends_with(filename, ".svg"))
    {
        throw std::logic_error("Only svg files are supported at this time.");
    }
    std::ofstream fs(filename);
    fs << *this;
    fs.close();
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>::ulps_plot(F hi_acc_impl, CoarseReal a, CoarseReal b,
             size_t samples, bool perturb_abscissas, int random_seed) : crop_color_("red")
{
    // Use digits10 for this comparison in case the two types have differeing radixes:
    static_assert(std::numeric_limits<PreciseReal>::digits10 >= std::numeric_limits<CoarseReal>::digits10, "PreciseReal must have higher precision that CoarseReal");
    if (samples < 10)
    {
        throw std::domain_error("Must have at least 10 samples, samples = " + std::to_string(samples));
    }
    if (b <= a)
    {
        throw std::domain_error("On interval [a,b], b > a is required.");
    }
    a_ = a;
    b_ = b;

    std::mt19937_64 gen;
    if (random_seed == -1)
    {
        std::random_device rd;
        gen.seed(rd());
    }
    // Boost's uniform_real_distribution can generate quad and multiprecision random numbers; std's cannot:
    boost::random::uniform_real_distribution<PreciseReal> dis(static_cast<PreciseReal>(a), static_cast<PreciseReal>(b));
    precise_abscissas_.resize(samples);
    coarse_abscissas_.resize(samples);

    if (perturb_abscissas)
    {
        for(size_t i = 0; i < samples; ++i)
        {
            precise_abscissas_[i] = dis(gen);
        }
        std::sort(precise_abscissas_.begin(), precise_abscissas_.end());
        for (size_t i = 0; i < samples; ++i)
        {
            coarse_abscissas_[i] = static_cast<CoarseReal>(precise_abscissas_[i]);
        }
    }
    else
    {
        for(size_t i = 0; i < samples; ++i)
        {
            coarse_abscissas_[i] = static_cast<CoarseReal>(dis(gen));
        }
        std::sort(coarse_abscissas_.begin(), coarse_abscissas_.end());
        for (size_t i = 0; i < samples; ++i)
        {
            precise_abscissas_[i] = static_cast<PreciseReal>(coarse_abscissas_[i]);
        }
    }

    precise_ordinates_.resize(samples);
    for (size_t i = 0; i < samples; ++i)
    {
        precise_ordinates_[i] = hi_acc_impl(precise_abscissas_[i]);
    }

    cond_.resize(samples, std::numeric_limits<PreciseReal>::quiet_NaN());
    for (size_t i = 0 ; i < samples; ++i)
    {
        PreciseReal y = precise_ordinates_[i];
        if (y != 0)
        {
            // Maybe cond_ is badly names; should it be half_cond_?
            cond_[i] = boost::math::tools::evaluation_condition_number(hi_acc_impl, precise_abscissas_[i])/2;
            // Half-ULP accuracy is the correctly rounded result, so make sure the envelop doesn't go below this:
            if (cond_[i] < 0.5)
            {
                cond_[i] = 0.5;
            }
        }
        // else leave it as nan.
    }
    clip_ = -1;
    width_ = 1100;
    envelope_color_ = "chartreuse";
    ulp_envelope_ = true;
    horizontal_lines_ = 8;
    vertical_lines_ = 10;
    title_ = "";
    background_color_ = "black";
    font_color_ = "white";
}

template<class F, typename PreciseReal, typename CoarseReal>
template<class G>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::add_fn(G g, std::string const & color)
{
    using std::abs;
    size_t samples = precise_abscissas_.size();
    std::vector<CoarseReal> ulps(samples);
    for (size_t i = 0; i < samples; ++i)
    {
        PreciseReal y_hi_acc = precise_ordinates_[i];
        PreciseReal y_lo_acc = static_cast<PreciseReal>(g(coarse_abscissas_[i]));
        PreciseReal absy = abs(y_hi_acc);
        PreciseReal dist = static_cast<PreciseReal>(nextafter(static_cast<CoarseReal>(absy), std::numeric_limits<CoarseReal>::max()) - static_cast<CoarseReal>(absy));
        ulps[i] = static_cast<CoarseReal>((y_lo_acc - y_hi_acc)/dist);
    }
    ulp_list_.emplace_back(ulps);
    colors_.emplace_back(color);
    return *this;
}

} // namespace boost::math::tools
#endif


/////////////////////////////////////////////////////////////
// Copyright (C) 2003-2006 Bryan Clark and Kenneth Esler   //
//                                                         //
// This program is free software; you can redistribute it  //
// and/or modify it under the terms of the GNU General     //
// Public License as published by the Free Software        //
// Foundation; either version 2 of the License, or         //
// (at your option) any later version.  This program is    //
// distributed in the hope that it will be useful, but     //
// WITHOUT ANY WARRANTY; without even the implied warranty //
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. //
// See the GNU General Public License for more details.    //
// For more information, please see the PIMC++ Home Page:  //
//           http://pathintegrals.info                     //
/////////////////////////////////////////////////////////////

#include "MatrixOps.h"
#include <blitz/mstruct.h>
#include "../config.h"

//#ifdef NOUNDERSCORE
//#define FORT(name) name
//#else
#define FORT(name) name ## _
//#endif

#define F77_DGELS  F77_FUNC(dgels,DGELS)
#define F77_ZGESVD F77_FUNC(zgesvd,ZGESVD)
#define F77_DGESVD F77_FUNC(dgesvd,DGESVD)
#define F77_DGETRF F77_FUNC(dgetrf,DGETRF)
#define F77_ZGETRF F77_FUNC(zgetrf,ZGETRF)
#define F77_DGETRI F77_FUNC(dgetri,DGETRI)
#define F77_ZGETRI F77_FUNC(zgetri,ZGETRI)
#define F77_DGEMM  F77_FUNC(dgemm,DGEMM)
#define F77_ZGEMM  F77_FUNC(zgemm,ZGEMM)
#define F77_DSYEVR F77_FUNC(dsyevr,DSYEVR)
#define F77_ZHEEVR F77_FUNC(zheevr,ZHEEVR)
#define F77_DGEMV  F77_FUNC(dgemv,DGEMV)
#define F77_DPOTRF  F77_FUNC(dpotrf,DPOTRF)

extern "C" void
F77_DGELS (char *transa, int *m, int *n, int *nrhs,
	   double *A, int *LDA, double *B,int *LDB,
	   double *work, int *LDWORK,int *INFO);

extern "C" void
F77_DGESVD (char *JOBU, char* JOBVT, int *M, int *N,
	    double *A, int *LDA, double *S, double *U,
	    int *LDU, double *VT, int *LDVT, double *work,
	    int *LWORK, int *INFO);

extern "C" void
F77_ZGESVD (char *JOBU, char* JOBVT, int *M, int *N,
	    complex<double> *A, int *LDA, double *S, complex<double> *U,
	    int *LDU, complex<double> *VT, int *LDVT, complex<double> *work,
	    int *LWORK, double *work2, int *INFO);

extern "C" void
F77_DGETRF(int *m, int *n, double A[], int *lda, int ipiv[], int *info);

extern "C" void
F77_DPOTRF(char *UPLO, int *n, double A[], int *lda, int *info);

extern "C" void
F77_ZGETRF(int *m, int *n, complex<double> A[],
	   int *lda, int ipiv[], int *info);

extern "C" void
F77_DGETRI (int *N, double A[], int *lda, int ipiv[], double work[],
	    int *lwork, int *info);

extern "C" void
F77_ZGETRI (int *N, complex<double> A[], int *lda, int ipiv[],
	    complex<double> work[], int *lwork, int *info);

extern "C" void
F77_DGEMM (char *transA, char *transB, int *m, int *n, int *k,
	   double *alpha, const double *A, int *lda, const double *B, int *ldb,
	   double *beta,  double *C, int *ldc);

extern "C" void
F77_ZGEMM (char *transA, char *transB, int *m, int *n, int *k,
	   complex<double> *alpha, const complex<double> *A, int *lda, const complex<double> *B,
	   int *ldb, complex<double> *beta,  complex<double> *C, int *ldc);

extern "C" void
F77_DSYEVR (char *JobType, char *Range, char *UpperLower,
	    int *N, double *Amat, int *LDA,
	    double *VL, double *VU,
	    int *IL, int *IU,
	    double *AbsTolerance, int *M,
	    double *EigVals,
	    double *EigVecs, int *LDEigVecs, int *ISuppZ,
	    double *Work, int *Lwork,
	    int *IWorkSpace, int *LIwork,
	    int *Info);

extern "C" void
F77_ZHEEVR (char *JobType, char *Range, char *UpperLower,
	    int *N, complex<double> *Amat, int *LDA,
	    double *VL, double *VU,
	    int *IL, int *IU,
	    double *AbsTolerance, int *M,
	    double *EigVals,
	    complex<double> *EigVecs, int *LDEigVecs,
	    int *ISuppZ,
	    complex<double> *Work, int *Lwork,
	    double *Rwork, int *LRwork,
	    int *IWorkSpace, int *LIwork,
	    int *Info);

const Array<double,2> operator*(const Array<double,2> &A,
				const Array<double,2> &B)
{
  int m = A.rows();
  int n = B.cols();
  int k = A.cols();
  assert (B.rows() == k);
  // We use "transpose" operation because we have C ordering, which fortran
  // thinks is transposed.
  char transA = 'T';
  char transB = 'T';
  double alpha = 1.0;
  double beta = 0.0;
  GeneralArrayStorage<2> colMajor;
  colMajor.ordering() = firstDim, secondDim;
  Array<double,2> C(m,n,colMajor);
  F77_DGEMM (&transA, &transB, &m, &n, &k, &alpha, A.data(), &k,
	     B.data(), &n, &beta, C.data(), &m);
  return C;
}

const Array<complex<double>,2> operator*(const Array<complex<double>,2> &A,
					 const Array<complex<double>,2> &B)
{
  int m = A.rows();
  int n = B.cols();
  int k = A.cols();
  assert (B.rows() == k);
  // We use "transpose" operation because we have C ordering, which fortran
  // thinks is transposed.
  char transA = 'T';
  char transB = 'T';
  complex<double> alpha(1.0, 0.0);
  complex<double> beta(0.0, 0.0);
  GeneralArrayStorage<2> colMajor;
  colMajor.ordering() = firstDim, secondDim;
  Array<complex<double>,2> C(m,n,colMajor);
  F77_ZGEMM (&transA, &transB, &m, &n, &k, &alpha, A.data(), &k,
	     B.data(), &n, &beta, C.data(), &m);
  return C;
}

void MatMult (const Array<double,2> &A, const Array<double,2> &B,
	      Array<double,2> &C)
{
  int m = A.rows();
  int n = B.cols();
  int k = A.cols();
  assert (B.rows() == k);
  // We use "transpose" operation because we have C ordering, which fortran
  // thinks is transposed.
  char transA = 'T';
  char transB = 'T';
  double alpha = 1.0;
  double beta = 0.0;
  GeneralArrayStorage<2> colMajor;
  colMajor.ordering() = firstDim, secondDim;
  F77_DGEMM (&transA, &transB, &m, &n, &k, &alpha, A.data(), &k,
	     B.data(), &n, &beta, C.data(), &m);
}

double
InnerProduct(const Array<double,1> &A,
	     const Array<double,1> &B)
{
  assert(A.size()==B.size());
  double total=0.0;
  for (int counter=0;counter<A.size();counter++)
    total+=A(counter)*B(counter);
  return total;

}

void
OuterProduct(const Array<double,1> &A,
	     const Array<double,1> &B,
	     Array<double,2> &AB)
{
  double total=0.0;
  AB.resize(A.size(),B.size());
  for (int i=0;i<A.size();i++)
    for (int j=0;j<B.size();j++)
      AB(i,j)=A(i)*B(j);
}

//Note that A gets corrupted in this process and b gets returned
//as the answer
void
LinearLeastSquares(Array<double,2> &A, Array<double,1> &x,
		   Array<double,1> &b)
{
  char trans;
  trans='T';
  ///These n and m are "transposed" because of Fortran ordering
  int n=A.rows();
  int m=A.cols();
  ///

  int ldb=b.size();
  //  cerr<<"The value of ldb is "<<ldb<<endl;
  Array<double,1> work(1);
  int ldwork=-1;
  int info=0;
  int nrhs=1;
  F77_DGELS(&trans,&m,&n,&nrhs,A.data(),&m,b.data(),&ldb,work.data(),
	    &ldwork,&info);
  work.resize((int)work(0));
  ldwork=work.size();
  F77_DGELS(&trans,&m,&n,&nrhs,A.data(),&m,b.data(),&ldb,work.data(),
	    &ldwork,&info);

}
void MatMult (const Array<complex<double>,2> &A, const Array<complex<double>,2> &B,
	      Array<complex<double>,2> &C)
{
  int m = A.rows();
  int n = B.cols();
  int k = A.cols();
  assert (B.rows() == k);
  // We use "transpose" operation because we have C ordering, which fortran
  // thinks is transposed.
  char transA = 'T';
  char transB = 'T';
  complex<double> alpha = 1.0;
  complex<double> beta = 0.0;
  GeneralArrayStorage<2> colMajor;
  colMajor.ordering() = firstDim, secondDim;
  F77_ZGEMM (&transA, &transB, &m, &n, &k, &alpha, A.data(), &k,
	     B.data(), &n, &beta, C.data(), &m);
  Transpose(C);
}

double Determinant (const Array<double,2> &A)
{
  int m = A.rows();
  int n = A.cols();
  assert (m == n);  // Cannot take a determinant of a non-square
		    // matrix
  if (A.rows() == 1)
    return (A(0,0));
  if (A.rows() == 2)
    return (A(0,0)*A(1,1)-A(0,1)*A(1,0));
  else {
    Array<double,2> LU(m,m);
    Array<int,1> ipiv(m);
    int info;
    LU = A;
    // Do LU factorization
    F77_DGETRF (&m, &n, LU.data(), &m, ipiv.data(), &info);
    double det = 1.0;
    int numPerm = 0;
    for (int i=0; i<m; i++) {
      det *= LU(i,i);
      numPerm += (ipiv(i) != (i+1));
    }
    if (numPerm & 1)
      det *= -1.0;

    return det;
  }
}

complex<double>
Determinant (const Array<complex<double>,2> &A)
{
  int m = A.rows();
  int n = A.cols();
  assert (m == n);  // Cannot take a determinant of a non-square
		    // matrix
  if (A.rows() == 1)
    return (A(0,0));
  if (A.rows() == 2)
    return (A(0,0)*A(1,1)-A(0,1)*A(1,0));
  else {
    Array<complex<double>,2> LU(m,m);
    Array<int,1> ipiv(m);
    int info;
    LU = A;
    // Do LU factorization
    F77_ZGETRF (&m, &n, LU.data(), &m, ipiv.data(), &info);
    complex<double> det = 1.0;
    int numPerm = 0;
    for (int i=0; i<m; i++) {
      det *= LU(i,i);
      numPerm += (ipiv(i) != (i+1));
    }
    if (numPerm & 1)
      det *= -1.0;

    return det;
  }
}

// Replaces A with its inverse by gauss-jordan elimination with full pivoting
// Adapted from Numerical Recipes in C
double GJInverse (Array<double,2> &A)
{

  const int maxSize = 2000;
  assert (A.cols() == A.rows());
  assert (A.cols() <= maxSize);
  int n = A.rows();

  if (n == 2) { // Special case for 2x2
    double a=A(0,0); double b=A(0,1);
    double c=A(1,0); double d=A(1,1);
    double detInv = 1.0/(a*d-b*c);
    A(0,0) = d*detInv;
    A(0,1) = -b*detInv;
    A(1,0) = -c*detInv;
    A(1,1) = a*detInv;
    return 1.0/detInv;
  }
  double det = 1.0;

  int colIndex[maxSize], rowIndex[maxSize], ipiv[maxSize];
  double big, dum, pivInv, temp;
  int icol, irow;

  for (int j=0; j<n; j++)
    ipiv[j] = -1;

  for (int i=0; i<n; i++) {
    big = 0.0;
    for (int j=0; j<n; j++)
      if (ipiv[j] != 0)
	for (int k=0; k<n; k++) {
	  if (ipiv[k] == -1) {
	    if (fabs(A(j,k)) >= big) {
	      big = fabs(A(j,k));
	      irow = j;
	      icol = k;
	    }
	  }
	  else if (ipiv[k] > 0) {
	    cerr << "GJInverse: Singular matrix!\n";
	    cerr << "A = " << A << endl;
	    abort();
	  }
	}
    ++(ipiv[icol]);

    if (irow != icol)
      for (int l=0; l<n; l++)
	swap (A(irow,l), A(icol,l));

    rowIndex[i] = irow;
    colIndex[i] = icol;
    if (A(icol,icol) == 0.0) {
      cerr << "GJInverse: Singular matrix!\n";
      cerr << "A = " << A << endl;
      abort();
    }
    det *= A(icol,icol);
    pivInv = 1.0/A(icol,icol);
    A(icol,icol) = 1.0;
    for (int l=0; l<n; l++)
      A(icol,l) *= pivInv;
    for (int ll=0; ll<n; ll++)
      if (ll != icol) {
	double dum = A(ll,icol);
	A(ll,icol) = 0.0;
	for (int l=0; l<n; l++)
	  A(ll,l) -= A(icol,l)*dum;
      }
  }
  // Now unscramble the permutations
  for (int l=n-1; l>=0; l--) {
    if (rowIndex[l] != colIndex[l]) {
      for (int k=0; k<n ; k++)
	swap (A(k,rowIndex[l]),A(k, colIndex[l]));
      det *= -1.0;
    }
  }
  return det;
}

double GJInversePartial (Array<double,2> &A)
{
  const int maxSize = 2000;
  assert (A.cols() == A.rows());
  assert (A.cols() <= maxSize);
  int n = A.rows();

  double det = 1.0;

  int colIndex[maxSize], rowIndex[maxSize], ipiv[maxSize];
  double big, dum, pivInv, temp;
  int icol, irow;

  for (int i=0; i<n; i++) {
    big = 0.0;
    int ipiv = 0;
    for (int j=i; j<n; j++)
      if (fabs(A(j,i)) < big) {
	big = fabs(A(j,i));
	ipiv = j;
      }
    // HACK
    // for (int j=0; j<n; j++)
    //   swap (A(i,j), A(ipiv,j));

    //ipiv = i;

    det *= A(ipiv,i);
    double pivInv = 1.0/A(ipiv,i);

    A(ipiv,i) = 1.0;
    for (int j=0; j<n; j++)
      A(ipiv,j) *= pivInv;
    for (int j=0; j<n; j++)
      if (j != ipiv) {
	double tmp = A(j,i);
	A(j,i) = 0.0;
	for (int k=0; k<n; k++)
	  A(j,k) -= A(ipiv,k)*tmp;
      }
  }
  return det;
}

inline void SwapRow (Array<double,2> A, int row1, int row2)
{
  int m = A.cols();
  for (int col=0; col<m; col++) {
    double temp = A(row1,col);
    A(row1,col) = A(row2,col);
    A(row2,col) = temp;
  }
}

// The cofactors of A are given by
// cof(A) = det(A) transpose(A^{-1})
void Cofactors (const Array<double,2> &A,
		Array<double,2> &cof,
		Array<double,2> &scratch)
{
  const int maxSize = 2000;
  int m = A.rows();
  int n = A.cols();
  assert (m == n);  // Cannot take cofactors of a non-square matrix
  assert (A.cols() < maxSize);
  int  ipiv[maxSize];
  int info;
  // Copy and transpose for FORTRAN ordering
  for (int i=0; i<m; i++)
    for (int j=0; j<m; j++)
      scratch(i,j) = A(j,i);
  // Do LU decomposition
  F77_DGETRF (&m, &n, scratch.data(), &m, ipiv, &info);
  // Now scratch contains LU matrix in fortran ordering with pivots in ipiv
  // Put identity matrix in cof
  cof = 0.0;
  for (int i=0; i<m; i++)
    cof(i,i) = 1.0;
  int numPerm = 0;
  // Now apply permutation matrix to cof
  for (int row=0; row<m; row++) {
    int ip = ipiv[row]-1;
    if (ip != row) {
      SwapRow(cof, row, ip);
      numPerm++;
    }
  }
}

void SVdecomp (Array<double,2> &A,
	       Array<double,2> &U, Array<double,1> &S,
	       Array<double,2> &V)
{
  int M = A.rows();
  int N = A.cols();
  Array<double,2> Atrans(M,N);
  // U will be Utrans after lapack call
  U.resize(min(M,N),M);
  V.resize(N,min(M,N));

  S.resize(min(N,M));
  Atrans = A;

  // Transpose U for FORTRAN ordering
  Transpose(Atrans);
  char JOBU  = 'S'; // return min (M,N) columns of U
  char JOBVT = 'S'; // return min (M,N) columns of V
  int LDA = M;
  int LDU = M;
  int LDVT = min(M,N);
  int LWORK = 10 * max(3*min(M,N)+max(M,N),5*min(M,N));
  Array<double,1> WORK(LWORK);
  int INFO;

  F77_DGESVD (&JOBU, &JOBVT, &M, &N, Atrans.data(), &LDA,
	      S.data(), U.data(), &LDU, V.data(), &LDVT,
	      WORK.data(), &LWORK, &INFO);
  assert (INFO == 0);
  // Transpose U to get back to C ordering
  // V was really Vtrans so we don't need to transpose
  Transpose(U);
}

void SVdecomp (Array<complex<double>,2> &A, Array<complex<double>,2> &U,
	       Array<double,1> &S, Array<complex<double>,2> &V)
{
  int M = A.rows();
  int N = A.cols();
  Array<complex<double>,2> Atrans(M,N);
  // U will be Utrans after lapack call
  U.resize(min(M,N),M);
  V.resize(N,min(M,N));

  S.resize(min(N,M));
  Atrans = A;

  // Transpose U for FORTRAN ordering
  Transpose(Atrans);
  char JOBU  = 'S'; // return min (M,N) columns of U
  char JOBVT = 'S'; // return min (M,N) columns of V
  int LDA = M;
  int LDU = M;
  int LDVT = min(M,N);
  int LWORK = 10 * max(3*min(M,N)+max(M,N),5*min(M,N));
  Array<complex<double>,1> WORK(LWORK);
  Array<double,1> WORK2(5*min(M,N));
  int INFO;

  F77_ZGESVD (&JOBU, &JOBVT, &M, &N, Atrans.data(), &LDA,
	      S.data(), U.data(), &LDU, V.data(), &LDVT,
	      WORK.data(), &LWORK, WORK2.data(), &INFO);
  assert (INFO == 0);
  // Transpose U to get back to C ordering
  // V was really Vtrans so we don't need to transpose
  Transpose(U);
}

void PolarOrthogonalize (Array<complex<double>,2> &A)
{
  int M = A.rows();
  int N = A.cols();
  if (M != N) {
    cerr << "Error:  nonsquare matrix in PolarOrthogonalize. Aborting.\n";
    abort();
  }
  Array<complex<double>,2> U, V;
  Array<double,1> S;

  SVdecomp (A, U, S, V);
  Transpose(V);
  for (int i=0; i<V.rows(); i++)
    for (int j=0; j<V.cols(); j++)
      V(i,j) = conj(V(i,j));
  A = U * V;
}

      // Adapted from Numerical Recipes in C
void LUdecomp (Array<double,2> &A, Array<int,1> &perm,
	       double &sign)
{
  int i, imax, j, k;
  int n = A.rows();
  double big, dum, sum, temp;
  Array<double,1> vv(n);
  sign = 1.0;

  perm.resize(n);

  for (i=0; i<n; i++) {
    big = 0.0;
    for (int j=0; j<n; j++)
      if (fabs(A(i,j)) > big) big = fabs(A(i,j));
    if (big == 0.0) {
      cerr << "Singularity in LUdecomp.\n";
      abort();
    }
    vv(i) = 1.0/big;
  }
  for (j=0; j<n; j++) {
    for (i=0; i<j; i++) {
      sum=A(i,j);
      for(k=0; k<i; k++)
	sum -= A(i,k)*A(k,j);
      A(i,j) = sum;
    }
    big=0.0;
    for (i=j; i<n; i++) {
      sum = A(i,j);
      for (k=0; k<j; k++)
	sum-=A(i,k)*A(k,j);
      A(i,j) = sum;
      if ((dum=vv(i)*fabs(sum)) >= big) {
	big = dum;
	imax = i;
      }
    }
    if (j != imax) {
      for (k=0; k<n; k++) {
	dum=A(imax,k);
	A(imax,k) = A(j,k);
	A(j,k) = dum;
      }
      sign = -sign;
      vv(imax) = vv(j);
    }
    perm(j)=imax;
    if (A(j,j) == 0.0)
      A(j,j) = 1.0e-200;
    if (j != (n-1)) {
      dum=1.0/A(j,j);
      for (i=j+1; i<n; i++)
	A(i,j) *= dum;
    }
  }
}

void LUsolve (Array<double,2> &LU, Array<int,1> &perm,
	      Array<double,1> &b)
{
  int i, ii=-1,ip,j;
  double sum;
  int n = LU.rows();

  for (i=0; i<n; i++) {
    ip = perm(i);
    sum = b(ip);
    b(ip) = b(i);
    if (ii>=0)
      for (j=ii; j<i; j++)
	sum -= LU(i,j)*b(j);
    else if (sum)
      ii = i;
    b(i) = sum;
  }
  for (i=n-1; i>=0; i--) {
    sum = b(i);
    for (j=i+1; j<n; j++)
      sum -= LU(i,j)*b(j);
    b(i) = sum/LU(i,i);
  }
}

Array<double,2> Inverse (Array<double,2> &A)
{
  Array<double,2> LU(A.rows(), A.cols()), Ainv(A.rows(), A.cols());
  Array<double,1> col(A.rows());
  Array<int,1> perm;
  double sign;

  LU = A;
  LUdecomp (LU, perm, sign);
  for (int j=0; j<A.rows(); j++) {
    for (int i=0; i<A.rows(); i++)
      col(i) = 0.0;
    col(j) = 1.0;
    LUsolve (LU, perm, col);
    for (int i=0; i<A.rows(); i++)
      Ainv(i,j) = col(i);
  }
  return (Ainv);
}

void SymmEigenPairs (const Array<scalar,2> &A, int NumPairs,
		     Array<scalar,1> &Vals,
		     Array<scalar,2> &Vectors)
{
  char JobType = 'V';    // Find eigenvectors and eignevalues
  char Range   = 'I';    // Find eigenpairs in a range of indices
  char UpperLower = 'U'; // Use upper triagle of A

  int N   = A.rows();
  double *Amat = new double[N*N];
  int LDA = N;
  double VL = 0.0;
  double VU = 0.0;
  int IL = 1;
  int IU = NumPairs;
  double AbsTolerance = -1.0;
  int NumComputed;
  double *EigVals = new double[N];
  double *EigVecs = new double[N*NumPairs];
  int LDEigVecs = N;
  int *ISuppZ = new int[2*NumPairs];
  int Info;

  // First do workspace query
  int Lwork = -1;
  int LIwork = -1;
  double WorkSize;
  int IWorkSize;

   F77_DSYEVR (&JobType, &Range, &UpperLower, &N, Amat, &LDA, &VL, &VU,
	       &IL, &IU, &AbsTolerance, &NumComputed, EigVals, EigVecs,
	       &LDEigVecs, ISuppZ, &WorkSize, &Lwork, &IWorkSize, &LIwork,
	       &Info);

   // Now allocate WorkSpace;
   Lwork = (int) floor(WorkSize+0.5);
   LIwork = IWorkSize;
   double *WorkSpace = new double[Lwork];
   int *IWorkSpace = new int[LIwork];

   // Copy A int Amat
   for (int row=0; row<N; row++)
     for (int col=0; col<N; col++)
       *(Amat+(col*N)+row) = A(row,col);

   F77_DSYEVR (&JobType, &Range, &UpperLower, &N, Amat, &LDA, &VL, &VU,
	       &IL, &IU, &AbsTolerance, &NumComputed, EigVals, EigVecs,
	       &LDEigVecs, ISuppZ, WorkSpace, &Lwork, IWorkSpace, &LIwork,
	       &Info);

   if (Info !=0)
     cerr << "Lapack error in DSYEVR: " << Info << endl;

   // Now copy over output of Vectors and Vals
   Vals.resize(NumPairs);
   Vectors.resize(NumPairs,N);

   for (int i=0; i<NumPairs; i++)
     {
       Vals(i) = *(EigVals+i);
       for (int j=0; j<N; j++)
	 Vectors(i,j) = *(EigVecs+(i*N)+j);
     }

   // Now free allocate memory
   delete[] Amat;
   delete[] EigVals;
   delete[] EigVecs;
   delete[] WorkSpace;
   delete[] IWorkSpace;
   delete[] ISuppZ;
}

void SymmEigenPairs (const Array<complex<double>,2> &A, int NumPairs,
		     Array<scalar,1> &Vals,
		     Array<complex<double>,2> &Vectors)
{
  char JobType = 'V';    // Find eigenvectors and eignevalues
  char Range   = 'I';    // Find eigenpairs in a range of indices
  char UpperLower = 'U'; // Use upper triagle of A

  int N   = A.rows();
  complex<double> *Amat = new complex<double>[N*N];
  int LDA = N;
  double VL = 0.0;
  double VU = 0.0;
  int IL = 1;
  int IU = NumPairs;
  double AbsTolerance = 0.0;
  int NumComputed;
  double *EigVals = new double[N];
  complex<double> *EigVecs = new complex<double>[N*NumPairs];
  int LDEigVecs = N;
  int *ISuppZ = new int[2*NumPairs];
  int Info;

  // First do workspace query
  int Lwork = -1;
  int LIwork = -1;
  int LRwork = -1;
  complex<double> WorkSize;
  double RWorkSize;
  int IWorkSize;

   F77_ZHEEVR(&JobType, &Range, &UpperLower, &N, Amat, &LDA, &VL, &VU,
	      &IL, &IU, &AbsTolerance, &NumComputed, EigVals, EigVecs,
	      &LDEigVecs, ISuppZ, &WorkSize, &Lwork, &RWorkSize, &LRwork,
	      &IWorkSize, &LIwork, &Info);
//    fprintf (stderr, "WorkSize  = %1.8f\n", WorkSize.real());
//    fprintf (stderr, "RWorkSize = %1.8f\n", RWorkSize);
//    fprintf (stderr, "IWorkSize = %d\n", IWorkSize);

   // Now allocate WorkSpace;
   Lwork = (int) floor(WorkSize.real()+0.5);
   LIwork = IWorkSize;
   LRwork = (int) floor (RWorkSize+0.5);
   complex<double> *WorkSpace = new complex<double>[Lwork];
   double * RWorkSpace = new double[LRwork];
   int *IWorkSpace = new int[LIwork];

   // Copy A int Amat
   for (int row=0; row<N; row++)
     for (int col=0; col<N; col++)
       *(Amat+(col*N)+row) = A(row,col);

   F77_ZHEEVR(&JobType, &Range, &UpperLower, &N, Amat, &LDA, &VL, &VU,
	      &IL, &IU, &AbsTolerance, &NumComputed, EigVals, EigVecs,
	      &LDEigVecs, ISuppZ, WorkSpace, &Lwork, RWorkSpace, &LRwork,
	      IWorkSpace, &LIwork, &Info);

   if (Info !=0) {
     fprintf (stderr, "Lapack error in zheevr.  Exitting.\n");
     exit(-1);
   }

   // Now copy over output of Vectors and Vals
   Vals.resize(NumPairs);
   Vectors.resize(NumPairs,N);

   for (int i=0; i<NumPairs; i++) {
     Vals(i) = *(EigVals+i);
     for (int j=0; j<N; j++)
       Vectors(i,j) = *(EigVecs+(i*N)+j);
   }

   // Now free allocate memory
   delete[] Amat;
   delete[] EigVals;
   delete[] EigVecs;
   delete[] WorkSpace;
   delete[] IWorkSpace;
   delete[] ISuppZ;

}

/// This function returns the determinant of A and replaces A with its
/// cofactors.
double
DetCofactors (Array<double,2> &A, Array<double,1> &work)
{
  const int maxN = 2000;
  int ipiv[maxN];
  int N = A.rows();
  int M = A.cols();
  assert (N == M);
  assert (N <= maxN);
  // First, transpose A for fortran ordering
//   for (int i=0; i<N; i++)
//     for (int j=0; j<i; j++) {
//       double tmp = A(i,j);
//       A(i,j) = A(j,i);
//       A(j,i) = tmp;
//     }
  Transpose(A);

  int info;
  // Do LU factorization
  F77_DGETRF (&N, &M, A.data(), &N, ipiv, &info);
  double det = 1.0;
  int numPerm = 0;
  for (int i=0; i<N; i++) {
    det *= A(i,i);
    numPerm += (ipiv[i] != (i+1));
  }
  if (numPerm & 1)
    det *= -1.0;

  int lwork = work.size();
  // Now, do inverse
  F77_DGETRI (&N, A.data(), &N, ipiv, work.data(), &lwork, &info);

  // Now, we have the transpose of Ainv.  Now, just multiply by det:
  A = det * A;
  // And we're done!
  return det;
}

int
DetCofactorsWorksize(int N)
{
  double work;
  double dummy;
  int info;
  int ipiv;
  int lwork = -1;

  F77_DGETRI(&N, &dummy, &N, &ipiv, &work, &lwork, &info);

  return ((int)ceil(work));
}

/// This function returns the determinant of A and replaces A with its
/// cofactors.
complex<double>
ComplexDetCofactors (Array<complex<double>,2> &A,
		     Array<complex<double>,1> &work)
{
  const int maxN = 2000;
  int ipiv[maxN];
  int N = A.rows();
  int M = A.cols();
  assert (N == M);
  assert (N <= maxN);
  // First, transpose A for fortran ordering
//   for (int i=0; i<N; i++)
//     for (int j=0; j<i; j++) {
//       double tmp = A(i,j);
//       A(i,j) = A(j,i);
//       A(j,i) = tmp;
//     }
  Transpose(A);

  int info;
  // Do LU factorization
  F77_ZGETRF (&N, &M, A.data(), &N, ipiv, &info);
  complex<double> det = 1.0;
  int numPerm = 0;
  for (int i=0; i<N; i++) {
    det *= A(i,i);
    numPerm += (ipiv[i] != (i+1));
  }
  if (numPerm & 1)
    det = -det;

  int lwork = work.size();
  // Now, do inverse
  F77_ZGETRI (&N, A.data(), &N, ipiv, work.data(), &lwork, &info);

  // Now, we have the transpose of Ainv.  Now, just multiply by det:
  A = det * A;
  // And we're done!
  return det;
}

int
ComplexDetCofactorsWorksize(int N)
{
  complex<double> work;
  complex<double> dummy;
  int info;
  int ipiv;
  int lwork = -1;

  F77_ZGETRI(&N, &dummy, &N, &ipiv, &work, &lwork, &info);

  return ((int)ceil(work.real()));
}

extern "C" void   F77_DGEMV (char *TRANS, const int *M, const int *N,
			     double *alpha, const void *A,
			     const int *LDA, const void *X,
			     const int *INCX, double *beta,
			     const void *Y, const int *INCY);

void
MatVecProd (Array<double,2> &A, Array<double,1> &x, Array<double,1> &Ax)
{
  assert (A.cols() == x.size());
  assert (A.rows() == Ax.size());

  double zero(0.0);
  double one(1.0);
  char trans = 'T';

  int n = A.rows();
  int m = A.cols();
  int inc = 1;

  F77_DGEMV(&trans, &m, &n, &one, A.data(), &m,
	    x.data(), &inc, &zero, Ax.data(), &inc);
}

void CholeskyBig (Array<double,2> &A)
{
  int n=A.extent(0);
  int lda=A.extent(1);
  int info;
  char upper='L';
  F77_DPOTRF(&upper,&n,A.data(),&lda,&info);
  for (int i=0;i<A.extent(0);i++)
    for (int j=0;j<i;j++)
      A(i,j)=0.0;

}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Scale0InplaceTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <vector>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/tensor.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>
#include <popart/topocons.hpp>

using namespace popart;

BOOST_AUTO_TEST_CASE(Inplace_Scale0) {

  //                 |-- slice [0,3 ], [0,3] - scale 1.1 -|
  //                 |-- slice [3,6 ], [3,6] - scale 2.2 -| - matmul -|
  //                 |
  // in0 (10 x 10) --|
  //

  auto test = [](bool branch77High) {
    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();
    TensorInfo shape0{"FLOAT", std::vector<int64_t>{6, 6}};
    auto in0 = builder->addInputTensor(shape0);

    auto sl11 = aiOnnx.slice({in0}, {6, 3}, {3, 0}, {0, 1});
    auto sl22 = aiOnnx.slice({in0}, {6, 6}, {3, 3}, {0, 1});

    builder->setInplacePreferences(sl11, {{"SliceInplace", 1000.0}});
    builder->setInplacePreferences(sl22, {{"SliceInplace", 1000.0}});

    float factor11 = 1.1 * (2 * branch77High - 1);
    float factor22 = 2.2 * (2 * branch77High - 1);

    auto sc11 = aiGraphcore.scale({sl11}, factor11);
    auto sc22 = aiGraphcore.scale({sl22}, factor22);

    builder->setInplacePreferences(sc11, {{"ScaleInplace", 10.0 + factor11}});
    builder->setInplacePreferences(sc22, {{"ScaleInplace", 10.0 + factor22}});

    auto out = aiOnnx.matmul({sc22, sc11});

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);

    // Create the IR
    auto dataFlow = DataFlow(1, {{out, AnchorReturnType("All")}});

    auto device = createTestDevice(TEST_TARGET);

    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                {},
                nullptr,
                *device,
                {},
                Patterns(PatternsLevel::NoPatterns)
                    .enableRuntimeAsserts(false)
                    .enableInPlace(true)});

    BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::MatMul).size() == 1);
    BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Slice).size() == 0);
    BOOST_CHECK(ir.opsOfType(Onnx::CustomOperators::SliceInplace).size() == 2);
    BOOST_CHECK(ir.opsOfType(Onnx::AiGraphcore::OpSet1::Scale).size() == 0);
    BOOST_CHECK(ir.opsOfType(Onnx::CustomOperators::ScaleInplace).size() == 2);
  };

  test(true);
  test(false);
}


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

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

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

#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/simd/round2even.hpp>
#include <boost/simd/function/simd/toint.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/pio2_1.hpp>
#include <boost/simd/constant/pio2_1t.hpp>
#include <boost/simd/constant/pio2_2.hpp>
#include <boost/simd/constant/pio2_2t.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/simd/constant/pio2_3t.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/minus.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD (rem_pio2_medium_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          , bd::generic_ < bd::integer_<A1>  >
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE void operator() ( A0 const& x, A1 & n, A0 & xr) BOOST_NOEXCEPT
    {
      n = bs::rem_pio2_medium(x, xr);
    }
  };

  BOOST_DISPATCH_OVERLOAD (rem_pio2_medium_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    using int_t = bd::as_integer_t<A0>;
    using result_t = std::pair<int_t, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const
    {
      A0 second;
      int_t const first = bs::rem_pio2_medium(a0,second);
      return {first, second};
    }
  };

  BOOST_DISPATCH_OVERLOAD (rem_pio2_medium_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    using int_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE  int_t operator() ( A0 const& t, A0 & xr) const BOOST_NOEXCEPT
    {
      const A0 fn = round2even(t*Twoopi<A0>());
      A0 r  = t-fn*Pio2_1<A0>();
      A0 w  = fn*Pio2_1t<A0>();
      A0 t2 = r;
      w  = fn*Pio2_2<A0>();
      r  = t2-w;
      w  = fn*Pio2_2t<A0>()-((t2-r)-w);
      t2 = r;
      w  = fn*Pio2_3<A0>();
      r  = t2-w;
      w  = fn*Pio2_3t<A0>()-((t2-r)-w);
      xr = r-w;
      return  bitwise_and(toint(fn), Three<int_t>());
    }
  };
} } }

#endif


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

#include <nt2/trigonometric/functions/atan2.hpp>
#include <nt2/trigonometric/functions/simd/common/impl/invtrig.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_gtz.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/negatenz.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/constants/pi.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/copysign.hpp>
#include <nt2/include/constants/one.hpp>
#endif
#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#include <nt2/include/functions/simd/is_nan.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( atan2_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(const typename A0::native_type a0_n,
                                  const typename A0::native_type a1_n) const
    {
      A0 a0 = a0_n;
      A0 a1 = a1_n;
      typedef typename meta::as_logical<A0>::type lA0;
#ifndef BOOST_SIMD_NO_INFINITIES
      lA0 test =  nt2::logical_and(nt2::is_inf(a0),  nt2::is_inf(a1));
      a0 =  nt2::if_else(test, nt2::copysign(One<A0>(), a0), a0);
      a1 =  nt2::if_else(test, nt2::copysign(One<A0>(), a1), a1);
#endif
      A0 z = details::invtrig_base<result_type,radian_tag, tag::simd_type>::kernel_atan(a0/a1);
      //A0 z = atan(abs(a0/a1));  // case a1 > 0,  a0 > 0
      z = nt2::negatenz(nt2::if_else(nt2::is_gtz(a1), z, nt2::Pi<A0>()-z), a0);
      z =  nt2::if_else( nt2::is_eqz(a0),
                         nt2::if_else_zero( nt2::is_ltz(a1),  nt2::Pi<A0>()),
                         z);
#ifdef BOOST_SIMD_NO_NANS
      return z;
#else
      return  nt2::if_nan_else( nt2::logical_or( nt2::is_nan(a0),  nt2::is_nan(a1)), z);
#endif
    }
  };
} }

#endif


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
// ML Data
#include <core/storage/sframe_data/sframe.hpp>
#include <core/storage/sframe_interface/unity_sframe.hpp>
#include <model_server/lib/variant_deep_serialize.hpp>

#include <core/storage/sframe_data/algorithm.hpp>
#include <ml/ml_data/metadata.hpp>

// Toolkits
#include <toolkits/supervised_learning/supervised_learning.hpp>
#include <toolkits/supervised_learning/supervised_learning_utils-inl.hpp>
#include <toolkits/supervised_learning/linear_svm_opt_interface.hpp>

// CoreML
#include <toolkits/coreml_export/linear_models_exporter.hpp>

// Solvers
#include <ml/optimization/utils.hpp>
#include <ml/optimization/constraints-inl.hpp>
#include <limits>

// Regularizer
#include <ml/optimization/regularizers-inl.hpp>
#include <ml/optimization/lbfgs.hpp>
#include <ml/optimization/newton_method-inl.hpp>
#include <ml/optimization/accelerated_gradient-inl.hpp>

// Utilities
#include <Eigen/SparseCore>
#include <cmath>

namespace turi {
namespace supervised {

/*
 * Linear SVM Model
 * ****************************************************************************
 */

/**
 * Destructor. Make sure bad things don't happen
 */
linear_svm::~linear_svm(){
}

/**
 * SVM supervised_learning init.
 */
void linear_svm::model_specific_init(const ml_data& data,
                                     const ml_data& valid_data) {

  if (ml_mdata->target_index_size() != 2){
    std::stringstream ss;
    ss << "Linear SVM currently only supports binary classification. "
       << "Use the boosted_trees_classifier for multi-class classification."
       << std::endl;
    log_and_throw(ss.str());
  }

  // Count the number of variables
  // --------------------------------------------------------------------------
  // Create an interface to the solver.
  size_t variables = get_number_of_coefficients(ml_mdata);

  // Examples per class
  state["num_classes"] = 2;
  state["num_examples_per_class"] =
                    to_variant(get_num_examples_per_class(ml_mdata));
  state["num_coefficients"] = variables;

  // Create an interface to the solver.
  // --------------------------------------------------------------------------
  scaled_logistic_svm_interface.reset(new
      linear_svm_scaled_logistic_opt_interface(data, valid_data, *this));
  coefs = DenseVector::Zero(variables);
}

/**
 * Set options
 */
void linear_svm::init_options(const std::map<std::string,
    flexible_type>&_opts){

  options.create_real_option(
      "convergence_threshold",
      "Convergence threshold for training",
      0.01,
      optimization::OPTIMIZATION_ZERO,
      optimization::OPTIMIZATION_INFTY,
      false);

  options.create_integer_option(
      "max_iterations",
      "Maximum number of iterations to perform during training",
      10,
      1,
      std::numeric_limits<int>::max(),
      false);

  options.create_categorical_option(
      "solver",
      "Solver used for training",
      "auto",
      {flexible_type("auto"), flexible_type("lbfgs")},
      false);

  options.create_real_option(
      "penalty",
      "Penalty on the mis-classification loss",
      1.0,
      optimization::OPTIMIZATION_ZERO,
      optimization::OPTIMIZATION_INFTY,
      false);

  options.create_integer_option(
      "lbfgs_memory_level",
      "Number of previous iterations to cache for LBFGS",
      11,
      1,
      std::numeric_limits<int>::max(),
      false);

  options.create_boolean_option(
      "feature_rescaling",
      "Rescale features to have unit L2-Norm",
      true,
      false);

  options.create_flexible_type_option(
      "class_weights",
      "Weights (during training) assigned to each class.",
      flex_undefined(),
      true);

  options.create_boolean_option(
      "disable_posttrain_evaluation",
      "Disable automatic computation of an evaluation report following training.",
      false,
      false);

  // Set options!
  options.set_options(_opts);
  add_or_update_state(flexmap_to_varmap(options.current_option_values()));

}

/**
 * Train a linear svm model.
 */
void linear_svm::train() {
  DASSERT_TRUE(scaled_logistic_svm_interface->num_variables() >= 0);
  if(get_option_value("feature_rescaling")){
    scaled_logistic_svm_interface->init_feature_rescaling();
  }

  // Set class weights
  flexible_type class_weights =
                      get_class_weights_from_options(options, ml_mdata);
  state["class_weights"] =  to_variant(class_weights);
  size_t i = 0;
  flex_dict _class_weights(variant_get_value<size_t>(state.at("num_classes")));
  for(const auto& kvp: class_weights.get<flex_dict>()){
    _class_weights[i++] =
        {ml_mdata->target_indexer()->immutable_map_value_to_index(kvp.first),
         kvp.second.get<flex_float>()};
  }
  scaled_logistic_svm_interface->set_class_weights(_class_weights);

  display_classifier_training_summary("SVM");
  size_t variables =  variant_get_value<size_t>(state.at("num_coefficients"));
  logprogress_stream << "Number of coefficients    : " << variables << std::endl;

  // Set the initial point and write initial output to screen
  // ---------------------------------------------------------------------------
  DenseVector init_point(variables);
  init_point.setZero();

  // Box constriants for L1 loss SVM
  float penalty = options.value("penalty");
  std::string solver = options.value("solver");

  // Auto solver
  if(solver == "auto"){
    solver = "lbfgs";
  }
  this->set_options({{"solver", solver}});

  // Call the solvers
  // ---------------------------------------------------------------------------
  optimization::solver_return stats;
  DenseVector is_regularized(variables);
  is_regularized.setOnes();
  is_regularized(variables - 1) = 0;

  // Set the regularization penalty
  DenseVector beta = is_regularized * 0.5 / penalty;
  std::shared_ptr<optimization::smooth_regularizer_interface> smooth_reg;
  smooth_reg.reset(new optimization::l2_norm(beta));

  // Make a copy of the options to use as solver options.
  std::map<std::string, flexible_type> solver_opts
    = options.current_option_values();
  if (solver == "lbfgs") {
    stats = turi::optimization::lbfgs_compat(scaled_logistic_svm_interface, init_point,
        solver_opts, smooth_reg);
  } else {
    std::ostringstream msg;
    msg << "Solver " << solver << " is not supported." << std::endl;
    msg << "Supported solvers are (auto, lbfgs)" << std::endl;
    log_and_throw(msg.str());
  }

  // Save final accuracies
  if(scaled_logistic_svm_interface->num_validation_examples() > 0) {
    state["validation_accuracy"] = scaled_logistic_svm_interface->get_validation_accuracy();
  }
  state["training_accuracy"] = scaled_logistic_svm_interface->get_training_accuracy();

  // Store the coefficients in the model
  // ---------------------------------------------------------------------------
  coefs = stats.solution;
  scaled_logistic_svm_interface->rescale_solution(coefs);
  sframe sf_coef = get_coefficients_as_sframe(coefs, ml_mdata);
  std::shared_ptr<unity_sframe> unity_coef = std::make_shared<unity_sframe>();
  unity_coef->construct_from_sframe(sf_coef);
  state["coefficients"] = to_variant(unity_coef);

  // Copy the training stats into the model
  // ---------------------------------------------------------------------------
  state["training_iterations"] = stats.iters;
  state["training_time"] = stats.solve_time;
  state["training_loss"] = stats.func_value;  // minimized negative log-likelihood
  state["training_solver_status"] = (std::string)
    translate_solver_status(stats.status);

  // Store progress table.
  std::shared_ptr<unity_sframe> unity_progress = std::make_shared<unity_sframe>();
  unity_progress->construct_from_sframe(stats.progress_table);
  state["progress"] = to_variant(unity_progress);

  smooth_reg.reset();

}

/**
 * Predict for a single example.
 */
flexible_type linear_svm::predict_single_example(
         const DenseVector& x,
         const prediction_type_enum& output_type){
  double margin = x.dot(coefs);
  switch (output_type) {
    // Margin
    case prediction_type_enum::MARGIN:
      return margin;

    // Class Index
    case prediction_type_enum::CLASS_INDEX:
      return (margin >= 0.0);

    // Class
    case prediction_type_enum::NA:
    case prediction_type_enum::CLASS:
    {
      size_t class_id = (margin >= 0.0);
      return  ml_mdata->target_indexer()->map_index_to_value(class_id);
    }

    // Not supported types
    case prediction_type_enum::PROBABILITY:
    case prediction_type_enum::MAX_PROBABILITY:
    case prediction_type_enum::RANK:
    case prediction_type_enum::PROBABILITY_VECTOR:
      log_and_throw("Output type not supported.");

  }

  log_and_throw(std::string("Configuration not supported"));
  ASSERT_UNREACHABLE();
}

/**
 * Predict for a single example.
 */
flexible_type linear_svm::predict_single_example(
         const SparseVector& x,
         const prediction_type_enum& output_type){
  double margin = x.dot(coefs);
  switch (output_type) {
    // Margin
    case prediction_type_enum::MARGIN:
      return margin;

    // Class Index
      case prediction_type_enum::CLASS_INDEX:
      return (margin >= 0.0);
    // Class
    case prediction_type_enum::NA:
    case prediction_type_enum::CLASS:
    {
      size_t class_id = (margin >= 0.0);
      return  ml_mdata->target_indexer()->map_index_to_value(class_id);
    }

    // Not supported
    case prediction_type_enum::PROBABILITY:
    case prediction_type_enum::MAX_PROBABILITY:
    case prediction_type_enum::RANK:
    case prediction_type_enum::PROBABILITY_VECTOR:
      log_and_throw("Output type not supported.");
  }

  log_and_throw(std::string("Configuration not supported"));
  ASSERT_UNREACHABLE();
}

/**
 * Classify.
 */
sframe linear_svm::classify(const ml_data& test_data,
                            const std::string& output_type){
  sframe sf_class;
  sf_class = sf_class.add_column(predict(test_data, "class"), "class");
  return sf_class;
}

/**
 * Fast Classify.
 */
gl_sframe linear_svm::fast_classify(
    const std::vector<flexible_type>& rows,
    const std::string& missing_value_action) {
  // Class predictions
  gl_sframe sf_class;
  sf_class.add_column(fast_predict(rows, missing_value_action, "class"),
		      "class");
  return sf_class;
}

/**
 * Turi Serialization Save
 */
void linear_svm::save_impl(turi::oarchive& oarc) const {

  // State
  variant_deep_save(state, oarc);

  // Everything else
  oarc << ml_mdata
       << metrics
       << coefs
       << options;

}

/**
 * Setter for coefficients vector.
 */
void linear_svm::set_coefs(const DenseVector& _coefs) {
  coefs = _coefs;
}

/**
 * Turi Serialization Load
 */
void linear_svm::load_version(turi::iarchive& iarc, size_t version) {
  ASSERT_MSG(version <= SVM_MODEL_VERSION,
        "This model version cannot be loaded. Please re-save your model.");
  if (version < 5) {
    log_and_throw("Cannot load a model saved using a version prior to GLC-1.7.");
  }

  // State
  variant_deep_load(state, iarc);

  // Everything else
  // GLC 1.3-
  iarc >> ml_mdata
       >> metrics
       >> coefs
       >> options;

  // GLC 1.7
  if (version < 5) {
    tracking_metrics = metrics;
    this->set_default_evaluation_metric();
  }
}

/**
 * Gets the model version number
 */
size_t linear_svm::get_version() const{
  // Version translator
  // -----------------------
  //  0 - Version 1.0
  //  1 - Version 1.1
  //  2 - Version 1.3
  //  3 - Version 1.4
  //  4 - Version 1.5
  //  5 - Version 1.7
  return SVM_MODEL_VERSION;
}

std::shared_ptr<coreml::MLModelWrapper> linear_svm::export_to_coreml() {

  std::map<std::string, flexible_type> context = {
    {"model_type", "linear_svm"},
    {"version", std::to_string(get_version())},
    {"class", name()},
    {"short_description", "Linear SVM Model."}};

  return export_linear_svm_as_model_asset(ml_mdata, coefs, context);
}

} // supervised
} // turicreate


#include <iostream>
#include <boost/format.hpp>
// Qt headers
#include <QtGui>
#include <CGAL/Qt/GraphicsViewNavigation.h>
#include <QLineF>
#include <QRectF>
#include <QApplication>
#include <QGraphicsScene>
#include <QGraphicsView>
// CGAL headers
#include <CGAL/Qt/PointsGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/utility.h>
// GraphicsView items and event filters (input classes)
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Largest_empty_iso_rectangle_2.h>
// the two base classes
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsRectItem>
#include <QGraphicsLineItem>
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Vector_2 Vector_2;
typedef K::Segment_2 Segment_2;
using namespace std;

int main(int argc, char **argv)
{
	CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> > * pgi;
	std::vector<Point_2> points;

	QApplication app(argc, argv);
	QGraphicsScene scene;

	FILE *fp = NULL;
	fp = fopen("../src/input.txt","r");
	int startx,starty,endx,endy;
	while(fscanf(fp,"%d %d %d %d",&startx,&starty,&endx,&endy)!=EOF){

		scene.addLine(QLineF(startx,starty,endx,endy));

	}

    scene.setSceneRect(0,0, 100, 100);

    QGraphicsView* view = new QGraphicsView(&scene);
    CGAL::Qt::GraphicsViewNavigation navigation;
    view->installEventFilter(&navigation);
    view->viewport()->installEventFilter(&navigation);
    view->setRenderHint(QPainter::Antialiasing);

    view->show();

    return app.exec();
}


/*******************************************************************************
* Copyright 2019 HITROS 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.
*******************************************************************************/

/* Author: Yu Fu */

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float64MultiArray.h>
#include <open_manipulator_msgs/SetKinematicsPose.h>
#include <open_manipulator_msgs/SetJointPosition.h>
#include <open_manipulator_msgs/KinematicsPose.h>
#include <open_manipulator_msgs/JointPosition.h>
#include <geometry_msgs/Pose.h>
#include <vector>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>
#include <Eigen/Eigen>
#include <omtb_control/omtb_kinematics.h>

using namespace std;

geometry_msgs::Pose get_pose(float position[])
{
  geometry_msgs::Pose object_pose;
  object_pose.position.x = position[0];
  object_pose.position.y = position[1];
  object_pose.position.z = position[2];
  float dist = sqrt((object_pose.position.x * object_pose.position.x) +
                   (object_pose.position.y * object_pose.position.y));
  float roll = 0.0;
  float pitch = 0.0;
  float yaw = 0.0;
  if (object_pose.position.y > 0)
      yaw = acos(object_pose.position.x / dist);
  else
      yaw = (-1) * acos(object_pose.position.x / dist);
  float cy = cos(yaw * 0.5);
  float sy = sin(yaw * 0.5);
  float cr = cos(roll * 0.5);
  float sr = sin(roll * 0.5);
  float cp = cos(pitch * 0.5);
  float sp = sin(pitch * 0.5);

  object_pose.orientation.w = cy * cr * cp + sy * sr * sp;
  object_pose.orientation.x = cy * sr * cp - sy * cr * sp;
  object_pose.orientation.y = cy * cr * sp + sy * sr * cp;
  object_pose.orientation.z = sy * cr * cp - cy * sr * sp;
  return object_pose;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "omtb_moveit");
  ros::NodeHandle node_handle("");
//  std::string  planning_group = argv[1];
//  std::string service_name;
//  service_name << planning_group << '/moveit/set_kinematics_pose';

  ros::ServiceClient arm_client = node_handle.serviceClient<open_manipulator_msgs::SetJointPosition>("robot1/arm/moveit/set_joint_position");
  ros::ServiceClient gripper_client = node_handle.serviceClient<open_manipulator_msgs::SetJointPosition>("robot1/gripper");

  static tf::TransformListener listener;
  tf::StampedTransform transform;
  try
  {
    listener.waitForTransform("robot1/odom", "robot1/link1", ros::Time(0), ros::Duration(5.0));
    listener.lookupTransform("robot1/odom", "robot1/link1", ros::Time(0), transform);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }
  Eigen::Quaterniond link1_to_world_q(transform.getRotation().getW(), transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
  Eigen::Isometry3d link1_to_world(link1_to_world_q);
  link1_to_world.pretranslate( Eigen::Vector3d( transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()));
  Eigen::Isometry3d end_to_world=Eigen::Isometry3d::Identity();
  float position1[3] = {0.161, 0.1, 0.200};
  end_to_world.pretranslate(Eigen::Vector3d(position1[0], position1[1], position1[2]));
  Eigen::Isometry3d end_to_link1 = end_to_world*link1_to_world.inverse();
  Eigen::Vector3d goal_to_link1_vector = end_to_link1.translation();
  std::vector<double> goal_vector;
  goal_vector.push_back(goal_to_link1_vector(0));
  goal_vector.push_back(goal_to_link1_vector(1));
  goal_vector.push_back(goal_to_link1_vector(2));
  OmtbKinematics solver;
  if (solver.inverse_kinematics(goal_vector))
  {
    open_manipulator_msgs::SetJointPosition arm_test;
  //  float position1[4] = {0.1, 0.5, 0.1 -0.6};

    open_manipulator_msgs::JointPosition arm_joint;
    arm_joint.position = solver.joint_position_;
    arm_joint.max_velocity_scaling_factor = 0.1;
    arm_joint.max_accelerations_scaling_factor = 0.1;

    arm_test.request.joint_position = arm_joint;
    arm_test.request.planning_group = "arm";

    if (arm_client.call(arm_test)) {
        if (arm_test.response.is_planned)
        {
          cout<<"success"<<endl;
        }
        else
        {
          cout<<"fail"<<endl;
        }
    }
    else {
        ROS_ERROR("Failed to call service!!");
        return 1;
    }
  }
  ros::Duration(2.0).sleep();
  open_manipulator_msgs::SetJointPosition gripper_test;
  open_manipulator_msgs::JointPosition gripper_joint;
  gripper_joint.position = {0.15};
  gripper_joint.max_velocity_scaling_factor = 0.1;
  gripper_joint.max_accelerations_scaling_factor = 0.1;
  gripper_test.request.planning_group = "arm";
  gripper_test.request.joint_position = gripper_joint;
  if (gripper_client.call(gripper_test)) {
      if (gripper_test.response.is_planned)
      {
        cout<<"success"<<endl;
      }
      else
      {
        cout<<"fail"<<endl;
      }
  }
  else {
      ROS_ERROR("Failed to call service!!");
      return 1;
  }
}


#include <mps/planner/pushing/oracle/HumanOracle.h>
#include <boost/math/constants/constants.hpp>
#include <mps/planner/util/Math.h>
#include <mps/planner/util/Logging.h>

using namespace mps::planner::pushing::oracle;
namespace b_math = boost::math;
namespace mps_logging = mps::planner::util::logging;

HumanOracle::Parameters::Parameters() {
    pushability_covariance.setIdentity();
    pushability_covariance(0, 0) = 0.1;
    pushability_covariance(1, 1) = 0.1;
    pushability_covariance(2, 2) = 0.78;
    optimal_push_distance = 0.2;
    push_distance_tolerance = 0.04;
    push_angle_tolerance = 0.03;
}

void HumanOracle::Parameters::computeInverses() {
    _inv_pushability_covariance = pushability_covariance.inverse();
}

HumanOracle::HumanOracle(RobotOraclePtr robot_oracle,
                         const std::vector<ObjectData>& object_data,
                         const Parameters& params) :
        _params(params),
        _robot_steerer(robot_oracle)
{
    _params.computeInverses();
    _rng = mps::planner::util::random::getDefaultRandomGenerator();
    _object_data = object_data;
}

HumanOracle::~HumanOracle() = default;

float HumanOracle::predictPushability(const Eigen::VectorXf &current_obj_state,
                                      const Eigen::VectorXf &next_obj_state,
                                      const unsigned int& obj_id) {
    assert(current_obj_state.size() == 3);
    assert(current_obj_state.size() == next_obj_state.size());
    // todo we assume object state is in SE(2)
    Eigen::Vector3f rel_se2(relativeSE2(next_obj_state, current_obj_state));
    // TODO maybe it's better to make this a pure function of the distance
    float maha_dist = std::sqrt(rel_se2.transpose().dot(_params._inv_pushability_covariance * rel_se2));
    if (maha_dist == 0.0) return std::numeric_limits<float>::max();
    return 1.0f / maha_dist;
}

void HumanOracle::projectToPushability(const Eigen::VectorXf& current_obj_state,
                                       const Eigen::VectorXf& next_obj_state,
                                       const float& num_std,
                                       const unsigned int& obj_id,
                                       Eigen::VectorXf& output)
{
    static const std::string log_prefix("[mps::planner::oracle::HumanOracle::projectToPushability]");
    assert(current_obj_state.size() == 3);
    assert(next_obj_state.size() == 3);
    mps_logging::logDebug(boost::format("Projecting state %1% given current object state %2%") % next_obj_state.transpose() % current_obj_state.transpose(), log_prefix);
    output.conservativeResize(3);
    Eigen::Vector3f rel_se2(relativeSE2(next_obj_state, current_obj_state));
    // TODO Do we need to treat R^2 and SO(2) separately here?
    float maha_dist = std::sqrt(rel_se2.transpose().dot(_params._inv_pushability_covariance * rel_se2));
    if (maha_dist == 0.0f) {
        output = current_obj_state;
    }
    if (maha_dist > num_std) { // only do sth if the next object state is further away than num_std standard deviations
        for (unsigned int i = 0; i < 3; ++i) {
            output[i] = current_obj_state[i] + num_std * 1.0f / maha_dist * rel_se2[i];
        }
        mps::planner::util::math::normalize_orientation(output[2]);
    } else {
        output = next_obj_state;
    }
    mps_logging::logDebug(boost::format("Output state is %1%") % output.transpose(), log_prefix);
}

float HumanOracle::predictFeasibility(const Eigen::VectorXf &current_robot_state,
                                      const Eigen::VectorXf &current_obj_state,
                                      const Eigen::VectorXf &next_obj_state,
                                      const unsigned int& obj_id)
{
    assert(current_obj_state.size() == 3 && next_obj_state.size() == 3);
    Eigen::Vector3f obj_diff(relativeSE2(next_obj_state, current_obj_state));
    Eigen::Vector3f rel_robot(relativeSE2(current_robot_state, current_obj_state));
    Eigen::Vector2f push_dir(obj_diff.head(2));
    Eigen::Vector2f approach_dir(-rel_robot.head(2));
    float robot_dist = approach_dir.norm();
    float object_dist_change = push_dir.norm();
    if (robot_dist == 0.0f)
    {
        // the robot is already colliding with the object, this is an infeasible state anyways
        return 0.0f;
    }
    float approach_angle;
    if (object_dist_change == 0.0f) {
        // we do not need to move the object, but maybe reorient it, as a simple heuristic set the approach angle to 0.0
        approach_angle = 0.0f;
    } else {
        approach_angle = std::acos(approach_dir.dot(push_dir) / (robot_dist * object_dist_change));
    }
    float args = std::pow(robot_dist - _params.optimal_push_distance, 2.0f) /
                 (2.0f * std::pow(_params.push_distance_tolerance, 2.0f));
    float distance_feasibility = std::exp(-args);
    args = std::pow(approach_angle, 2.0f) /
           (2.0f * std::pow(_params.push_angle_tolerance, 2.0f));
    float angle_feasibility = std::exp(-args);
    return distance_feasibility * angle_feasibility;
}

void HumanOracle::predictAction(const Eigen::VectorXf &current_robot_state,
                                const Eigen::VectorXf &current_obj_state,
                                const Eigen::VectorXf &next_obj_state,
                                const unsigned int& obj_id,
                                Eigen::VectorXf &control)
{
    static const std::string log_prefix("[HumanOracle::predictAction]");
    Eigen::Vector3f rel_obj(relativeSE2(next_obj_state, current_obj_state));
    Eigen::Vector3f next_robot_state(next_obj_state[0], next_obj_state[1], current_robot_state[2]);
    if (rel_obj.head(2).norm() != 0.0) {
        Eigen::Vector2f heading = rel_obj.head(2) / rel_obj.head(2).norm();
        // we essentially move the robot as far as the next object state is away from the current object state.
        // since the current robot state must be with some offset to the object, we will not overshoot
        // Move robot to object goal state, but subtract half of object size
        float obj_size = fmax(_object_data[obj_id].width, _object_data[obj_id].height);
        next_robot_state.head(2) -= heading * obj_size / 2.0;
        std::vector<Eigen::VectorXf> controls;
        _robot_steerer->steer(current_robot_state, next_robot_state, controls);
        assert(not controls.empty());
        control = controls.at(0);
    } else {
        mps_logging::logWarn("HumanOracle was asked to provide an action for non-translational push. "
                             "This oracle is not capable of dealing with this, returning null action", log_prefix);
        control.setZero(5);
    }
}

void HumanOracle::sampleFeasibleState(const Eigen::VectorXf &current_obj_state,
                                      const Eigen::VectorXf &next_obj_state,
                                      const unsigned int& obj_id,
                                      Eigen::VectorXf &new_robot_state)
{
    double pushing_dist = _rng->gaussian(_params.optimal_push_distance, _params.push_distance_tolerance);
    double angle = _rng->gaussian(0.0, _params.push_angle_tolerance);
    Eigen::Vector3f rel_obj(relativeSE2(next_obj_state, current_obj_state));
    if (rel_obj.norm() == 0.0) {
        rel_obj[0] = 0.0001 * (0.5f - _rng->uniform01());
        rel_obj[1] = 0.0001 * (0.5f - _rng->uniform01());
    }
    Eigen::Vector2f pushing_dir = rel_obj.head(2).normalized();
    Eigen::Rotation2Df rotation((float)angle);
    if (new_robot_state.size() < 3) new_robot_state.resize(3);
    // the idea is to be offset from the current_obj config by pushing_dist and angle
    new_robot_state.head(2) = current_obj_state.head(2);
    Eigen::Vector2f pushing_offset = pushing_dist * rotation.toRotationMatrix() * pushing_dir;
    new_robot_state.head(2) -= pushing_offset.head(2);
    // orient the robot so that it faces into the desired pushing direction
    new_robot_state[2] = std::atan2(pushing_dir[1], pushing_dir[0]) - 1.57f; // TODO the -1.57 is specific to the floating end-effector we use
}

float HumanOracle::getMaximalPushingDistance() const {
    return std::sqrt(2.0f * _params.pushability_covariance(0,0) + 2.0f * _params.pushability_covariance(1,1))
            + 0.01f * std::sqrt(_params.pushability_covariance(2, 2));
}

Eigen::Vector3f HumanOracle::relativeSE2(const Eigen::VectorXf& state, const Eigen::VectorXf& ref) {
    assert(state.size() >= 3);
    assert(ref.size() >= 3);
    Eigen::Vector3f dir(state[0] - ref[0], state[1] - ref[1], state[2] - ref[2]);
    dir[2] = mps::planner::util::math::shortest_direction_so2(ref[2], state[2]);
    return dir;
}


#include <iostream>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using namespace std;

int main()
{
	MatrixXd Pborg(3,1);
	Pborg << 0,
			 2,
			 0;
	cout << Pborg << endl;
	MatrixXd Pb(3,1);
	Pb << 2,
		  2,
		  2;
	cout << Pb << endl;
	cout << "\n\n Ponto final" << endl;
	MatrixXd Pa(3,1);
	Pa << Pborg + Pb;
	cout << Pa;
}


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

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_ILOGB_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_ILOGB_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( ilogb_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int_<A0>, bs::sse_>
                         )
  {
    using result = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0)
    {
      using vtype = bd::as_integer_t<A0,unsigned>;
      return simd::bitwise_cast<A0>(if_zero_else( is_lez(a0)
                                                , ilogb(simd::bitwise_cast<vtype>(a0))
                                                )
                                   );
    }
  };
  BOOST_DISPATCH_OVERLOAD ( ilogb_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( const A0 & a0)
    {
#define MKN8 simd::bitwise_cast<A0>
      const A0 mask =  Constant<A0, 0x7f>();
      A0 n = a0;
      A0 i = One<A0>();
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      n = bitwise_and(MKN8(_mm_srli_epi16(n, 1)), mask);
      i = if_plus(is_nez(n), i, One<A0>());
      return dec(i);
    }
#undef MKN8
  };

  BOOST_DISPATCH_OVERLOAD ( ilogb_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                          )
  {
    using result = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0)
    {
      auto s0 = split(a0);
      return bitwise_cast<result>(group(ilogb(s0[0]), ilogb(s0[1])));
    }
  };
  BOOST_DISPATCH_OVERLOAD ( ilogb_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::unsigned_<A0>, bs::sse_>
                          )
  {
    using result = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0)
    {
      return simd::bitwise_cast<A0>(bs::exponent(tofloat(a0)));
    }
  };
} } }

#endif


// copyright (c) Jacob N. Smith; leave this here; use at your whim
#include <cctbx/boost_python/flex_fwd.h>
#include <cctbx/maptbx/coordinate_transformers.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>

namespace {
  typedef
    cctbx::maptbx::transform<
      cctbx::cartesian<double>,
      cctbx::grid_point<signed long> >
        tf_c_g;
  typedef
    cctbx::maptbx::transform<
      cctbx::grid_point<signed long>,
      cctbx::cartesian<double> >
        tf_g_c;
}
SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(tf_c_g)
SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(tf_g_c)

namespace cctbx { namespace maptbx { namespace boost_python {

namespace {

struct transform_coordinate_wrappers {

  typedef scitbx::mat3<double>              matrix3;

  typedef cctbx::fractional<double>                fractional;
  typedef cctbx::grid_point<signed long>           grid_point;
  typedef cctbx::cartesian<double>                 cartesian;
  typedef af::tiny<signed long,dimension_3>        extents;

  typedef transform<fractional,fractional>        frac2frac;
  typedef transform<fractional,cartesian>          frac2cart;
  typedef transform<fractional,grid_point>        frac2grid;

  typedef transform<cartesian,fractional>          cart2frac;
  typedef transform<cartesian,cartesian>          cart2cart;
  typedef transform<cartesian,grid_point>          cart2grid;

  typedef transform<grid_point,fractional>        grid2frac;
  typedef transform<grid_point,cartesian>          grid2cart;
  typedef transform<grid_point,grid_point>        grid2grid;

  static void wrap() {

    using namespace boost::python;

    class_<frac2frac>("frac2frac",init<>())
      .def("__call__",&frac2frac::operator(),arg("coordinate"))
      .def("inverse",&frac2frac::inverse);

    class_<frac2cart>("frac2cart",no_init)
      .def(init<matrix3>())
      .def("__call__",&frac2cart::operator(),arg("coordinate"))
      .def("inverse",&frac2cart::inverse);

    class_<frac2grid>("frac2grid",no_init)
      .def(init<extents>())
      .def("__call__",&frac2grid::operator(),arg("coordinate"))
      .def("fractional_transform",&frac2grid::strange_transform,arg("coordinate"))
      .def("floor_transform",&frac2grid::floor_transform,arg("coordinate"))
      .def("inverse",&frac2grid::inverse);

    class_<cart2frac>("cart2frac",no_init)
      .def(init<matrix3>())
      .def("__call__",&cart2frac::operator(),arg("coordinate"))
      .def("inverse",&cart2frac::inverse);

    class_<cart2cart>("cart2cart",init<>())
      .def("__call__",&cart2cart::operator(),arg("coordinate"))
      .def("inverse",&cart2cart::inverse);

    class_<cart2grid>("cart2grid",no_init)
      .def(init<matrix3,extents>())
      .def("__call__",&cart2grid::operator(),arg("coordinate"))
      .def("inverse",&cart2grid::inverse);

    class_<grid2frac>("grid2frac",no_init)
      .def(init<extents>())
      .def("__call__",&grid2frac::operator(),arg("coordinate"))
      .def("inverse",&grid2frac::inverse);

    class_<grid2cart>("grid2cart",no_init)
      .def(init<extents,matrix3>())
      .def("__call__",&grid2cart::operator(),arg("coordinate"))
      .def("inverse",&grid2cart::inverse);

    class_<grid2grid>("grid2grid",init<>())
      .def("__call__",&grid2grid::operator(),arg("coordinate"))
      .def("inverse",&grid2grid::inverse);

  }
};

} // namespace <anoymous>

void wrap_coordinate_transformers () {
  transform_coordinate_wrappers::wrap();
}

}}} // namespace cctbx::maptbx::boost_python