/*  raycasting %{Cpp:License:ClassName} - Yann BOUCHER (yann) 12/04/2016
 *
**
**
**            DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
**                    Version 2, December 2004
**
** Copyright (C) 2004 Sam Hocevar <sam@hocevar.net>
**
** Everyone is permitted to copy and distribute verbatim or modified
** copies of this license document, and changing it is allowed as long
** as the name is changed.
**
**            DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
**   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
**
**  0. You just DO WHAT THE FUCK YOU WANT TO.
*/

#include "raycasting.hpp"

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <algorithm>
#include <iostream>

#include <cmath>

#include <Thor/Vectors.hpp>
#include <Thor/Graphics/ColorGradient.hpp>

#include "camera.hpp"
#include "map.hpp"
#include "drawableactor.hpp"

#include "utils/mathutility.hpp"
#include "utils/graphicsutility.hpp"
#include "utils/utility.hpp"

namespace Rayfun
{

namespace Raycasting
{

std::vector<RaycastResult> castRay(const sf::Vector2d &t_begin, const sf::Vector2d &t_dir, Map& t_map, HitMode t_hitmode)
{
    const sf::Vector2d rayDir = thor::unitVector(t_dir);
    auto mapPos = sf::Vector2i(t_begin);

    const sf::Vector2d deltaDist = {
        std::sqrt(1 + (t_dir.y * t_dir.y) / (t_dir.x * t_dir.x)),
        std::sqrt(1 + (t_dir.x * t_dir.x) / (t_dir.y * t_dir.y))
    };

    sf::Vector2d sideDist;
    const sf::Vector2i step = { rayDir.x < 0 ? -1 : 1, rayDir.y < 0 ? -1 : 1 };

    if (rayDir.x < 0)
    {
        sideDist.x = (t_begin.x - mapPos.x) * deltaDist.x;
    }
    else
    {
        sideDist.x = (mapPos.x + 1 - t_begin.x) * deltaDist.x;
    }
    if (rayDir.y < 0)
    {
        sideDist.y = (t_begin.y - mapPos.y) * deltaDist.y;
    }
    else
    {
        sideDist.y = (mapPos.y + 1 - t_begin.y) * deltaDist.y;
    }

    Side side { Side::North };
    bool hit { false };
    std::vector<RaycastResult> results;
    const auto correctedDir = thor::rotatedVector(rayDir, -90.0);

    while (true)
    {

        do
        {
            if (sideDist.x < sideDist.y) // hit on x size
            {
                sideDist.x += deltaDist.x;
                mapPos.x += step.x;
                side = correctedDir.y > 0 ? Side::North : Side::South;
            }
            else // hit on y side
            {
                sideDist.y += deltaDist.y;
                mapPos.y += step.y;
                side = correctedDir.x > 0 ? Side::East : Side::West;
            }

            if (BOOST_UNLIKELY(mapPos.x < 0) || BOOST_UNLIKELY(mapPos.y < 0) ||
                    BOOST_UNLIKELY(static_cast<unsigned>(mapPos.x) >= t_map.size().x) ||
                    BOOST_UNLIKELY(static_cast<unsigned>(mapPos.y) >= t_map.size().y)) // Outside of map bounds
            {
                if (results.empty())
                {
                    results.push_back({ side });
                }
                return results;
            }
            else
            {
                switch (t_hitmode)
                {
                    case HitMode::Visibility:
                        hit = t_map.tileAt(sf::Vector2s(mapPos)).isWall;
                        break;
                    case HitMode::Clipping:
                        hit = t_map.tileAt(sf::Vector2s(mapPos)).clip[side];
                        break;

                }
            }
        } while (!hit);

        double perpWallDist { 0 };

        if (side == Side::North || side == Side::South)
        {
            perpWallDist = (mapPos.x - t_begin.x + (1 - step.x) / 2.f) / t_dir.x;
        }
        else
        {
            perpWallDist = (mapPos.y - t_begin.y + (1 - step.y) / 2.f) / t_dir.y;
        }
        if (!std::isfinite(perpWallDist))
        {
            perpWallDist = 0;
        }

        //mapPos = thor::rotated(sf::Vector2d(mapPos), 90);

        // FIXME : replace all '64' with actual code !!

        auto& tileHit = t_map.tileAt(sf::Vector2s(mapPos));
        if (tileHit.tex[side])
        {
            if (side == Side::North || side == Side::South)
            {

                double wallX { }; //where exactly the wall was hit
                wallX = t_begin.y + perpWallDist * t_dir.y;
                wallX -= std::floor(wallX);
                wallX = 1 - wallX;

                unsigned texX = static_cast<unsigned>(wallX * static_cast<double>(
                                                          64));

                if (t_dir.x > 0)
                {
                    if (static_cast<int>(64 - texX - 1) < 0)
                    {
                        texX = 0;
                    }
                    else
                    {
                        texX = 64 - texX - 1;
                    }
                }

                sf::Vector2d hitPos = sf::Vector2d(mapPos) + sf::Vector2d(0, wallX);

                results.push_back({ side, tileHit, hitPos, mapPos,
                                    Utility::distance(t_begin, hitPos), perpWallDist });

                if (t_hitmode == HitMode::Clipping /*|| !Utility::imageStripContainsAlpha(*tileHit.tex[side], texX)*/)
                {
                    return results;
                }
            }
            else
            {

                double wallX { }; //where exactly the wall was hit
                wallX = t_begin.x + perpWallDist * t_dir.x;
                wallX -= std::floor(wallX);
                wallX = 1 - wallX;

                unsigned texX = static_cast<unsigned>(wallX * static_cast<double>(
                                                          64));

                if (t_dir.y < 0)
                {
                    if (static_cast<int>(64 - texX - 1) < 0)
                    {
                        texX = 0;
                    }
                    else
                    {
                        texX = 64 - texX - 1;
                    }
                }

                sf::Vector2d hitPos = sf::Vector2d(mapPos) + sf::Vector2d(wallX, 0);

                results.push_back({ side, tileHit, hitPos, mapPos,
                                    Utility::distance(t_begin, hitPos), perpWallDist });

                if (t_hitmode == HitMode::Clipping /*|| !Utility::imageStripContainsAlpha(*tileHit.tex[side], texX)*/)
                {
                    return results;
                }
            }

        }

    }

}

//std::vector<sf::Uint8> render(const Camera& t_cam, const Map& t_map, bool bilinear_filtering,
//                              bool t_bilinear_sprites)
//{
//    std::vector<sf::Uint8> renderTarget;
//    renderTarget.resize(t_cam.screenSize().x * t_cam.screenSize().y * 4);
//    std::fill(renderTarget.begin(), renderTarget.end(), 0);

//    std::vector<double> zbuffer;
//    zbuffer.resize(t_cam.screenSize().x);

//    std::sort(t_map.sprites.begin(), t_map.sprites.end(), [&t_cam](std::unique_ptr<DrawableActor>& rhs,
//              std::unique_ptr<DrawableActor>& lhs){
//        return Utility::distance(rhs->pos, t_cam.pos()) > Utility::distance(lhs->pos, t_cam.pos());
//    });

//    static Utility::LookupTable<double> lut(t_cam.screenSize().x, [&t_cam](size_t i){
//        return 2.f * i / t_cam.screenSize().x - 1;
//    });

//#pragma omp parallel for simd
//    for (size_t i = 0; i < t_cam.screenSize().x; ++i)
//    {
//        double cameraX = lut[i];
//        //double cameraX = 2.f * i / t_cam.screenSize().x - 1;
//        const sf::Vector2d dir = t_cam.direction() + t_cam.plane() * cameraX;
//        const auto& results = castRay(t_cam.pos(), dir, const_cast<Map&>(t_map), HitMode::Visibility);

//        for (size_t index { results.size() }; index > 0; --index)
//        {
//            const auto result = results[index - 1];
//            const long lineHeight = static_cast<long>(t_cam.screenSize().y / result.perpDistance) *
//                                    std::min<double>(t_cam.screenSize().x, t_cam.screenSize().y)
//                                    / std::max<double>(t_cam.screenSize().x, t_cam.screenSize().y);
//            const long drawStart = -lineHeight / 2 + static_cast<long>(t_cam.screenSize().y) / 2;

//            if (index == results.size())
//            {
//                zbuffer[i] = result.perpDistance;
//            }

//            long drawStartClamped = drawStart;
//            if (drawStart < 0)
//            {
//                drawStartClamped = 0;
//            }
//            const long drawEnd = static_cast<long>(lineHeight / 2 +
//                                                   static_cast<long>(t_cam.screenSize().y) / 2);
//            long drawEndClamped = drawEnd;
//            if (drawEnd >= static_cast<long>(t_cam.screenSize().y))
//            {
//                drawEndClamped = static_cast<long>(t_cam.screenSize().y) - 1;
//            }

//            if (BOOST_LIKELY((bool)result.tileHit) && BOOST_LIKELY(result.tileHit->tex[result.side] != nullptr))
//            {
//                const double wallX = { result.side == Side::North || result.side == Side::South ?
//                                       result.hitPos.y - std::floor(result.hitPos.y) : result.hitPos.x - std::floor(result.hitPos.x)
//                                     };

//                double texX = wallX * static_cast<double>(
//                                  result.tileHit->tex[result.side]->getSize().x);

//                if (((result.side == Side::North || result.side == Side::South) && dir.x > 0) ||
//                        ((result.side == Side::East || result.side == Side::West) && dir.y < 0))
//                {
//                    texX = static_cast<double>(result.tileHit->tex[result.side]->getSize().x) - texX;
//                }

//                const unsigned char brightness = Utility::clamp(
//                                                     t_map.ambientLight +
//                                                     result.tileHit->brigthnessMap[result.side][static_cast<unsigned>(texX)]
//                                                 , 0, 255);

//                for (size_t j { static_cast<size_t>(drawStartClamped) }; static_cast<long>(j) < drawEndClamped; ++j)
//                {
//                    const double texY = Utility::map<double>(static_cast<double>(j),
//                                                             static_cast<double>(drawStart),
//                                                             static_cast<double>(drawEnd), 0,
//                                                             static_cast<double>(result.tileHit->tex[result.side]->getSize().y));

//                    sf::Color color;
//                    if (bilinear_filtering)
//                    {
//                        color = Utility::bilinearFilter(*result.tileHit->tex[result.side], texX, texY);
//                    }
//                    else
//                    {
//                        color = result.tileHit->tex[result.side]->getPixel(texX, texY);
//                        if (result.tileHit->decals[result.side] && texX < result.tileHit->decals[result.side]->getSize().x &&
//                                texY < result.tileHit->decals[result.side]->getSize().y)
//                        {
//                            color = thor::blendColors(result.tileHit->tex[result.side]->getPixel(texX, texY),
//                                    result.tileHit->decals[result.side]->getPixel(texX, texY),
//                                    result.tileHit->decals[result.side]->getPixel(texX, texY).a / 255.f);
//                        }
//                    }
//                    Utility::setBrightness(color, brightness);
//                    color = detail::pixelIntensity(color, result.distance);
//                    const size_t offset = (t_cam.screenSize().x * j + i) * 4;
//                    if (BOOST_UNLIKELY(color.a != 0xFF))
//                    {
//                        sf::Color oldColor;
//                        oldColor.r = renderTarget[offset + 0];
//                        oldColor.g = renderTarget[offset + 1];
//                        oldColor.b = renderTarget[offset + 2];

//                        oldColor = detail::pixelIntensity(oldColor, result.distance);

//                        if (result.side == Side::East || result.side == Side::West)
//                        {
//                            Utility::setBrightness(color, 150);
//                        }

//                        sf::Color blendedColor = thor::blendColors(oldColor, color, color.a / 255.f);

//                        renderTarget[offset + 0] = blendedColor.r;
//                        renderTarget[offset + 1] = blendedColor.g;
//                        renderTarget[offset + 2] = blendedColor.b;
//                        renderTarget[offset + 3] = 0xFF;
//                    }
//                    else
//                    {
//                        if (result.side == Side::East || result.side == Side::West)
//                        {
//                            Utility::setBrightness(color, 150);
//                        }
//                        renderTarget[offset + 0] = color.r;
//                        renderTarget[offset + 1] = color.g;
//                        renderTarget[offset + 2] = color.b;
//                        renderTarget[offset + 3] = 0xFF;
//                    }
//                }

//                sf::Vector2d floorWall {};
//                switch (result.side)
//                {
//                    case Side::South:
//                        floorWall.x = result.mapPos.x;
//                        floorWall.y = result.mapPos.y + (1 - wallX);
//                        break;

//                    case Side::North:
//                        floorWall.x = result.mapPos.x + 1;
//                        floorWall.y = result.mapPos.y + (1 - wallX);
//                        break;

//                    case Side::East:
//                        floorWall.x = result.mapPos.x + (1 - wallX);
//                        floorWall.y = result.mapPos.y;
//                        break;

//                    case Side::West:
//                        floorWall.x = result.mapPos.x + (1 - wallX);
//                        floorWall.y = result.mapPos.y + 1;
//                        break;
//                }

//                if (drawEndClamped < 0)
//                {
//                    drawEndClamped = static_cast<long>(t_cam.screenSize().y);
//                }

//                //detail::floorCasting(t_cam, t_map, result, wallX, bilinear_filtering, drawEndClamped, i, renderTarget);
//            }
//        }
//    }

//    // Sprite casting

//    detail::spriteCasting(t_cam, t_map.sprites, zbuffer, t_bilinear_sprites, t_map.ambientLight, renderTarget);

//    return renderTarget;
//}

//namespace detail
//{

//void spriteCasting(const Camera &t_cam, const std::vector<std::unique_ptr<DrawableActor> > &t_sprites, const std::vector<double> &t_zbuffer, bool t_bilinear_sprites, unsigned char t_ambientLight, std::vector<sf::Uint8> &t_image)
//{
//    const double invDet = 1.0 / (t_cam.plane().x * t_cam.direction().y -
//                                 t_cam.direction().x * t_cam.plane().y); //required for correct matrix multiplication

//    for (const auto& sprite_ref : t_sprites)
//    {
//        auto& sprite = *sprite_ref;
//        sf::Image spriteTex = sprite.renderImage();

//        sf::Vector2d spritePos = sprite.pos - t_cam.pos();

//        //transform sprite with the inverse camera matrix
//        // [ planeX   dirX ] -1                                       [ dirY      -dirX ]
//        // [               ]       =  1/(planeX*dirY-dirX*planeY) *   [                 ]
//        // [ planeY   dirY ]                                          [ -planeY  planeX ]

//        sf::Vector2d transform;
//        transform.x = invDet * (t_cam.direction().y * spritePos.x - t_cam.direction().x * spritePos.y);
//        transform.y = invDet * (-t_cam.plane().y * spritePos.x + t_cam.plane().x * spritePos.y);

//        const int zScreen = (spriteTex.getSize().x * sprite.scale.y - sprite.z) / transform.y;

//        int spriteScreenX = static_cast<int>((t_cam.screenSize().x / 2) * (1 + transform.x / transform.y));

//        double ratioX = std::min<double>(spriteTex.getSize().x, spriteTex.getSize().y)
//                        / std::max<double>(spriteTex.getSize().x, spriteTex.getSize().y);

//        const int spriteHeight = static_cast<int>(std::abs(t_cam.screenSize().y / transform.y))
//                                 * sprite.scale.y;

//        int drawStartY = -spriteHeight / 2 + t_cam.screenSize().y / 2 + zScreen;
//        if (drawStartY < 0)
//        {
//            drawStartY = 0;
//        }
//        size_t drawEndY = spriteHeight / 2 + t_cam.screenSize().y / 2 + zScreen;
//        if (drawEndY >= t_cam.screenSize().y)
//        {
//            drawEndY = t_cam.screenSize().y - 1;
//        }

//        const int spriteWidth = std::abs(t_cam.screenSize().y / (transform.y)) * ratioX * sprite.scale.x;
//        int drawStartX = -spriteWidth / 2 + spriteScreenX;
//        if (drawStartX < 0)
//        {
//            drawStartX = 0;
//        }
//        int drawEndX = spriteWidth / 2 + spriteScreenX;
//        if (drawEndX >= int(t_cam.screenSize().x))
//        {
//            drawEndX = t_cam.screenSize().x - 1;
//        }
//        for (int stripe = drawStartX; stripe < drawEndX; ++stripe)
//        {
//            double texX = (stripe - (-spriteWidth / 2 + spriteScreenX)) * double(spriteTex.getSize().x) / double(spriteWidth);

//            if (transform.y > 0 && stripe < t_cam.screenSize().x && transform.y < t_zbuffer[stripe] && texX >= 0)
//            {
//                for (int y = drawStartY; y < drawEndY; ++y)
//                {
//                    const double d = (y - zScreen) - t_cam.screenSize().y / 2 + spriteHeight / 2;
//                    const double texY = d * spriteTex.getSize().y / double(spriteHeight);
//                    sf::Color color;
//                    if (t_bilinear_sprites)
//                    {
//                        color = Utility::bilinearFilter(spriteTex, texX, texY);
//                    }
//                    else
//                    {
//                        color = spriteTex.getPixel(size_t(texX), size_t(texY));
//                    }
//                    Utility::setBrightness(color, t_ambientLight);
//                    color = pixelIntensity(color, Utility::distance(t_cam.pos(), sprite.pos));
//                    if (color.a == 255)
//                    {
//                        t_image[(t_cam.screenSize().x * y + stripe) * 4 + 0] = color.r;
//                        t_image[(t_cam.screenSize().x * y + stripe) * 4 + 1] = color.g;
//                        t_image[(t_cam.screenSize().x * y + stripe) * 4 + 2] = color.b;
//                        t_image[(t_cam.screenSize().x * y + stripe) * 4 + 3] = 0xFF;
//                    }
//                }
//            }
//        }

//    }
//}

//void floorCasting(const Camera &t_cam, const Map &t_map, const RaycastResult &t_result, double t_wallX, bool t_bilinear_filtering, long t_drawEnd, size_t t_screenX, std::vector<sf::Uint8> &t_image)
//{

//    sf::Vector2d floorWall {};
//    switch (t_result.side)
//    {
//        case Side::South:
//            floorWall.x = t_result.mapPos.x;
//            floorWall.y = t_result.mapPos.y + (1 - t_wallX);
//            break;

//        case Side::North:
//            floorWall.x = t_result.mapPos.x + 1;
//            floorWall.y = t_result.mapPos.y + (1 - t_wallX);
//            break;

//        case Side::East:
//            floorWall.x = t_result.mapPos.x + (1 - t_wallX);
//            floorWall.y = t_result.mapPos.y;
//            break;

//        case Side::West:
//            floorWall.x = t_result.mapPos.x + (1 - t_wallX);
//            floorWall.y = t_result.mapPos.y + 1;
//            break;
//    }

//    static Utility::LookupTable<double> lut(t_cam.screenSize().y, [&t_cam](size_t i)
//    {
//        return t_cam.screenSize().y / (2.f * i - t_cam.screenSize().y);
//    });
//#pragma omp parallel for simd
//    for (size_t j = static_cast<size_t>(t_drawEnd) + 1; j < t_cam.screenSize().y; ++j)
//    {
//        //const double currentDist = t_cam.screenSize().y / (2.f * j - t_cam.screenSize().y);
//        const double currentDist = lut[j];
//        const double weight = currentDist / t_result.perpDistance;
//        const sf::Vector2d floorPos = weight * floorWall +
//                                      (1.0 - weight) * t_cam.pos();

//        const auto sector = t_map.sectorAt(sf::Vector2s(floorPos));

//        if (BOOST_LIKELY((bool)sector))
//        {
//            const sf::Image& floor = sector.get().floor;
//            const sf::Image& ceiling = sector.get().floor;
//            sf::Vector2d floorTexCoord {};

//            floorTexCoord.x = (floorPos.x - std::floor(floorPos.x)) * floor.getSize().x;
//            floorTexCoord.y = (floorPos.y - std::floor(floorPos.y)) * floor.getSize().y;

//            sf::Vector2d ceilTexCoord {};

//            ceilTexCoord.x = (floorPos.x - std::floor(floorPos.x)) * ceiling.getSize().x;
//            ceilTexCoord.y = (floorPos.y - std::floor(floorPos.y)) * ceiling.getSize().y;

//            sf::Color floorTexel;
//            sf::Color ceilTexel;

//            if (t_bilinear_filtering)
//            {
//                floorTexel = Utility::bilinearFilter(floor, floorTexCoord.x, floorTexCoord.y);
//                ceilTexel = Utility::bilinearFilter(ceiling, ceilTexCoord.x, ceilTexCoord.y);
//            }
//            else
//            {
//                floorTexel = floor.getPixel(static_cast<unsigned>(floorTexCoord.x),
//                                            static_cast<unsigned>(floorTexCoord.y));
//                ceilTexel = ceiling.getPixel(static_cast<unsigned>(ceilTexCoord.x),
//                                             static_cast<unsigned>(ceilTexCoord.y));
//            }

//            Utility::setBrightness(floorTexel, t_map.ambientLight);
//            Utility::setBrightness(ceilTexel, t_map.ambientLight);

//            Utility::setBrightness(floorTexel, 150);

//            floorTexel = pixelIntensity(floorTexel, currentDist);
//            ceilTexel = pixelIntensity(ceilTexel, currentDist);

//            const auto floorPos = (t_cam.screenSize().x * (j-1) + t_screenX) * 4;
//            t_image[floorPos + 0] = floorTexel.r;
//            t_image[floorPos + 1] = floorTexel.g;
//            t_image[floorPos + 2] = floorTexel.b;
//            t_image[floorPos + 3] = floorTexel.a;

//            const auto ceilPos = (t_cam.screenSize().x * (t_cam.screenSize().y - j) + t_screenX) * 4;
//            t_image[ceilPos + 0] = ceilTexel.r;
//            t_image[ceilPos + 1] = ceilTexel.g;
//            t_image[ceilPos + 2] = ceilTexel.b;
//            t_image[ceilPos + 3] = ceilTexel.a;
//        }
//    }
//}

//sf::Color pixelIntensity(sf::Color t_pixel, double t_dist)
//{
//    sf::Color returnColor = t_pixel;
//    if (t_dist < 0) t_dist = 0;
//    unsigned multiplier = 10;
//    double intensity = 0.5 / t_dist * multiplier;
//    if (intensity > 1) intensity = 1;
//    else if (intensity < 0) intensity = 0;

//    returnColor.r = t_pixel.r * intensity;
//    returnColor.g = t_pixel.g * intensity;
//    returnColor.b = t_pixel.b * intensity;

//    return returnColor;
//}

//}

bool rayIntersectsSprite(const sf::Vector2d &t_begin, const sf::Vector2d &t_dir, Map &t_map,
                         DrawableActor*& t_hitSprite, size_t t_maxDistance)
{
    typedef boost::geometry::model::d2::point_xy<double> Point;
    boost::geometry::model::linestring<Point> line;
    Point origin = {t_begin.x, t_begin.y};
    line.push_back(origin);
    line.push_back({t_begin.x + t_dir.x * t_maxDistance, t_begin.y + t_dir.y * t_maxDistance});

    for (auto& sprite : t_map.sprites)
    {
        sf::Image img = sprite->renderImage();
        boost::geometry::model::polygon<Point> spriteRect;
        double largestSide = std::max<double>(img.getSize().x, img.getSize().y);
        double side = img.getSize().x / largestSide;
        std::vector<Point> points =
        {
            {sprite->pos.x - side / 2, sprite->pos.x - side / 2},
            {sprite->pos.x - side / 2, sprite->pos.x + side / 2},
            {sprite->pos.x + side / 2, sprite->pos.x + side / 2},
            {sprite->pos.x + side / 2, sprite->pos.x - side / 2}
        };
        boost::geometry::assign_points(spriteRect, points);
        boost::geometry::model::multi_point<Point> intersectionPoints;
        if (boost::geometry::intersection(line, spriteRect, intersectionPoints) && !intersectionPoints.empty())
        {
            auto result = castRay(t_begin, t_dir, t_map, HitMode::Visibility).front();
            if (boost::geometry::distance(origin, intersectionPoints.front()) <= result.distance)
            {
                t_hitSprite = sprite.get();
                return true;
            }
        }
    }

    return false;
}

} // namespace Raycasting

} // namespace Rayfun


