#include <algorithm>
#include <cstdint>
#include <iostream>
#include <stdexcept>
#include <string>

#ifndef CPPDEBUG /* Ubuntu's Boost does not provide binaries compatible with libstdc++'s debug mode so we just reduce functionality here */
#include <boost/program_options.hpp>
#endif

#include "libiop/algebra/fields/gf64.hpp"
#include "libiop/algebra/fields/gf128.hpp"
#include "libiop/algebra/fields/gf192.hpp"
#include "libiop/algebra/fields/gf256.hpp"
#include "libiop/algebra/fields/utils.hpp"

#include "boost_profile.cpp"
#include "libiop/snark/aurora_snark.hpp"
#include "libiop/bcs/bcs_common.hpp"
#include "libiop/protocols/aurora_iop.hpp"
#include "libiop/protocols/ldt/fri/argument_size_optimizer.hpp"
#include "libiop/relations/examples/r1cs_examples.hpp"
#include <libff/algebra/curves/edwards/edwards_pp.hpp>
#include <libff/algebra/curves/alt_bn128/alt_bn128_pp.hpp>

#ifndef CPPDEBUG
bool process_prover_command_line(const int argc, const char** argv,
                                 options &options, bool heuristic_fri_soundness, bool optimize_localization)
{
    namespace po = boost::program_options;

    try
    {

        po::options_description desc = gen_options(options);
        desc.add_options()
             ("optimize_localization", po::value<bool>(&optimize_localization)->default_value(false))
             ("heuristic_fri_soundness", po::value<bool>(&heuristic_fri_soundness)->default_value(true));

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return false;
        }

        po::notify(vm);
        options.hash_enum = static_cast<libiop::bcs_hash_type>(options.hash_enum_val);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libiop;

template<typename FieldT, typename hash_type>
void print_argument_size(
    aurora_snark_parameters<FieldT, hash_type> params,
    const r1cs_constraint_system<FieldT> &constraint_system,
    aurora_snark_argument<FieldT, hash_type> argument)
{
    /* We go through registration on the verifier to know what the domains look like */
    bcs_verifier<FieldT, hash_type> verifier(params.bcs_params_, argument);
    aurora_iop<FieldT> full_protocol(verifier, constraint_system, params.iop_params_);
    full_protocol.register_interactions();
    verifier.seal_interaction_registrations();
    full_protocol.register_queries();
    verifier.seal_query_registrations();
    const bool holographic = false;
    print_detailed_transcript_data<FieldT, hash_type>(
        holographic,
        argument,
        params.bcs_params_,
        verifier);
}

template<typename FieldT, typename hash_type>
void instrument_aurora_snark(options &options,
                            LDT_reducer_soundness_type ldt_reducer_soundness_type,
                            FRI_soundness_type fri_soundness_type,
                            bool &optimize_localization)

{
    // TODO: Unhard code this
    const size_t RS_extra_dimensions = 3 + (options.make_zk ? 0 : 2);
    const size_t fri_localization_parameter = 2;
    field_subset_type domain_type = affine_subspace_type;
    if (options.is_multiplicative) {
        domain_type = multiplicative_coset_type;
    }

    for (std::size_t log_n = options.log_n_min; log_n <= options.log_n_max; ++log_n)
    {
        print_separator();

        const std::size_t n = 1ul << log_n;
        /* k+1 needs to be a power of 2 (proof system artifact) so we just fix it to 15 here */
        const std::size_t k = 15;
        const std::size_t m = n - 1;
        r1cs_example<FieldT> example = generate_r1cs_example<FieldT>(n, k, m);

        aurora_snark_parameters<FieldT, hash_type> parameters(
            options.security_level,
            ldt_reducer_soundness_type,
            fri_soundness_type,
            options.hash_enum,
            fri_localization_parameter,
            RS_extra_dimensions,
            options.make_zk,
            domain_type,
            example.constraint_system_.num_constraints(),
            example.constraint_system_.num_variables());

        std::vector<std::size_t> localization_parameter_array;
        if (optimize_localization)
        {
            const size_t codeword_domain_dim = parameters.iop_params_.codeword_domain_dim();
            size_t num_query_sets = parameters.iop_params_.FRI_params_.query_repetitions();
            size_t interactive_repetitions =
                parameters.iop_params_.FRI_params_.interactive_repetitions() *
                parameters.iop_params_.LDT_reducer_params_.num_output_LDT_instances();
            const size_t max_tested_degree_bound =
                parameters.iop_params_.encoded_aurora_params_.max_tested_degree_bound();
            const size_t hash_size = (parameters.bcs_params_.security_parameter + 3) / 4;
            std::vector<size_t> oracle_locality_vector = parameters.iop_params_.locality_vector();
            // if (parameters.iop_params_.make_zk())
            // {
            //     /* Handle the zk leaves for the SNARK. TODO: Where should this go? */
            //     oracle_locality_vector[0] += 1;
            // }
            localization_parameter_array =
                compute_argument_size_optimal_localization_parameters<FieldT>(
                    oracle_locality_vector, codeword_domain_dim,
                    num_query_sets, interactive_repetitions,
                    max_tested_degree_bound, hash_size);

            parameters.reset_fri_localization_parameters(localization_parameter_array);
        }

        enter_block("Check satisfiability of R1CS example");
        const bool is_satisfied = example.constraint_system_.is_satisfied(
            example.primary_input_, example.auxiliary_input_);
        assert(is_satisfied);
        leave_block("Check satisfiability of R1CS example");
        printf("\n");
        print_indent(); printf("* R1CS number of constraints: %zu\n", example.constraint_system_.num_constraints());
        print_indent(); printf("* R1CS number of variables: %zu\n", example.constraint_system_.num_variables());
        print_indent(); printf("* R1CS number of variables for primary input: %zu\n", example.primary_input_.size());
        print_indent(); printf("* R1CS number of variables for auxiliary input: %zu\n", example.auxiliary_input_.size());
        print_indent(); printf("* R1CS size of constraint system (bytes): %zu\n", example.constraint_system_.size_in_bytes());
        print_indent(); printf("* R1CS size of primary input (bytes): %zu\n", example.primary_input_.size() * sizeof(FieldT));
        print_indent(); printf("* R1CS size of auxiliary input (bytes): %zu\n", example.auxiliary_input_.size() * sizeof(FieldT));
        printf("\n");
        const aurora_snark_argument<FieldT, hash_type> proof = aurora_snark_prover<FieldT, hash_type>(
            example.constraint_system_,
            example.primary_input_,
            example.auxiliary_input_,
            parameters);

        print_argument_size(parameters, example.constraint_system_, proof);

        const bool bit = aurora_snark_verifier<FieldT, hash_type>(
            example.constraint_system_,
            example.primary_input_,
            proof,
            parameters);

        printf("\n\n");

        print_indent(); printf("* Verifier satisfied: %s\n", bit ? "true" : "false");
    }
}

int main(int argc, const char * argv[])
{
    /* Set up R1CS */

    options default_vals;

    bool optimize_localization = false;
    bool heuristic_fri_soundness = true;

#ifdef CPPDEBUG
    /* set reasonable defaults */
    if (argc > 1)
    {
        printf("There is no argument parsing in CPPDEBUG mode.");
        exit(1);
    }
    libiop::UNUSED(argv);

#else
    if (!process_prover_command_line(argc, argv, default_vals, heuristic_fri_soundness, optimize_localization))
    {
        return 1;
    }
#endif
    /** TODO: eventually get a string from program options, and then have a from string methods in protocols */
    LDT_reducer_soundness_type ldt_reducer_soundness_type = LDT_reducer_soundness_type::proven;
    if (default_vals.heuristic_ldt_reducer_soundness)
    {
        ldt_reducer_soundness_type = LDT_reducer_soundness_type::optimistic_heuristic;
    }
    FRI_soundness_type fri_soundness_type = FRI_soundness_type::proven;
    if (heuristic_fri_soundness) {
        fri_soundness_type = FRI_soundness_type::heuristic;
    }
    start_profiling();

    printf("Selected parameters:\n");
    printf("- log_n_min = %zu\n", default_vals.log_n_min);
    printf("- log_n_max = %zu\n", default_vals.log_n_max);
    printf("- security_level = %zu\n", default_vals.security_level);
    printf("- LDT_reducer_soundness_type = %s\n", LDT_reducer_soundness_type_to_string(ldt_reducer_soundness_type));
    printf("- FRI_soundness_type = %s\n", FRI_soundness_type_to_string(fri_soundness_type));
    printf("- is_multiplicative = %s\n", default_vals.is_multiplicative ? "true" : "false");
    printf("- field_size = %zu\n", default_vals.field_size);
    printf("- make_zk = %s\n", default_vals.make_zk ? "true" : "false");
    printf("- hash_enum = %s\n", bcs_hash_type_names[default_vals.hash_enum]);

    if (default_vals.is_multiplicative) {
        switch (default_vals.field_size) {
            case 181:
                edwards_pp::init_public_params();
                instrument_aurora_snark<edwards_Fr, binary_hash_digest>(
                    default_vals, ldt_reducer_soundness_type,
                    fri_soundness_type, optimize_localization);
                break;
            case 256:
                libff::alt_bn128_pp::init_public_params();

                if (default_vals.hash_enum == libiop::blake2b_type)
                {
                    instrument_aurora_snark<libff::alt_bn128_Fr, binary_hash_digest>(
                        default_vals, ldt_reducer_soundness_type,
                        fri_soundness_type, optimize_localization);
                }
                else
                {
                    instrument_aurora_snark<libff::alt_bn128_Fr, libff::alt_bn128_Fr>(
                        default_vals, ldt_reducer_soundness_type,
                        fri_soundness_type, optimize_localization);
                }
                break;
            default:
                throw std::invalid_argument("Field size not supported.");
        }

    } else {
        switch (default_vals.field_size)
        {
            case 64:
                instrument_aurora_snark<gf64, binary_hash_digest>(
                    default_vals, ldt_reducer_soundness_type, fri_soundness_type, optimize_localization);
                break;
            case 128:
                instrument_aurora_snark<gf128, binary_hash_digest>(
                    default_vals, ldt_reducer_soundness_type, fri_soundness_type, optimize_localization);
                break;
            case 192:
                instrument_aurora_snark<gf192, binary_hash_digest>(
                    default_vals, ldt_reducer_soundness_type, fri_soundness_type, optimize_localization);
                break;
            case 256:
                instrument_aurora_snark<gf256, binary_hash_digest>(
                    default_vals, ldt_reducer_soundness_type, fri_soundness_type, optimize_localization);
                break;
            default:
                throw std::invalid_argument("Field size not supported.");
        }
    }
}


#include "astroutils.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <math.h>
#include "libwcs/wcs.h"
#include "libwcs/fitsfile.h"
#include "libwcs/wcscat.h"
#include "libwcs/lwcs.h"

#include <iostream>
#include <vector>
#include <string>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string.hpp>
using namespace boost::algorithm;

extern void setsys();

AstroUtils::AstroUtils()
{
}

void AstroUtils::GetCenterCoords(std::string file, double *coords)
{
    WorldCoor *wc = AstroUtils().GetWCSFITS((char*) file.c_str(), 1);
    AstroUtils().xy2sky(file, wc->nxpix/2.0, wc->nypix/2.0, coords, WCS_GALACTIC);
    wcsfree(wc);
}

/// values = [dl, db]
void AstroUtils::GetRectSize(std::string file, double *values)
{
    WorldCoor *wc = AstroUtils().GetWCSFITS((char*) file.c_str(), 1);
    double sky_coords[2], delta[2];
    AstroUtils().xy2sky(file, 0, 0, sky_coords, WCS_GALACTIC);

    /* dl */
    AstroUtils().xy2sky(file, wc->nxpix, 0, delta, WCS_GALACTIC);
    values[0] = abs(delta[0] - sky_coords[0]);

    /* db */
    AstroUtils().xy2sky(file, 0, wc->nypix, delta, WCS_GALACTIC);
    values[1] = abs(delta[1] - sky_coords[1]);

    wcsfree(wc);
}

double AstroUtils::GetRadiusSize(std::string file)
{
    double rect[2];
    AstroUtils().GetRectSize(file, rect);
    double radius = fmax(rect[0], rect[1]) / 2.0;
    return radius;
}

void AstroUtils::GetBounds(std::string file, double *top, double *bottom, double *right, double *left)
{
    double tl[2], br[2];
    WorldCoor *wc = AstroUtils().GetWCSFITS((char*) file.c_str(), 1);
    AstroUtils().xy2sky(file, 0, wc->nypix, tl, WCS_GALACTIC);
    AstroUtils().xy2sky(file, wc->nxpix, 0, br, WCS_GALACTIC);
    *top = tl[1];
    *left = tl[0];
    *bottom = br[1];
    *right = br[0];
    wcsfree(wc);
}

bool AstroUtils::CheckOverlap(std::string f1, std::string f2, bool full)
{
    if (full) {
        // Full overlap
        return CheckFullOverlap(f1, f2) || CheckFullOverlap(f2, f1);
    } else {
        // Check partial overlap
        double T1, B1, R1, L1;
        AstroUtils().GetBounds(f1, &T1, &B1, &R1, &L1);

        double T2, B2, R2, L2;
        AstroUtils().GetBounds(f2, &T2, &B2, &R2, &L2);

        return L1 > R2 && R1 < L2 && T1 > B2 && B1 < T2;
    }
}

bool AstroUtils::CheckFullOverlap(std::string f1, std::string f2)
{
    double T1, B1, R1, L1;
    AstroUtils().GetBounds(f1, &T1, &B1, &R1, &L1);

    double T2, B2, R2, L2;
    AstroUtils().GetBounds(f2, &T2, &B2, &R2, &L2);

    // returns true if f2 is completely inside f1
    return R2 > R1 && L2 < L1 && T2 < T1 && B2 > B1;
}

double AstroUtils::arcsecPixel(std::string file)
{
    char *fn = new char[file.length() + 1];
    strcpy(fn, file.c_str());

    struct WorldCoor *wcs;
    char *header;
    double cra, cdec, dra, ddec, secpix;
    int wp, hp;
    int sysout = 0;
    double eqout = 0.0;

    header = GetFITShead(fn, 0);
    wcs = GetFITSWCS(fn, header, 0, &cra, &cdec, &dra, &ddec, &secpix, &wp, &hp, &sysout, &eqout);
    wcsfree(wcs);

    return secpix;
}

void AstroUtils::xy2sky(std::string map, float x, float y, double* coord, int wcs_type)
{
    struct WorldCoor *wcs;
    char *fn = new char[map.length() + 1];
    static char coorsys[16];
    char wcstring[64];
    char lstr = 64;
    *coorsys = 0;

    strcpy(fn, map.c_str());
    wcs = GetWCSFITS(fn, 0);
    wcs->sysout = wcs_type;
    // force the set of wcs in degree
    setwcsdeg(wcs,1);

    if (wcs_type == WCS_GALACTIC)
    {
        wcs->eqout = 2000.0;
    }

    if (pix2wcst(wcs, x, y, wcstring, lstr))
    {
        std::string str(wcstring);
        std::vector<std::string> tokens;
        trim(str);
        split(tokens, str, is_any_of(" "), boost::token_compress_on);
        coord[0] = atof(tokens[0].c_str());
        coord[1] = atof(tokens[1].c_str());
    }

    delete [] fn;
    wcsfree(wcs);
}

int AstroUtils::getSysOut(std::string file)
{
    char *fn = new char[file.length() + 1];
    strcpy(fn, file.c_str());

    struct WorldCoor *wcs;
    char *header;
    double cra, cdec, dra, ddec, secpix;
    int wp, hp;
    int sysout = 0;
    double eqout = 0.0;

    header = GetFITShead(fn, 0);
    wcs = GetFITSWCS(fn, header, 1, &cra, &cdec, &dra, &ddec, &secpix, &wp, &hp, &sysout, &eqout);
    wcsfree(wcs);

    delete [] fn;

    return sysout;
}

void AstroUtils::getRotationAngle(std::string file)
{
    char *fn = new char[file.length() + 1];
    strcpy(fn, file.c_str());

    struct WorldCoor *wcs;
    char *header;
    double cra, cdec, dra, ddec, secpix;
    int wp, hp;
    int sysout = 0;
    double eqout = 0.0;

    header = GetFITShead(fn, 0);
    wcs = GetFITSWCS(fn, header, 1, &cra, &cdec, &dra, &ddec, &secpix, &wp, &hp, &sysout, &eqout);
    wcsoutinit(wcs, (char*) "GALACTIC");

    int off;
    double xc, xn, xe, yc, yn, ye;

    /* If image is one-dimensional, leave rotation angle alone */
    if (wcs->nxpix < 1.5 || wcs->nypix < 1.5) {
        wcs->imrot = wcs->rot;
        wcs->pa_north = wcs->rot + 90.0;
        wcs->pa_east = wcs->rot + 180.0;
        return;
    }

    /* Do not try anything if image is LINEAR (not Cartesian projection) */
    if (wcs->syswcs == WCS_LINEAR)
        return;

    wcs->xinc = fabs (wcs->xinc);
    wcs->yinc = fabs (wcs->yinc);

    /* Compute position angles of North and East in image */
    xc = wcs->xrefpix;
    yc = wcs->yrefpix;
    pix2wcs (wcs, xc, yc, &cra, &cdec);
    if (wcs->coorflip) {
        wcs2pix (wcs, cra+wcs->yinc, cdec, &xe, &ye, &off);
        wcs2pix (wcs, cra, cdec+wcs->xinc, &xn, &yn, &off);
    }
    else {
        wcs2pix (wcs, cra+wcs->xinc, cdec, &xe, &ye, &off);
        wcs2pix (wcs, cra, cdec+wcs->yinc, &xn, &yn, &off);
    }
    wcs->pa_north = raddeg (atan2 (yn-yc, xn-xc));
    if (wcs->pa_north < -90.0)
        wcs->pa_north = wcs->pa_north + 360.0;
    wcs->pa_east = raddeg (atan2 (ye-yc, xe-xc));
    if (wcs->pa_east < -90.0)
        wcs->pa_east = wcs->pa_east + 360.0;

    /* Compute image rotation angle from North */
    if (wcs->pa_north < -90.0)
        wcs->imrot = 270.0 + wcs->pa_north;
    else
        wcs->imrot = wcs->pa_north - 90.0;

    /* Compute CROTA */
    if (wcs->coorflip) {
        wcs->rot = wcs->imrot + 90.0;
        if (wcs->rot < 0.0)
            wcs->rot = wcs->rot + 360.0;
    }
    else
        wcs->rot = wcs->imrot;
    if (wcs->rot < 0.0)
        wcs->rot = wcs->rot + 360.0;
    if (wcs->rot >= 360.0)
        wcs->rot = wcs->rot - 360.0;

    /* Set image mirror flag based on axis orientation */
    wcs->imflip = 0;
    if (wcs->pa_east - wcs->pa_north < -80.0 &&
            wcs->pa_east - wcs->pa_north > -100.0)
        wcs->imflip = 1;
    if (wcs->pa_east - wcs->pa_north < 280.0 &&
            wcs->pa_east - wcs->pa_north > 260.0)
        wcs->imflip = 1;
    if (wcs->pa_north - wcs->pa_east > 80.0 &&
            wcs->pa_north - wcs->pa_east < 100.0)
        wcs->imflip = 1;
    if (wcs->coorflip) {
        if (wcs->imflip)
            wcs->yinc = -wcs->yinc;
    }
    else {
        if (!wcs->imflip)
            wcs->xinc = -wcs->xinc;
    }

    delete [] fn;
    wcsfree(wcs);
}

bool AstroUtils::sky2xy(std::string map, double ra, double dec, double* coord)
{
    char *fn = new char[map.length() + 1];
    strcpy(fn, map.c_str());

    struct WorldCoor *wcs;
    char *header;
    double cra, cdec, dra, ddec, secpix;
    int wp, hp;
    int sysout = 0;
    double eqout = 0.0;
    double x, y;
    int sysin;
    char csys[16];
    double eqin = 0.0;
    int offscale;

    header = GetFITShead(fn, 0);
    wcs = GetFITSWCS(fn, header, 0, &cra, &cdec, &dra, &ddec, &secpix, &wp, &hp, &sysout, &eqout);

    if (wcs->prjcode < 0)
        strcpy (csys, "PIXEL");
    else if (wcs->prjcode < 2)
        strcpy (csys, "LINEAR");
    else
        strcpy (csys, wcs->radecsys);

    sysin = wcscsys (csys);
    eqin = wcsceq (csys);

    if (wcs->syswcs > 0 && wcs->syswcs != 6 && wcs->syswcs != 10)
        wcscon (sysin, wcs->syswcs, eqin, eqout, &ra, &dec, wcs->epoch);

    wcsc2pix (wcs, ra, dec, csys, &x, &y, &offscale);

    coord[0] = x;
    coord[1] = y;
    coord[2] = secpix;

    delete [] fn;
    delete [] header;
    wcsfree(wcs);

    return true;
}

static double secpix0 = PSCALE;		/* Set image scale--override header */
static int usecdelt = 0;		    /* Use CDELT if 1, else CD matrix */
static int hp0 = 0;			        /* Initial height of image */
static int wp0 = 0;			        /* Initial width of image */
static double ra0 = -99.0;		    /* Initial center RA in degrees */
static double dec0 = -99.0;		    /* Initial center Dec in degrees */
static int comsys = WCS_J2000;		/* Command line center coordinte system */
static int ptype0 = -1;			    /* Projection type to fit */
static int  nctype = 28;		    /* Number of possible projections */
static char ctypes[32][4];		    /* 3-letter codes for projections */
static double xref0 = -99999.0;		/* Reference pixel X coordinate */
static double yref0 = -99999.0;		/* Reference pixel Y coordinate */
static double secpix2 = PSCALE;		/* Set image scale 2--override header */
static double *cd0 = NULL;		    /* Set CD matrix--override header */
static double rot0 = 361.0;		    /* Initial image rotation */
static char *dateobs0 = NULL;		/* Initial DATE-OBS value in FITS date format */

WorldCoor* AstroUtils::GetFITSWCS(char *filename, char	*header, int verbose, double *cra, double *cdec,double	*dra, double *ddec, double *secpix, int *wp,int	*hp,int	*sysout ,double	*eqout)
{
    int naxes;
    double eq1, x, y;
    double ra1, dec1, dx, dy;
    double xmin, xmax, ymin, ymax, ra2, dec2, ra3, dec3, ra4, dec4;
    double dra0, dra1, dra2, dra3, dra4;
    struct WorldCoor *wcs;
    char rstr[64], dstr[64], cstr[16];

    /* Initialize WCS structure from possibly revised FITS header */
    wcs = ChangeFITSWCS (filename, header, verbose);
    if (wcs == NULL) {
        return (NULL);
    }
    *hp = (int) wcs->nypix;
    *wp = (int) wcs->nxpix;

    /* If incomplete WCS in header, drop out */
    if (nowcs (wcs)) {
        setwcsfile (filename);
        /* wcserr(); */
        if (verbose)
            fprintf (stderr,"Insufficient information for initial WCS\n");
        return (NULL);
    }

    /* If in linear coordinates, do not print as sexigesimal */
    if (wcs->sysout < 1 || wcs->sysout == 6 || wcs->sysout == 10)
        wcs->degout = 1;

    /* Set flag to get appropriate equinox for catalog search */
    if (!*sysout)
        *sysout = wcs->syswcs;
    if (*eqout == 0.0)
        *eqout = wcs->equinox;
    eq1 = wcs->equinox;
    if (wcs->coorflip) {
        ra1 = wcs->crval[1];
        dec1 = wcs->crval[0];
    }
    else {
        ra1 = wcs->crval[0];
        dec1 = wcs->crval[1];
    }

    /* Print reference pixel position and value */
    if (verbose && (eq1 != *eqout || wcs->syswcs != *sysout)) {
        if (wcs->degout) {
            deg2str (rstr, 32, ra1, 6);
            deg2str (dstr, 32, dec1, 6);
        }
        else {
            ra2str (rstr, 32, ra1, 3);
            dec2str (dstr, 32, dec1, 2);
        }
        wcscstr (cstr, wcs->syswcs, wcs->equinox, wcs->epoch);
        fprintf (stderr,"Reference pixel (%.2f,%.2f) %s %s %s\n",
                 wcs->xrefpix, wcs->yrefpix, rstr, dstr, cstr);
    }

    /* Get coordinates of corners for size for catalog searching */
    dx = wcs->nxpix;
    dy = wcs->nypix;
    xmin = 0.5;
    ymin = 0.5;
    xmax = 0.5 + dx;
    ymax = 0.5 + dy;
    pix2wcs (wcs, xmin, ymin, &ra1, &dec1);
    pix2wcs (wcs, xmin, ymax, &ra2, &dec2);
    pix2wcs (wcs, xmax, ymin, &ra3, &dec3);
    pix2wcs (wcs, xmax, ymax, &ra4, &dec4);

    /* Convert search corners to output coordinate system and equinox */
    if (wcs->syswcs > 0 && wcs->syswcs != 6 && wcs->syswcs != 10) {
        wcscon (wcs->syswcs,*sysout,wcs->equinox,*eqout,&ra1,&dec1,wcs->epoch);
        wcscon (wcs->syswcs,*sysout,wcs->equinox,*eqout,&ra2,&dec2,wcs->epoch);
        wcscon (wcs->syswcs,*sysout,wcs->equinox,*eqout,&ra3,&dec3,wcs->epoch);
        wcscon (wcs->syswcs,*sysout,wcs->equinox,*eqout,&ra4,&dec4,wcs->epoch);
    }

    /* Find center and convert to output coordinate system and equinox */
    x = 0.5 + (dx * 0.5);
    y = 0.5 + (dy * 0.5);
    pix2wcs (wcs, x, y, cra, cdec);
    if (wcs->syswcs > 0 && wcs->syswcs != 6 && wcs->syswcs != 10)
        wcscon (wcs->syswcs,*sysout,wcs->equinox,*eqout,cra,cdec,wcs->epoch);

    /* Find maximum half-width in declination */
    *ddec = fabs (dec1 - *cdec);
    if (fabs (dec2 - *cdec) > *ddec)
        *ddec = fabs (dec2 - *cdec);
    if (fabs (dec3 - *cdec) > *ddec)
        *ddec = fabs (dec3 - *cdec);
    if (fabs (dec4 - *cdec) > *ddec)
        *ddec = fabs (dec4 - *cdec);

    /* Find maximum half-width in right ascension */
    dra0 = (dx / dy) * (*ddec / cos (*cdec));
    dra1 = ra1 - *cra;
    dra2 = ra2 - *cra;
    if (*cra < 0 && *cra + dra0 > 0.0) {
        dra1 = -(dra1 - 360.0);
        dra2 = -(dra2 - 360.0);
    }
    if (dra1 > 180.0)
        dra1 = dra1 - 360.0;
    else if (dra1 < -180.0)
        dra1 = dra1 + 360.0;
    else if (dra1 < 0.0)
        dra1 = -dra1;
    if (dra2 > 180.0)
        dra2 = dra2 - 360.0;
    else if (dra2 < -180.0)
        dra2 = dra2 + 360.0;
    else if (dra2 < 0.0)
        dra2 = -dra2;
    dra3 = *cra - ra3;
    dra4 = *cra - ra4;
    if (*cra > 0 && *cra - dra0 < 0.0) {
        dra3 = dra3 + 360.0;
        dra4 = dra4 + 360.0;
    }
    if (dra3 > 180.0)
        dra3 = dra3 - 360.0;
    else if (dra3 < -180.0)
        dra3 = dra3 + 360.0;
    else if (dra3 < 0.0)
        dra3 = -dra3;
    if (dra4 > 180.0)
        dra4 = dra4 - 360.0;
    else if (dra4 < -180.0)
        dra4 = dra4 + 360.0;
    else if (dra4 < 0.0)
        dra4 = -dra4;
    *dra = dra1;
    if (dra2 > *dra)
        *dra = dra2;
    if (dra3 > *dra)
        *dra = dra3;
    if (dra4 > *dra)
        *dra = dra4;

    /* wcssize (wcs, cra, cdec, dra, ddec); */

    /* Set reference pixel to center of image if it has not been set */
    if (wcs->xref == -999.0 && wcs->yref == -999.0) {
        wcs->xref = *cra;
        wcs->cel.ref[0] = *cra;
        wcs->crval[0] = *cra;
        wcs->yref = *cdec;
        wcs->cel.ref[1] = *cdec;
        wcs->crval[1] = *cdec;
        ra1 = *cra;
        dec1 = *cdec;
        if (wcs->xrefpix == 0.0 && wcs->yrefpix == 0.0) {
            wcs->xrefpix = 0.5 + (double) wcs->nxpix * 0.5;
            wcs->yrefpix = 0.5 + (double) wcs->nypix * 0.5;
        }
        wcs->xinc = *dra * 2.0 / (double) wcs->nxpix;
        wcs->yinc = *ddec * 2.0 / (double) wcs->nypix;
        /* hchange (header,"PLTRAH","PLT0RAH");
    wcs->plate_fit = 0; */
    }

    /* Convert center to desired coordinate system */
    else if (wcs->syswcs != *sysout && wcs->equinox != *eqout) {
        wcscon (wcs->syswcs, *sysout, wcs->equinox, *eqout, &ra1, &dec1, wcs->epoch);
        if (wcs->coorflip) {
            wcs->yref = ra1;
            wcs->xref = dec1;
        }
        else {
            wcs->xref = ra1;
            wcs->yref = dec1;
        }
    }

    /* Compute plate scale to return if it was not set on the command line */
    if (secpix0 <= 0.0) {
        pix2wcs (wcs, wcs->xrefpix-0.5, wcs->yrefpix, &ra1, &dec1);
        pix2wcs (wcs, wcs->xrefpix+0.5, wcs->yrefpix, &ra2, &dec2);
        *secpix = 3600.0 * wcsdist (ra1, dec1, ra2, dec2);
    }

    wcs->crval[0] = wcs->xref;
    wcs->crval[1] = wcs->yref;
    if (wcs->coorflip) {
        wcs->cel.ref[0] = wcs->crval[1];
        wcs->cel.ref[1] = wcs->crval[0];
    }
    else {
        wcs->cel.ref[0] = wcs->crval[0];
        wcs->cel.ref[1] = wcs->crval[1];
    }

    if (wcs->syswcs > 0 && wcs->syswcs != 6 && wcs->syswcs != 10) {
        wcs->cel.flag = 0;
        wcs->wcsl.flag = 0;
    }
    else {
        wcs->lin.flag = LINSET;
        wcs->wcsl.flag = WCSSET;
    }

    wcs->equinox = *eqout;
    wcs->syswcs = *sysout;
    wcs->sysout = *sysout;
    wcs->eqout = *eqout;
    wcs->sysin = *sysout;
    wcs->eqin = *eqout;
    wcscstr (cstr,*sysout,*eqout,wcs->epoch);
    strcpy (wcs->radecsys, cstr);
    strcpy (wcs->radecout, cstr);
    strcpy (wcs->radecin, cstr);
    wcsininit (wcs, wcs->radecsys);
    wcsoutinit (wcs, wcs->radecsys);

    naxes = wcs->naxis;
    if (naxes < 1 || naxes > 9) {
        naxes = wcs->naxes;
        wcs->naxis = naxes;
    }

    if (usecdelt) {
        hputnr8 (header, "CDELT1", 9, wcs->xinc);
        if (naxes > 1) {
            hputnr8 (header, "CDELT2", 9, wcs->yinc);
            hputnr8 (header, "CROTA2", 9, wcs->rot);
        }
        hdel (header, "CD1_1");
        hdel (header, "CD1_2");
        hdel (header, "CD2_1");
        hdel (header, "CD2_2");
    }
    else {
        hputnr8 (header, "CD1_1", 9, wcs->cd[0]);
        if (naxes > 1) {
            hputnr8 (header, "CD1_2", 9, wcs->cd[1]);
            hputnr8 (header, "CD2_1", 9, wcs->cd[2]);
            hputnr8 (header, "CD2_2", 9, wcs->cd[3]);
        }
    }

    /* Print reference pixel position and value */
    if (verbose) {
        if (wcs->degout) {
            deg2str (rstr, 32, ra1, 6);
            deg2str (dstr, 32, dec1, 6);
        }
        else {
            ra2str (rstr, 32, ra1, 3);
            dec2str (dstr, 32, dec1, 2);
        }
        wcscstr (cstr,*sysout,*eqout,wcs->epoch);
        fprintf (stderr,"Reference pixel (%.2f,%.2f) %s %s %s\n",
                 wcs->xrefpix, wcs->yrefpix, rstr, dstr, cstr);
    }

    /* Image size for catalog search */
    if (verbose) {
        if (wcs->degout) {
            deg2str (rstr, 32, *cra, 6);
            deg2str (dstr, 32, *cdec, 6);
        }
        else {
            ra2str (rstr, 32, *cra, 3);
            dec2str (dstr, 32, *cdec, 2);
        }
        wcscstr (cstr, *sysout, *eqout, wcs->epoch);
        fprintf (stderr,"Search at %s %s %s", rstr, dstr, cstr);
        if (wcs->degout) {
            deg2str (rstr, 32, *dra, 6);
            deg2str (dstr, 32, *ddec, 6);
        }
        else {
            ra2str (rstr, 32, *dra, 3);
            dec2str (dstr, 32, *ddec, 2);
        }
        fprintf (stderr," +- %s %s\n", rstr, dstr);
        fprintf (stderr,"Image width=%d height=%d, %g arcsec/pixel\n",
                 *wp, *hp, *secpix);
    }

    return (wcs);
}

WorldCoor* AstroUtils::ChangeFITSWCS(char *filename, char* header, int verbose)
{
    int nax, i, hp, wp;
    double xref, yref, degpix, secpix;
    struct WorldCoor *wcs;
    char temp[16];
    char *cwcs;

    /* Set the world coordinate system from the image header */
    if (strlen (filename) > 0)
    {
        cwcs = strchr (filename, '%');
        if (cwcs != NULL)
            cwcs++;
    }

    if (!strncmp (header, "END", 3)) {
        cwcs = NULL;
        for (i = 0; i < 2880; i++)
            header[i] = (char) 32;
        hputl (header, "SIMPLE", 1);
        hputi4 (header, "BITPIX", 0);
        hputi4 (header, "NAXIS", 2);
        hputi4 (header, "NAXIS1", 1);
        hputi4 (header, "NAXIS2", 1);
    }

    /* Set image dimensions */
    nax = 0;
    if (hp0 > 0 || wp0 > 0) {
        hp = hp0;
        wp = wp0;
        if (hp > 0 && wp > 0)
            nax = 2;
        else
            nax = 1;
        hputi4 (header, "NAXIS", nax);
        hputi4 (header, "NAXIS1", wp);
        hputi4 (header, "NAXIS2", hp);
    }
    else if (hgeti4 (header,"NAXIS",&nax) < 1 || nax < 1) {
        if (hgeti4 (header, "WCSAXES", &nax) < 1)
            return (NULL);
        else {
            if (hgeti4 (header, "IMAGEW", &wp) < 1)
                return (NULL);
            if (hgeti4 (header, "IMAGEH", &wp) < 1)
                return (NULL);
        }
    }
    else {
        if (hgeti4 (header,"NAXIS1",&wp) < 1)
            return (NULL);
        if (hgeti4 (header,"NAXIS2",&hp) < 1)
            return (NULL);
    }

    /* Set plate center from command line, if it is there */
    if (ra0 > -99.0 && dec0 > -99.0) {
        hputnr8 (header, "CRVAL1" ,8,ra0);
        hputnr8 (header, "CRVAL2" ,8,dec0);
        hputra (header, "RA", ra0);
        hputdec (header, "DEC", dec0);
        if (comsys == WCS_B1950) {
            hputi4 (header, "EPOCH", 1950);
            hputi4 (header, "EQUINOX", 1950);
            hputs (header, "RADECSYS", "FK4");
        }
        else {
            hputi4 (header, "EPOCH", 2000);
            hputi4 (header, "EQUINOX", 2000);
            if (comsys == WCS_GALACTIC)
                hputs (header, "RADECSYS", "GALACTIC");
            else if (comsys == WCS_ECLIPTIC)
                hputs (header, "RADECSYS", "ECLIPTIC");
            else if (comsys == WCS_ICRS)
                hputs (header, "RADECSYS", "ICRS");
            else
                hputs (header, "RADECSYS", "FK5");
        }
        if (hgetr8 (header, "SECPIX", &secpix)) {
            degpix = secpix / 3600.0;
            hputnr8 (header, "CDELT1", 8, -degpix);
            hputnr8 (header, "CDELT2", 8, degpix);
            hdel (header, "CD1_1");
            hdel (header, "CD1_2");
            hdel (header, "CD2_1");
            hdel (header, "CD2_2");
        }
    }
    if (ptype0 > -1 && ptype0 < nctype) {
        strcpy (temp,"RA---");
        strcat (temp, ctypes[ptype0]);
        hputs (header, "CTYPE1", temp);
        strcpy (temp,"DEC--");
        strcat (temp, ctypes[ptype0]);
        hputs (header, "CTYPE2", temp);
    }

    /* Set reference pixel from command line, if it is there */
    if (xref0 > -99999.0 && yref0 > -99999.0) {
        hputr8 (header, "CRPIX1", xref0);
        hputr8 (header, "CRPIX2", yref0);
    }
    else if (hgetr8 (header, "CRPIX1", &xref) < 1) {
        xref = 0.5 + (double) wp / 2.0;
        yref = 0.5 + (double) hp / 2.0;
        hputnr8 (header, "CRPIX1", 3, xref);
        hputnr8 (header, "CRPIX2", 3, yref);
    }

    /* Set plate scale from command line, if it is there */
    if (secpix0 != 0.0 || cd0 != NULL) {
        if (secpix2 != 0.0) {
            secpix = 0.5 * (secpix0 + secpix2);
            hputnr8 (header, "SECPIX1", 5, secpix0);
            hputnr8 (header, "SECPIX2", 5, secpix2);
            degpix = -secpix0 / 3600.0;
            hputnr8 (header, "CDELT1", 8, degpix);
            degpix = secpix2 / 3600.0;
            hputnr8 (header, "CDELT2", 8, degpix);
            hdel (header, "CD1_1");
            hdel (header, "CD1_2");
            hdel (header, "CD2_1");
            hdel (header, "CD2_2");
        }
        else if (secpix0 != 0.0) {
            secpix = secpix0;
            hputnr8 (header, "SECPIX", 5, secpix);
            degpix = secpix / 3600.0;
            hputnr8 (header, "CDELT1", 8, -degpix);
            hputnr8 (header, "CDELT2", 8, degpix);
            hdel (header, "CD1_1");
            hdel (header, "CD1_2");
            hdel (header, "CD2_1");
            hdel (header, "CD2_2");
        }
        else {
            hputr8 (header, "CD1_1", cd0[0]);
            hputr8 (header, "CD1_2", cd0[1]);
            hputr8 (header, "CD2_1", cd0[2]);
            hputr8 (header, "CD2_2", cd0[3]);
            hdel (header, "CDELT1");
            hdel (header, "CDELT2");
            hdel (header, "CROTA1");
            hdel (header, "CROTA2");
        }
        if (!ksearch (header,"CRVAL1")) {
            hgetra (header, "RA", &ra0);
            hgetdec (header, "DEC", &dec0);
            hputnr8 (header, "CRVAL1", 8, ra0);
            hputnr8 (header, "CRVAL2", 8, dec0);
        }
        if (!ksearch (header,"CRPIX1")) {
            xref = (double) wp / 2.0;
            yref = (double) hp / 2.0;
            hputnr8 (header, "CRPIX1", 3, xref);
            hputnr8 (header, "CRPIX2", 3, yref);
        }
        if (!ksearch (header,"CTYPE1")) {
            if (comsys == WCS_GALACTIC) {
                hputs (header, "CTYPE1", "GLON-TAN");
                hputs (header, "CTYPE2", "GLAT-TAN");
            }
            else {
                hputs (header, "CTYPE1", "RA---TAN");
                hputs (header, "CTYPE2", "DEC--TAN");
            }
        }
    }

    /* Set rotation angle from command line, if it is there */
    if (rot0 < 361.0) {
        hputnr8 (header, "CROTA1", 5, rot0);
        hputnr8 (header, "CROTA2", 5, rot0);
    }

    /* Set observation date for epoch, if it is there */
    if (dateobs0 != NULL)
        hputs (header, "DATE-OBS", dateobs0);

    /* Initialize WCS structure from FITS header */
    wcs = wcsinitn (header, cwcs);

    /* If incomplete WCS in header, drop out */
    if (nowcs (wcs)) {
        setwcsfile (filename);
        /* wcserr(); */
        if (verbose)
            fprintf (stderr,"Insufficient information for initial WCS\n");
        return (NULL);
    }
    return (wcs);
}

WorldCoor * AstroUtils::GetWCSFITS(char *filename, int verbose)
{
    char *header;		    /* FITS header */
    struct WorldCoor *wcs;	/* World coordinate system structure */
    char *cwcs;			    /* Multiple wcs string (name or character) */

    /* Read the FITS or IRAF image file header */
    header = GetFITShead (filename, verbose);
    if (header == NULL)
        return (NULL);

    verbose=true;

    /* Set the world coordinate system from the image header */
    cwcs = strchr (filename, '%');
    if (cwcs != NULL)
        cwcs++;
    wcs = wcsinitn (header, cwcs);
    if (wcs == NULL) {
        setwcsfile (filename);
        if (verbose)
            wcserr ();
    }
    free (header);

    return (wcs);
}

char * AstroUtils::GetFITShead(char * filename, int verbose)
{
    char *header;		/* FITS header */
    int lhead;			/* Maximum number of bytes in FITS header */
    char *irafheader;   /* IRAF image header */
    int nbiraf, nbfits;

    /* Open IRAF image if .imh extension is present */
    if (isiraf (filename)) {
        if ((irafheader = irafrhead (filename, &nbiraf)) != NULL) {
            if ((header = iraf2fits (filename, irafheader, nbiraf, &lhead)) == NULL) {
                if (verbose)
                    fprintf (stderr, "Cannot translate IRAF header %s\n",filename);
                free (irafheader);
                irafheader = NULL;
                return (NULL);
            }
            free (irafheader);
            irafheader = NULL;
        }
        else {
            if (verbose)
                fprintf (stderr, "Cannot read IRAF header file %s\n", filename);
            return (NULL);
        }
    }
    else if (istiff (filename) || isgif (filename) || isjpeg (filename)) {
        if ((header = fitsrtail (filename, &lhead, &nbfits)) == NULL) {
            if (verbose)
                fprintf (stderr, "TIFF file %s has no appended header\n", filename);
            return (NULL);
        }
    }

    /* Open FITS file if .imh extension is not present */
    else {
        if ((header = fitsrhead (filename, &lhead, &nbfits)) == NULL) {
            if (verbose)
                /* fprintf (stderr, "Cannot read FITS file %s\n", filename); */
                fitserr ();
            return (NULL);
        }
    }

    return (header);
}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_SPARSE_
#define _BOOST_UBLAS_OPERATION_SPARSE_

#include <boost/numeric/ublas/traits.hpp>

// These scaled additions were borrowed from MTL unashamedly.
// But Alexei Novakov had a lot of ideas to improve these. Thanks.

namespace boost { namespace numeric { namespace ublas {

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI,
                 row_major_tag) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

        // ISSUE why is there a dense vector here?
        vector<value_type> temporary (e2 ().size2 ());
        temporary.clear ();
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, row_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
            size_type jb (temporary.size ());
            size_type je (0);
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                // temporary.plus_assign (*it2 * row (e2 (), it2.index2 ()));
                matrix_row<expression2_type> mr (e2 (), it2.index2 ());
                typename matrix_row<expression2_type>::const_iterator itr (mr.begin ());
                typename matrix_row<expression2_type>::const_iterator itr_end (mr.end ());
                while (itr != itr_end) {
                    size_type j (itr.index ());
                    temporary (j) += *it2 * *itr;
                    jb = (std::min) (jb, j);
                    je = (std::max) (je, j);
                    ++ itr;
                }
                ++ it2;
            }
            for (size_type j = jb; j < je + 1; ++ j) {
                if (temporary (j) != value_type/*zero*/()) {
                    // FIXME we'll need to extend the container interface!
                    // m.push_back (it1.index1 (), j, temporary (j));
                    // FIXME What to do with adaptors?
                    // m.insert (it1.index1 (), j, temporary (j));
                    if (triangular_restriction::other (it1.index1 (), j))
                        m (it1.index1 (), j) = temporary (j);
                    temporary (j) = value_type/*zero*/();
                }
            }
            ++ it1;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI,
                 column_major_tag) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

        // ISSUE why is there a dense vector here?
        vector<value_type> temporary (e1 ().size1 ());
        temporary.clear ();
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, column_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
            size_type ib (temporary.size ());
            size_type ie (0);
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                // column (m, it2.index2 ()).plus_assign (*it1 * column (e1 (), it1.index1 ()));
                matrix_column<expression1_type> mc (e1 (), it1.index1 ());
                typename matrix_column<expression1_type>::const_iterator itc (mc.begin ());
                typename matrix_column<expression1_type>::const_iterator itc_end (mc.end ());
                while (itc != itc_end) {
                    size_type i (itc.index ());
                    temporary (i) += *it1 * *itc;
                    ib = (std::min) (ib, i);
                    ie = (std::max) (ie, i);
                    ++ itc;
                }
                ++ it1;
            }
            for (size_type i = ib; i < ie + 1; ++ i) {
                if (temporary (i) != value_type/*zero*/()) {
                    // FIXME we'll need to extend the container interface!
                    // m.push_back (i, it2.index2 (), temporary (i));
                    // FIXME What to do with adaptors?
                    // m.insert (i, it2.index2 (), temporary (i));
                    if (triangular_restriction::other (i, it2.index2 ()))
                        m (i, it2.index2 ()) = temporary (i);
                    temporary (i) = value_type/*zero*/();
                }
            }
            ++ it2;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI, bool init = true) {
        typedef typename M::value_type value_type;
        typedef TRI triangular_restriction;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return sparse_prod (e1, e2, m, triangular_restriction (), orientation_category ());
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 TRI) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        // FIXME needed for c_matrix?!
        // return sparse_prod (e1, e2, m, triangular_restriction (), false);
        return sparse_prod (e1, e2, m, triangular_restriction (), true);
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return sparse_prod (e1, e2, m, full (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        // FIXME needed for c_matrix?!
        // return sparse_prod (e1, e2, m, full (), false);
        return sparse_prod (e1, e2, m, full (), true);
    }

}}}

