// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// This code was entirely written by Nathan Wagner
// and is in the public domain.

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_ISEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_ISEA_HPP

#include <sstream>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace isea
    {
            static const double epsilon = std::numeric_limits<double>::epsilon();

            /* sqrt(5)/M_PI */
            static const double isea_scale = 0.8301572857837594396028083;
            /* 26.565051177 degrees */
            static const double v_lat = 0.46364760899944494524;
            /* 52.62263186 */
            static const double e_rad = 0.91843818702186776133;
            /* 10.81231696 */
            static const double f_rad = 0.18871053072122403508;
            /* R tan(g) sin(60) */
            static const double table_g = 0.6615845383;
            /* H = 0.25 R tan g = */
            static const double table_h = 0.1909830056;
            //static const double RPRIME = 0.91038328153090290025;
            static const double isea_std_lat = 1.01722196792335072101;
            static const double isea_std_lon = .19634954084936207740;

            template <typename T>
            inline T deg30_rad() { return T(30) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T deg120_rad() { return T(120) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T deg72_rad() { return T(72) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T deg90_rad() { return geometry::math::half_pi<T>(); }
            template <typename T>
            inline T deg144_rad() { return T(144) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T deg36_rad() { return T(36) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T deg108_rad() { return T(108) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T deg180_rad() { return geometry::math::pi<T>(); }

            inline bool downtri(int tri) { return (((tri - 1) / 5) % 2 == 1); }

            /*
             * Proj 4 provides its own entry points into
             * the code, so none of the library functions
             * need to be global
             */

            struct hex {
                    int iso;
                    int x, y, z;
            };

            /* y *must* be positive down as the xy /iso conversion assumes this */
            inline
            int hex_xy(struct hex *h) {
                if (!h->iso) return 1;
                if (h->x >= 0) {
                    h->y = -h->y - (h->x+1)/2;
                } else {
                    /* need to round toward -inf, not toward zero, so x-1 */
                    h->y = -h->y - h->x/2;
                }
                h->iso = 0;

                return 1;
            }

            inline
            int hex_iso(struct hex *h) {
                if (h->iso) return 1;

                if (h->x >= 0) {
                    h->y = (-h->y - (h->x+1)/2);
                } else {
                    /* need to round toward -inf, not toward zero, so x-1 */
                    h->y = (-h->y - (h->x)/2);
                }

                h->z = -h->x - h->y;
                h->iso = 1;
                return 1;
            }

            template <typename T>
            inline
            int hexbin2(T const& width, T x, T y, int *i, int *j)
            {
                T z, rx, ry, rz;
                T abs_dx, abs_dy, abs_dz;
                int ix, iy, iz, s;
                struct hex h;

                static const T cos_deg30 = cos(deg30_rad<T>());

                x = x / cos_deg30; /* rotated X coord */
                y = y - x / 2.0; /* adjustment for rotated X */

                /* adjust for actual hexwidth */
                x /= width;
                y /= width;

                z = -x - y;

                rx = floor(x + 0.5);
                ix = (int)rx;
                ry = floor(y + 0.5);
                iy = (int)ry;
                rz = floor(z + 0.5);
                iz = (int)rz;

                s = ix + iy + iz;

                if (s) {
                    abs_dx = fabs(rx - x);
                    abs_dy = fabs(ry - y);
                    abs_dz = fabs(rz - z);

                    if (abs_dx >= abs_dy && abs_dx >= abs_dz) {
                        ix -= s;
                    } else if (abs_dy >= abs_dx && abs_dy >= abs_dz) {
                        iy -= s;
                    } else {
                        iz -= s;
                    }
                }
                h.x = ix;
                h.y = iy;
                h.z = iz;
                h.iso = 1;

                hex_xy(&h);
                *i = h.x;
                *j = h.y;
                return ix * 100 + iy;
            }

            //enum isea_poly { isea_none = 0, isea_icosahedron = 20 };
            //enum isea_topology { isea_hexagon=6, isea_triangle=3, isea_diamond=4 };
            enum isea_address_form {
                isea_addr_geo, isea_addr_q2di, isea_addr_seqnum,
                isea_addr_interleave, isea_addr_plane, isea_addr_q2dd,
                isea_addr_projtri, isea_addr_vertex2dd, isea_addr_hex
            };

            template <typename T>
            struct isea_dgg {
                //isea_poly         polyhedron; /* ignored, icosahedron */
                T                 o_lat, o_lon, o_az; /* orientation, radians */
                int               pole; /* true if standard snyder */
                //isea_topology     topology; /* ignored, hexagon */
                int               aperture; /* valid values depend on partitioning method */
                int               resolution;
                T                 radius; /* radius of the earth in meters, ignored 1.0 */
                isea_address_form output; /* an isea_address_form */
                int               triangle; /* triangle of last transformed point */
                int               quad; /* quad of last transformed point */
                unsigned long     serial;
            };

            template <typename T>
            struct isea_pt {
                T x, y;
            };

            template <typename T>
            struct isea_geo {
                T lon, lat;
            };

            template <typename T>
            struct isea_address {
                int    type; /* enum isea_address_form */
                int    number;
                T      x,y; /* or i,j or lon,lat depending on type */
            };

            /* ENDINC */

            enum snyder_polyhedron {
                snyder_poly_hexagon = 0, snyder_poly_pentagon = 1,
                snyder_poly_tetrahedron = 2, snyder_poly_cube = 3,
                snyder_poly_octahedron = 4, snyder_poly_dodecahedron = 5,
                snyder_poly_icosahedron = 6
            };

            template <typename T>
            struct snyder_constants {
                T          g, G, theta, ea_w, ea_a, ea_b, g_w, g_a, g_b;
            };

            template <typename T>
            inline const snyder_constants<T> * constants()
            {
                /* TODO put these in radians to avoid a later conversion */
                static snyder_constants<T> result[] = {
                    {23.80018260, 62.15458023, 60.0, 3.75, 1.033, 0.968, 5.09, 1.195, 1.0},
                    {20.07675127, 55.69063953, 54.0, 2.65, 1.030, 0.983, 3.59, 1.141, 1.027},
                    {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
                    {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
                    {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
                    {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
                    {37.37736814, 36.0, 30.0, 17.27, 1.163, 0.860, 13.14, 1.584, 1.0}
                };
                return result;
            }

            template <typename T>
            inline const isea_geo<T> * vertex()
            {
                static isea_geo<T> result[] = {
                    { 0.0,              deg90_rad<T>()},
                    { deg180_rad<T>(),  v_lat},
                    {-deg108_rad<T>(),  v_lat},
                    {-deg36_rad<T>(),   v_lat},
                    { deg36_rad<T>(),   v_lat},
                    { deg108_rad<T>(),  v_lat},
                    {-deg144_rad<T>(), -v_lat},
                    {-deg72_rad<T>(),  -v_lat},
                    { 0.0,             -v_lat},
                    { deg72_rad<T>(),  -v_lat},
                    { deg144_rad<T>(), -v_lat},
                    { 0.0,             -deg90_rad<T>()}
                };
                return result;
            }

            /* TODO make an isea_pt array of the vertices as well */

            static int      tri_v1[] = {0, 0, 0, 0, 0, 0, 6, 7, 8, 9, 10, 2, 3, 4, 5, 1, 11, 11, 11, 11, 11};

            /* triangle Centers */
            template <typename T>
            inline const isea_geo<T> * icostriangles()
            {
                static isea_geo<T> result[] = {
                    { 0.0,              0.0},
                    {-deg144_rad<T>(),  e_rad},
                    {-deg72_rad<T>(),   e_rad},
                    { 0.0,              e_rad},
                    { deg72_rad<T>(),   e_rad},
                    { deg144_rad<T>(),  e_rad},
                    {-deg144_rad<T>(),  f_rad},
                    {-deg72_rad<T>(),   f_rad},
                    { 0.0,              f_rad},
                    { deg72_rad<T>(),   f_rad},
                    { deg144_rad<T>(),  f_rad},
                    {-deg108_rad<T>(), -f_rad},
                    {-deg36_rad<T>(),  -f_rad},
                    { deg36_rad<T>(),  -f_rad},
                    { deg108_rad<T>(), -f_rad},
                    { deg180_rad<T>(), -f_rad},
                    {-deg108_rad<T>(), -e_rad},
                    {-deg36_rad<T>(),  -e_rad},
                    { deg36_rad<T>(),  -e_rad},
                    { deg108_rad<T>(), -e_rad},
                    { deg180_rad<T>(), -e_rad},
                };
                return result;
            }

            template <typename T>
            inline T az_adjustment(int triangle)
            {
                T          adj;

                isea_geo<T> v;
                isea_geo<T> c;

                v = vertex<T>()[tri_v1[triangle]];
                c = icostriangles<T>()[triangle];

                /* TODO looks like the adjustment is always either 0 or 180 */
                /* at least if you pick your vertex carefully */
                adj = atan2(cos(v.lat) * sin(v.lon - c.lon),
                        cos(c.lat) * sin(v.lat)
                        - sin(c.lat) * cos(v.lat) * cos(v.lon - c.lon));
                return adj;
            }

            template <typename T>
            inline isea_pt<T> isea_triangle_xy(int triangle)
            {
                isea_pt<T>  c;
                T Rprime = 0.91038328153090290025;

                triangle = (triangle - 1) % 20;

                c.x = table_g * ((triangle % 5) - 2) * 2.0;
                if (triangle > 9) {
                    c.x += table_g;
                }
                switch (triangle / 5) {
                case 0:
                    c.y = 5.0 * table_h;
                    break;
                case 1:
                    c.y = table_h;
                    break;
                case 2:
                    c.y = -table_h;
                    break;
                case 3:
                    c.y = -5.0 * table_h;
                    break;
                default:
                    /* should be impossible */
                    BOOST_THROW_EXCEPTION( projection_exception() );
                };
                c.x *= Rprime;
                c.y *= Rprime;

                return c;
            }

            /* snyder eq 14 */
            template <typename T>
            inline T sph_azimuth(T const& f_lon, T const& f_lat, T const& t_lon, T const& t_lat)
            {
                T          az;

                az = atan2(cos(t_lat) * sin(t_lon - f_lon),
                       cos(f_lat) * sin(t_lat)
                       - sin(f_lat) * cos(t_lat) * cos(t_lon - f_lon)
                    );
                return az;
            }

            /* coord needs to be in radians */
            template <typename T>
            inline int isea_snyder_forward(isea_geo<T> * ll, isea_pt<T> * out)
            {
                static T const two_pi = detail::two_pi<T>();
                static T const d2r = geometry::math::d2r<T>();

                int             i;

                /*
                 * spherical distance from center of polygon face to any of its
                 * vertexes on the globe
                 */
                T          g;

                /*
                 * spherical angle between radius vector to center and adjacent edge
                 * of spherical polygon on the globe
                 */
                T          G;

                /*
                 * plane angle between radius vector to center and adjacent edge of
                 * plane polygon
                 */
                T          theta;

                /* additional variables from snyder */
                T          q, Rprime, H, Ag, Azprime, Az, dprime, f, rho,
                                x, y;

                /* variables used to store intermediate results */
                T          cot_theta, tan_g, az_offset;

                /* how many multiples of 60 degrees we adjust the azimuth */
                int             Az_adjust_multiples;

                snyder_constants<T> c;

                /*
                 * TODO by locality of reference, start by trying the same triangle
                 * as last time
                 */

                /* TODO put these constants in as radians to begin with */
                c = constants<T>()[snyder_poly_icosahedron];
                theta = c.theta * d2r;
                g = c.g * d2r;
                G = c.G * d2r;

                for (i = 1; i <= 20; i++) {
                    T          z;
                    isea_geo<T> center;

                    center = icostriangles<T>()[i];

                    /* step 1 */
                    z = acos(sin(center.lat) * sin(ll->lat)
                         + cos(center.lat) * cos(ll->lat) * cos(ll->lon - center.lon));

                    /* not on this triangle */
                    if (z > g + 0.000005) { /* TODO DBL_EPSILON */
                        continue;
                    }

                    Az = sph_azimuth(center.lon, center.lat, ll->lon, ll->lat);

                    /* step 2 */

                    /* This calculates "some" vertex coordinate */
                    az_offset = az_adjustment<T>(i);

                    Az -= az_offset;

                    /* TODO I don't know why we do this.  It's not in snyder */
                    /* maybe because we should have picked a better vertex */
                    if (Az < 0.0) {
                        Az += two_pi;
                    }
                    /*
                     * adjust Az for the point to fall within the range of 0 to
                     * 2(90 - theta) or 60 degrees for the hexagon, by
                     * and therefore 120 degrees for the triangle
                     * of the icosahedron
                     * subtracting or adding multiples of 60 degrees to Az and
                     * recording the amount of adjustment
                     */

                    Az_adjust_multiples = 0;
                    while (Az < 0.0) {
                        Az += deg120_rad<T>();
                        Az_adjust_multiples--;
                    }
                    while (Az > deg120_rad<T>() + epsilon) {
                        Az -= deg120_rad<T>();
                        Az_adjust_multiples++;
                    }

                    /* step 3 */
                    cot_theta = 1.0 / tan(theta);
                    tan_g = tan(g);    /* TODO this is a constant */

                    /* Calculate q from eq 9. */
                    /* TODO cot_theta is cot(30) */
                    q = atan2(tan_g, cos(Az) + sin(Az) * cot_theta);

                    /* not in this triangle */
                    if (z > q + 0.000005) {
                        continue;
                    }
                    /* step 4 */

                    /* Apply equations 5-8 and 10-12 in order */

                    /* eq 5 */
                    /* Rprime = 0.9449322893 * R; */
                    /* R' in the paper is for the truncated */
                    Rprime = 0.91038328153090290025;

                    /* eq 6 */
                    H = acos(sin(Az) * sin(G) * cos(g) - cos(Az) * cos(G));

                    /* eq 7 */
                    /* Ag = (Az + G + H - deg180_rad) * M_PI * R * R / deg180_rad; */
                    Ag = Az + G + H - deg180_rad<T>();

                    /* eq 8 */
                    Azprime = atan2(2.0 * Ag, Rprime * Rprime * tan_g * tan_g - 2.0 * Ag * cot_theta);

                    /* eq 10 */
                    /* cot(theta) = 1.73205080756887729355 */
                    dprime = Rprime * tan_g / (cos(Azprime) + sin(Azprime) * cot_theta);

                    /* eq 11 */
                    f = dprime / (2.0 * Rprime * sin(q / 2.0));

                    /* eq 12 */
                    rho = 2.0 * Rprime * f * sin(z / 2.0);

                    /*
                     * add back the same 60 degree multiple adjustment from step
                     * 2 to Azprime
                     */

                    Azprime += deg120_rad<T>() * Az_adjust_multiples;

                    /* calculate rectangular coordinates */

                    x = rho * sin(Azprime);
                    y = rho * cos(Azprime);

                    /*
                     * TODO
                     * translate coordinates to the origin for the particular
                     * hexagon on the flattened polyhedral map plot
                     */

                    out->x = x;
                    out->y = y;

                    return i;
                }

                /*
                 * should be impossible, this implies that the coordinate is not on
                 * any triangle
                 */

                //fprintf(stderr, "impossible transform: %f %f is not on any triangle\n",
                //    ll->lon * geometry::math::r2d<double>(), ll->lat * geometry::math::r2d<double>());
                std::stringstream ss;
                ss << "impossible transform: " << ll->lon * geometry::math::r2d<T>()
                   << " " << ll->lat * geometry::math::r2d<T>() << " is not on any triangle.";

                BOOST_THROW_EXCEPTION( projection_exception(ss.str()) );

                /* not reached */
                return 0;        /* supresses a warning */
            }

            /*
             * return the new coordinates of any point in orginal coordinate system.
             * Define a point (newNPold) in orginal coordinate system as the North Pole in
             * new coordinate system, and the great circle connect the original and new
             * North Pole as the lon0 longitude in new coordinate system, given any point
             * in orginal coordinate system, this function return the new coordinates.
             */

            /* formula from Snyder, Map Projections: A working manual, p31 */
            /*
             * old north pole at np in new coordinates
             * could be simplified a bit with fewer intermediates
             *
             * TODO take a result pointer
             */
            template <typename T>
            inline isea_geo<T> snyder_ctran(isea_geo<T> * np, isea_geo<T> * pt)
            {
                static T const pi = detail::pi<T>();
                static T const two_pi = detail::two_pi<T>();

                isea_geo<T> npt;
                T           alpha, phi, lambda, lambda0, beta, lambdap, phip;
                T           sin_phip;
                T           lp_b;    /* lambda prime minus beta */
                T           cos_p, sin_a;

                phi = pt->lat;
                lambda = pt->lon;
                alpha = np->lat;
                beta = np->lon;
                lambda0 = beta;

                cos_p = cos(phi);
                sin_a = sin(alpha);

                /* mpawm 5-7 */
                sin_phip = sin_a * sin(phi) - cos(alpha) * cos_p * cos(lambda - lambda0);

                /* mpawm 5-8b */

                /* use the two argument form so we end up in the right quadrant */
                lp_b = atan2(cos_p * sin(lambda - lambda0),
                   (sin_a * cos_p * cos(lambda - lambda0) + cos(alpha) * sin(phi)));

                lambdap = lp_b + beta;

                /* normalize longitude */
                /* TODO can we just do a modulus ? */
                lambdap = fmod(lambdap, two_pi);
                while (lambdap > pi)
                    lambdap -= two_pi;
                while (lambdap < -pi)
                    lambdap += two_pi;

                phip = asin(sin_phip);

                npt.lat = phip;
                npt.lon = lambdap;

                return npt;
            }

            template <typename T>
            inline isea_geo<T> isea_ctran(isea_geo<T> * np, isea_geo<T> * pt, T const& lon0)
            {
                static T const pi = detail::pi<T>();
                static T const two_pi = detail::two_pi<T>();

                isea_geo<T> npt;

                np->lon += pi;
                npt = snyder_ctran(np, pt);
                np->lon -= pi;

                npt.lon -= (pi - lon0 + np->lon);

                /*
                 * snyder is down tri 3, isea is along side of tri1 from vertex 0 to
                 * vertex 1 these are 180 degrees apart
                 */
                npt.lon += pi;
                /* normalize longitude */
                npt.lon = fmod(npt.lon, two_pi);
                while (npt.lon > pi)
                    npt.lon -= two_pi;
                while (npt.lon < -pi)
                    npt.lon += two_pi;

                return npt;
            }

            /* in radians */

            /* fuller's at 5.2454 west, 2.3009 N, adjacent at 7.46658 deg */

            template <typename T>
            inline int isea_grid_init(isea_dgg<T> * g)
            {
                if (!g)
                    return 0;

                //g->polyhedron = isea_icosahedron;
                g->o_lat = isea_std_lat;
                g->o_lon = isea_std_lon;
                g->o_az = 0.0;
                g->aperture = 4;
                g->resolution = 6;
                g->radius = 1.0;
                //g->topology = isea_hexagon;

                return 1;
            }

            template <typename T>
            inline int isea_orient_isea(isea_dgg<T> * g)
            {
                if (!g)
                    return 0;
                g->o_lat = isea_std_lat;
                g->o_lon = isea_std_lon;
                g->o_az = 0.0;
                return 1;
            }

            template <typename T>
            inline int isea_orient_pole(isea_dgg<T> * g)
            {
                static T const half_pi = detail::half_pi<T>();

                if (!g)
                    return 0;
                g->o_lat = half_pi;
                g->o_lon = 0.0;
                g->o_az = 0;
                return 1;
            }

            template <typename T>
            inline int isea_transform(isea_dgg<T> * g, isea_geo<T> * in,
                                      isea_pt<T> * out)
            {
                isea_geo<T> i, pole;
                int         tri;

                pole.lat = g->o_lat;
                pole.lon = g->o_lon;

                i = isea_ctran(&pole, in, g->o_az);

                tri = isea_snyder_forward(&i, out);
                out->x *= g->radius;
                out->y *= g->radius;
                g->triangle = tri;

                return tri;
            }

            template <typename T>
            inline void isea_rotate(isea_pt<T> * pt, T const& degrees)
            {
                static T const d2r = geometry::math::d2r<T>();
                static T const two_pi = detail::two_pi<T>();

                T          rad;

                T          x, y;

                rad = -degrees * d2r;
                while (rad >= two_pi) rad -= two_pi;
                while (rad <= -two_pi) rad += two_pi;

                x = pt->x * cos(rad) + pt->y * sin(rad);
                y = -pt->x * sin(rad) + pt->y * cos(rad);

                pt->x = x;
                pt->y = y;
            }

            template <typename T>
            inline int isea_tri_plane(int tri, isea_pt<T> *pt, T const& radius)
            {
                isea_pt<T> tc; /* center of triangle */

                if (downtri(tri)) {
                    isea_rotate(pt, 180.0);
                }
                tc = isea_triangle_xy<T>(tri);
                tc.x *= radius;
                tc.y *= radius;
                pt->x += tc.x;
                pt->y += tc.y;

                return tri;
            }

            /* convert projected triangle coords to quad xy coords, return quad number */
            template <typename T>
            inline int isea_ptdd(int tri, isea_pt<T> *pt)
            {
                int             downtri, quad;

                downtri = (((tri - 1) / 5) % 2 == 1);
                quad = ((tri - 1) % 5) + ((tri - 1) / 10) * 5 + 1;

                isea_rotate(pt, downtri ? 240.0 : 60.0);
                if (downtri) {
                    pt->x += 0.5;
                    /* pt->y += cos(30.0 * M_PI / 180.0); */
                    pt->y += .86602540378443864672;
                }
                return quad;
            }

            template <typename T>
            inline int isea_dddi_ap3odd(isea_dgg<T> *g, int quad, isea_pt<T> *pt, isea_pt<T> *di)
            {
                static T const pi = detail::pi<T>();

                isea_pt<T> v;
                T          hexwidth;
                T          sidelength;    /* in hexes */
                int        d, i;
                int        maxcoord;
                hex        h;

                /* This is the number of hexes from apex to base of a triangle */
                sidelength = (math::pow(T(2), g->resolution) + T(1)) / T(2);

                /* apex to base is cos(30deg) */
                hexwidth = cos(pi / 6.0) / sidelength;

                /* TODO I think sidelength is always x.5, so
                 * (int)sidelength * 2 + 1 might be just as good
                 */
                maxcoord = (int) (sidelength * 2.0 + 0.5);

                v = *pt;
                hexbin2(hexwidth, v.x, v.y, &h.x, &h.y);
                h.iso = 0;
                hex_iso(&h);

                d = h.x - h.z;
                i = h.x + h.y + h.y;

                /*
                 * you want to test for max coords for the next quad in the same
                 * "row" first to get the case where both are max
                 */
                if (quad <= 5) {
                    if (d == 0 && i == maxcoord) {
                        /* north pole */
                        quad = 0;
                        d = 0;
                        i = 0;
                    } else if (i == maxcoord) {
                        /* upper right in next quad */
                        quad += 1;
                        if (quad == 6)
                            quad = 1;
                        i = maxcoord - d;
                        d = 0;
                    } else if (d == maxcoord) {
                        /* lower right in quad to lower right */
                        quad += 5;
                        d = 0;
                    }
                } else if (quad >= 6) {
                    if (i == 0 && d == maxcoord) {
                        /* south pole */
                        quad = 11;
                        d = 0;
                        i = 0;
                    } else if (d == maxcoord) {
                        /* lower right in next quad */
                        quad += 1;
                        if (quad == 11)
                            quad = 6;
                        d = maxcoord - i;
                        i = 0;
                    } else if (i == maxcoord) {
                        /* upper right in quad to upper right */
                        quad = (quad - 4) % 5;
                        i = 0;
                    }
                }

                di->x = d;
                di->y = i;

                g->quad = quad;
                return quad;
            }

            template <typename T>
            inline int isea_dddi(isea_dgg<T> *g, int quad, isea_pt<T> *pt, isea_pt<T> *di)
            {
                isea_pt<T> v;
                T          hexwidth;
                int        sidelength;    /* in hexes */
                hex        h;

                if (g->aperture == 3 && g->resolution % 2 != 0) {
                    return isea_dddi_ap3odd(g, quad, pt, di);
                }
                /* todo might want to do this as an iterated loop */
                if (g->aperture >0) {
                    sidelength = (int) (math::pow(T(g->aperture), T(g->resolution / T(2))) + T(0.5));
                } else {
                    sidelength = g->resolution;
                }

                hexwidth = 1.0 / sidelength;

                v = *pt;
                isea_rotate(&v, -30.0);
                hexbin2(hexwidth, v.x, v.y, &h.x, &h.y);
                h.iso = 0;
                hex_iso(&h);

                /* we may actually be on another quad */
                if (quad <= 5) {
                    if (h.x == 0 && h.z == -sidelength) {
                        /* north pole */
                        quad = 0;
                        h.z = 0;
                        h.y = 0;
                        h.x = 0;
                    } else if (h.z == -sidelength) {
                        quad = quad + 1;
                        if (quad == 6)
                            quad = 1;
                        h.y = sidelength - h.x;
                        h.z = h.x - sidelength;
                        h.x = 0;
                    } else if (h.x == sidelength) {
                        quad += 5;
                        h.y = -h.z;
                        h.x = 0;
                    }
                } else if (quad >= 6) {
                    if (h.z == 0 && h.x == sidelength) {
                        /* south pole */
                        quad = 11;
                        h.x = 0;
                        h.y = 0;
                        h.z = 0;
                    } else if (h.x == sidelength) {
                        quad = quad + 1;
                        if (quad == 11)
                            quad = 6;
                        h.x = h.y + sidelength;
                        h.y = 0;
                        h.z = -h.x;
                    } else if (h.y == -sidelength) {
                        quad -= 4;
                        h.y = 0;
                        h.z = -h.x;
                    }
                }
                di->x = h.x;
                di->y = -h.z;

                g->quad = quad;
                return quad;
            }

            template <typename T>
            inline int isea_ptdi(isea_dgg<T> *g, int tri, isea_pt<T> *pt,
                                 isea_pt<T> *di)
            {
                isea_pt<T> v;
                int        quad;

                v = *pt;
                quad = isea_ptdd(tri, &v);
                quad = isea_dddi(g, quad, &v, di);
                return quad;
            }

            /* q2di to seqnum */
            template <typename T>
            inline int isea_disn(isea_dgg<T> *g, int quad, isea_pt<T> *di)
            {
                int             sidelength;
                int             sn, height;
                int             hexes;

                if (quad == 0) {
                    g->serial = 1;
                    return g->serial;
                }
                /* hexes in a quad */
                hexes = (int) (math::pow(T(g->aperture), T(g->resolution)) + T(0.5));
                if (quad == 11) {
                    g->serial = 1 + 10 * hexes + 1;
                    return g->serial;
                }
                if (g->aperture == 3 && g->resolution % 2 == 1) {
                    height = (int) (math::pow(T(g->aperture), T((g->resolution - 1) / T(2))));
                    sn = ((int) di->x) * height;
                    sn += ((int) di->y) / height;
                    sn += (quad - 1) * hexes;
                    sn += 2;
                } else {
                    sidelength = (int) (math::pow(T(g->aperture), T(g->resolution / T(2))) + T(0.5));
                    sn = (int) ((quad - 1) * hexes + sidelength * di->x + di->y + 2);
                }

                g->serial = sn;
                return sn;
            }

            /* TODO just encode the quad in the d or i coordinate
             * quad is 0-11, which can be four bits.
             * d' = d << 4 + q, d = d' >> 4, q = d' & 0xf
             */
            /* convert a q2di to global hex coord */
            template <typename T>
            inline int isea_hex(isea_dgg<T> *g, int tri, isea_pt<T> *pt,
                                isea_pt<T> *hex)
            {
                isea_pt<T> v;
#ifdef BOOST_GEOMETRY_PROJECTIONS_FIXME
                int sidelength;
                int d, i, x, y;
#endif // BOOST_GEOMETRY_PROJECTIONS_FIXME
                int quad;

                quad = isea_ptdi(g, tri, pt, &v);

                hex->x = ((int)v.x << 4) + quad;
                hex->y = v.y;

                return 1;
#ifdef BOOST_GEOMETRY_PROJECTIONS_FIXME
                d = (int)v.x;
                i = (int)v.y;

                /* Aperture 3 odd resolutions */
                if (g->aperture == 3 && g->resolution % 2 != 0) {
                    int offset = (int)(pow(T(3.0), T(g->resolution - 1)) + 0.5);

                    d += offset * ((g->quad-1) % 5);
                    i += offset * ((g->quad-1) % 5);

                    if (quad == 0) {
                        d = 0;
                        i = offset;
                    } else if (quad == 11) {
                        d = 2 * offset;
                        i = 0;
                    } else if (quad > 5) {
                        d += offset;
                    }

                    x = (2*d - i) /3;
                    y = (2*i - d) /3;

                    hex->x = x + offset / 3;
                    hex->y = y + 2 * offset / 3;
                    return 1;
                }

                /* aperture 3 even resolutions and aperture 4 */
                sidelength = (int) (pow(T(g->aperture), T(g->resolution / 2.0)) + 0.5);
                if (g->quad == 0) {
                    hex->x = 0;
                    hex->y = sidelength;
                } else if (g->quad == 11) {
                    hex->x = sidelength * 2;
                    hex->y = 0;
                } else {
                    hex->x = d + sidelength * ((g->quad-1) % 5);
                    if (g->quad > 5) hex->x += sidelength;
                    hex->y = i + sidelength * ((g->quad-1) % 5);
                }

                return 1;
#endif // BOOST_GEOMETRY_PROJECTIONS_FIXME
            }

            template <typename T>
            inline isea_pt<T> isea_forward(isea_dgg<T> *g, isea_geo<T> *in)
            {
                int        tri;
                isea_pt<T> out, coord;

                tri = isea_transform(g, in, &out);

                if (g->output == isea_addr_plane) {
                    isea_tri_plane(tri, &out, g->radius);
                    return out;
                }

                /* convert to isea standard triangle size */
                out.x = out.x / g->radius * isea_scale;
                out.y = out.y / g->radius * isea_scale;
                out.x += 0.5;
                out.y += 2.0 * .14433756729740644112;

                switch (g->output) {
                case isea_addr_projtri:
                    /* nothing to do, already in projected triangle */
                    break;
                case isea_addr_vertex2dd:
                    g->quad = isea_ptdd(tri, &out);
                    break;
                case isea_addr_q2dd:
                    /* Same as above, we just don't print as much */
                    g->quad = isea_ptdd(tri, &out);
                    break;
                case isea_addr_q2di:
                    g->quad = isea_ptdi(g, tri, &out, &coord);
                    return coord;
                    break;
                case isea_addr_seqnum:
                    isea_ptdi(g, tri, &out, &coord);
                    /* disn will set g->serial */
                    isea_disn(g, g->quad, &coord);
                    return coord;
                    break;
                case isea_addr_hex:
                    isea_hex(g, tri, &out, &coord);
                    return coord;
                    break;
                }

                return out;
            }
            /*
             * Proj 4 integration code follows
             */

            template <typename T>
            struct par_isea
            {
                isea_dgg<T> dgg;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_isea_spheroid
                : public base_t_f<base_isea_spheroid<T, Parameters>, T, Parameters>
            {
                par_isea<T> m_proj_parm;

                inline base_isea_spheroid(const Parameters& par)
                    : base_t_f<base_isea_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    isea_pt<T> out;
                    isea_geo<T> in;

                    in.lon = lp_lon;
                    in.lat = lp_lat;

                    isea_dgg<T> copy = this->m_proj_parm.dgg;
                    out = isea_forward(&copy, &in);

                    xy_x = out.x;
                    xy_y = out.y;
                }

                static inline std::string get_name()
                {
                    return "isea_spheroid";
                }

            };

            template <typename T>
            inline void isea_orient_init(srs::detail::proj4_parameters const& params,
                                         par_isea<T>& proj_parm)
            {
                std::string opt = pj_get_param_s(params, "orient");
                if (! opt.empty()) {
                    if (opt == std::string("isea")) {
                        isea_orient_isea(&proj_parm.dgg);
                    } else if (opt == std::string("pole")) {
                        isea_orient_pole(&proj_parm.dgg);
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_ellipsoid_use_required) );
                    }
                }
            }

            template <typename T>
            inline void isea_orient_init(srs::dpar::parameters<T> const& params,
                                         par_isea<T>& proj_parm)
            {
                typename srs::dpar::parameters<T>::const_iterator
                    it = pj_param_find(params, srs::dpar::orient);
                if (it != params.end()) {
                    srs::dpar::value_orient o = static_cast<srs::dpar::value_orient>(it->template get_value<int>());
                    if (o == srs::dpar::orient_isea) {
                        isea_orient_isea(&proj_parm.dgg);
                    } else if (o == srs::dpar::orient_pole) {
                        isea_orient_pole(&proj_parm.dgg);
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_ellipsoid_use_required) );
                    }
                }
            }

            template <typename T>
            inline void isea_mode_init(srs::detail::proj4_parameters const& params,
                                       par_isea<T>& proj_parm)
            {
                std::string opt = pj_get_param_s(params, "mode");
                if (! opt.empty()) {
                    if (opt == std::string("plane")) {
                        proj_parm.dgg.output = isea_addr_plane;
                    } else if (opt == std::string("di")) {
                        proj_parm.dgg.output = isea_addr_q2di;
                    } else if (opt == std::string("dd")) {
                        proj_parm.dgg.output = isea_addr_q2dd;
                    } else if (opt == std::string("hex")) {
                        proj_parm.dgg.output = isea_addr_hex;
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_ellipsoid_use_required) );
                    }
                }
            }

            template <typename T>
            inline void isea_mode_init(srs::dpar::parameters<T> const& params,
                                       par_isea<T>& proj_parm)
            {
                typename srs::dpar::parameters<T>::const_iterator
                    it = pj_param_find(params, srs::dpar::mode);
                if (it != params.end()) {
                    srs::dpar::value_mode m = static_cast<srs::dpar::value_mode>(it->template get_value<int>());
                    if (m == srs::dpar::mode_plane) {
                        proj_parm.dgg.output = isea_addr_plane;
                    } else if (m == srs::dpar::mode_di) {
                        proj_parm.dgg.output = isea_addr_q2di;
                    } else if (m == srs::dpar::mode_dd) {
                        proj_parm.dgg.output = isea_addr_q2dd;
                    } else if (m == srs::dpar::mode_hex) {
                        proj_parm.dgg.output = isea_addr_hex;
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_ellipsoid_use_required) );
                    }
                }
            }

            // Icosahedral Snyder Equal Area
            template <typename Params, typename T>
            inline void setup_isea(Params const& params, par_isea<T>& proj_parm)
            {
                std::string opt;

                isea_grid_init(&proj_parm.dgg);

                proj_parm.dgg.output = isea_addr_plane;
            /*        proj_parm.dgg.radius = par.a; / * otherwise defaults to 1 */
                /* calling library will scale, I think */

                isea_orient_init(params, proj_parm);

                pj_param_r<srs::spar::azi>(params, "azi", srs::dpar::azi, proj_parm.dgg.o_az);
                pj_param_r<srs::spar::lon_0>(params, "lon_0", srs::dpar::lon_0, proj_parm.dgg.o_lon);
                pj_param_r<srs::spar::lat_0>(params, "lat_0", srs::dpar::lat_0, proj_parm.dgg.o_lat);
                // TODO: this parameter is set below second time
                pj_param_i<srs::spar::aperture>(params, "aperture", srs::dpar::aperture, proj_parm.dgg.aperture);
                // TODO: this parameter is set below second time
                pj_param_i<srs::spar::resolution>(params, "resolution", srs::dpar::resolution, proj_parm.dgg.resolution);

                isea_mode_init(params, proj_parm);

                // TODO: pj_param_exists -> pj_get_param_b ?
                if (pj_param_exists<srs::spar::rescale>(params, "rescale", srs::dpar::rescale)) {
                    proj_parm.dgg.radius = isea_scale;
                }

                if (pj_param_i<srs::spar::resolution>(params, "resolution", srs::dpar::resolution, proj_parm.dgg.resolution)) {
                    /* empty */
                } else {
                    proj_parm.dgg.resolution = 4;
                }

                if (pj_param_i<srs::spar::aperture>(params, "aperture", srs::dpar::aperture, proj_parm.dgg.aperture)) {
                    /* empty */
                } else {
                    proj_parm.dgg.aperture = 3;
                }
            }

    }} // namespace detail::isea
    #endif // doxygen

    /*!
        \brief Icosahedral Snyder Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
        \par Projection parameters
         - orient (string)
         - azi: Azimuth (or Gamma) (degrees)
         - lon_0: Central meridian (degrees)
         - lat_0: Latitude of origin (degrees)
         - aperture (integer)
         - resolution (integer)
         - mode (string)
         - rescale
        \par Example
        \image html ex_isea.gif
    */
    template <typename T, typename Parameters>
    struct isea_spheroid : public detail::isea::base_isea_spheroid<T, Parameters>
    {
        template <typename Params>
        inline isea_spheroid(Params const& params, Parameters const& par)
            : detail::isea::base_isea_spheroid<T, Parameters>(par)
        {
            detail::isea::setup_isea(params, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_isea, isea_spheroid, isea_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_F(isea_entry, isea_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(isea_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(isea, isea_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_ISEA_HPP


// Copyright 2005-2009 Daniel James.
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(FBOOST_FUNCTIONAL_HASH_DETAIL_FLOAT_FUNCTIONS_HPP)
#define FBOOST_FUNCTIONAL_HASH_DETAIL_FLOAT_FUNCTIONS_HPP

#include <boost/config/no_tr1/cmath.hpp>

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

// The C++ standard requires that the C float functions are overloarded
// for float, double and long double in the std namespace, but some of the older
// library implementations don't support this. On some that don't, the C99
// float functions (frexpf, frexpl, etc.) are available.
//
// Some of this is based on guess work. If I don't know any better I assume that
// the standard C++ overloaded functions are available. If they're not then this
// means that the argument is cast to a double and back, which is inefficient
// and will give pretty bad results for long doubles - so if you know better
// let me know.

// STLport:
#if defined(__SGI_STL_PORT) || defined(_STLPORT_VERSION)
#  if (defined(__GNUC__) && __GNUC__ < 3 && (defined(linux) || defined(__linux) || defined(__linux__))) || defined(__DMC__)
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
#  elif defined(BOOST_MSVC) && BOOST_MSVC < 1300
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
#  else
#    define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS
#  endif

// Roguewave:
//
// On borland 5.51, with roguewave 2.1.1 the standard C++ overloads aren't
// defined, but for the same version of roguewave on sunpro they are.
#elif defined(_RWSTD_VER)
#  if defined(__BORLANDC__)
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
#    define BOOST_HASH_C99_NO_FLOAT_FUNCS
#  elif defined(__DECCXX)
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
#  else
#    define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS
#  endif

// libstdc++ (gcc 3.0 onwards, I think)
#elif defined(__GLIBCPP__) || defined(__GLIBCXX__)
#  define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS

// SGI:
#elif defined(__STL_CONFIG_H)
#  if defined(linux) || defined(__linux) || defined(__linux__)
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
#  else
#    define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS
#  endif

// vxWorks. It has its own math library, but uses Dinkumware STL
#elif defined(__VXWORKS__)
#  define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS

// Dinkumware.
#elif (defined(_YVALS) && !defined(__IBMCPP__)) || defined(_CPPLIB_VER)
// Some versions of Visual C++ don't seem to have the C++ overloads but they
// all seem to have the c99 float overloads
#  if defined(BOOST_MSVC)
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
// On other platforms the C++ overloads seem to have been introduced sometime
// before 402.
#  elif defined(_CPPLIB_VER) && (_CPPLIB_VER >= 402)
#    define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS
#  else
#    define BOOST_HASH_USE_C99_FLOAT_FUNCS
#  endif

// Digital Mars
#elif defined(__DMC__)
#  define BOOST_HASH_USE_C99_FLOAT_FUNCS

// Use overloaded float functions by default.
#else
#  define BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS
#endif

namespace gboost
{
    namespace hash_detail
    {

        inline float call_ldexp(float v, int exp)
        {
            using namespace std;
#if defined(BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS) || \
    defined(BOOST_HASH_C99_NO_FLOAT_FUNCS)
            return ldexp(v, exp);
#else
            return ldexpf(v, exp);
#endif
        }

        inline double call_ldexp(double v, int exp)
        {
            using namespace std;
            return ldexp(v, exp);
        }

        inline long double call_ldexp(long double v, int exp)
        {
            using namespace std;
#if defined(BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS)
            return ldexp(v, exp);
#else
            return ldexpl(v, exp);
#endif
        }

        inline float call_frexp(float v, int* exp)
        {
            using namespace std;
#if defined(BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS) || \
    defined(BOOST_HASH_C99_NO_FLOAT_FUNCS)
            return frexp(v, exp);
#else
            return frexpf(v, exp);
#endif
        }

        inline double call_frexp(double v, int* exp)
        {
            using namespace std;
            return frexp(v, exp);
        }

        inline long double call_frexp(long double v, int* exp)
        {
            using namespace std;
#if defined(BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS)
            return frexp(v, exp);
#else
            return frexpl(v, exp);
#endif
        }
    }
}

#if defined(BOOST_HASH_USE_C99_FLOAT_FUNCS)
#undef BOOST_HASH_USE_C99_FLOAT_FUNCS
#endif

#if defined(BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS)
#undef BOOST_HASH_USE_OVERLOAD_FLOAT_FUNCS
#endif

#if defined(BOOST_HASH_C99_NO_FLOAT_FUNCS)
#undef BOOST_HASH_C99_NO_FLOAT_FUNCS
#endif

#endif


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Image.h>
#include <vw/Plate/PlateFile.h>
#include <vw/Plate/TileManipulation.h>
using namespace vw;
using namespace vw::platefile;

#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <boost/math/distributions/fisher_f.hpp>

using namespace std;

// --- Functions to Apply --------------------------

// Reduce Base class
//   defines interface
template <typename ImplT>
struct ReduceBase {
  inline ImplT& impl() { return static_cast<ImplT&>(*this); }
  inline ImplT const& impl() const { return static_cast<ImplT const&>(*this); }

  template <class PixelT>
  inline void operator()( list<ImageView<PixelT> > const& input,
                          list<TileHeader> const& input_header,
                          ImageView<PixelT> & output) {
    impl()(input,input_header,output);
  }
};

// Weighted Average Implementation
//   mimick this with own functions
struct WeightedAverage : public ReduceBase<WeightedAverage> {

  template <class PixelT>
  inline void operator()( list<ImageView<PixelT> > const& input,
                          list<TileHeader> const& /*input_header*/,
                          ImageView<PixelT> & output) {
    // Input Images will always have an alpha channel. That is a requirement of PlateFiles.
    int num_channels = CompoundNumChannels<PixelT>::value;
    std::vector<ImageView<float32> > sum_weighted_data;
    for ( uint8 i = 0; i < num_channels-1; i++ )
      sum_weighted_data.push_back(ImageView<float32>(input.front().cols(),
                                                     input.front().rows()));
    ImageView<float32> summed_weights(input.front().cols(),
                                      input.front().rows());

    // Summing multiple images
    typedef typename list< ImageView<PixelT > >::const_iterator image_iter;
    for ( image_iter image = input.begin();
          image != input.end(); image++ ) {
      summed_weights += channel_cast<float32>(select_channel(*image,num_channels-1));
      // Iterating over non alpha channels
      for ( uint8 i = 0; i < num_channels-1; i++ )
        sum_weighted_data[i] += channel_cast<float32>(select_channel(*image,num_channels-1))*channel_cast<float32>(select_channel(*image,i));
    }

    // Normalizing
    for ( uint8 i = 0; i < num_channels-1; i++ )
      sum_weighted_data[i] /= summed_weights;
    output.set_size(input.front().cols(),
                    input.front().rows());
    for ( uint8 i = 0; i < num_channels-1; i++ )
      select_channel(output,i) = sum_weighted_data[i];
    // Setting output alpha
    select_channel(output,num_channels-1) = threshold(summed_weights,0,
                                                      ChannelRange<typename PixelChannelType<PixelT>::type>::min(),
                                                      ChannelRange<typename PixelChannelType<PixelT>::type>::max());
  }
};

// Weighted Variance Implementation
struct WeightedVar2 : public ReduceBase<WeightedVar2> {

  template <class PixelT>
  inline void operator()( list<ImageView<PixelT> > const& input,
                          list<TileHeader> const& /*input_header*/,
                          ImageView<PixelT> & var2) {
    // Input Images will always have an alpha channel. That is a requirement of PlateFiles.
    int num_channels = CompoundNumChannels<PixelT>::value;
    std::vector<ImageView<float32> > sum_weighted_data; //store the mean
    std::vector<ImageView<float32> > sum_weighted_data2; //store the variance
    std::vector<ImageView<float32> > sum_weighted_stdv; //store the standard deviation

    for ( uint8 i = 0; i < num_channels-1; i++ ){
      sum_weighted_data.push_back(ImageView<float32>(input.front().cols(),
                                                     input.front().rows()));
      sum_weighted_data2.push_back(ImageView<float32>(input.front().cols(),
                                                     input.front().rows()));
      sum_weighted_stdv.push_back(ImageView<float32>(input.front().cols(),
                                                    input.front().rows()));
    }

    ImageView<float32> summed_weights(input.front().cols(),
                                      input.front().rows());

    // Summing multiple images
    typedef typename list< ImageView<PixelT > >::const_iterator image_iter;
    for ( image_iter image = input.begin();image != input.end(); image++ ) {
      summed_weights += channel_cast<float32>(select_channel(*image,num_channels-1));

      // Iterating over non alpha channels
      for ( uint8 i = 0; i < num_channels-1; i++ ){
        sum_weighted_data[i] += channel_cast<float32>(select_channel(*image,num_channels-1))*channel_cast<float32>(select_channel(*image,i));

        sum_weighted_data2[i] += channel_cast<float32>(select_channel(*image,num_channels-1))
                                 *channel_cast<float32>(select_channel(*image,i))*channel_cast<float32>(select_channel(*image,i));
      }
    }

    // Normalizing
    for ( uint8 i = 0; i < num_channels-1; i++ ){
      sum_weighted_data[i] /= summed_weights;
      sum_weighted_data2[i] = sum_weighted_data2[i]/summed_weights - sum_weighted_data[i]*sum_weighted_data[i];
      sum_weighted_stdv[i]=sqrt(clamp(sum_weighted_data2[i],0, 999999999));
    }

    var2.set_size(input.front().cols(),
                  input.front().rows());
    for ( uint8 i = 0; i < num_channels-1; i++ )
      select_channel(var2,i) = sum_weighted_stdv[i];
    // Setting output alpha
    select_channel(var2,num_channels-1) =
      threshold(summed_weights,0,
                ChannelRange<typename PixelChannelType<PixelT>::type>::min(),
                ChannelRange<typename PixelChannelType<PixelT>::type>::max());
  }
};

// Robust Mean Implementation
struct RobustMean : public ReduceBase<RobustMean> {
private:
  float smart_weighted_mean( vector<float> & weights,
                             vector<float> const& samples,
                             float const sign_level=0.3f,
                             float const learn_rate=0.1f,
                             float const error_tol=1e-5f,
                             int32 const max_iter=1000 ) {
    namespace bm = boost::math;
    switch ( samples.size() ) {
    case 1:
      weights[0]=1;
      return samples[0];
    case 2:
      weights[0]=1;
      weights[1]=1;
      return (samples[0]+samples[1])/2;
    default:

      float weighted_mean = 0;
      std::vector<float> prev_wt = weights; // the previous weights
      for (int i = 0; i < max_iter; ++i) {

        // accumulation of all sums
        float SW1 = 0;    // sum of weights
        float SW2 = 0;    // sum of squared weights
        float SWX = 0;    // weighted sum of samples
        float WX2 = 0;    // weighted sum of squared data
        for (size_t j = 0; j < samples.size(); ++j) {
          SW1 += weights[j];
          SW2 += weights[j]*weights[j];
          SWX += weights[j]*samples[j];
          WX2 += weights[j]*samples[j]*samples[j];
        }

        // weighted mean
        weighted_mean = SWX/SW1;

        float DN = SW1*WX2-SWX*SWX; // denominator
        float v2 = SW1-SW2/SW1;     // the second degree of freedom
        float sse_wt = 0;           // squared sum of weight differences
        for (size_t j = 0; j < samples.size(); ++j) {

          // F statistic
          float DF = samples[j]-weighted_mean; // difference from the mean
          float FS = (SW1*SW1-SW2)*DF*DF/DN;   // F statistic

          // degree of freedom
          float v1 = 1-weights[j]/SW1;         // the first degree of freedom

          // p-value calculation
          float p_value = 1;                   // p-value
          if (SW1 > 1 && DN > 0) {             // basic assumptions
            bm::fisher_f dist(v1,v2);
            p_value = boost::numeric_cast<float>(1-cdf(dist, FS));
          }

          // gradient decent update
          weights[j] += learn_rate*(p_value-sign_level);
          if ( weights[j] < 0 ) weights[j] = 0; // 0.0 is lower bound of weight
          if ( weights[j] > 1 ) weights[j] = 1; // 1.0 is upper bound of weight

          // squared sum of weight differences
          sse_wt += (weights[j]-prev_wt[j])*(weights[j]-prev_wt[j]);
          prev_wt[j] = weights[j];              // update previous weight

        }

        // terminal condition: mean squared difference of weights is
        // smaller than the tolerance
        if ( sse_wt/float(weights.size()) < error_tol ) break;
      }

      return  weighted_mean;
    }
  }

public:
  template <class PixelT>
  inline void operator()( list<ImageView<PixelT> > const& input,
                          list<TileHeader> const& /*input_header*/,
                          ImageView<PixelT> & output ) {
    uint8 num_v_channels = CompoundNumChannels<PixelT>::value-1;
    output.set_size( input.front().cols(),
                     input.front().rows() );

    // Iterating through every pixel within a tile
    for ( int32 ix = 0; ix < input.front().cols(); ix++ ) {
      for ( int32 iy = 0; iy < input.front().rows(); iy++ ) {
        vector<std::vector<float> > t_samples;
        t_samples.resize(num_v_channels);
        vector<float> t_weight;

        // Seperating data out into vectors, only loading up data that
        // is not empty.
        typedef typename list<ImageView<PixelT> >::const_iterator iter_type;
        for ( iter_type iter = input.begin();
              iter != input.end(); iter++ ) {
          if ( (*iter)(ix,iy)[num_v_channels] == 0 )
            continue;
          t_weight.push_back( float((*iter)(ix,iy)[num_v_channels])/float(ChannelRange<typename PixelChannelType<PixelT>::type>::max()) );
          for ( uint8 iz = 0; iz < num_v_channels; iz++ )
            t_samples[iz].push_back( (*iter)(ix,iy)[iz] );
        }

        if ( t_weight.empty() ) {
          output(ix,iy) = PixelT();
          continue;
        }

        typedef typename PixelChannelType<PixelT>::type ChannelT;
        output(ix,iy)[num_v_channels] = ChannelRange<ChannelT>::max();
        for ( uint8 iz = 0; iz < num_v_channels; iz++ ) {
          vector<float> copy = t_weight;
          output(ix,iy)[iz] =
            boost::numeric_cast<ChannelT>(smart_weighted_mean( copy,
                                                               t_samples[iz]));
        }
      } // iy - end loop
    }   // ix - first loop
  }     // end operator()
};

// --- Standard Terminal Argument ------------------

// Standard Arguments
struct Options {
  // Input
  Url url;
  int32 level;
  TransactionOrNeg start_trans_id, end_trans_id;
  std::string start_description;
  bool finish;

  // Output
  string function;
  TransactionOrNeg transaction_id;

  // For spawning multiple jobs
  int32 job_id, num_jobs;
};

void handle_arguments(int argc, char *argv[], Options& opt) {
  po::options_description general_options("Perform weighted averages of all layers within a tile inside a plate file");
  general_options.add_options()
    ("job_id,j", po::value(&opt.job_id)->default_value(0), "")
    ("num_jobs,n", po::value(&opt.num_jobs)->default_value(1), "")
    ("begin_transaction", po::value(&opt.start_trans_id)->default_value(0), "Input starting transaction ID range.")
    ("end_transaction", po::value(&opt.end_trans_id), "Input ending transaction ID range.")
    ("level,l", po::value(&opt.level)->default_value(-1), "Level inside the plate in which to process. -1 will error out and show the number of levels available.")
    ("function,f", po::value(&opt.function)->default_value("WeightedAvg"), "Functions that are available are [WeightedAvg RobustMean WeightedVar]")
    ("transaction-id,t",po::value(&opt.transaction_id)->default_value(2000), "Transaction id to write to")
    ("start", po::value(&opt.start_description), "Starts a multi-part plate reduce.")
    ("finish", po::bool_switch(&opt.finish)->default_value(false), "Finish a multi-part plate reduce")
    ("help,h", "Display this help message");

  po::options_description hidden_options("");
  hidden_options.add_options()
    ("input-file", po::value(&opt.url), "");

  po::options_description options("");
  options.add(general_options).add(hidden_options);

  po::positional_options_description p;
  p.add("input-file",-1);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(options).positional(p).run(), vm );
    po::notify( vm );
  } catch (const po::error& e) {
    vw_throw( ArgumentErr() << "Error parsing input:\n\t"
              << e.what() << options );
  }

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " <plate_filename> [options]\n";

  if ( vm.count("help") || vm.count("input-file") != 1 || opt.transaction_id.newest())
    vw_throw( ArgumentErr() << usage.str() << general_options );
}

// --- Meta Application of Above Functions ----------

// apply_reduce
template <typename ReduceT, class PixelT>
void apply_reduce( boost::shared_ptr<PlateFile> platefile,
                   std::list<BBox2i> const& workunits,
                   Options& opt, ReduceBase<ReduceT>& reduce) {

  TerminalProgressCallback tpc("plate.platereduce", "Processing");
  double inc_tpc = 1.0/float(workunits.size());
  BOOST_FOREACH( const BBox2i& workunit, workunits) {
    tpc.report_incremental_progress(inc_tpc);
    for ( int ix = 0; ix < workunit.width(); ix++ ) {
      for ( int iy = 0; iy < workunit.height(); iy++ ) {
        Vector2i location(ix,iy);
        location += workunit.min();

        // Polling for Tiles
        std::list<TileHeader> tile_records;
        tile_records = platefile->search_by_location(location[0],
                                                     location[1],
                                                     opt.level,
                                                     TransactionRange(opt.start_trans_id, opt.end_trans_id));

        // No Tiles? No Problem!
        if (tile_records.empty())
          continue;

        // Loading images
        std::list<ImageView<PixelT> > tiles;
        BOOST_FOREACH( const TileHeader& tile, tile_records ) {
          ImageView<PixelT> new_tile;
          platefile->read( new_tile, location[0],
                           location[1], opt.level,
                           tile.transaction_id(), true );
          tiles.push_back(new_tile);
        }

        // Calling function
        ImageView<PixelT> result;
        reduce(tiles, tile_records, result);

        platefile->write_update(result, location[0], location[1], opt.level);
      }
    }
  }
  tpc.report_finished();
}

// Function that runs the apply_reduce over the plate file
template <typename ReduceT>
void do_run( Options& opt, ReduceBase<ReduceT>& reduce ) {
  boost::shared_ptr<PlateFile> platefile =
    boost::shared_ptr<PlateFile>( new PlateFile(opt.url) );

  if ( !opt.start_description.empty() ) {
    platefile->transaction_begin(opt.start_description, opt.transaction_id );
    vw_out() << "Transaction started with ID = " << platefile->transaction_id() << "\n";
    vw_out() << "Plate has " << platefile->num_levels() << " levels.\n";
    exit(0);
  }

  platefile->transaction_resume(opt.transaction_id.promote());

  if ( opt.finish ) {
    // Update the read cursor when the snapshot is complete!
    platefile->transaction_end(true);
    vw_out() << "Transaction " << opt.transaction_id << " complete.\n";
    exit(0);
  }

  if ( opt.level < 0 || opt.level >= boost::numeric_cast<int32>(platefile->num_levels()) ) {
    vw_throw( ArgumentErr() << "Incorrect level selection, "
              << opt.level << ".\n\nPlatefile " << opt.url.string() << " has "
              << platefile->num_levels() << " levels internally.\n" );
  }

  // This is arbitrary, just needed to divide up jobs
  int32 region_size = 1 << opt.level;
  BBox2i full_region(0,0,region_size,region_size);
  std::list<BBox2i> workunits = bbox_tiles(full_region,4,4);
  std::list<BBox2i> mworkunits;
  int32 count = 0;
  BOOST_FOREACH(const BBox2i& c, workunits) {
    if (count==opt.num_jobs)
      count=0;
    if (count==opt.job_id)
      mworkunits.push_back(c);
    count++;
  }
  vw_out() << "Job " << opt.job_id << "/" << opt.num_jobs << " has "
           << mworkunits.size() << " work units.\n";

  platefile->audit_log()
    << "Started multi-part reduce (t_id = " << opt.transaction_id
    << ") -- level:" << opt.level
    << " jobid:" << opt.job_id << "/" << opt.num_jobs << "\n";

  platefile->write_request();

  switch(platefile->pixel_format()) {
  case VW_PIXEL_GRAYA:
    switch(platefile->channel_type()) {
    case VW_CHANNEL_UINT8:
      apply_reduce<ReduceT, PixelGrayA<uint8> >(platefile, mworkunits,
                                                opt, reduce);
      break;
    case VW_CHANNEL_INT16:
      apply_reduce<ReduceT, PixelGrayA<int16> >(platefile, mworkunits,
                                                opt, reduce);
      break;
    case VW_CHANNEL_FLOAT32:
      apply_reduce<ReduceT, PixelGrayA<float32> >(platefile, mworkunits,
                                                  opt, reduce);
      break;
    default:
      vw_throw(InputErr() << "Platefile contains unsupported channel type.\n" );
    }
    break;
  case VW_PIXEL_RGBA:
    switch(platefile->channel_type()) {
    case VW_CHANNEL_UINT8:
      apply_reduce<ReduceT, PixelRGBA<uint8> >(platefile, mworkunits,
                                               opt, reduce);
      break;
    default:
      vw_throw(InputErr() << "Platefile contains unsupported channel type.\n" );
    }
    break;
  default:
    vw_throw(InputErr() << "Platefile contains a pixel type thats unsupported.\n" );
  }

  platefile->write_complete();
  platefile->audit_log()
    << "Finished multi-part reduce (t_id = " << opt.transaction_id
    << ") -- level:" << opt.level
    << " jobid:" << opt.job_id << "/" << opt.num_jobs << "\n";
}

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

  Options opt;
  try {
    handle_arguments( argc, argv, opt );

    // Handing out jobs now
    boost::to_lower(opt.function);
    if ( opt.function == "weightedavg" ) {
      WeightedAverage f;
      do_run<WeightedAverage>( opt, f );
    } else if ( opt.function == "robustmean" ) {
      RobustMean f;
      do_run<RobustMean>( opt, f );
    }
    else if ( opt.function == "weightedvar" ) {
      WeightedVar2 f;
      do_run<WeightedVar2>( opt, f );
    } else {
      vw_throw( ArgumentErr() << "Unknown function, " << opt.function << "\n" );
    }

  } catch ( const ArgumentErr& e ) {
    vw_out() << e.what() << std::endl;
    return 1;
  } catch ( const Exception& e ) {
    std::cerr << "Error: " << e.what() << std::endl;
    return 1;
  }

  return 0;
}


#include <iostream>
#include <optional>
#include <sstream>
#include <stdexcept>

#include <ceres/ceres.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <Eigen/Dense>
#include <boost/asio.hpp>

#include "farm_ng/calibration/calibrate_multi_view_apriltag_rig.pb.h"
#include "farm_ng/calibration/calibrate_multi_view_lidar.pb.h"
#include "farm_ng/calibration/local_parameterization.h"
#include "farm_ng/calibration/multi_view_apriltag_rig_calibrator.h"
#include "farm_ng/calibration/multi_view_lidar_model.pb.h"

#include "farm_ng/perception/apriltag.h"
#include "farm_ng/perception/camera_model.h"
#include "farm_ng/perception/image_loader.h"
#include "farm_ng/perception/point_cloud.h"

#include "farm_ng/core/blobstore.h"
#include "farm_ng/core/event_log_reader.h"
#include "farm_ng/core/init.h"
#include "farm_ng/core/ipc.h"

#include <opencv2/imgproc.hpp>
#include "farm_ng/perception/tensor.h"
#include "farm_ng/perception/time_series.h"

#include <Eigen/Dense>
DEFINE_bool(interactive, false, "receive program args via eventbus");
DEFINE_string(output_config, "", "Output the config to a file.");
DEFINE_string(
    config, "/blobstore/logs/calibration_capture/multi_view_lidar_config.json",
    "Load config from a file rather than args.");

DEFINE_string(name, "", "Name of calibration output.");
DEFINE_string(event_log, "", "Path to event log containing input data.");
DEFINE_string(
    calibrate_multi_view_apriltag_rig_result, "",
    "Path to result of calibrate_multi_view_apriltag_rig, containing camera "
    "rig and apriltag rig to calibrate the LIDARs with respect to.");

namespace fs = boost::filesystem;

namespace farm_ng::calibration {

std::vector<std::tuple<perception::ApriltagRig::Node, std::vector<int>, double>>
ApriltagRigPointCloudMatch(const perception::ApriltagRig& rig,
                           const Eigen::Matrix3Xd& points_rig,
                           double tolerance) {
  std::vector<
      std::tuple<perception::ApriltagRig::Node, std::vector<int>, double>>
      node_indices;
  for (auto node : rig.nodes()) {
    Sophus::SE3d tag_pose_rig =
        ProtoToSophus(node.pose(), node.frame_name(), rig.name());
    Eigen::Matrix3Xd points_tag =
        perception::TransformPoints(tag_pose_rig, points_rig);
    // half the tag size + 1 pixel of border (note tag_size/8 is 1 apriltag
    // pixel)
    double half_tag = node.tag_size() / 8.0 + node.tag_size() / 2.0;

    auto [indices, error] = perception::AxisAlignedBoxFilter(
        points_tag, half_tag, half_tag, tolerance);
    if (indices.empty()) {
      continue;
    }

    node_indices.push_back({node, indices, error.z()});
  }
  return node_indices;
}

struct PointPlaneCostFunctor {
  PointPlaneCostFunctor(const Eigen::Vector3d& point_a,
                        const Eigen::Vector3d& normal_a,
                        const Eigen::Matrix3Xd& points_b,
                        const perception::SE3Map& a_pose_b_map)
      : point_a_(point_a),
        normal_a_(normal_a),
        points_b_(points_b),
        a_pose_b_map_(a_pose_b_map) {}

  template <class T>
  bool operator()(T const* const raw_a_pose_b, T* raw_residuals) const {
    Eigen::Map<Eigen::Matrix<T, 1, Eigen::Dynamic>> residuals(raw_residuals, 1,
                                                              points_b_.cols());
    auto a_pose_b = a_pose_b_map_.Map(raw_a_pose_b);
    residuals = (normal_a_.cast<T>().transpose() *
                 (perception::TransformPoints<T>(a_pose_b, points_b_.cast<T>())
                      .colwise() -
                  point_a_.cast<T>()));
    return true;
  }
  Eigen::Vector3d point_a_;
  Eigen::Vector3d normal_a_;
  Eigen::Matrix3Xd points_b_;
  perception::SE3Map a_pose_b_map_;
};

void SavePlyFilesInTagRig(MultiViewLidarModel model, std::string prefix) {
  perception::PoseGraph posegraph;
  posegraph.AddPoses(model.camera_rig().camera_pose_rig());
  posegraph.AddPoses(model.lidar_poses());
  posegraph = posegraph.AveragePoseGraph(model.camera_rig().name());
  std::ofstream report_csv(prefix + "_report.csv");
  report_csv << "# frame_number, point_coud, tag_id, error";
  perception::ImageLoader loader;
  for (int i = 0; i < model.measurements_size(); ++i) {
    const auto& m_i0 = model.measurements(i);
    std::map<std::string, cv::Mat> images;
    std::map<std::string, perception::CameraModel> cameras;

    for (auto mvd : m_i0.multi_view_detections().detections_per_view()) {
      std::string camera_name = mvd.image().camera_model().frame_name();
      images[camera_name] = loader.LoadImage(mvd.image());
      if (images[camera_name].channels() == 1) {
        cv::cvtColor(images[camera_name], images[camera_name],
                     cv::COLOR_GRAY2BGR);
      }
      cameras[camera_name] = mvd.image().camera_model();
    }

    Sophus::SE3d camera_rig_pose_apriltag_rig =
        ProtoToSophus(m_i0.camera_rig_pose_apriltag_rig(),
                      model.camera_rig().name(), model.apriltag_rig().name());
    for (perception::PointCloud cloud :
         m_i0.multi_view_pointclouds().point_clouds_per_view()) {
      Sophus::SE3d cloud_pose_camera_rig = posegraph.CheckAverageAPoseB(
          cloud.frame_name(), model.camera_rig().name());
      Sophus::SE3d cloud_pose_apriltag_rig =
          cloud_pose_camera_rig * camera_rig_pose_apriltag_rig;
      auto points_cloud = PointCloudGetData(cloud, "xyz");
      auto points_apriltag_rig = perception::TransformPoints<double>(
          cloud_pose_apriltag_rig.inverse(), points_cloud);
      Eigen::Matrix3Xd colors = Eigen::Matrix3Xd::Zero(3, points_cloud.cols());
      Eigen::MatrixXd count = Eigen::Matrix3Xd::Zero(1, points_cloud.cols());
      for (auto camera_image : images) {
        perception::PoseGraph c_pg =
            posegraph.AveragePoseGraph(camera_image.first);
        Sophus::SE3d camera_pose_cloud =
            c_pg.CheckAverageAPoseB(camera_image.first, cloud.frame_name());

        auto points_camera = perception::TransformPoints<double>(
            camera_pose_cloud, points_cloud);
        const auto& camera_model = cameras[camera_image.first];
        cv::Rect roi(cv::Point(0, 0), camera_image.second.size());
        for (int i = 0; i < points_camera.cols(); ++i) {
          const Eigen::Vector3d& point_camera = points_camera.col(i);
          if (point_camera.z() < 0.01) {
            continue;
          }
          auto xyf =
              perception::ProjectPointToPixel(camera_model, point_camera);
          cv::Point xy(xyf.x() + 0.5, xyf.y() + 0.5);
          if (roi.contains(xy)) {
            cv::Vec3b c = camera_image.second.at<cv::Vec3b>(xy);
            colors(0, i) = c[0] / 255.0;
            colors(1, i) = c[1] / 255.0;
            colors(2, i) = c[2] / 255.0;
            count(0, i) += 1.0;
          }
        }
      }
      std::vector<Eigen::Vector3d> points_valid;
      std::vector<Eigen::Vector3d> color_valid;
      for (int i = 0; i < count.cols(); ++i) {
        if (count(0, i) > 0) {
          color_valid.push_back(colors.col(i));
          points_valid.push_back(points_apriltag_rig.col(i));
        }
      }

      perception::SavePly(
          prefix + cloud.frame_name() + std::to_string(i) + ".ply",
          Eigen::Map<const Eigen::MatrixXd>(points_valid[0].data(),
                                            3, points_valid.size()),
          Eigen::Map<const Eigen::MatrixXd>(color_valid[0].data(),
                                            3, color_valid.size()));

      auto apriltag_rig_matches = ApriltagRigPointCloudMatch(
          model.apriltag_rig(), points_apriltag_rig, 0.1);
      for (auto [node, indices, error] : apriltag_rig_matches) {
        LOG(INFO) << node.frame_name() << "<->" << cloud.frame_name()
                  << "  matches: " << indices.size() << " error: " << error;
        report_csv << i << ", " << cloud.frame_name() << "," << node.id()
                   << ", " << error << std::endl;
        perception::SavePly(
            prefix + std::to_string(node.id()) + cloud.frame_name() +
                std::to_string(i) + ".ply",
            perception::SelectPoints(points_apriltag_rig, indices));
      }
    }
  }
}

MultiViewLidarModel Solve(MultiViewLidarModel model) {
  perception::PoseGraph posegraph;
  posegraph.AddPoses(model.camera_rig().camera_pose_rig());
  posegraph.AddPoses(model.lidar_poses());
  posegraph = posegraph.AveragePoseGraph(model.camera_rig().name());
  ceres::Problem problem;

  for (auto pose : posegraph.MutablePoseEdges()) {
    problem.AddParameterBlock(pose->GetAPoseB().data(),
                              Sophus::SE3d::num_parameters,
                              new LocalParameterizationSE3);
  }
  for (auto camera_pose : model.camera_rig().camera_pose_rig()) {
    problem.SetParameterBlockConstant(
        posegraph.MutablePoseEdge(camera_pose.frame_a(), camera_pose.frame_b())
            ->GetAPoseB()
            .data());
  }

  std::map<int, ApriltagRigTagStats> tag_stats;
  std::map<int,
           std::vector<std::tuple<PerImageRmse, ceres::ResidualBlockId, int>>>
      tag_id_to_per_image_rmse_block_id;

  for (int i = 0; i < model.measurements_size(); ++i) {
    const auto& m_i0 = model.measurements(i);
    Sophus::SE3d camera_rig_pose_apriltag_rig =
        ProtoToSophus(m_i0.camera_rig_pose_apriltag_rig(),
                      model.camera_rig().name(), model.apriltag_rig().name());
    for (const auto& cloud :
         m_i0.multi_view_pointclouds().point_clouds_per_view()) {
      auto* cloud_rig_edge = posegraph.MutablePoseEdge(
          cloud.frame_name(), model.camera_rig().name());
      Sophus::SE3d cloud_pose_camera_rig = cloud_rig_edge->GetAPoseBMapped(
          cloud.frame_name(), model.camera_rig().name());
      Sophus::SE3d cloud_pose_apriltag_rig =
          cloud_pose_camera_rig * camera_rig_pose_apriltag_rig;
      auto points_cloud = PointCloudGetData(cloud, "xyz");
      auto points_apriltag_rig = perception::TransformPoints<double>(
          cloud_pose_apriltag_rig.inverse(), points_cloud);
      auto apriltag_rig_matches = ApriltagRigPointCloudMatch(
          model.apriltag_rig(), points_apriltag_rig, 0.1);
      for (auto [node, indices, error] : apriltag_rig_matches) {
        LOG(INFO) << node.frame_name() << "<->" << cloud.frame_name()
                  << "  matches: " << indices.size() << " error: " << error;
        Sophus::SE3d apriltag_rig_pose_tag = perception::ProtoToSophus(
            node.pose(), model.apriltag_rig().name(), node.frame_name());
        Sophus::SE3d camera_rig_pose_tag =
            camera_rig_pose_apriltag_rig * apriltag_rig_pose_tag;
        // tag normal
        Eigen::Vector3d normal_camera_rig =
            camera_rig_pose_tag.rotationMatrix().col(2);
        Eigen::Vector3d point_camera_rig = camera_rig_pose_tag.translation();

        auto tag_points_cloud = perception::SelectPoints(points_cloud, indices);
        ceres::CostFunction* cost_function1 =
            new ceres::AutoDiffCostFunction<PointPlaneCostFunctor,
                                            ceres::DYNAMIC,
                                            Sophus::SE3d::num_parameters>(
                new PointPlaneCostFunctor(
                    point_camera_rig, normal_camera_rig, tag_points_cloud,
                    cloud_rig_edge->GetAPoseBMap(model.camera_rig().name(),
                                                 cloud.frame_name())),
                tag_points_cloud.cols());
        auto block_id = problem.AddResidualBlock(
            cost_function1, new ceres::CauchyLoss(0.05),
            cloud_rig_edge->GetAPoseB().data());

        ApriltagRigTagStats& stats = tag_stats[node.id()];
        stats.set_tag_id(node.id());
        stats.set_n_frames(stats.n_frames() + 1);

        PerImageRmse per_image_rmse;
        per_image_rmse.set_frame_number(i);
        per_image_rmse.set_camera_name(cloud.frame_name());

        tag_id_to_per_image_rmse_block_id[node.id()].push_back(
            std::make_tuple(per_image_rmse, block_id, tag_points_cloud.cols()));
      }
    }
  }

  // Set solver options (precision / method)
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.gradient_tolerance = 1e-20;
  options.function_tolerance = 1e-20;
  options.parameter_tolerance = 1e-20;
  options.max_num_iterations = 2000;

  // Solve
  ceres::Solver::Summary summary;
  options.logging_type = ceres::PER_MINIMIZER_ITERATION;
  options.minimizer_progress_to_stdout = false;
  ceres::Solve(options, &problem, &summary);
  LOG(INFO) << summary.FullReport() << std::endl;

  double total_rmse = 0.0;
  double total_count = 0;
  std::vector<ApriltagRigTagStats> out_tag_stats;
  model.clear_tag_stats();
  for (auto it : tag_id_to_per_image_rmse_block_id) {
    int tag_id = it.first;
    auto& stats = tag_stats[tag_id];
    for (auto per_image_rmse_block : it.second) {
      auto [image_rmse, block, n_residuals] = per_image_rmse_block;
      double cost;
      Eigen::Matrix<double, 1, Eigen::Dynamic> residuals(1, n_residuals);
      problem.EvaluateResidualBlockAssumingParametersUnchanged(
          block, false, &cost, residuals.data(), nullptr);

      double squared_mean = residuals.squaredNorm() / residuals.cols();

      total_rmse += squared_mean;
      total_count += 1;

      stats.set_tag_rig_rmse(stats.tag_rig_rmse() + squared_mean);

      image_rmse.set_rmse(std::sqrt(squared_mean));
      stats.add_per_image_rmse()->CopyFrom(image_rmse);
    }
    stats.set_tag_rig_rmse(std::sqrt(stats.tag_rig_rmse() / stats.n_frames()));
    model.add_tag_stats()->CopyFrom(stats);
  }
  double rmse = std::sqrt(total_rmse / total_count);
  LOG(INFO) << "model rmse (meters): " << rmse << "\n";
  model.set_rmse(rmse);
  model.mutable_lidar_poses()->CopyFrom(posegraph.ToNamedSE3Poses());
  return model;
}

class CalibrateMultiViewLidarProgram {
 public:
  CalibrateMultiViewLidarProgram(
      core::EventBus& bus, CalibrateMultiViewLidarConfiguration configuration,
      bool interactive)
      : bus_(bus), timer_(bus_.get_io_service()) {
    if (interactive) {
      status_.mutable_input_required_configuration()->CopyFrom(configuration);
    } else {
      set_configuration(configuration);
    }
    bus_.AddSubscriptions({"^" + bus_.GetName() + "/"});
    bus_.GetEventSignal()->connect(
        std::bind(&CalibrateMultiViewLidarProgram::on_event, this,
                  std::placeholders::_1));
    on_timer(boost::system::error_code());
  }

  int run() {
    while (status_.has_input_required_configuration()) {
      bus_.get_io_service().run_one();
    }
    LOG(INFO) << "config:\n" << configuration_.DebugString();

    CHECK(configuration_.has_event_log()) << "Please specify an event log.";

    fs::path output_dir =
        fs::path(configuration_.event_log().path()).parent_path();

    // Output under the same directory as the dataset.
    core::SetArchivePath((output_dir / "multi_view_lidar_model").string());

    MultiViewLidarModel model;
    model.mutable_lidar_poses()->CopyFrom(configuration_.lidar_poses());

    auto mv_rig_result = core::ReadProtobufFromResource<
        calibration::CalibrateMultiViewApriltagRigResult>(
        configuration_.calibrate_multi_view_apriltag_rig_result());

    LOG(INFO) << mv_rig_result.DebugString();
    model.mutable_camera_rig()->CopyFrom(
        core::ReadProtobufFromResource<perception::MultiViewCameraRig>(
            mv_rig_result.camera_rig_solved()));
    const auto& camera_rig = model.camera_rig();
    model.mutable_apriltag_rig()->CopyFrom(
        core::ReadProtobufFromResource<perception::ApriltagRig>(
            mv_rig_result.apriltag_rig_solved()));
    const auto& apriltag_rig = model.apriltag_rig();
    std::map<std::string, perception::TimeSeries<core::Event>> event_series;

    std::set<std::string> lidar_names;
    if (configuration_.include_lidars_size() > 0) {
      for (auto lidar_name : configuration_.include_lidars()) {
        lidar_names.insert(lidar_name);
      }
    }
    core::EventLogReader log_reader(configuration_.event_log());
    while (true) {
      core::Event event;
      try {
        event = log_reader.ReadNext();
      } catch (const std::runtime_error& e) {
        break;
      }

      if (event.data().Is<perception::PointCloud>()) {
        if (configuration_.include_lidars_size() == 0) {
          lidar_names.insert(event.name());
        }
      }
      event_series[event.name()].insert(event);
    }
    CHECK_GT(lidar_names.size(), 0);

    auto time_window =
        google::protobuf::util::TimeUtil::MillisecondsToDuration(1000);

    perception::ApriltagsFilter tag_filter;
    std::string root_camera_name = camera_rig.root_camera_name();

    int steady_count = 2;

    for (auto event : event_series[root_camera_name + "/apriltags"]) {
      perception::ApriltagDetections detections;
      CHECK(event.data().UnpackTo(&detections));
      if (!tag_filter.AddApriltags(detections, steady_count, 7)) {
        continue;
      }

      MultiViewLidarModel::Measurement measurement;
      for (auto lidar_name : lidar_names) {
        auto closest_event =
            event_series[lidar_name].FindNearest(event.stamp(), time_window);
        if (!closest_event) {
          LOG(WARNING) << "No closest event for: " << lidar_name
                       << " within time_window: "
                       << time_window.ShortDebugString();
          continue;
        }
        auto* cloud = measurement.mutable_multi_view_pointclouds()
                          ->add_point_clouds_per_view();
        CHECK(closest_event->data().UnpackTo(cloud))
            << closest_event->name() << " is not a PointCloud.";
      }
      auto* mv_detections = measurement.mutable_multi_view_detections();
      for (auto camera : camera_rig.cameras()) {
        std::string apriltags_topic = camera.frame_name() + "/apriltags";
        auto closest_event = event_series[apriltags_topic].FindNearest(
            event.stamp(), time_window);
        CHECK(closest_event->data().UnpackTo(
            mv_detections->add_detections_per_view()))
            << closest_event->name() << " is not an ApriltagDetections.";
      }
      // std::optional<std::tuple<perception::NamedSE3Pose, double,
      //                         std::vector<ApriltagRigTagStats>>>
      auto camera_rig_pose_est = EstimateMultiViewCameraRigPoseApriltagRig(
          camera_rig, apriltag_rig, *mv_detections);
      if (!camera_rig_pose_est) {
        LOG(WARNING) << "Couldn't estimate camera pose for frame: "
                     << event.name() << " " << event.stamp().ShortDebugString();
        continue;
      }
      auto [pose, rmse, stats] = *camera_rig_pose_est;
      measurement.mutable_camera_rig_pose_apriltag_rig()->CopyFrom(pose);
      model.add_measurements()->CopyFrom(measurement);
    }
    SavePlyFilesInTagRig(model, "/blobstore/scratch/init_");
    model = Solve(model);
    SavePlyFilesInTagRig(model, "/blobstore/scratch/solved_");

    auto model_bin = core::GetUniqueArchiveResource(
        "multi_view_lidar_model", "pb",
        core::ContentTypeProtobufBinary<MultiViewLidarModel>());
    core::WriteProtobufToBinaryFile(model_bin.second, model);

    auto temp_model = model;
    temp_model.clear_measurements();

    auto model_json = core::GetUniqueArchiveResource(
        "multi_view_lidar_model", "json",
        core::ContentTypeProtobufJson<MultiViewLidarModel>());
    LOG(INFO) << "writing: " << model_json.second.string();
    core::WriteProtobufToJsonFile(model_json.second, temp_model);

    LOG(INFO) << "Measurements: " << model.measurements_size();
    CalibrateMultiViewLidarResult result;

    send_status();
    return 0;
  }

  void send_status() {
    bus_.Send(core::MakeEvent(bus_.GetName() + "/status", status_));
  }

  void on_timer(const boost::system::error_code& error) {
    if (error) {
      LOG(WARNING) << "timer error: " << __PRETTY_FUNCTION__ << error;
      return;
    }
    timer_.expires_from_now(boost::posix_time::millisec(1000));
    timer_.async_wait(std::bind(&CalibrateMultiViewLidarProgram::on_timer, this,
                                std::placeholders::_1));

    send_status();
  }

  bool on_configuration(const core::Event& event) {
    CalibrateMultiViewLidarConfiguration configuration;
    if (!event.data().UnpackTo(&configuration)) {
      return false;
    }
    LOG(INFO) << configuration.ShortDebugString();
    set_configuration(configuration);
    return true;
  }

  void set_configuration(CalibrateMultiViewLidarConfiguration configuration) {
    configuration_ = configuration;
    status_.clear_input_required_configuration();
    send_status();
  }

  void on_event(const core::Event& event) {
    CHECK(event.name().rfind(bus_.GetName() + "/", 0) == 0);
    if (on_configuration(event)) {
      return;
    }
  }

 private:
  core::EventBus& bus_;
  boost::asio::deadline_timer timer_;
  CalibrateMultiViewLidarConfiguration configuration_;
  CalibrateMultiViewLidarStatus status_;
  CalibrateMultiViewLidarResult result_;
};

}  // namespace farm_ng::calibration

void Cleanup(farm_ng::core::EventBus& bus) {}

int Main(farm_ng::core::EventBus& bus) {
  farm_ng::calibration::CalibrateMultiViewLidarConfiguration config;
  if (!FLAGS_config.empty()) {
    config = farm_ng::core::ReadProtobufFromJsonFile<
        farm_ng::calibration::CalibrateMultiViewLidarConfiguration>(
        FLAGS_config);
    farm_ng::calibration::CalibrateMultiViewLidarProgram program(
        bus, config, FLAGS_interactive);
    return program.run();
  } else {
    config.set_name(FLAGS_name);
    config.mutable_event_log()->CopyFrom(
        farm_ng::core::EventLogResource(FLAGS_event_log));
    config.mutable_calibrate_multi_view_apriltag_rig_result()->CopyFrom(
        farm_ng::core::ProtobufJsonResource<
            farm_ng::calibration::CalibrateMultiViewApriltagRigResult>(
            FLAGS_calibrate_multi_view_apriltag_rig_result));
  }
  if (!FLAGS_output_config.empty()) {
    farm_ng::core::WriteProtobufToJsonFile(FLAGS_output_config, config);
    return 0;
  }
  LOG(ERROR) << "Please provide a config.";
  return -1;
}
int main(int argc, char* argv[]) {
  return farm_ng::core::Main(argc, argv, &Main, &Cleanup);
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, 6, 5> test_float_6_5(const Eigen::Matrix<float, 6, 5>& M) { return M; }
void export_float_6_5() { boost::python::def("test_float_6_5", test_float_6_5); }