#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/io.hpp>
#include <iostream>
using namespace boost::units;
using namespace boost::units::si;
using namespace std;
class vectorunit
{
  private:
    quantity<length> x;
    quantity<length> y;
    quantity<length> z;

  public:
    vectorunit(quantity<length> vx, quantity<length> vy, quantity<length> vz)
    {
        x = vx;
        y = vy;
        z = vz;
    }
    void display_vector()
    {
        cout << "(" << x << "," << y << "," << z << ")" << endl;
    }
};

int main(int argc, char const *argv[])
{
    double x1, y1, z1;
    cout << "Enter 1st vector coordinate" << endl;
    cin >> x1 >> y1 >> z1;
    quantity<length> nx(x1 * meters);
    quantity<length> ny(y1 * meters);
    quantity<length> nz(z1 * meters);
    vectorunit result_vector(nx,ny,nz);
    result_vector.display_vector();
        return 0;
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>
#include <functional>

#include "eqeqc.h"
#include "../parameters.h"
#include "../geometry.h"

CHARGEFW2_METHOD(EQeqC)

Eigen::VectorXd EQeqC::EE_system(const std::vector<const Atom *> &atoms, double total_charge) const {

    size_t n = atoms.size();

    const double lambda = 1.2;
    const double k = 14.4;
    double H_electron_affinity = -2.0; // Exception for hydrogen mentioned in the article

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);
    Eigen::VectorXd J = Eigen::VectorXd::Zero(n);
    Eigen::VectorXd X = Eigen::VectorXd::Zero(n);

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = *atoms[i];
        if (atom_i.element().symbol() == "H") {
            X(i) = (atom_i.element().ionization_potential() + H_electron_affinity) / 2;
            J(i) = atom_i.element().ionization_potential() - H_electron_affinity;
        } else {
            X(i) = (atom_i.element().ionization_potential() + atom_i.element().electron_affinity()) / 2;
            J(i) = atom_i.element().ionization_potential() - atom_i.element().electron_affinity();
        }
    }

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = *atoms[i];
        A(i, i) = J(i);
        b(i) = -X(i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = *atoms[j];
            double a = std::sqrt(J(i) * J(j)) / k;
            double Rij = distance(atom_i, atom_j);
            double overlap = std::exp(-a * a * Rij * Rij) * (2 * a - a * a * Rij - 1 / Rij);
            auto x = lambda * k / 2 * (1 / Rij + overlap);
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A(n, n) = 0;
    b(n) = total_charge;

    return A.partialPivLu().solve(b).head(n);
}