#endif


// Copyright 2016 Robert Maier, Technical University Munich
#include "dvo.hpp"

#include <iostream>
#include <sstream>
#include <string>

#include <Eigen/Cholesky>
#include <sophus/se3.hpp>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

DVO::DVO() :
    numPyramidLevels_(2),
    useWeights_(true),
    numIterations_(500),
    algo_(LevenbergMarquardt)
    //algo_(GaussNewton)
{
}

DVO::~DVO()
{
    for (int i = 0; i < numPyramidLevels_; ++i)
    {
        delete[] J_[i];
        delete[] residuals_[i];
        delete[] weights_[i];
    }
}

void DVO::init(int w, int h, const Eigen::Matrix3f &K)
{
    // pyramid level size
    int wDown = w;
    int hDown = h;
    int n = wDown*hDown;
    sizePyramid_.push_back(cv::Size(wDown, hDown));

    // gradients
    cv::Mat gradX = cv::Mat::zeros(h, w, CV_32FC1);
    gradX_.push_back(gradX);
    cv::Mat gradY = cv::Mat::zeros(h, w, CV_32FC1);
    gradY_.push_back(gradY);

    // Jacobian
    float* J = new float[n*6];
    J_.push_back(J);
    // residuals
    float* residuals = new float[n];
    residuals_.push_back(residuals);
    // per-residual weights
    float* weights = new float[n];
    weights_.push_back(weights);

    // camera matrix
    kPyramid_.push_back(K);

    for (int i = 1; i < numPyramidLevels_; ++i)
    {
        // pyramid level size
        wDown = wDown / 2;
        hDown = hDown / 2;
        int n = wDown*hDown;
        sizePyramid_.push_back(cv::Size(wDown, hDown));

        // gradients
        cv::Mat gradXdown = cv::Mat::zeros(hDown, wDown, CV_32FC1);
        gradX_.push_back(gradXdown);
        cv::Mat gradYdown = cv::Mat::zeros(hDown, wDown, CV_32FC1);
        gradY_.push_back(gradYdown);

        // Jacobian
        float* J = new float[n*6];
        J_.push_back(J);
        // residuals
        float* residuals = new float[n];
        residuals_.push_back(residuals);
        // per-residual weights
        float* weights = new float[n];
        weights_.push_back(weights);

        // downsample camera matrix
        Eigen::Matrix3f kDown = kPyramid_[i-1];
        kDown(0, 2) += 0.5f;
        kDown(1, 2) += 0.5f;
        kDown.topLeftCorner(2, 3) = kDown.topLeftCorner(2, 3) * 0.5f;
        kDown(0, 2) -= 0.5f;
        kDown(1, 2) -= 0.5f;
        kPyramid_.push_back(kDown);
        //std::cout << "Camera matrix (level " << i << "): " << kDown << std::endl;
    }
}

void DVO::convertSE3ToTf(const Vec6f &xi, Eigen::Matrix3f &rot, Eigen::Vector3f &t)
{
    // rotation
    Sophus::SE3f se3 = Sophus::SE3f::exp(xi);
    Eigen::Matrix4f mat = se3.matrix();
    rot = mat.topLeftCorner(3, 3);
    t = mat.topRightCorner(3, 1);
}

void DVO::convertSE3ToTf(const Vec6f &xi, Eigen::Matrix4f &pose)
{
    Sophus::SE3f se3 = Sophus::SE3f::exp(xi);
    pose = se3.matrix();
}

void DVO::convertTfToSE3(const Eigen::Matrix3f &rot, const Eigen::Vector3f &t, Vec6f &xi)
{
    Sophus::SE3f se3(rot, t);
    xi = Sophus::SE3f::log(se3);
}

void DVO::convertTfToSE3(const Eigen::Matrix4f &pose, Vec6f &xi)
{
    Eigen::Matrix3f rot = pose.topLeftCorner(3, 3);
    Eigen::Vector3f t = pose.topRightCorner(3, 1);
    convertTfToSE3(rot, t, xi);
}

cv::Mat DVO::downsampleGray(const cv::Mat &gray)
{
    const float* ptrIn = (const float*)gray.data;
    int w = gray.cols;
    int h = gray.rows;
    int wDown = w/2;
    int hDown = h/2;

    cv::Mat grayDown = cv::Mat::zeros(hDown, wDown, gray.type());
    float* ptrOut = (float*)grayDown.data;
    for (size_t y = 0; y < hDown; ++y)
    {
        for (size_t x = 0; x < wDown; ++x)
        {
            float sum = 0.0f;
            sum += ptrIn[2*y * w + 2*x] * 0.25f;
            sum += ptrIn[2*y * w + 2*x+1] * 0.25f;
            sum += ptrIn[(2*y+1) * w + 2*x] * 0.25f;
            sum += ptrIn[(2*y+1) * w + 2*x+1] * 0.25f;
            ptrOut[y*wDown + x] = sum;
        }
    }

    return grayDown;
}