std::vector<double> EQeqC::calculate_charges(const Molecule &molecule) const {
    size_t n = molecule.atoms().size();

    auto f = [this](const std::vector<const Atom *> &atoms, double total_charge) -> Eigen::VectorXd {
        return EE_system(atoms, total_charge);
    };

    Eigen::VectorXd q = solve_EE(molecule, f);

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = molecule.atoms()[i];
        double correction = 0;
        for (size_t j = 0; j < n; j++) {
            if (i == j)
                continue;
            const auto &atom_j = molecule.atoms()[j];
            double tkk = parameters_->atom()->parameter(atom::Dz)(atom_i) - parameters_->atom()->parameter(atom::Dz)(atom_j);
            double bkk = std::exp(-parameters_->common()->parameter(common::alpha) *
                                  (distance(atom_i, atom_j) - atom_i.element().covalent_radius() -
                                   atom_j.element().covalent_radius()));
            correction += tkk * bkk;
        }
        q(i) += correction;
    }
    return std::vector<double>(q.data(), q.data() + q.size());
}


/**
 * \file RLSFilter.cpp
 */

#include "RLSFilter.h"
#include <ATK/Core/TypeTraits.h>
#include <ATK/Core/Utilities.h>

#include <Eigen/Core>

#include <cstdint>
#include <complex>
#include <stdexcept>

namespace ATK
{
  template<typename DataType_>
  class RLSFilter<DataType_>::RLSFilterImpl
  {
  public:
    using PType = Eigen::Matrix<DataType_, Eigen::Dynamic, Eigen::Dynamic>;
    using wType = Eigen::Matrix<DataType_, Eigen::Dynamic, 1>;
    using xType = Eigen::Map<const wType>;

    explicit RLSFilterImpl(gsl::index size)
      :P(PType::Identity(size, size) / DataType(size)), w(wType::Zero(size))
    {
    }

    void learn(const xType& x, DataType_ target, DataType_ actual)
    {
      auto alpha = target - actual;
      auto xreverse = x.reverse();

      wType g = (P * xreverse) / ((xreverse.adjoint() * P * xreverse)(0,0) + static_cast<DataType>(memory));
      PType pupdate = (g * (xreverse.adjoint() * P));
      w = w + TypeTraits<DataType>::conj(alpha) * g;
      P = (P - (pupdate + pupdate.transpose()) / 2) * memory;
    }

    void set_P(const DataType_* P)
    {
      this->P = Eigen::Map<const PType>(P, this->P.rows(), this->P.cols());
    }

    const DataType_* get_P() const
    {
      return P.data();
    }

    void set_w(const DataType_* w)
    {
      this->w = xType(w, this->w.rows(), 1);
    }

    const DataType_* get_w() const
    {
      return w.data();
    }

    PType P;
    wType w;
    double memory = 0.99;
  };

  template<typename DataType_>
  RLSFilter<DataType_>::RLSFilter(gsl::index size)
  :Parent(1, 1), impl(std::make_unique<RLSFilterImpl>(size)), global_size(size)
  {
    input_delay = size + 1;
  }