cv::Mat DVO::downsampleDepth(const cv::Mat &depth)
{
    const float* ptrIn = (const float*)depth.data;
    int w = depth.cols;
    int h = depth.rows;
    int wDown = w/2;
    int hDown = h/2;

    // downscaling by averaging the inverse depth
    cv::Mat depthDown = cv::Mat::zeros(hDown, wDown, depth.type());
    float* ptrOut = (float*)depthDown.data;
    for (size_t y = 0; y < hDown; ++y)
    {
        for (size_t x = 0; x < wDown; ++x)
        {
            float d0 = ptrIn[2*y * w + 2*x];
            float d1 = ptrIn[2*y * w + 2*x+1];
            float d2 = ptrIn[(2*y+1) * w + 2*x];
            float d3 = ptrIn[(2*y+1) * w + 2*x+1];

            int cnt = 0;
            float sum = 0.0f;
            if (d0 != 0.0f)
            {
                sum += 1.0f / d0;
                ++cnt;
            }
            if (d1 != 0.0f)
            {
                sum += 1.0f / d1;
                ++cnt;
            }
            if (d2 != 0.0f)
            {
                sum += 1.0f / d2;
                ++cnt;
            }
            if (d3 != 0.0f)
            {
                sum += 1.0f / d3;
                ++cnt;
            }

            if (cnt > 0)
            {
                float dInv = sum / float(cnt);
                if (dInv != 0.0f)
                    ptrOut[y*wDown + x] = 1.0f / dInv;
            }
        }
    }

    return depthDown;
}