  template<typename DataType_>
  RLSFilter<DataType_>::~RLSFilter()
  {
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_size(gsl::index size)
  {
    if(size == 0)
    {
      throw ATK::RuntimeError("Size must be strictly positive");
    }

    impl->P = RLSFilterImpl::PType::Identity(size, size) / DataType(size);
    impl->w = typename RLSFilterImpl::wType(size, 1);
    input_delay = size+1;
    this->global_size = size;
  }

  template<typename DataType_>
  gsl::index RLSFilter<DataType_>::get_size() const
  {
    return global_size;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_memory(double memory)
  {
    if(memory >= 1)
    {
      throw ATK::RuntimeError("Memory must be less than 1");
    }
    if(memory <= 0)
    {
      throw ATK::RuntimeError("Memory must be strictly positive");
    }

    impl->memory = memory;
  }

  template<typename DataType_>
  double RLSFilter<DataType_>::get_memory() const
  {
    return impl->memory;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_learning(bool learning)
  {
    this->learning = learning;
  }

  template<typename DataType_>
  bool RLSFilter<DataType_>::get_learning() const
  {
    return learning;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::process_impl(gsl::index size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];

    for(gsl::index i = 0; i < size; ++i)
    {
      typename RLSFilterImpl::xType x(input - global_size + i, global_size, 1);

      // compute next sample
      output[i] = impl->w.adjoint().dot(x.reverse());

      if(learning)
      {
        //update w and P
        impl->learn(x, input[i], output[i]);
      }
    }
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_P(const DataType_* P)
  {
     impl->set_P(P);
  }

  template<typename DataType_>
  const DataType_* RLSFilter<DataType_>::get_P() const
  {
    return impl->get_P();
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_w(const DataType_* w)
  {
    impl->set_w(w);
  }

  template<typename DataType_>
  const DataType_* RLSFilter<DataType_>::get_w() const
  {
    return impl->get_w();
  }

  template class RLSFilter<double>;
#if ATK_ENABLE_INSTANTIATION
  template class RLSFilter<float>;
  template class RLSFilter<std::complex<float>>;
  template class RLSFilter<std::complex<double>>;
#endif
}


#include <boost/math/special_functions/binomial.hpp>


#include "FEMConstraint.h"
#include <iostream>
#include <Eigen/LU>
using namespace FEM;
bool
FEMConstraint::
ComputeDeformationGradient(const Eigen::VectorXd& x)
{
	Eigen::Vector2d x0(x.block<2,1>(mi0*2,0));

	Eigen::Matrix2d Ds;

	Ds.block<2,1>(0,0) = x.block<2,1>(mi1*2,0)-x0;
	Ds.block<2,1>(0,1) = x.block<2,1>(mi2*2,0)-x0;

	mCacheDs = Ds;
	mCacheF = mCacheDs * mInvDm;

	return true;
}

FEMConstraint::
FEMConstraint(const double& stiffness,const double& poisson_ratio,int i0,int i1,int i2,double vol,const Eigen::Matrix2d& invDm)
	:Constraint(stiffness),
	mMu(stiffness/((1.0+poisson_ratio))),mLambda(stiffness*poisson_ratio/((1.0+poisson_ratio)*(1-2.0*poisson_ratio))),
	mi0(i0),mi1(i1),mi2(i2),mVol(vol),mInvDm(invDm),
	mCacheDs(Eigen::Matrix2d::Zero()),
	mCacheF(Eigen::Matrix2d::Zero())
{
}

void
FEMConstraint::
AddInversionFreePosition(Eigen::VectorXd& x)
{
	ComputeDeformationGradient(x);
	double vol = mCacheF.determinant();
	if(vol<0)
	{
		Eigen::Vector2d proj_0;
		Eigen::Vector2d proj_1;
		Eigen::Vector2d proj_2;

		Eigen::Vector2d p0,p1,p2;
		p0 = x.block<2,1>(2*mi0,0);
		p1 = x.block<2,1>(2*mi1,0);
		p2 = x.block<2,1>(2*mi2,0);

		proj_0 = (((p2-p1).dot(p0-p1))/((p2-p1).squaredNorm()))*(p2-p1) + p1;
		proj_1 = (((p2-p0).dot(p1-p0))/((p2-p0).squaredNorm()))*(p2-p0) + p0;
		proj_2 = (((p1-p0).dot(p2-p0))/((p1-p0).squaredNorm()))*(p1-p0) + p0;

		double len_0 = (p0-proj_0).squaredNorm();
		double len_1 = (p1-proj_1).squaredNorm();
		double len_2 = (p2-proj_2).squaredNorm();
		double eps = 1E-7;

		if(len_0<len_1&&len_0<len_2)
			x.block<2,1>(mi0*2,0) = proj_0+eps*(proj_0-p0);
		else if(len_1<len_0&&len_1<len_2)
			x.block<2,1>(mi1*2,0) = proj_1+eps*(proj_1-p1);
		else
			x.block<2,1>(mi2*2,0) = proj_2+eps*(proj_2-p2);
	}
}
int
FEMConstraint::
GetDof()
{
	return 2;
}
int
FEMConstraint::
GetNumHessianTriplets()
{
	return 36;
}
void
FEMConstraint::
AddOffset(const int& offset)
{
	mi0+=offset;
	mi1+=offset;
	mi2+=offset;
}

const int&
FEMConstraint::
GetI0()
{
	return mi0;
}
const int&
FEMConstraint::
GetI1()
{
	return mi1;
}
const int&
FEMConstraint::
GetI2()
{
	return mi2;
}


#include <iostream>
#include <Eigen/Core>
#include "iris/iris.h"

int main(int argc, char** argv) {
  iris::IRISProblem problem(2);
  problem.setSeedPoint(Eigen::Vector2d(0.1, 0.1));

  Eigen::MatrixXd obs(2,2);
  // Inflate a region inside a 1x1 box
  obs << 0, 1,
         0, 0;
  problem.addObstacle(obs);
  obs << 1, 1,
         0, 1;
  problem.addObstacle(obs);
  obs << 1, 0,
         1, 1;
  problem.addObstacle(obs);
  obs << 0, 0,
         1, 0;
  problem.addObstacle(obs);

  iris::IRISOptions options;
  iris::IRISRegion region = inflate_region(problem, options);

  std::cout << "C: " << region.ellipsoid.getC() << std::endl;
  std::cout << "d: " << region.ellipsoid.getD() << std::endl;
  return 0;
}


#include "lelantus_test_fixture.h"

#include "../range_prover.h"
#include "../range_verifier.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

BOOST_FIXTURE_TEST_SUITE(lelantus_range_proof_tests, LelantusTestingSetup)

BOOST_AUTO_TEST_CASE(prove_verify)
{
    uint64_t n = 64;
    uint64_t m = 4;
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();

    //creating generators g, h vectors
    auto g_ = RandomizeGroupElements(n * m);
    auto h_ = RandomizeGroupElements(n * m);

    auto serials = RandomizeScalars(m);
    auto randoms = RandomizeScalars(m);

    std::vector<secp_primitives::Scalar> v_s;
    std::vector<secp_primitives::GroupElement> V;
    for(uint64_t i = 0; i < m; ++i){
        v_s.emplace_back(701 + i);
        V.push_back(g_gen * v_s.back() +  h_gen1 * randoms[i] + h_gen2 * serials[i]);
    }

    RangeProver rangeProver(g_gen, h_gen1, h_gen2, g_, h_, n);
    RangeProof proof;
    rangeProver.batch_proof(v_s, serials, randoms, proof);

    RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n);
    BOOST_CHECK(rangeVerifier.verify_batch(V, proof));
}

BOOST_AUTO_TEST_CASE(out_of_range_notVerify)
{
    uint64_t n = 4;
    uint64_t m = 4;
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();

    //creating generators g, h vectors
    auto g_ = RandomizeGroupElements(n * m);
    auto h_ = RandomizeGroupElements(n * m);

    auto randoms = RandomizeScalars(m);
    auto serials = RandomizeScalars(m);

    auto testF = [&] (std::vector<Scalar> const v_s) {
        std::vector<GroupElement> V;
        for (uint64_t i = 0; i < m; ++i) {
            V.push_back(g_gen * v_s[i] +  h_gen1 * randoms[i] + h_gen2 * serials[i]);
        }

        lelantus::RangeProver rangeProver(g_gen, h_gen1, h_gen2, g_, h_, n);
        lelantus::RangeProof proof;
        rangeProver.batch_proof(v_s, serials, randoms, proof);

        lelantus::RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n);
        BOOST_CHECK(!rangeVerifier.verify_batch(V, proof));
    };

    // All values are out of range
    std::vector<Scalar> vs;
    for(uint64_t i = 0; i < m; ++i){
        vs.emplace_back(17 + i);
    }
    testF(vs);

    // [0, 2 ^ n - 1]
    Scalar l(uint64_t(0));
    Scalar r((1 << n) - 1);

    // One value is out of range
    vs = {l, l + 1, r, r + 1};
    testF(vs);

    vs = {l - 1, l, r - 1, r};
    testF(vs);
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"
#include <chrono>

using namespace std;
using namespace cv;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);

void myBundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d[],
    const Mat& K,
    Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d[2];
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d[0].push_back ( keypoints_1[m.queryIdx].pt );
        pts_2d[1].push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d[1], K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    ::myBundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

class VertexCamera : public g2o::BaseVertex<6, Sophus::SE3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        Eigen::Map<const Vector6d> update_vec(update);
        setEstimate(Sophus::SE3d::exp(update_vec) * _estimate);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {
        return false;
    }
    virtual bool write( ostream& out ) const {
        return false;
    }
};

class VertexPointXYZ : public g2o::BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual bool read( istream& in ) {
        return false;
    }
    virtual bool write( ostream& out ) const {
        return false;
    }

    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Map<const Eigen::Vector3d> v(update);
      _estimate += v;
    }
};

class EdgeProject : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPointXYZ, VertexCamera>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeProject() {
        _cam = nullptr;
        resizeParameters(1);
        installParameter(_cam, 0);
    }

    virtual bool read( istream& in ) {
        return false;
    }
    virtual bool write( ostream& out ) const {
        return false;
    }

    void computeError()  {
      const VertexCamera* v1 = static_cast<const VertexCamera*>(_vertices[1]);
      const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]);
      const g2o::CameraParameters * cam
        = static_cast<const g2o::CameraParameters *>(_cam);
      _error = _measurement - cam->cam_map(v1->estimate() * (v2->estimate()));

    }

    g2o::CameraParameters * _cam;
};

// reference: T=
//     0.99778  -0.0522171   0.0413239   -0.129224
//    0.051047    0.998279   0.0288811 -0.00970178
//  -0.0427608  -0.0267076    0.998728   0.0601133
//           0           0           0           1

void myBundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d[],
    const Mat& K,
    Mat& R, Mat& t )
{
// 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    std::unique_ptr<Block::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>(); // 线性方程求解器
    std::unique_ptr<Block> solver_ptr = g2o::make_unique<Block> ( move(linearSolver) );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( move(solver_ptr) );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    VertexCamera* pose[2]; // camera pose
    pose[0] = new VertexCamera();
    pose[0]->setId(0);
    pose[0]->setEstimate(Sophus::SE3d());
    optimizer.addVertex(pose[0]);

    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose[1] = new VertexCamera();
    pose[1]->setId ( 1 );
    pose[1]->setEstimate ( Sophus::SE3d (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose[1] );

    int index = 2;
    for ( const Point3f p:points_3d )   // landmarks
    {
        VertexPointXYZ* point = new VertexPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 2;
    assert(points_2d[0].size() == points_2d[1].size());
    for (size_t i = 0; i < points_2d[0].size(); i++)
    {
        for (size_t j = 0; j < 2; j++) {
            EdgeProject* edge = new EdgeProject();
            edge->setId ( index );
            // cout << index << endl;
            edge->setVertex ( 0, dynamic_cast<VertexPointXYZ*> ( optimizer.vertex ( index / 2 + 1 ) ) );
            edge->setVertex ( 1, pose[j] );
            edge->setMeasurement ( Eigen::Vector2d ( points_2d[j][i].x, points_2d[j][i].y ) );
            edge->setParameterId ( 0,0 );
            edge->setInformation ( Eigen::Matrix2d::Identity() );
            optimizer.addEdge ( edge );
            index++;
        }
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T1="<<endl<<pose[0]->estimate().matrix() <<endl;
    cout<<"T2="<<endl<<pose[1]->estimate().matrix() <<endl;
    cout<<"relative T="<<endl<<(pose[0]->estimate().inverse() * pose[1]->estimate()).matrix() <<endl;
}

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    std::unique_ptr<Block::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>(); // 线性方程求解器
    std::unique_ptr<Block> solver_ptr = g2o::make_unique<Block> ( move(linearSolver) );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( move(solver_ptr) );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}


/*
BSD 3-Clause License

Copyright (c) 2020, Jack Miles Hunt
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef GPLIB_KERNEL_HEADER
#define GPLIB_KERNEL_HEADER

#include <Aliases.hpp>

#include <vector>
#include <algorithm>
#include <iostream>
#include <optional>
#include <cmath>

#include <Eigen/Dense>

namespace GPLib {
    //Available kernel types enumerated here.
    enum class KernelType : short {
        SQUARED_EXPONENTIAL
    };

    template<typename T>
    class Kernel {
    protected:
        ParameterSet<T> params;
        std::vector<std::string> validParams;

    protected:
        void verifyParam(const std::string& var) const {
            assert(params.find(var) != params.end());
        }

        void verifyParams() {
            // Check for missing parameters.
            for (const auto& p : validParams) {
                verifyParam(p);
            }

            // Check for invalid parameters.
            for (const auto p : params) {
                if (std::find(validParams.begin(), validParams.end(), p.first) == validParams.end()) {
                    std::cout << "WARNING: Surplus parameter " + p.first + " being removed from parameter set!";
                    params.erase(p.first);
                }
            }
        }

    protected:
        Kernel(const std::vector< std::string >& validParams, const ParameterSet<T>& params) {
            //
        }

    public:
        virtual ~Kernel() {
            //
        }

    public:
        ParameterSet<T> getParameters() const {
            return params;
        }

        void setParameters(const ParameterSet<T>& params) {
            this->params = params;
            verifyParams();
        }

        virtual T f(const Vector<T>& a, const Vector<T>& b) const = 0;

        virtual KernelGradient<T> df(const Vector<T>& a, const Vector<T>& b,
                                  const std::optional<std::string>& gradVar = std::nullopt) const = 0;

        virtual Vector<T> dfda(const Vector<T>& a, const Vector<T>& b) const = 0;

        virtual Vector<T> dfdb(const Vector<T>& a, const Vector<T>& b) const = 0;
    };
}

#endif


#include "MexPackUnpack.h"
#include "mex.h"
#include <Eigen>

using namespace MexPackUnpackTypes;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // We expect two real double matrices (treated as eigen maps), a scalar double, and a string
  MexUnpacker<EDRM,  EDRM,double,std::string> my_unpack(nrhs, prhs);

  try {

    auto [a,b,c,d] = my_unpack.unpackMex(); //a,b are Eigen maps, c is double, d is std::string
    Eigen::MatrixXd e = a+b*c;
    d+=std::string(" appended to the string");
    MexPacker<Eigen::MatrixXd,std::string> my_pack(nlhs, plhs);
    my_pack.PackMex(e,d);    // We return an eigen Matrix and a string

  } catch (std::string s) {
    mexPrintf(s.data());
  }

}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <iterator>
#include <vector>
#include <list>
// Use boost::queue instead of std::queue because std::queue doesn't
// model Buffer; it has to top() function. -Jeremy
#include <boost/pending/queue.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace std;
using namespace boost;
/*
  This example does a best-first-search (using dijkstra's) and
  simultaneously makes a copy of the graph (assuming the graph is
  connected).

  Example Graph: (p. 90 "Data Structures and Network Algorithms", Tarjan)

              g
            3+ +2
            / 1 \
           e+----f
           |+0 5++
           | \ / |
         10|  d  |12
           |8++\7|
           +/ | +|
           b 4|  c
            \ | +
            6+|/3
              a

  Sample Output:
a --> c d
b --> a d
c --> f
d --> c e f
e --> b g
f --> e g
g -->
Starting graph:
a(32767); c d
c(32767); f
d(32767); c e f
f(32767); e g
e(32767); b g
g(32767);
b(32767); a d
Result:
a(0); d c
d(4); f e c
c(3); f
f(9); g e
e(4); g b
g(7);
b(14); d a

*/

typedef property< vertex_color_t, default_color_type,
    property< vertex_distance_t, int > >
    VProperty;
typedef int weight_t;
typedef property< edge_weight_t, weight_t > EProperty;

typedef adjacency_list< vecS, vecS, directedS, VProperty, EProperty > Graph;

template < class Tag >
struct endl_printer : public boost::base_visitor< endl_printer< Tag > >
{
    typedef Tag event_filter;
    endl_printer(std::ostream& os) : m_os(os) {}
    template < class T, class Graph > void operator()(T, Graph&)
    {
        m_os << std::endl;
    }
    std::ostream& m_os;
};
template < class Tag > endl_printer< Tag > print_endl(std::ostream& os, Tag)
{
    return endl_printer< Tag >(os);
}

template < class PA, class Tag >
struct edge_printer : public boost::base_visitor< edge_printer< PA, Tag > >
{
    typedef Tag event_filter;

    edge_printer(PA pa, std::ostream& os) : m_pa(pa), m_os(os) {}

    template < class T, class Graph > void operator()(T x, Graph& g)
    {
        m_os << "(" << get(m_pa, source(x, g)) << "," << get(m_pa, target(x, g))
             << ") ";
    }
    PA m_pa;
    std::ostream& m_os;
};
template < class PA, class Tag >
edge_printer< PA, Tag > print_edge(PA pa, std::ostream& os, Tag)
{
    return edge_printer< PA, Tag >(pa, os);
}

template < class NewGraph, class Tag >
struct graph_copier
: public boost::base_visitor< graph_copier< NewGraph, Tag > >
{
    typedef Tag event_filter;

    graph_copier(NewGraph& graph) : new_g(graph) {}

    template < class Edge, class Graph > void operator()(Edge e, Graph& g)
    {
        add_edge(source(e, g), target(e, g), new_g);
    }

private:
    NewGraph& new_g;
};
template < class NewGraph, class Tag >
inline graph_copier< NewGraph, Tag > copy_graph(NewGraph& g, Tag)
{
    return graph_copier< NewGraph, Tag >(g);
}

template < class Graph, class Name > void print(Graph& G, Name name)
{
    typename boost::graph_traits< Graph >::vertex_iterator ui, uiend;
    for (boost::tie(ui, uiend) = vertices(G); ui != uiend; ++ui)
    {
        cout << name[*ui] << " --> ";
        typename boost::graph_traits< Graph >::adjacency_iterator vi, viend;
        for (boost::tie(vi, viend) = adjacent_vertices(*ui, G); vi != viend;
             ++vi)
            cout << name[*vi] << " ";
        cout << endl;
    }
}

int main(int, char*[])
{
    // Name and ID numbers for the vertices
    char name[] = "abcdefg";
    enum
    {
        a,
        b,
        c,
        d,
        e,
        f,
        g,
        N
    };

    Graph G(N);
    boost::property_map< Graph, vertex_index_t >::type vertex_id
        = get(vertex_index, G);

    std::vector< weight_t > distance(N, (numeric_limits< weight_t >::max)());
    typedef boost::graph_traits< Graph >::vertex_descriptor Vertex;
    std::vector< Vertex > parent(N);

    typedef std::pair< int, int > E;

    E edges[] = { E(a, c), E(a, d), E(b, a), E(b, d), E(c, f), E(d, c), E(d, e),
        E(d, f), E(e, b), E(e, g), E(f, e), E(f, g) };

    int weight[] = { 3, 4, 6, 8, 12, 7, 0, 5, 10, 3, 1, 2 };

    for (int i = 0; i < 12; ++i)
        add_edge(edges[i].first, edges[i].second, weight[i], G);

    print(G, name);

    adjacency_list< listS, vecS, directedS,
        property< vertex_color_t, default_color_type > >
        G_copy(N);

    cout << "Starting graph:" << endl;

    std::ostream_iterator< int > cout_int(std::cout, " ");
    std::ostream_iterator< char > cout_char(std::cout, " ");

    boost::queue< Vertex > Q;
    boost::breadth_first_search(G, vertex(a, G), Q,
        make_bfs_visitor(boost::make_list(
            write_property(make_iterator_property_map(name, vertex_id, name[0]),
                cout_char, on_examine_vertex()),
            write_property(make_iterator_property_map(
                               distance.begin(), vertex_id, distance[0]),
                cout_int, on_examine_vertex()),
            print_edge(make_iterator_property_map(name, vertex_id, name[0]),
                std::cout, on_examine_edge()),
            print_endl(std::cout, on_finish_vertex()))),
        get(vertex_color, G));

    std::cout << "about to call dijkstra's" << std::endl;

    parent[vertex(a, G)] = vertex(a, G);
    boost::dijkstra_shortest_paths(G, vertex(a, G),
        distance_map(make_iterator_property_map(
                         distance.begin(), vertex_id, distance[0]))
            .predecessor_map(make_iterator_property_map(
                parent.begin(), vertex_id, parent[0]))
            .visitor(
                make_dijkstra_visitor(copy_graph(G_copy, on_examine_edge()))));

    cout << endl;
    cout << "Result:" << endl;
    boost::breadth_first_search(G, vertex(a, G),
        visitor(make_bfs_visitor(boost::make_list(
            write_property(make_iterator_property_map(name, vertex_id, name[0]),
                cout_char, on_examine_vertex()),
            write_property(make_iterator_property_map(
                               distance.begin(), vertex_id, distance[0]),
                cout_int, on_examine_vertex()),
            print_edge(make_iterator_property_map(name, vertex_id, name[0]),
                std::cout, on_examine_edge()),
            print_endl(std::cout, on_finish_vertex())))));

    return 0;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, log1p_exp) {
  using stan::math::log1p_exp;

  // exp(10000.0) overflows
  EXPECT_FLOAT_EQ(10000.0, log1p_exp(10000.0));
  EXPECT_FLOAT_EQ(0.0, log1p_exp(-10000.0));
}

TEST(MathFunctions, log1p_exp_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log1p_exp(nan));
}


#include <catch/catch2.hpp>

#include <orient/from_rotation_matrix.hpp>

#include <Eigen/Geometry>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h>

TEST_CASE("angleAxisFromRotationMatrix")
{
  Eigen::Vector3d aa;
  SECTION("Zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("Random"){
    std::srand(1);
    aa = Eigen::Vector3d::Random();
  }
  SECTION("half_PI_angle"){
    std::srand(2);
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / (2*aa.norm());
  }
  SECTION("minus_half_PI_angle"){
    std::srand(3);
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / (2*aa.norm());
  }
  SECTION("PI_angle"){
    std::srand(4);
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / aa.norm();
  }
  SECTION("minus_PI_angle"){
    std::srand(5);
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / aa.norm();
  }
  SECTION("2PI_angle"){
    std::srand(6);
    aa = Eigen::Vector3d::Random();
    aa *= 2*M_PI / aa.norm();
  }
  SECTION("minus_2PI_angle"){
    std::srand(7);
    aa = Eigen::Vector3d::Random();
    aa *= -2*M_PI / aa.norm();
  }
  SECTION("special1"){
    aa << M_PI,0,0;
  }
  SECTION("special2"){
    aa << 0,M_PI,0;
  }
  SECTION("special3"){
    aa << 0,0,M_PI;
  }
  SECTION("special4"){
    const auto a = M_PI/std::sqrt(2);
    aa << a,a,0;
  }
  SECTION("special5"){
    const auto a = M_PI/std::sqrt(2);
    aa << a,0,a;
  }
  SECTION("special6"){
    const auto a = M_PI/std::sqrt(2);
    aa << 0,a,a;
  }

  const Eigen::Matrix3d R = gtsam::Rot3::Rodrigues(aa).matrix();
  const Eigen::Vector3d actual_aa = orient::angleAxisFromRotationMatrix(R);
  const Eigen::Matrix3d actual_R = gtsam::Rot3::Rodrigues(actual_aa).matrix();
  CHECK( actual_R.isApprox(R) );
}

TEST_CASE("quaternionFromRotationMatrix")
{
  Eigen::Vector3d aa;
  SECTION("Zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("Random"){
    std::srand(0);
    aa = Eigen::Vector3d::Random();
  }
  SECTION("half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / (2 * aa.norm());
  }
  SECTION("minus_half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / (2*aa.dot(aa));
  }
  SECTION("PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / aa.norm();
  }
  SECTION("minus_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / aa.norm();
  }
  SECTION("2PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= 2*M_PI / aa.norm();
  }
  SECTION("minus_2PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -2*M_PI / aa.norm();
  }
  SECTION("special1"){
    aa << M_PI,0,0;
  }
  SECTION("special2"){
    aa << 0,M_PI,0;
  }
  SECTION("special3"){
    aa << 0,0,M_PI;
  }
  SECTION("special4"){
    const auto a = M_PI/std::sqrt(2);
    aa << a,a,0;
  }
  SECTION("special5"){
    const auto a = M_PI/std::sqrt(2);
    aa << a,0,a;
  }
  SECTION("special6"){
    const auto a = M_PI/std::sqrt(2);
    aa << 0,a,a;
  }

  Eigen::Matrix3d R = gtsam::Rot3::Rodrigues(aa).matrix();
  Eigen::Quaterniond equat{R};
  Eigen::Vector4d expected = (Eigen::Vector4d() << equat.w(), equat.vec()).finished();
  const Eigen::Vector4d actual = orient::quaternionFromRotationMatrix(R);
  CHECK( actual.isApprox( expected ) );
}

TEST_CASE("angleAxisFromRotationMatrix_derivative")
{
  std::srand(7);
  Eigen::Vector3d aa;
  SECTION("Random"){
    std::srand(0);
    aa = Eigen::Vector3d::Random();
  }
  SECTION("half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / (2 * aa.norm());
  }
  SECTION("minus_half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / (2*aa.dot(aa));
  }
  Eigen::Matrix3d R = gtsam::Rot3::Rodrigues(aa).matrix();

  const auto [v, J] = orient::angleAxisFromRotationMatrixWD(R);
  auto num = gtsam::numericalDerivative11(orient::angleAxisFromRotationMatrix<double>, R);
  CHECK( v.isApprox(orient::angleAxisFromRotationMatrix(R)) );
  CHECK( J.isApprox(num, 1e-8) );
}

TEST_CASE("quaternionFromRotationMatrix_derivative")
{
  std::srand(8);
  Eigen::Vector3d aa;
  SECTION("Zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("Random"){
    std::srand(0);
    aa = Eigen::Vector3d::Random();
  }
  SECTION("half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / (2 * aa.norm());
  }
  SECTION("minus_half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / (2*aa.dot(aa));
  }

  Eigen::Matrix3d R = gtsam::Rot3::Rodrigues(aa).matrix();

  const auto [v, J] = orient::quaternionFromRotationMatrixWD(R);
  auto num = gtsam::numericalDerivative11(orient::quaternionFromRotationMatrix<double>, R);
  CHECK( v.isApprox(orient::quaternionFromRotationMatrix(R)) );
  CHECK( J.isApprox(num, 1e-10) );
}


#ifndef STAN_MATH_FWD_MAT_FUN_TRACE_QUAD_FORM_HPP
#define STAN_MATH_FWD_MAT_FUN_TRACE_QUAD_FORM_HPP

#include <boost/type_traits.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/fwd/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/fun/trace.hpp>
#include <stan/math/fwd/core.hpp>

namespace stan {
  namespace math {

    template<int RA, int CA, int RB, int CB, typename T>
    inline fvar<T>
    trace_quad_form(const Eigen::Matrix<fvar<T>, RA, CA> &A,
                    const Eigen::Matrix<fvar<T>, RB, CB> &B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);
      return trace(multiply(transpose(B),
                            multiply(A, B)));
    }

    template<int RA, int CA, int RB, int CB, typename T>
    inline fvar<T>
    trace_quad_form(const Eigen::Matrix<fvar<T>, RA, CA> &A,
                    const Eigen::Matrix<double, RB, CB> &B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);
      return trace(multiply(transpose(B),
                            multiply(A, B)));
    }

    template<int RA, int CA, int RB, int CB, typename T>
    inline fvar<T>
    trace_quad_form(const Eigen::Matrix<double, RA, CA> &A,
                    const Eigen::Matrix<fvar<T>, RB, CB> &B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);
      return trace(multiply(transpose(B),
                            multiply(A, B)));
    }
  }
}

#endif


/* boost random/cauchy_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: cauchy_distribution.hpp,v 1.1 2007/02/12 18:25:54 irving Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP
#define BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP

#include <cmath>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::tan;
#endif

// Cauchy distribution: p(x) = sigma/(pi*(sigma**2 + (x-median)**2))
template<class RealType = double>
class cauchy_distribution
{
public:
  typedef RealType input_type;
  typedef RealType result_type;

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit cauchy_distribution(result_type median = result_type(0),
                               result_type sigma = result_type(1))
    : _median(median), _sigma(sigma) { }

  // compiler-generated copy ctor and assignment operator are fine

  result_type median() const { return _median; }
  result_type sigma() const { return _sigma; }
  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
    // Can we have a boost::mathconst please?
    const result_type pi = result_type(3.14159265358979323846);
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::tan;
#endif
    return _median + _sigma * tan(pi*(eng()-result_type(0.5)));
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const cauchy_distribution& cd)
  {
    os << cd._median << " " << cd._sigma;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, cauchy_distribution& cd)
  {
    is >> std::ws >> cd._median >> std::ws >> cd._sigma;
    return is;
  }
#endif

private:
  result_type _median, _sigma;
};

} // namespace boost

#endif // BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  VectorXd v(10);
v.resize(3);
RowVector3d w;
w.resize(3); // this is legal, but has no effect
cout << "v: " << v.rows() << " rows, " << v.cols() << " cols" << endl;
cout << "w: " << w.rows() << " rows, " << w.cols() << " cols" << endl;

  return 0;
}


#include <stdio.h>
#include <iostream>
#include <iomanip>

#include <Eigen/Core>

#include "../libfovis/initial_homography_estimation.hpp"
#include <bot_core/bot_core.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <GL/gl.h>

using namespace std;

#define dump(var) (cerr<<" "#var<<" =[\n"<< setprecision (3)<<var<<"];"<<endl)

static Eigen::ArrayXf flattenMatrix(Eigen::MatrixXf &m)
{
  return Eigen::Map<Eigen::ArrayXf>(m.data(), m.rows() * m.cols());
}

static void warpImage(const uint8_t * image, int width, int height, int rowstride, const Eigen::Matrix3f &H,
    uint8_t * warped_image)
{
  //setup the utility matrices
  Eigen::MatrixXf x = Eigen::VectorXf::LinSpaced(width, 0, width - 1).transpose().replicate(height, 1);
  Eigen::MatrixXf y = Eigen::VectorXf::LinSpaced(height, 0, height - 1).replicate(1, width);
  Eigen::MatrixXf xx = flattenMatrix(x);
  Eigen::MatrixXf yy = flattenMatrix(y);

  Eigen::MatrixXf imageHomogeneousPoints(3, xx.rows());
  imageHomogeneousPoints.row(0) = xx.transpose();
  imageHomogeneousPoints.row(1) = yy.transpose();
  imageHomogeneousPoints.row(2).setOnes();

  Eigen::MatrixXf warpedHomogeneousPoints;
  warpedHomogeneousPoints = H * imageHomogeneousPoints;

  Eigen::MatrixXf warped = Eigen::MatrixXf(height, width);

  const double defaultValue = 128;
  //Bilinear interpolation
  for (int i = 0; i < warpedHomogeneousPoints.cols(); i++) {
    double val;
    Eigen::Vector2f pt = warpedHomogeneousPoints.col(i).head(2) / warpedHomogeneousPoints(2, i);
    Eigen::Vector2i fipt(floor(pt(0)), floor(pt(1)));
    Eigen::Vector2i cipt(ceil(pt(0)), ceil(pt(1)));
    if (0 <= pt(0) && pt(0) < width - 1 && 0 <= pt(1) && pt(1) < height - 1) {
      double x1 = pt(0) - fipt(0);
      double y1 = pt(1) - fipt(1);
      double x2 = 1 - x1;
      double y2 = 1 - y1;
      val = x2 * y2 * image[fipt(1) * rowstride + fipt(0)] + x1 * y2 * image[fipt(1) * rowstride + fipt(0) + 1] + x2
          * y1 * image[(fipt(1) + 1) * rowstride + fipt(0)] + x1 * y1 * image[(fipt(1) + 1) * rowstride + fipt(0) + 1];

    }
    else if (0 <= fipt(0) && fipt(0) < width && 0 <= fipt(1) && fipt(1) < height) {
      val = image[fipt(1) * rowstride + fipt(0)];
    }
    else if (0 <= cipt(0) && cipt(0) < width && 0 <= cipt(1) && cipt(1) < height) {
      val = image[cipt(1) * rowstride + cipt(0)];
    }
    else
      val = defaultValue; //templateImage(i / width, i % width); //default to the same as template, so error is 0

    warped_image[(i % height) * rowstride + (i / height)] = val; //Eigen is Column-major
  }
}

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

  uint8_t* src_im = (uint8_t*) malloc(640 * 480 * sizeof(uint8_t));

  int width, height, rowstride;
  bot_pgm_read_fname("test_im.pgm", &src_im, &width, &height, &rowstride);

  Eigen::MatrixXf ref_image = Eigen::MatrixXf::Zero(60, 80);
  ref_image.block(15, 25, 30, 30).setConstant(255.0);

  Eigen::Matrix3f H_warp;
  H_warp.setZero();

  double u = 2;
  double v = 1;
  double theta = 1 * M_PI / 180.0;
  int downsample = 0;
  if (argc >= 2)
    u = atof(argv[1]);

  if (argc >= 3)
    v = atof(argv[2]);

  if (argc >= 4)
    theta = atof(argv[3]) * M_PI / 180.0;

  if (argc >= 5)
    downsample = atoi(argv[4]);

  H_warp(0, 0) = cos(theta);
  H_warp(0, 1) = sin(theta);
  H_warp(1, 0) = -sin(theta);
  H_warp(1, 1) = cos(theta);

  H_warp(0, 2) = u;
  H_warp(1, 2) = v;
  H_warp(2, 2) = 1;

  uint8_t* warped_im = (uint8_t*) malloc(width * height * sizeof(uint8_t));
  warpImage(src_im, width, height, rowstride, H_warp, warped_im);

  fovis::InitialHomographyEstimator rotation_estimator;
  rotation_estimator.setTemplateImage(warped_im, width, height, rowstride, downsample);

  rotation_estimator.setTestImage(src_im, width, height, rowstride, downsample);
  double finalRMS = 0;
  Eigen::Matrix3f H_est = rotation_estimator.track(Eigen::Matrix3f::Identity(), 10, &finalRMS);

  double scale_factor = 1 << downsample;
  Eigen::Matrix3f S = Eigen::Matrix3f::Identity() * scale_factor;
  S(2,2)=1;

  //scale homography up to the full size image
  H_est = S * H_est * S.inverse();

  dump(H_warp);
  dump(H_est);
  fprintf(stderr, "finalRMS=%f\n", finalRMS);

  //draw results
  lcm_t * lcm = lcm_create(NULL);
  bot_lcmgl_t* lcmgl = bot_lcmgl_init(lcm, "Init_Homography_Test\n");

  //convert to pixel coords
  bot_lcmgl_push_matrix(lcmgl);
  bot_lcmgl_rotated(lcmgl, -90, 0, 0, 1);
  bot_lcmgl_scalef(lcmgl, 10.0 / width, -10.0 / width, 1);

  // template image
  bot_lcmgl_color3f(lcmgl, 1, 1, 1);
  int template_texid = bot_lcmgl_texture2d(lcmgl, warped_im, width, height, rowstride, BOT_LCMGL_LUMINANCE,
      BOT_LCMGL_COMPRESS_NONE);

  bot_lcmgl_push_matrix(lcmgl);
  bot_lcmgl_translated(lcmgl, 0, height + 10, 0);
  bot_lcmgl_texture_draw_quad(lcmgl, template_texid, 0, 0, 0, 0, height, 0, width, height, 0, width, 0, 0);
  bot_lcmgl_pop_matrix(lcmgl);

  // test image
  bot_lcmgl_color3f(lcmgl, 1, 1, 1);
  int gray_texid = bot_lcmgl_texture2d(lcmgl, src_im, width, height, rowstride, BOT_LCMGL_LUMINANCE,
      BOT_LCMGL_COMPRESS_NONE);
  bot_lcmgl_texture_draw_quad(lcmgl, gray_texid, 0, 0, 0, 0, height, 0, width, height, 0, width, 0, 0);

  //draw the ESM homography estimate
  bot_lcmgl_line_width(lcmgl, 2.0);
  bot_lcmgl_color3f(lcmgl, 1, 1, 0);
  bot_lcmgl_begin(lcmgl, GL_LINE_STRIP);
  Eigen::MatrixXf vertices(5, 3);
  vertices << 0, 0, 1, width, 0, 1, width, height, 1, 0, height, 1, 0, 0, 1;

  Eigen::MatrixXf warpedPoints = H_est * vertices.transpose();
  warpedPoints.row(0) = warpedPoints.row(0).array() / warpedPoints.row(2).array();
  warpedPoints.row(1) = warpedPoints.row(1).array() / warpedPoints.row(2).array();
  for (int i = 0; i < warpedPoints.cols(); i++) {
    bot_lcmgl_vertex2f(lcmgl, warpedPoints(0, i), warpedPoints(1, i));
  }
  bot_lcmgl_end(lcmgl);

  bot_lcmgl_pop_matrix(lcmgl);

  bot_lcmgl_switch_buffer(lcmgl);

  return 0;
}


//
// Created by Esha Uboweja on 12/4/16.
//

#include <armadillo>
#include "BinaryLayer.h"
#include "TestBinaryLayer.h"

//#define DEBUG 0

using namespace bd;

bool TestBinaryLayer::test_binarizeMat_single(uint rows, uint cols) {
    arma::mat values = arma::randn(rows, cols);

    BinaryLayer bl = BinaryLayer(cols, rows);

#ifdef DEBUG
    std::cout << "[test_binarizeMat_single] arma input:\n" << values << std::endl;
#endif

    // Compare Armadillo result and BinaryMatrix result
    double armaAlpha = arma::mean(arma::mean(arma::abs(values)));
    arma::umat armaResult(rows, cols);
    armaResult.zeros();
    armaResult.elem(arma::find(values >= 0)).ones();
    armaResult.elem(arma::find(values < 0)).zeros();
    bl.binarizeMat(values);

#ifdef DEBUG
    std::cout << "[test_binarizeMat_single] Arma alpha: " << armaAlpha << " Arma output:\n" << armaResult << std::endl;
    std::cout << "[test_binarizeMat_single] Binarized alpha: " << bl.alpha() << " output (bl):\n";
    bl.binMtx()->print();
    std::cout << std::endl;
#endif

    return (bl.alpha() == armaAlpha) && bl.binMtx()->equalsArmaMat(armaResult);
}

bool TestBinaryLayer::test_binarizeMat() {
    return test_binarizeMat_single()
        && test_binarizeMat_single(5, 3)
        && test_binarizeMat_single(28, 9)
        && test_binarizeMat_single(81, 102);
}

bool TestBinaryLayer::test_operatorMult_single(uint rows1, uint cols1, uint rows2, uint cols2) {
     // Generate 2 random binary matrices
    arma::umat input2D_a = BinaryMatrix::randomArmaUMat(rows1, cols1);
    BinaryLayer bl_a(input2D_a);
    arma::umat input2D_b = BinaryMatrix::randomArmaUMat(rows2, cols2);
    BinaryLayer bl_b(input2D_b);

#ifdef DEBUG
    std::cout << "[test_operatorMult_single] arma input 1: \n" << input2D_a << std::endl;
    std::cout << "[test_operatorMult_single] binary matrix input 1: \n";
    bl_a.binMtx()->print();
    std::cout << std::endl;
    std::cout << "[test_operatorMult_single] arma input 2: \n" << input2D_b << std::endl;
    std::cout << "[test_operatorMult_single] binary matrix input 2: \n";
    bl_b.binMtx()->print();
    std::cout << std::endl;
#endif

    // XNOR product
    BinaryLayer result = bl_a * bl_b;

    // ARMA product
    arma::imat ia = arma::conv_to<imat>::from(input2D_a);
    ia.replace(0, -1);
    arma::imat ib = arma::conv_to<imat>::from(input2D_b);
    ib.replace(0, -1);
    arma::imat ires = ia % ib;
    ires.replace(-1, 0);
    arma::umat armaResult = arma::conv_to<umat>::from(ires);

#ifdef DEBUG
    ires.replace(0, -1);
    std::cout << "[test_operatorMult_single] Arma integer result: \n" << ires << std::endl;
    std::cout << "[test_operatorMult_single] Arma result: \n" << armaResult << std::endl;
    std::cout << "[test_operatorMult_single] Binary layer result: \n";
    result.binMtx()->print();
    std::cout << std::endl;
#endif

    return result.binMtx()->equalsArmaMat(armaResult);
}

bool TestBinaryLayer::test_operatorMult_invalid(uint rows1, uint cols1, uint rows2, uint cols2) {
    try {
        test_operatorMult_single(rows1, cols1, rows2, cols2);
    } catch (std::exception e) {
        return true;
    }
    // didn't raise exception
    return false;
}

bool TestBinaryLayer::test_operatorMult() {
    return test_operatorMult_single()
        && test_operatorMult_single(5, 6, 5, 6)
        && test_operatorMult_invalid(5, 6, 7, 8);
}

bool TestBinaryLayer::test_im2col_single(uint rows, uint cols, uint block_width, uint block_height, uint padding,
                                         uint stride) {
    // Generate a random binary matrix
    arma::umat input2D = BinaryMatrix::randomArmaUMat(rows, cols);
    BinaryLayer bl(input2D);

#ifdef DEBUG
    std::cout << "[test_im2col_single] arma input:\n" << input2D << std::endl;
    std::cout << "[test_im2col_single] bm input:\n" ;
    bl.binMtx()->print();
    std::cout << std::endl;
#endif

    // Compare im2col result for binary matrix and arma
    arma::umat armaResult = BinaryMatrix::im2colArmaMat(input2D, block_width, block_height, padding, stride);
    BinaryLayer blResult = bl.im2col(block_width, block_height, padding, stride);

#ifdef DEBUG
    std::cout << "[test_im2col_single] arma result:\n" << armaResult << std::endl;
    std::cout << "[test_im2col_single] bm result:\n" ;
    blResult.binMtx()->print();
    std::cout << std::endl;
#endif

    return blResult.binMtx()->equalsArmaMat(armaResult);
}

bool TestBinaryLayer::test_im2col_invalid(uint rows, uint cols, uint block_width, uint block_height, uint padding, uint stride) {
    try {
        test_im2col_single(rows, cols, block_width, block_height, padding, stride);
    } catch (std::exception e) {
        return true;
    }
    std::cerr << "[test_im2col_invalid] Test didn't raise exception\n";
    // This test should raise an exception
    return false;
}

bool TestBinaryLayer::test_im2col() {
    return test_im2col_single()
        && test_im2col_single(3, 3)
        && test_im2col_single(3, 3, 3, 3, 1, 1)
        && test_im2col_single(3, 3, 3, 3, 1, 2)
        && test_im2col_single(5, 5, 3, 3, 1, 2)
        && test_im2col_single(7, 9, 5, 5, 2, 1)
        && test_im2col_invalid(8, 6, 3, 3, 1, 2)
        && test_im2col_invalid(7, 9, 5, 5, 3, 1)
        && test_im2col_invalid(10, 10, 3, 3, 0, 2);
}

void TestBinaryLayer::runAllTests() {
    std::cout << "----Testing BinaryLayer class functions...\n";
    bool result = test_binarizeMat() && test_operatorMult() && test_im2col();
    std::cout << "[TestBinaryLayer] Tests completed! Result = " << (result? "PASSED" : "FAILED") << std::endl;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::Col;
using arma::cross;
using arma::join_rows;
using arma::join_horiz;
using arma::join_cols;
using arma::join_vert;
using arma::kron;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedGenMatGenColVec : public Expected {
    public:
      ExpectedGenMatGenColVec() {
        cout << "Compute ExpectedGenMatGenColVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::GenMat,
          InputClass::GenColVec
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genMat = *static_cast<Mat<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _genColVec = *static_cast<Col<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          expectedArmaCross();
          expectedArmaJoin_rows();
          expectedArmaJoin_horiz();
          expectedArmaJoin_cols();
          expectedArmaJoin_vert();
          expectedArmaKron();
          expectedMatPlus();
          expectedMatMinus();
          expectedMatTimes();
          expectedMatElemTimes();
          expectedMatElemDivide();
          expectedMatEquals();
          expectedMatNonEquals();
          expectedMatGreaterThan();
          expectedMatLessThan();
          expectedMatStrictGreaterThan();
          expectedMatStrictLessThan();
        }

        cout << "done." << endl;
      }

    protected:
      Mat<double> _genMat;
      Col<double> _genColVec;

      void expectedArmaCross() {
        cout << "- Compute expectedArmaCross() ... ";

        Mat<double> tempGenMat = Mat<double>(_genMat);
        tempGenMat.resize(3, 1);
        Col<double> tempGenColVec = Col<double>(_genColVec);
        tempGenColVec.resize(3);

        save<double>("Arma.cross", cross(tempGenMat, tempGenColVec));

        cout << "done." << endl;
      }

      void expectedArmaJoin_rows() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedArmaJoin_rows() ... ";
        save<double>("Arma.join_rows", join_rows(_genMat, _genColVec));
        cout << "done." << endl;
      }

      void expectedArmaJoin_horiz() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedArmaJoin_horiz() ... ";
        save<double>("Arma.join_horiz", join_horiz(_genMat, _genColVec));
        cout << "done." << endl;
      }

      void expectedArmaJoin_cols() {
        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedArmaJoin_cols() ... ";
        save<double>("Arma.join_cols", join_cols(_genMat, _genColVec));
        cout << "done." << endl;
      }

      void expectedArmaJoin_vert() {
        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedArmaJoin_vert() ... ";
        save<double>("Arma.join_vert", join_cols(_genMat, _genColVec));
        cout << "done." << endl;
      }

      void expectedArmaKron() {
        cout << "- Compute expectedArmaKron() ... ";
        save<double>("Arma.kron", kron(_genMat, _genColVec));
        cout << "done." << endl;
      }

      void expectedMatPlus() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatPlus() ... ";
        save<double>("Mat.plus", _genMat + _genColVec);
        cout << "done." << endl;
      }

      void expectedMatMinus() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatMinus() ... ";
        save<double>("Mat.minus", _genMat - _genColVec);
        cout << "done." << endl;
      }

      void expectedMatTimes() {
        if(_genMat.n_cols != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedMatTimes() ... ";
        save<double>("Mat.times", _genMat * _genColVec);
        cout << "done." << endl;
      }

      void expectedMatElemTimes() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatElemTimes() ... ";
        save<double>("Mat.elemTimes", _genMat % _genColVec);
        cout << "done." << endl;
      }

      void expectedMatElemDivide() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatElemDivide() ... ";
        save<double>("Mat.elemDivide", _genMat / _genColVec);
        cout << "done." << endl;
      }

      void expectedMatEquals() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatEquals() ... ";

        Mat<uword> expected = _genMat == _genColVec;
        save<uword>("Mat.equals", expected);

        cout << "done." << endl;
      }

      void expectedMatNonEquals() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatNonEquals() ... ";

        Mat<uword> expected = _genMat != _genColVec;
        save<uword>("Mat.nonEquals", expected);

        cout << "done." << endl;
      }

      void expectedMatGreaterThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatGreaterThan() ... ";

        Mat<uword> expected = _genMat >= _genColVec;
        save<uword>("Mat.greaterThan", expected);

        cout << "done." << endl;
      }

      void expectedMatLessThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatLessThan() ... ";

        Mat<uword> expected = _genMat <= _genColVec;
        save<uword>("Mat.lessThan", expected);

        cout << "done." << endl;
      }

      void expectedMatStrictGreaterThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatStrictGreaterThan() ... ";

        Mat<uword> expected = _genMat > _genColVec;
        save<uword>("Mat.strictGreaterThan", expected);

        cout << "done." << endl;
      }

      void expectedMatStrictLessThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedMatElemDivide() ... ";

        Mat<uword> expected = _genMat < _genColVec;
        save<uword>("Mat.strictLessThan", expected);

        cout << "done." << endl;
      }

  };
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief heterogeneous_unit.cpp

\details
Test heterogeneous units and quantities.

Output:
@verbatim

//[heterogeneous_unit_output_1
1.5 m
1 g
1.5 m g
1.5 m g^-1

1 N
1 kg s^-2

1 cm kg s^-2
1 cm m^-1 kg s^-2
//]

//[heterogeneous_unit_output_2
1.5 cm m
0.015 m^2
//]

@endverbatim
**/