void DVO::computeGradient(const cv::Mat &gray, cv::Mat &gradient, int direction)
{
    int dirX = 1;
    int dirY = 0;
    if (direction == 1)
    {
        dirX = 0;
        dirY = 1;
    }

    // compute gradient manually using finite differences
    int w = gray.cols;
    int h = gray.rows;
    const float* ptrIn = (const float*)gray.data;
    gradient.setTo(0);
    float* ptrOut = (float*)gradient.data;

    int yStart = dirY;
    int yEnd = h - dirY;
    int xStart = dirX;
    int xEnd = w - dirX;
    for (size_t y = yStart; y < yEnd; ++y)
    {
        for (size_t x = xStart; x < xEnd; ++x)
        {
            float v0;
            float v1;
            if (direction == 1)
            {
                // y-direction
                v0 = ptrIn[(y-1)*w + x];
                v1 = ptrIn[(y+1)*w + x];
            }
            else
            {
                // x-direction
                v0 = ptrIn[y*w + (x-1)];
                v1 = ptrIn[y*w + (x+1)];
            }
            ptrOut[y*w + x] = 0.5f * (v1 - v0);
        }
    }
}

float DVO::interpolate(const float* ptrImgIntensity, float x, float y, int w, int h)
{
    float valCur = std::numeric_limits<float>::quiet_NaN();

#if 0
    // direct lookup, no interpolation
    int x0 = static_cast<int>(x + 0.5f);
    int y0 = static_cast<int>(y + 0.5f);
    if (x0 >= 0 && x0 < w && y0 >= 0 && y0 < h)
        valCur = ptrImgIntensity[y0*w + x0];
#else
    //bilinear interpolation
    int x0 = static_cast<int>(x);
    int y0 = static_cast<int>(y);
    int x1 = x0 + 1;
    int y1 = y0 + 1;

    float x1_weight = x - static_cast<float>(x0);
    float y1_weight = y - static_cast<float>(y0);
    float x0_weight = 1.0f - x1_weight;
    float y0_weight = 1.0f - y1_weight;

    if (x0 < 0 || x0 >= w)
        x0_weight = 0.0f;
    if (x1 < 0 || x1 >= w)
        x1_weight = 0.0f;
    if (y0 < 0 || y0 >= h)
        y0_weight = 0.0f;
    if (y1 < 0 || y1 >= h)
        y1_weight = 0.0f;
    float w00 = x0_weight * y0_weight;
    float w10 = x1_weight * y0_weight;
    float w01 = x0_weight * y1_weight;
    float w11 = x1_weight * y1_weight;

    float sumWeights = w00 + w10 + w01 + w11;
    float sum = 0.0f;
    if (w00 > 0.0f)
        sum += ptrImgIntensity[y0*w + x0] * w00;
    if (w01 > 0.0f)
        sum += ptrImgIntensity[y1*w + x0] * w01;
    if (w10 > 0.0f)
        sum += ptrImgIntensity[y0*w + x1] * w10;
    if (w11 > 0.0f)
        sum += ptrImgIntensity[y1*w + x1] * w11;

    if (sumWeights > 0.0f)
        valCur = sum / sumWeights;
#endif

    return valCur;
}

float DVO::calculateError(const float* residuals, int n)
{
    float error = 0.0f;
    int numValid = 0;
    for (int i = 0; i < n; ++i)
    {
        if (residuals[i] != 0.0f)
        {
            error += residuals[i] * residuals[i];
            ++numValid;
        }
    }
    if (numValid > 0)
        error = error / static_cast<float>(numValid);
    return error;
}

void DVO::calculateErrorImage(const float* residuals, int w, int h, cv::Mat &errorImage)
{
    cv::Mat imgResiduals = cv::Mat::zeros(h, w, CV_32FC1);
    float* ptrResiduals = (float*)imgResiduals.data;

    // fill residuals image
    for (size_t y = 0; y < h; ++y)
    {
        for (size_t x = 0; x < w; ++x)
        {
            size_t off = y*w + x;
            if (residuals[off] != 0.0f)
                ptrResiduals[off] = residuals[off];
        }
    }

    imgResiduals.convertTo(errorImage, CV_8SC1, 127.0);
}

void DVO::calculateError(const cv::Mat &grayRef, const cv::Mat &depthRef,
                         const cv::Mat &grayCur, const cv::Mat &depthCur,
                         const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                         float* residuals)
{
    // create residual image
    int w = grayRef.cols;
    int h = grayRef.rows;

    // camera intrinsics
    float fx = K(0, 0);
    float fy = K(1, 1);
    float cx = K(0, 2);
    float cy = K(1, 2);
    float fxInv = 1.0f / fx;
    float fyInv = 1.0f / fy;

    // convert SE3 to rotation matrix and translation vector
    Eigen::Matrix3f rotMat;
    Eigen::Vector3f t;
    convertSE3ToTf(xi, rotMat, t);

    const float* ptrGrayRef = (const float*)grayRef.data;
    const float* ptrDepthRef = (const float*)depthRef.data;
    const float* ptrGrayCur = (const float*)grayCur.data;
    const float* ptrDepthCur = (const float*)depthCur.data;

    for (size_t y = 0; y < h; ++y)
    {
        for (size_t x = 0; x < w; ++x)
        {
            size_t off = y*w + x;
            float residual = 0.0f;

            // project 2d point back into 3d using its depth
            float dRef = ptrDepthRef[y*w + x];
            if (dRef > 0.0)
            {
                float x0 = (static_cast<float>(x) - cx) * fxInv;
                float y0 = (static_cast<float>(y) - cy) * fyInv;
                float scale = 1.0f;
                //scale = std::sqrt(x0*x0 + y0*y0 + 1.0f);
                dRef = dRef * scale;
                x0 = x0 * dRef;
                y0 = y0 * dRef;

                // transform reference 3d point into current frame
                // reference 3d point
                Eigen::Vector3f pt3Ref(x0, y0, dRef);
                Eigen::Vector3f pt3Cur = rotMat * pt3Ref + t;
                if (pt3Cur[2] > 0.0f)
                {
                    // project 3d point to 2d
                    Eigen::Vector3f pt2CurH = K * pt3Cur;
                    float ptZinv = 1.0f / pt2CurH[2];
                    float px = pt2CurH[0] * ptZinv;
                    float py = pt2CurH[1] * ptZinv;

                    // interpolate residual
                    float valCur = interpolate(ptrGrayCur, px, py, w, h);
                    if (!std::isnan(valCur))
                    {
                        float valRef = ptrGrayRef[off];
                        float valDiff = valRef - valCur;
                        residual = valDiff;
                    }
                }
            }
            residuals[off] = residual;
        }
    }
}

void DVO::calculateMeanStdDev(const float* residuals, float &mean, float &stdDev, int n)
{
    float meanVal = 0.0f;
    for (int i = 0; i < n; ++i)
        meanVal += residuals[i];
    mean = meanVal / static_cast<float>(n);

    float variance = 0.0f;
    for (int i = 0; i < n; ++i)
        variance += (residuals[i] - mean) * (residuals[i] - mean);
    stdDev = std::sqrt(variance);
}

void DVO::computeWeights(const float* residuals, float* weights, int n)
{
#if 0
    // no weighting
    for (int i = 0; i < n; ++i)
        weights[i] = 1.0f;
#if 0
    // squared residuals
    for (int i = 0; i < n; ++i)
        residuals[i] = residuals[i] * residuals[i];
    return;
#endif
#endif

    // compute mean and standard deviation
    float mean, stdDev;
    calculateMeanStdDev(residuals, mean, stdDev, n);

    // compute robust Huber weights

    float k = 1.345f * stdDev;
    //float k = 0.15f * stdDev;
    for (int i = 0; i < n; ++i)
    {
        float w;
        if (std::abs(residuals[i]) <= k)
            w = 1.0f;
        else
            w = k / std::abs(residuals[i]);
        weights[i] = w;
    }
    /*
    #if 0
    // compute Tukey weights
    float k = 4.6851f * stdDev;
    for (int i = 0; i<n; i++)
    {
        float w;
        if (std::abs(residuals[i]) <= k)
            w = std::pow((1.0f-residuals[i]*residuals[i] / (k*k)),2);
        else
            w = 0.0f;
        weights[i] = w;
    }

    // compute Cauchy weights
    float k = 1.35f * stdDev;
    for (int i = 0; i<n; i++)
    {
        float w;
        w = 1.0f/(1+(residuals[i]*residuals[i] / (k*k)));
        weights[i] = w;
    }
    */

}

void DVO::applyWeights(const float* weights, float* residuals, int n)
{
    for (size_t i = 0; i < n; ++i)
    {
        // weight residual
        residuals[i] = residuals[i] * weights[i];
    }
}