#define MCS_USE_DEMANGLING
//#define MCS_USE_BOOST_REGEX_DEMANGLING

#include <iostream>

#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/detail/utility.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>

using namespace boost::units;

int main()
{
    //[heterogeneous_unit_snippet_1
    quantity<si::length>        L(1.5*si::meter);
    quantity<cgs::mass>         M(1.0*cgs::gram);

    std::cout << L << std::endl
              << M << std::endl
              << L*M << std::endl
              << L/M << std::endl
              << std::endl;

    std::cout << 1.0*si::meter*si::kilogram/pow<2>(si::second) << std::endl
              << 1.0*si::meter*si::kilogram/pow<2>(si::second)/si::meter
              << std::endl << std::endl;

    std::cout << 1.0*cgs::centimeter*si::kilogram/pow<2>(si::second) << std::endl
              << 1.0*cgs::centimeter*si::kilogram/pow<2>(si::second)/si::meter
              << std::endl << std::endl;
    //]

    //[heterogeneous_unit_snippet_2
    quantity<si::area>      A(1.5*si::meter*cgs::centimeter);

    std::cout << 1.5*si::meter*cgs::centimeter << std::endl
              << A << std::endl
              << std::endl;
    //]

    return 0;
}


//   Copyright 2018 Dhruvesh Nikhilkumar Patel
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
//
#include <boost/numeric/ublas/matrix.hpp>
#include "../include/Polynomial.ipp"
#include <gmpxx.h>
#include <iostream>
int main()
{
   using namespace boost::numeric::ublas;
   using coefT = mpz_class;
   using entryT= Polynomial<coefT>;
   entryT p1 {1,2,3};
   entryT p2 {4,5,6};
   entryT p3 {1};
   entryT p4 {7,8,9,10};
   matrix<entryT> m (2,2);
   m(0,0)=p1;
   m(0,1)=p2;
   m(1,0)=p3;
   m(1,1)=p4;
   std::cout<<"m(0,0)= "<<m(0,0)<<std::endl;
   std::cout<<"m(0,1)= "<<m(0,1)<<std::endl;
   std::cout<<"m(1,0)= "<<m(1,0)<<std::endl;
   std::cout<<"m(1,1)= "<<m(1,1)<<std::endl;

   matrix<entryT> m2 (2,2);
   m2 = prod(m,m);
   std::cout<<"m2=m*m"<<std::endl;
   std::cout<<"m2(0,0)= "<<m2(0,0)<<std::endl;
   std::cout<<"m2(0,1)= "<<m2(0,1)<<std::endl;
   std::cout<<"m2(1,0)= "<<m2(1,0)<<std::endl;
   std::cout<<"m2(1,1)= "<<m2(1,1)<<std::endl;

   return 0;
}