void DVO::deriveNumeric(const cv::Mat &grayRef, const cv::Mat &depthRef,
                                  const cv::Mat &grayCur, const cv::Mat &depthCur,
                                  const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                                  float* residuals, float* J)
{
    float epsilon = 1e-6;
    float scale = 1.0f / epsilon;

    int w = grayRef.cols;
    int h = grayRef.rows;
    int n = w*h;

    // calculate per-pixel residuals
    calculateError(grayRef, depthRef, grayCur, depthCur, xi, K, residuals);

    // create and fill Jacobian column by column
    float* residualsInc = new float[n];
    for (int j = 0; j < 6; ++j)
    {
        Eigen::VectorXf unitVec = Eigen::VectorXf::Zero(6);
        unitVec[j] = epsilon;

        // left-multiplicative increment on SE3
        Eigen::VectorXf xiEps = Sophus::SE3f::log(Sophus::SE3f::exp(unitVec) * Sophus::SE3f::exp(xi));

        calculateError(grayRef, depthRef, grayCur, depthCur, xiEps, K, residualsInc);
        for (int i = 0; i < n; ++i)
            J[i*6 + j] = (residualsInc[i] - residuals[i]) * scale;
    }
    delete[] residualsInc;
}

void DVO::compute_JtR(const float* J, const float* residuals, Vec6f &b, int validRows)
{
    int n = 6;
    int m = validRows;

    // compute b = Jt*r
    for (int j = 0; j < n; ++j)
    {
        float val = 0.0f;
        for (int i = 0; i < m; ++i)
            val += J[i*6 + j] * residuals[i];
        b[j] = val;
    }
}

void DVO::compute_JtJ(const float* J, Mat6f &A, const float* weights, int validRows, bool useWeights)
{
    int n = 6;
    int m = validRows;

    // compute A = Jt*J
    for (int k = 0; k < n; ++k)
    {
        for (int j = 0; j < n; ++j)
        {
            float val = 0.0f;
            for (int i = 0; i < m; ++i)
            {
                float valSqr = J[i*6 + j] * J[i*6 + k];
                if (useWeights)
                    valSqr *= weights[i];
                val += valSqr;
            }
            A(k, j) = val;
        }
    }
}

void DVO::deriveAnalytic(const cv::Mat &grayRef, const cv::Mat &depthRef,
                   const cv::Mat &grayCur, const cv::Mat &depthCur,
                   const cv::Mat &gradX, const cv::Mat &gradY,
                   const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                   float* residuals, float* J)
{
    // reference input images
    int w = grayRef.cols;
    int h = grayRef.rows;
    int n = w*h;
    const float* ptrDepthRef = (const float*)depthRef.data;

    // camera intrinsics
    float fx = K(0, 0);
    float fy = K(1, 1);
    float cx = K(0, 2);
    float cy = K(1, 2);
    float fxInv = 1.0f / fx;
    float fyInv = 1.0f / fy;

    // convert SE3 to rotation matrix and translation vector
    Eigen::Matrix3f rotMat;
    Eigen::Vector3f t;
    convertSE3ToTf(xi, rotMat, t);

    // calculate per-pixel residuals
    calculateError(grayRef, depthRef, grayCur, depthCur, xi, K, residuals);

    // reference gradient images
    const float* ptrGradX = (const float*)gradX.data;
    const float* ptrGradY = (const float*)gradY.data;

    // create and fill Jacobian row by row
    float residualRowJ[6];
    for (size_t y = 0; y < h; ++y)
    {
        for (size_t x = 0; x < w; ++x)
        {
            size_t off = y*w + x;

            // project 2d point back into 3d using its depth
            float dRef = ptrDepthRef[y*w + x];
            if (dRef > 0.0f)
            {
                float x0 = (static_cast<float>(x) - cx) * fxInv;
                float y0 = (static_cast<float>(y) - cy) * fyInv;
                float scale = 1.0f;
                //scale = std::sqrt(x0*x0 + y0*y0 + 1.0);
                dRef = dRef * scale;
                x0 = x0 * dRef;
                y0 = y0 * dRef;

                // transform reference 3d point into current frame
                // reference 3d point
                Eigen::Vector3f pt3Ref(x0, y0, dRef);
                Eigen::Vector3f pt3 = rotMat * pt3Ref + t;
                if (pt3[2] > 0.0f)
                {
                    // project 3d point to 2d
                    Eigen::Vector3f pt2CurH = K * pt3;
                    float ptZinv = 1.0f / pt2CurH[2];
                    float px = pt2CurH[0] * ptZinv;
                    float py = pt2CurH[1] * ptZinv;

                    // compute interpolated image gradient
                    float dX = interpolate(ptrGradX, px, py, w, h);
                    float dY = interpolate(ptrGradY, px, py, w, h);
                    if (!std::isnan(dX) && !std::isnan(dY))
                    {
                        dX = fx * dX;
                        dY = fy * dY;
                        float pt3Zinv = 1.0f / pt3[2];

                        // shorter computation
                        residualRowJ[0] = dX * pt3Zinv;
                        residualRowJ[1] = dY * pt3Zinv;
                        residualRowJ[2] = - (dX * pt3[0] + dY * pt3[1]) * pt3Zinv * pt3Zinv;
                        residualRowJ[3] = - (dX * pt3[0] * pt3[1]) * pt3Zinv * pt3Zinv - dY * (1 + (pt3[1] * pt3Zinv) * (pt3[1] * pt3Zinv));
                        residualRowJ[4] = + dX * (1.0 + (pt3[0] * pt3Zinv) * (pt3[0] * pt3Zinv)) + (dY * pt3[0] * pt3[1]) * pt3Zinv * pt3Zinv;
                        residualRowJ[5] = (- dX * pt3[1] + dY * pt3[0]) * pt3Zinv;
                    }
                }
            }

            // set 1x6 Jacobian row for current residual
            // invert Jacobian according to kerl2012msc.pdf (necessary?)
            for (int j = 0; j < 6; ++j)
                J[off*6 + j] = - residualRowJ[j];
        }
    }
}

void DVO::buildPyramid(const cv::Mat &depth, const cv::Mat &gray, std::vector<cv::Mat> &depthPyramid, std::vector<cv::Mat> &grayPyramid)
{
    grayPyramid.push_back(gray);
    depthPyramid.push_back(depth);

    for (int i = 1; i < numPyramidLevels_; ++i)
    {
        // downsample grayscale image
        cv::Mat grayDown = downsampleGray(grayPyramid[i-1]);
        grayPyramid.push_back(grayDown);

        // downsample depth image
        cv::Mat depthDown = downsampleDepth(depthPyramid[i-1]);
        depthPyramid.push_back(depthDown);
    }
}

void DVO::align(const cv::Mat &depthRef, const cv::Mat &grayRef, const cv::Mat &depthCur, const cv::Mat &grayCur, Eigen::Matrix4f &pose)
{
    // downsampling
    std::vector<cv::Mat> grayRefPyramid;
    std::vector<cv::Mat> depthRefPyramid;
    buildPyramid(depthRef, grayRef, depthRefPyramid, grayRefPyramid);

    std::vector<cv::Mat> grayCurPyramid;
    std::vector<cv::Mat> depthCurPyramid;
    buildPyramid(depthCur, grayCur, depthCurPyramid, grayCurPyramid);

    align(depthRefPyramid, grayRefPyramid, depthCurPyramid, grayCurPyramid, pose);
}

void DVO::align(const std::vector<cv::Mat> &depthRefPyramid, const std::vector<cv::Mat> &grayRefPyramid,
                const std::vector<cv::Mat> &depthCurPyramid, const std::vector<cv::Mat> &grayCurPyramid,
                Eigen::Matrix4f &pose)
{
    Vec6f xi;
    convertTfToSE3(pose, xi);

    Vec6f lastXi = Vec6f::Zero();

    int maxLevel = numPyramidLevels_-1;
    int minLevel = 1;
    float initGradDescStepSize = 1e-3f;
    float gradDescStepSize = initGradDescStepSize;

    Mat6f A;
    Mat6f diagMatA = Mat6f::Identity();
    Vec6f delta;

    for (int lvl = maxLevel; lvl >= minLevel; --lvl)
    {
        float lambda = 0.1f;

        int w = sizePyramid_[lvl].width;
        int h = sizePyramid_[lvl].height;
        int n = w*h;

        cv::Mat grayRef = grayRefPyramid[lvl];
        cv::Mat depthRef = depthRefPyramid[lvl];
        cv::Mat grayCur = grayCurPyramid[lvl];
        cv::Mat depthCur = depthCurPyramid[lvl];
        Eigen::Matrix3f kLevel = kPyramid_[lvl];
        //std::cout << "level " << level << " (size " << depthRef.cols << "x" << depthRef.rows << ")" << std::endl;

        // compute gradient images
        computeGradient(grayCur, gradX_[lvl], 0);
        computeGradient(grayCur, gradY_[lvl], 1);

        float errorLast = std::numeric_limits<float>::max();
        for (int itr = 0; itr < numIterations_; ++itr)
        {
            // compute residuals and Jacobian
#if 0
            deriveNumeric(grayRef, depthRef, grayCur, depthCur, xi, kLevel, residuals_[lvl], J_[lvl]);
#else
            deriveAnalytic(grayRef, depthRef, grayCur, depthCur, gradX_[lvl], gradY_[lvl], xi, kLevel, residuals_[lvl], J_[lvl]);
#endif

#if 0
            // compute and show error image
            cv::Mat errorImage;
            calculateErrorImage(residuals_[level], grayRef.cols, grayRef.rows, errorImage);
            std::stringstream ss;
            ss << dataFolder << "residuals_" << level << "_";
            ss << std::setw(2) << std::setfill('0') << itr << ".png";
            cv::imwrite(ss.str(), errorImage);
            cv::imshow("error", errorImage);
            cv::waitKey(100);
#endif

            // calculate error
            float error = calculateError(residuals_[lvl], n);

            if (useWeights_)
            {
                // compute robust weights
                computeWeights(residuals_[lvl], weights_[lvl], n);
                // apply robust weights
                applyWeights(weights_[lvl], residuals_[lvl], n);
            }

            // compute update
            Vec6f b;
            compute_JtR(J_[lvl], residuals_[lvl], b, n);

            if (algo_ == GradientDescent)
            {
                // Gradient Descent
                delta = -gradDescStepSize * b * (1.0f / b.norm());
            }
            else if (algo_ == GaussNewton)
            {
                // Gauss-Newton algorithm
                compute_JtJ(J_[lvl], A, weights_[lvl], n, useWeights_);
                // solve using Cholesky LDLT decomposition
                delta = -(A.ldlt().solve(b));
            }
            else if (algo_ == LevenbergMarquardt)
            {
                // Levenberg-Marquardt algorithm
                compute_JtJ(J_[lvl], A, weights_[lvl], n, useWeights_);
                diagMatA.diagonal() = lambda * A.diagonal();
                delta = -((A + diagMatA).ldlt().solve(b));
            }

            // apply update: left-multiplicative increment on SE3
            lastXi = xi;
            xi = Sophus::SE3f::log(Sophus::SE3f::exp(delta) * Sophus::SE3f::exp(xi));
#if 0
            std::cout << "delta = " << delta.transpose() << " size = " << delta.rows() << " x " << delta.cols() << std::endl;
            std::cout << "xi = " << xi.transpose() << std::endl;
#endif

            // compute error again
            error = calculateError(residuals_[lvl], n);

            if (algo_ == LevenbergMarquardt)
            {
                if (error >= errorLast)
                {
                    lambda = lambda * 5.0f;
                    xi = lastXi;

                    if (lambda > 5.0f)
                        break;
                }
                else
                {
                    lambda = lambda / 1.5f;
                }
            }
            else if (algo_ == GaussNewton)
            {
                // break if no improvement (0.99 or 0.995)
                if (error / errorLast > 0.995f)
                    break;
            }
            else if (algo_ == GradientDescent)
            {
                if (error >= errorLast)
                {
                    gradDescStepSize = gradDescStepSize * 0.5f;
                    if (gradDescStepSize <= initGradDescStepSize * 0.01f)
                        gradDescStepSize = initGradDescStepSize * 0.01f;
                    xi = lastXi;
                }
                else
                {
                    gradDescStepSize = gradDescStepSize * 2.0f;
                    if (gradDescStepSize >= initGradDescStepSize * 100.0f)
                        gradDescStepSize = initGradDescStepSize * 100.0f;

                    // break if no improvement (0.99 or 0.995)
                    if (error / errorLast > 0.995f)
                        break;
                }
            }

            errorLast = error;
        }
    }

    // store to output pose
    convertSE3ToTf(xi, pose);
}


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <cap/mp_values.h>
#include <cap/energy_storage_device.h>
#include <deal.II/base/types.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/base/quadrature_lib.h>
#include <iostream>

namespace cap
{

// reads database for finite element model and write database for equivalent
// circuit model
void compute_equivalent_circuit(
    boost::property_tree::ptree const &input_database,
    boost::property_tree::ptree &output_database)
{
  // TODO: of course we could clear the database or just overwrite but for
  // now let's just throw an exception if it is not empty
  if (!output_database.empty())
    throw std::runtime_error("output_database was not empty...");

  auto to_meters = [](double const &cm)
  {
    return 0.01 * cm;
  };
  auto to_square_meters = [](double const &cm2)
  {
    return 0.0001 * cm2;
  };

  double const cross_sectional_area =
      to_square_meters(input_database.get<double>("geometry.geometric_area"));
  // clang-format off
  double const electrode_width = to_meters(input_database.get<double>("geometry.anode_electrode_thickness"));
  double const separator_width = to_meters(input_database.get<double>("geometry.separator_thickness"      ));
  double const collector_width = to_meters(input_database.get<double>("geometry.anode_collector_thickness"));
  // clang-format on

  // getting the material parameters values
  std::shared_ptr<boost::property_tree::ptree> material_properties_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database.get_child("material_properties"));
  MPValuesParameters<2> mp_values_params(material_properties_database);
  std::shared_ptr<boost::property_tree::ptree> geometry_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database.get_child("geometry"));
  // build dummy cell iterator and set its material id. Because we use a dummy
  // triangulation, we can use MPI_COMM_WORLD.
  std::shared_ptr<dealii::distributed::Triangulation<2>> triangulation(
      new dealii::distributed::Triangulation<2>(MPI_COMM_WORLD));
  dealii::GridGenerator::hyper_cube(*triangulation);
  dealii::FE_Q<2> fe(1);
  dealii::DoFHandler<2> dof_handler(*triangulation);
  dof_handler.distribute_dofs(fe);
  mp_values_params.geometry = std::make_shared<Geometry<2>>(
      triangulation,
      std::make_shared<std::unordered_map<
          std::string, std::set<dealii::types::material_id>>>(
          std::initializer_list<std::pair<
              std::string const, std::set<dealii::types::material_id>>>{
              {"anode", std::set<dealii::types::material_id>{1}},
              {"separator", std::set<dealii::types::material_id>{2}},
              {"cathode", std::set<dealii::types::material_id>{3}},
              {"collector", std::set<dealii::types::material_id>{4, 5}}}),
      std::make_shared<std::unordered_map<
          std::string, std::set<dealii::types::material_id>>>(
          std::initializer_list<std::pair<
              std::string const, std::set<dealii::types::boundary_id>>>{
              {"anode", std::set<dealii::types::boundary_id>{1}},
              {"cathode", std::set<dealii::types::boundary_id>{2}}}));

  std::shared_ptr<MPValues<2>> mp_values =
      std::make_shared<SuperCapacitorMPValues<2>>(mp_values_params);
  dealii::DoFHandler<2>::active_cell_iterator cell = dof_handler.begin_active();
  dealii::FEValues<2> fe_values(fe, dealii::QGauss<2>(1),
                                dealii::update_default);
  fe_values.reinit(cell);
  // electrode
  cell->set_material_id(1); // <- matches the anode material_id in the
                            //    initializer list
  std::vector<double> electrode_solid_electrical_conductivity_values(1);
  std::vector<double> electrode_liquid_electrical_conductivity_values(1);
  mp_values->get_values("solid_electrical_conductivity", fe_values,
                        electrode_solid_electrical_conductivity_values);
  mp_values->get_values("liquid_electrical_conductivity", fe_values,
                        electrode_liquid_electrical_conductivity_values);
  double const electrode_resistivity =
      (1.0 / electrode_solid_electrical_conductivity_values[0] +
       1.0 / electrode_liquid_electrical_conductivity_values[0] +
       1.0 / (electrode_solid_electrical_conductivity_values[0] +
              electrode_liquid_electrical_conductivity_values[0])) /
      3.0;
  double const electrode_resistance =
      electrode_resistivity * electrode_width / cross_sectional_area;
  std::vector<double> electrode_specific_capacitance_values(1);
  mp_values->get_values("specific_capacitance", fe_values,
                        electrode_specific_capacitance_values);
  double const electrode_capacitance =
      electrode_specific_capacitance_values[0] * electrode_width *
      cross_sectional_area;
  std::vector<double> electrode_exchange_current_density_values(1);
  mp_values->get_values("faradaic_reaction_coefficient", fe_values,
                        electrode_exchange_current_density_values);
  double const electrode_leakage_resistance =
      1.0 / (electrode_exchange_current_density_values[0] * electrode_width *
             cross_sectional_area);
  std::cout << "ELECTRODE\n";
  std::cout << "    specific_capacitance="
            << electrode_specific_capacitance_values[0] << "\n";
  std::cout << "    solid_electrical_conductivity="
            << electrode_solid_electrical_conductivity_values[0] << "\n";
  std::cout << "    liquid_electrical_conductivity="
            << electrode_liquid_electrical_conductivity_values[0] << "\n";
  std::cout << "    exchange_current_density="
            << electrode_exchange_current_density_values[0] << "\n";
  std::cout << "    width=" << electrode_width << "\n";
  std::cout << "    cross_sectional_area=" << cross_sectional_area << "\n";
  // separator
  cell->set_material_id(2); // <- matches the separator material_id in the
                            //    initializer list
  std::vector<double> separator_liquid_electrical_conductivity_values(1);
  mp_values->get_values("liquid_electrical_conductivity", fe_values,
                        separator_liquid_electrical_conductivity_values);
  double const separator_resistivity =
      1.0 / separator_liquid_electrical_conductivity_values[0];
  double const separator_resistance =
      separator_resistivity * separator_width / cross_sectional_area;
  std::cout << "SEPARATOR\n";
  std::cout << "    liquid_electrical_conductivity="
            << separator_liquid_electrical_conductivity_values[0] << "\n";
  std::cout << "    width=" << separator_width << "\n";
  std::cout << "    cross_sectional_area=" << cross_sectional_area << "\n";
  // collector
  cell->set_material_id(4); // <- matches the collector  material_id in the
                            //    initializer list
  std::vector<double> collector_solid_electrical_conductivity_values(1);
  mp_values->get_values("solid_electrical_conductivity", fe_values,
                        collector_solid_electrical_conductivity_values);
  double const collector_resistivity =
      1.0 / collector_solid_electrical_conductivity_values[0];
  double const collector_resistance =
      collector_resistivity * collector_width / cross_sectional_area;
  std::cout << "COLLECTOR\n";
  std::cout << "    solid_electrical_conductivity="
            << collector_solid_electrical_conductivity_values[0] << "\n";
  std::cout << "    width=" << collector_width << "\n";
  std::cout << "    cross_sectional_area=" << cross_sectional_area << "\n";

  std::cout << "electrode_capacitance=" << electrode_capacitance << "\n";
  std::cout << "electrode_resistance=" << electrode_resistance << "\n";
  std::cout << "electrode_leakage_resistance=" << electrode_leakage_resistance
            << "\n";
  std::cout << "separator_resistance=" << separator_resistance << "\n";
  std::cout << "collector_resistance=" << collector_resistance << "\n";

  // compute the effective resistance and capacitance
  double const sandwich_capacitance = electrode_capacitance / 2.0;
  double const sandwich_resistance = 2.0 * electrode_resistance +
                                     separator_resistance +
                                     2.0 * collector_resistance;
  double const sandwich_leakage_resistance = 2.0 * electrode_leakage_resistance;
  std::cout << "sandwich_capacitance=" << sandwich_capacitance << "\n";
  std::cout << "sandwich_resistance=" << sandwich_resistance << "\n";
  std::cout << "sandwich_leakage_resistance=" << sandwich_leakage_resistance
            << "\n";

  output_database.put("capacitance", sandwich_capacitance);
  output_database.put("series_resistance", sandwich_resistance);
  output_database.put("parallel_resistance", sandwich_leakage_resistance);
  if (std::isfinite(sandwich_leakage_resistance))
    output_database.put("type", "ParallelRC");
  else
    output_database.put("type", "SeriesRC");
}

class EquivalentCircuitBuilder : public EnergyStorageDeviceBuilder
{
public:
  EquivalentCircuitBuilder()
  {
    register_energy_storage_device("EquivalentCircuit", this);
  }
  std::unique_ptr<EnergyStorageDevice>
  build(boost::property_tree::ptree const &ptree,
        boost::mpi::communicator const &comm) override
  {
    boost::property_tree::ptree other;
    compute_equivalent_circuit(ptree, other);
    return EnergyStorageDevice::build(other, comm);
  }
} global_EquivalentCircuitBuilder;

} // end namespace cap