#include "localization_graph.h"
#include "debug_functions.h"
#include "features.h"
#include <Eigen/src/Core/Matrix.h>

#ifdef USE_OPEN3D
    #include <Open3D/Geometry/Geometry.h>
    #include <Open3D/Geometry/PointCloud.h>
    #include <Open3D/Geometry/TriangleMesh.h>
    #include <Open3D/Visualization/Utility/DrawGeometry.h>
    #include <Open3D/IO/ClassIO/TriangleMeshIO.h>
    #include <Open3D/IO/ClassIO/PointCloudIO.h>
#endif

#include <algorithm>
#include <cstdint>
// #include <fmt/format.h>
#include <iostream>
#include <memory>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/base.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/matx.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <stdexcept>
#include <utility>

using namespace k3d;

LGraph::LGraph()
#ifdef TRANSMIT_POSES
    : stream_handle(networking::acquire_stream_handle(STREAM_IP, STREAM_PORT))
#endif
{
    // double identity_vec[3*3] = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
    // identity_mat = utilities::restrucure_mat<double>(identity_vec, 3, 3);
}

bool LGraph::localize_frame(std::shared_ptr<Frame> frame)
{
    current_frame_points.clear();
    current_frame_point_colors.clear();

	frames.push_back(frame);

    bool status = false;

    // cannot localize if only 1 frame
    if (frames.size() == 1)
        status = true;

    // localize using essential matrix decomposition
    else if (frames.size() == 2)
        status = localize_frame_essential(frames[0], frame);

    // localize using PnP
    else
        status = localize_frame_pnp(frames[frames.size() - 2], frame);

    // stream points and frame to the remote device
    if (status)
    {
        #ifdef TRANSMIT_POSES
        networking::stream_points_camera(current_frame_points, current_frame_point_colors, frames.back(), stream_handle);
        #endif
    }

    return status;
}

bool LGraph::localize_frame_essential(const std::shared_ptr<Frame> ref_frame, std::shared_ptr<Frame> frame)
{
    std::vector<std::pair<uint32_t, uint32_t>> matches =
#if defined USE_FLANN_ESSENTIAL
        features::match_features_flann(ref_frame->descriptors, frame->descriptors);
#else
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);
#endif

    matches = features::radius_distance_filter_matches(matches, ref_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);

    if (matches.size() < MIN_MATCH_FEATURE_COUNT)
        throw std::runtime_error("not enough feature matches: " + std::to_string(matches.size()));

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);

    std::vector<cv::Point2f> x1, x2;
    x1.reserve(matches.size());
    x2.reserve(matches.size());

    // populate the 2d feature position vectors
    for (const auto& m : matches)
    {
        x1.push_back(ref_frame->keypoints[m.first].pt);
        x2.push_back(frame->keypoints[m.second].pt);
    }
    cv::undistortPoints(x1, x1, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);
    cv::undistortPoints(x2, x2, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

    cv::Mat mask;
    const cv::Mat essential = cv::findEssentialMat(x1, x2, frame->params.intr, cv::RANSAC, 0.999, 0.11, mask);

    std::cout << essential << "\n";

    cv::Mat local_R, local_t;
    cv::recoverPose(essential, x1, x2, frame->params.intr, local_R, local_t, mask);

    std::cout << local_R << "\n\n" << local_t << "\n";

    Eigen::Matrix3d dR;
    Eigen::Vector3d dt;
    cv::cv2eigen(local_R, dR);
    cv::cv2eigen(local_t, dt);

    frame->position = dt;
    frame->rotation = dR;

    const auto T_P = utilities::TP_from_Rt(dR.transpose(), -dR.transpose() * dt, frame->params.intr);
    frame->transformation = T_P.first;
    frame->projection = T_P.second;
    cv::eigen2cv(T_P.second, frame->projection_cv);

    #ifdef LOG
    const  Eigen::AngleAxisd ax(dR);
    std::cout << "essential angle: " << ax.angle() * (180. / 3.1415) << "\n";
    #endif

    create_landmarks_from_matches(ref_frame, frame, matches);

    // visualize_camera_tracks(true);

    return true;
}

void LGraph::create_landmarks_from_matches(const std::shared_ptr<Frame> ref_frame,
        const std::shared_ptr<Frame> frame, const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    for (auto& m : matches)
    {
        Landmark lm;

        lm.descriptors.push_back(features::get_individual_descriptor(ref_frame->descriptors, m.first));
        lm.descriptors.push_back(features::get_individual_descriptor(frame->descriptors, m.second));

        std::vector<cv::Point2f> x1 = { ref_frame->keypoints[m.first].pt };
        std::vector<cv::Point2f> x2 = { frame->keypoints[m.second].pt };

        cv::undistortPoints(x1, x1, frame->params.intr, ref_frame->params.distortion, cv::noArray(), ref_frame->params.intr);
        cv::undistortPoints(x2, x2, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

        lm.feature_2d_points.push_back(x1[0]);
        lm.feature_2d_points.push_back(x2[0]);

        lm.triangulate_2d_points.push_back(x1[0]);
        lm.triangulate_2d_points.push_back(x2[0]);

        lm.view_frames.push_back(ref_frame);
        lm.view_frames.push_back(frame);

        lm.triangulate_frames.push_back(ref_frame);
        lm.triangulate_frames.push_back(frame);

// use multiview triangulation. Considerably slower.
#if false
        const cv::Point3d new_p3d = features::triangulate_multiview_eigen(lm.triangulate_2d_points, [&lm] {
            std::vector<Mat34> pmats;
            for (const auto& f : lm.triangulate_frames)
                pmats.push_back(f->projection);
            return pmats;
        }());
// use 2view DLT. Fast.
#else
        cv::Mat p4d;
        cv::triangulatePoints(ref_frame->projection_cv, frame->projection_cv, x1, x2, p4d);
        const cv::Point3f new_p3d = cv::Point3f(
                    p4d.at<float>(0, 0) / p4d.at<float>(3, 0),
                    p4d.at<float>(1, 0) / p4d.at<float>(3, 0),
                    p4d.at<float>(2, 0) / p4d.at<float>(3, 0));
#endif

        // if the triangulated point is behind the camera or too far away -> ignore
        if (!utilities::point_in_front(frame->projection, Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z)) ||
            Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z).norm() > TRIANGULATE_DISTANCE_OUTLIER)
            continue;

        lm.location = new_p3d;
        const cv::Vec3b col = frame->rgb->at<cv::Vec3b>(x2[0].y, x2[0].x);
        lm.color = Eigen::Vector3d((float)(col.val[2]) / 255.0, (float)(col.val[1]) / 255.0, (float)(col.val[0]) / 255.0);

        lm.normal = frame->position - Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z);

        landmarks.push_back(lm);

        // <feature index, landmark index>
        frame->feature_landmark_lookup.push_back(std::make_pair(m.second, landmarks.size() - 1));

        // these will be sent over the network to the remote device
        current_frame_points.push_back(Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z));
        current_frame_point_colors.push_back(lm.color);
    }
}

void LGraph::update_landmarks(const std::shared_ptr<Frame> frame,
        std::vector<uint32_t>& feature_ids, std::vector<uint32_t>& landmark_ids)
{
    for (int ii = 0; ii < feature_ids.size(); ii++)
    {
        Landmark& lm = landmarks[landmark_ids[ii]];

        // const std::vector<cv::Point2f> x1 = { lm.first_feature_point };
        // const std::vector<cv::Point2f> x2 = { frame->keypoints[feature_ids[ii]].pt };

        // cv::Mat p4d;
        // cv::triangulatePoints(lm.first_frame->projection_cv, frame->projection_cv, x1, x2, p4d);
        // const cv::Point3f p3d = cv::Point3f(
        //             p4d.at<float>(0, 0) / p4d.at<float>(3, 0),
        //             p4d.at<float>(1, 0) / p4d.at<float>(3, 0),
        //             p4d.at<float>(2, 0) / p4d.at<float>(3, 0));

        // // if the triangulated point is behind the camera -> ignore
        // if (utilities::point_infront_of_camera(frame->projection, Eigen::Vector3d(p3d.x, p3d.y, p3d.z)) &&
        //     Eigen::Vector3d(p3d.x, p3d.y, p3d.z).norm() < TRIANGULATE_DISTANCE_OUTLIER)
        // {
        //     lm.location += p3d;
        //     lm.location /= 2.0;
        // }

        frame->feature_landmark_lookup.push_back(std::make_pair(feature_ids[ii], landmark_ids[ii]));
        lm.descriptors.push_back(features::get_individual_descriptor(frame->descriptors, feature_ids[ii]));
        lm.feature_2d_points.push_back(frame->keypoints[feature_ids[ii]].pt);
        lm.view_frames.push_back(frame);

        // for performance's sake skip the incremential 3d improvement
        continue;

        const Eigen::Vector3d test_p3d (lm.location.x, lm.location.y, lm.location.z);

        // find a triangulatable frame
        for (int jj = lm.triangulate_frames.size() - 1; jj >= 0; jj--)
        {
            const double frame_angle = utilities::calculate_triangulation_angle(lm.triangulate_frames[jj]->position, frame->position, test_p3d);

            // std::cout << frame_angle << ", " << RAD2DEG(frame_angle) << "\n";

            if (RAD2DEG(frame_angle) < MIN_TRIANGULATION_ANGLE)
                continue;

            lm.triangulate_2d_points.push_back(frame->keypoints[feature_ids[ii]].pt);

            const cv::Point3d new_p3d = features::triangulate_multiview_eigen(lm.feature_2d_points, [&lm, &frame] {
                std::vector<Mat34> pmats;
                for (const auto& f : lm.view_frames)
                    pmats.push_back(f->projection);
                pmats.push_back(frame->projection);
                return pmats;
            }());

            if (std::isnan(new_p3d.x) || std::isnan(new_p3d.y) || std::isnan(new_p3d.z) ||
                std::isinf(new_p3d.x) || std::isinf(new_p3d.y) || std::isinf(new_p3d.z))
            {
                lm.triangulate_2d_points.pop_back();
                continue;
            }

            // if the triangulated point is behind the camera or too far away -> ignore
            if (!utilities::point_in_front(frame->projection, Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z)) ||
                Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z).norm() > TRIANGULATE_DISTANCE_OUTLIER)
            {
                lm.triangulate_2d_points.pop_back();
                continue;
            }

            lm.location = new_p3d;
            lm.triangulate_frames.push_back(frame);

            std::cout << ii << " updated, " << frame_angle << ", " << RAD2DEG(frame_angle) << "\n";

            break;
        }
    }
}

bool LGraph::localize_frame_pnp(const std::shared_ptr<Frame> prev_frame, std::shared_ptr<Frame> frame)
{
    std::vector<std::pair<uint32_t, uint32_t>> matches =
#if defined USE_FLANN
        features::match_features_flann(prev_frame->descriptors, frame->descriptors);
#else
        features::match_features_bf_crosscheck(prev_frame->descriptors, frame->descriptors);
#endif

    // matches = features::radius_distance_filter_matches(matches, prev_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);
    matches = homography_filter_matches(prev_frame, frame, matches);

    // DEBUG_visualize_matches(*prev_frame->rgb, *frame->rgb, matches, prev_frame->keypoints, frame->keypoints);

    // find landmark points for PnP
    std::vector<cv::Point3f> lm_points;
    std::vector<uint32_t> lm_ids;
    std::vector<cv::Point2f> feature_points;
    std::vector<uint32_t> feature_ids;

    for (const auto& lfc : find_landmark_feature_matches(prev_frame, matches))
    {
        feature_points.push_back(frame->keypoints[lfc.first].pt);
        feature_ids.push_back(lfc.first);

        lm_points.push_back(landmarks[lfc.second].location);
        lm_ids.push_back(lfc.second);
    }

    if (feature_points.size() < MIN_MATCH_FEATURE_COUNT)
    {
        std::cout << "could not localize frame, not enough matches: " << feature_points.size() << "\n";
        this->frames.pop_back();
        return false;
    }

    cv::undistortPoints(feature_points, feature_points, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

    cv::Mat rcv, tcv, rcv_mat;
    cv::eigen2cv(prev_frame->position, tcv);
    cv::eigen2cv(prev_frame->rotation, rcv);

    std::vector<int> inliers;
    cv::solvePnPRansac(lm_points, feature_points, frame->params.intr, frame->params.distortion, rcv, tcv, true, 1000, 8.0, 0.988, inliers);
    cv::Rodrigues(rcv, rcv_mat);

    Eigen::Matrix3d dR;
    Eigen::Vector3d dt;
    cv::cv2eigen(rcv_mat, dR);
    cv::cv2eigen(tcv, dt);

    frame->rotation = dR;
    frame->position = dt;

    const auto T_P = utilities::TP_from_Rt(dR.transpose(), -dR.transpose() * dt, frame->params.intr);
    frame->transformation = T_P.first;
    frame->projection = T_P.second;
    cv::eigen2cv(T_P.second, frame->projection_cv);

    // reject the frame if it has too high location magnitude
    if (frame->position.norm() - prev_frame->position.norm() > FRAME_POSITION_DISTANCE_DEVIATION)
    {
        std::cout << "frame rejected for having an outlier position of " << frame->position.transpose() <<
            " compared to the previous frame pose of " << prev_frame->position.transpose() << "\n";

        frames.pop_back();
        return false;
    }

    // update matched landmarks using lookup
    update_landmarks(frame, feature_ids, lm_ids);

    if (feature_points.size() < MIN_MATCH_TRIANGULATE_NEW_COUNT)
    {
        // std::cout << "low number of active landmarks reached, creating new ones\n";
        new_landmarks_standalone(frame, lm_points);
    }

    if (frames.size() % DENSE_POINTS_EVERY_NTH_FRAME == 0)
    {
        std::cout << "project more points\n";
        project_more_points(find_triangulatable_point_frame(frame, lm_points, MIN_TRIANGULATION_ANGLE_LIBERAL), frame);
    }

    return true;
}

void LGraph::project_more_points(const std::shared_ptr<Frame> ref_frame, const std::shared_ptr<Frame> frame)
{
    // compute dense features
    const auto [ref_kps, ref_desc] = features::detect_features_orb(ref_frame->rgb, true);
    const auto [frame_kps, frame_desc] = features::detect_features_orb(frame->rgb, true);

    std::vector<std::pair<uint32_t, uint32_t>> matches =
        features::match_features_flann(ref_desc, frame_desc, KNN_DISTANCE_RATIO_LIBERAL);

    matches = features::radius_distance_filter_matches(matches, ref_kps, frame_kps, FEATURE_DIST_MAX_RADIUS);
    // matches = homography_filter_matches(ref_frame, frame, matches);

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);

    std::vector<cv::Point2f> x1, x2;

    for (const auto& m : matches)
    {
        x1.push_back(ref_kps[m.first].pt);
        x2.push_back(frame_kps[m.second].pt);
    }

    cv::undistortPoints(x1, x1, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);
    cv::undistortPoints(x2, x2, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

    cv::Mat p4ds;
    cv::triangulatePoints(ref_frame->projection_cv, frame->projection_cv, x1, x2, p4ds);

    // std::vector<Eigen::Vector3d> debug_points, debug_colors;

    // loop over all triangulated points and convert from homography to cartecian
    for (int ii = 0; ii < p4ds.cols; ii++)
    {
        const Eigen::Vector3d new_p3d (
                    p4ds.at<float>(0, ii) / p4ds.at<float>(3, ii),
                    p4ds.at<float>(1, ii) / p4ds.at<float>(3, ii),
                    p4ds.at<float>(2, ii) / p4ds.at<float>(3, ii));

        // if the triangulated point is behind the camera or too far away -> ignore
        if (!utilities::point_in_front(frame->projection, new_p3d) || new_p3d.norm() > TRIANGULATE_DISTANCE_OUTLIER)
            continue;

        extra_3d_points.push_back(new_p3d);

        const cv::Vec3b col = frame->rgb->at<cv::Vec3b>(x2[ii].y, x2[ii].x);
        extra_3d_points_colors.push_back(Eigen::Vector3d((float)(col.val[2]) / 255.0, (float)(col.val[1]) / 255.0, (float)(col.val[0]) / 255.0));

        extra_3d_points_normals.push_back(frame->position - new_p3d);

        // add to streamable points
        current_frame_points.push_back(new_p3d);
        current_frame_point_colors.push_back(extra_3d_points_colors.back());

        // debug_points.push_back(new_p3d);
        // debug_colors.push_back(Eigen::Vector3d((float)(col.val[2]) / 255.0, (float)(col.val[1]) / 255.0, (float)(col.val[0]) / 255.0));
    }

    // auto extra_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(debug_points));
    // extra_cloud->colors_ = debug_colors;

    // std::shared_ptr<open3d::geometry::TriangleMesh> camera_mesh = std::make_shared<open3d::geometry::TriangleMesh>(open3d::geometry::TriangleMesh());
    // open3d::io::ReadTriangleMeshFromOBJ("../assets/debug_camera_mesh.obj", *camera_mesh, false);
    // camera_mesh->Transform(frame->transformation);

    // open3d::visualization::DrawGeometries({ extra_cloud, camera_mesh });
}

std::vector<std::pair<uint32_t, uint32_t>> LGraph::find_landmark_feature_matches(const std::shared_ptr<Frame> ref_frame,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    // <2d feature, 3d landmark>
    std::vector<std::pair<uint32_t, uint32_t>> feature_lm_correspondences;
    feature_lm_correspondences.reserve(std::min(matches.size(), ref_frame->feature_landmark_lookup.size()));

    for (const auto& flm : ref_frame->feature_landmark_lookup)
    {
        for (const auto& match : matches)
        {
            // feature match was found in landmark lookup
            if (match.first == flm.first)
                feature_lm_correspondences.push_back(std::make_pair(match.second, flm.second));
        }
    }

    feature_lm_correspondences.shrink_to_fit();
    return feature_lm_correspondences;
}

std::vector<std::pair<uint32_t, uint32_t>> LGraph::backpropagate_future_matches(const std::shared_ptr<Frame> ref_frame,
        const std::shared_ptr<Frame> frame,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    throw std::runtime_error("this does not work, to the surprise of absolutely no one ");

    // <2d feature, 3d landmark>
    std::vector<std::pair<uint32_t, uint32_t>> feature_lm_correspondences;
    std::vector<uint32_t> ref_feature_ids;
    feature_lm_correspondences.reserve(std::min(matches.size(), ref_frame->feature_landmark_lookup.size()));

    // collect landmark point ids and current frame feature point ids
    // find prev-current matched form prev-landmarks
    for (const auto& flm : ref_frame->feature_landmark_lookup)
    {
        for (const auto& match : matches)
        {
            // feature match was found in landmark lookup
            if (match.first == flm.first)
            {
                feature_lm_correspondences.push_back(std::make_pair(match.second, flm.second));
                ref_feature_ids.push_back(match.first);
            }
        }
    }

    // not enough matches, backpropagate new ones
    // if (feature_lm_correspondences.size() < MIN_MATCH_FEATURE_COUNT)
    if (frames.size() > 5)
    {
        std::vector<std::pair<uint32_t, uint32_t>> new_matches;
        std::shared_ptr<Frame> latest_frame;

        // find a frame until STATISTICAL_FEATURE_COUNT condition not met
        for (int ii = frames.size() - 4; ii >= 0; ii--)
        {
            latest_frame = frames[ii];

            new_matches = features::match_features_flann(latest_frame->descriptors, ref_frame->descriptors);
            new_matches = features::take_only_indexed_matches(new_matches, ref_feature_ids);

            // new_matches = features::match_features_bf_crosscheck(latest_frame->descriptors, ref_desc);

            // new_matches = homography_filter_matches(latest_frame, ref_frame, new_matches);
            // new_matches = features::radius_distance_filter_matches(new_matches, ref_kps, latest_frame->keypoints, frame_kps;

            // DEBUG_visualize_matches(*latest_frame->rgb, *ref_frame->rgb, new_matches, latest_frame->keypoints, ref_frame->keypoints);

            // std::cout << ref_feature_ids.size() << ", " << new_matches.size() << ", " << matches.size() << "\n";

            // frame found, stop searching
            if (new_matches.size() < 250)
                break;
        }

        feature_lm_correspondences.shrink_to_fit();
        create_landmarks_from_matches(latest_frame, frame, new_matches);

        feature_lm_correspondences.clear();

        for (const auto& flm : ref_frame->feature_landmark_lookup)
        {
            for (const auto& match : matches)
            {
                // feature match was found in landmark lookup
                if (match.first == flm.first)
                {
                    feature_lm_correspondences.push_back(std::make_pair(match.second, flm.second));
                    ref_feature_ids.push_back(match.first);
                }
            }
        }
    }

    return feature_lm_correspondences;
}

void LGraph::new_landmarks_standalone(const std::shared_ptr<Frame> frame, const std::vector<cv::Point3f>& tr_angle_points)
{
    /**
     *  - find a triangulatable frame
     *  - match and filter features
     *  - triangulate
     *  - add to frame->feature_landmark_lookup
     */

    std::shared_ptr<Frame> ref_frame = find_triangulatable_point_frame(frame, tr_angle_points);

    std::vector<std::pair<uint32_t, uint32_t>> matches =
#if defined USE_FLANN
        features::match_features_flann(ref_frame->descriptors, frame->descriptors);
#else
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);
#endif

    // if (matches.size() > HOMOGRAPHY_MIN_FEATURE_COUNT)
    //     matches = homography_filter_matches(ref_frame, frame, matches);

    matches = features::radius_distance_filter_matches(matches, ref_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);
    // visualize_camera_tracks(true);

    // std::cout << "created new landmarks: " << matches.size() << "\n";

    // create landmarks and add to feature_landmark_lookup
    create_landmarks_from_matches(ref_frame, frame, matches);
}

void LGraph::backpropagate_new_landmarks_homography(const std::shared_ptr<Frame> frame,
        const cv::Point3f tr_angle_point)
{
    // TODO: find the largest difference frame to match against

    std::shared_ptr<Frame> ref_frame = nullptr;
    const Eigen::Vector3d tr_point (tr_angle_point.x, tr_angle_point.y, tr_angle_point.z);

    if (frames.size() < 4)
        return;

    for (int ii = frames.size() - 3; ii >= 0; ii--)
    {
        const double frame_angle = utilities::calculate_triangulation_angle(frames[ii]->position, frame->position, tr_point);

        if (RAD2DEG(frame_angle) < MIN_TRIANGULATION_ANGLE)
            continue;

        ref_frame = frames[ii];
        break;
    }

    if (!ref_frame)
        return;

    std::vector<std::pair<uint32_t, uint32_t>> matches =
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);
    matches = homography_filter_matches(ref_frame, frame, matches);

    // filter matches for already existing landmarks

    std::vector<std::pair<uint32_t, uint32_t>> new_matches;
    for (const auto& m : matches)
    {
        // find if the feature id already exists in teh landmark lookup
        const auto it = std::find_if(ref_frame->feature_landmark_lookup.begin(), ref_frame->feature_landmark_lookup.end(),
            [&m](const std::pair<uint32_t, uint32_t>& feature_landmark) {
                return feature_landmark.first == m.first;
            });

        // the feature was not found --> create new landmarks
        if (it == ref_frame->feature_landmark_lookup.end())
            new_matches.push_back(m);
    }

    create_landmarks_from_matches(ref_frame, frame, new_matches);

    std::cout << "matches " << matches.size() << " out of " << new_matches.size() << "\n";

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);
    // visualize_camera_tracks(true);

}

std::vector<std::pair<uint32_t, uint32_t>> LGraph::homography_filter_matches(
        const std::shared_ptr<Frame> ref_frame, const std::shared_ptr<Frame> frame,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    if (matches.size() < HOMOGRAPHY_MIN_FEATURE_COUNT)
        throw std::runtime_error("not enough matches for computing homography");

    std::vector<cv::Point2f> fpoints1, fpoints2;
    fpoints1.reserve(matches.size());
    fpoints2.reserve(matches.size());

    for (const auto& m : matches)
    {
        fpoints1.push_back(ref_frame->keypoints[m.first].pt);
        fpoints2.push_back(frame->keypoints[m.second].pt);
    }

    cv::Mat homography;

    homography = findHomography(fpoints1, fpoints2, cv::RANSAC, HOMOGRAPHY_RANSAC_THRESHOLD, cv::noArray());

    std::vector<std::pair<uint32_t, uint32_t>> good_matches;
    good_matches.reserve(matches.size());

    for (size_t ii = 0; ii < matches.size(); ii++)
    {
        cv::Mat col = cv::Mat::ones(3, 1, CV_64F);
        col.at<double>(0) = fpoints1[ii].x;
        col.at<double>(1) = fpoints1[ii].y;
        col = homography * col;
        col /= col.at<double>(2);

        const double dist = sqrt(pow(col.at<double>(0) - fpoints2[ii].x, 2) + pow(col.at<double>(1) - fpoints2[ii].y, 2));
        if (dist < HOMOGRAPHY_FILTER_MAX_DIST)
            good_matches.push_back(matches[ii]);
    }

    good_matches.shrink_to_fit();
    return good_matches;
}

void LGraph::backpropagate_new_landmarks(const std::shared_ptr<Frame> frame,
        const std::shared_ptr<Frame> ref_frame,
        const std::vector<uint32_t>& ref_frame_fids_inv,
        const std::vector<uint32_t>& current_frame_fids_inv,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches,
        const cv::Point3f tr_angle_point)
{
    // traverse the frames backwards, matching the features, until
    // a frame is found which has enough triangulatable movement

    // Create a data structure for holding the to-be-triangulated (tbt) feature chains.
    // Is also used as a lookup for pruning the triangulation list
    std::unordered_map<uint32_t, std::vector<uint32_t>> feature_chain;
    feature_chain.reserve(current_frame_fids_inv.size());

    // used to map feature ids from frame -> last_frame
    std::map<uint32_t, uint32_t> first_current_feature_lookup;

    for (int ii = 0; ii < current_frame_fids_inv.size(); ii++)
    {
        feature_chain.insert(std::make_pair(current_frame_fids_inv[ii], std::vector<uint32_t> { ref_frame_fids_inv[ii] }));
        first_current_feature_lookup.insert(std::make_pair(current_frame_fids_inv[ii], ref_frame_fids_inv[ii]));
    }

    std::vector<uint32_t> last_frame_fids = ref_frame_fids_inv;
    cv::Mat last_frame_descriptor = features::descriptor_from_feature_ids(ref_frame->descriptors, last_frame_fids);
    std::shared_ptr<Frame> last_frame = ref_frame;

    // -3: -1 for size, -2 for frame, -3 for ref_frame
    for (int ii = frames.size() - 3; ii >= 0; ii--)
    {
        const std::shared_ptr<Frame> current_frame = frames[ii];;

        // match the current latest descriptors against a new frame
        std::vector<std::pair<uint32_t, uint32_t>> matches =
            features::match_features_bf_crosscheck(last_frame_descriptor, current_frame->descriptors);
        matches = features::radius_distance_filter_matches(matches, last_frame->keypoints, current_frame->keypoints, FEATURE_DIST_MAX_RADIUS);

        std::cout << "matches size: " << matches.size() << "\n";

        last_frame_fids.clear();

        // populate tbt feature chain lookup
        for (const auto& match : matches)
        {
            // find match.first in first_current_feature_lookup, acquire the "key", use the key to
            // index into feature_chain, append match.second
            const auto key_iter = std::find_if(
                first_current_feature_lookup.begin(),
                first_current_feature_lookup.end(),
                [match](const auto& fcfl) { return fcfl.second == match.first; });

            if (key_iter == first_current_feature_lookup.end())
                continue;

            const uint32_t key = key_iter->first;
            feature_chain.at(key).push_back(match.second);

            // append the current frame feature id, next iteration last frame id
            last_frame_fids.push_back(match.second);
        }

        // check if enough triangulatable distance

        // if enough movement, stop iterating and triangulate
        throw std::runtime_error("todo");

        last_frame = current_frame;
        last_frame_descriptor = features::descriptor_from_feature_ids(current_frame->descriptors, last_frame_fids);
    }
}

std::shared_ptr<Frame> LGraph::find_triangulatable_movement_frame(const std::shared_ptr<Frame> frame, const double angle_threshold)
{
    // use angle to find good frame

    const Eigen::Vector3d current_pos = frame->position;

    for (int ii = frames.size() - 2; ii >= 0; ii--)
    {
        const std::shared_ptr<Frame> ref_frame = frames[ii];

        if ((ref_frame->position - current_pos).norm() > angle_threshold)
            return ref_frame;
    }

    return nullptr;
}

std::shared_ptr<Frame> LGraph::find_triangulatable_point_frame(const std::shared_ptr<Frame> frame,
        const std::vector<cv::Point3f>& tr_angle_points, const double angle_threshold)
{
    const Eigen::Vector3d current_pos = frame->position;

    // take the average of all of the triangulated points
    const Eigen::Vector3d tr_point_eigen = [&tr_angle_points] {
        cv::Point3f trp (0.0, 0.0, 0.0);
        for (const auto& p : tr_angle_points)
            trp += p;
        return Eigen::Vector3d(trp.x, trp.y, trp.z) / (double)tr_angle_points.size();

    }();

    for (int ii = frames.size() - 2; ii >= 0; ii--)
    {
        const std::shared_ptr<Frame> ref_frame = frames[ii];

        const double frame_angle = utilities::calculate_triangulation_angle(frames[ii]->position, frame->position, tr_point_eigen);

        if (RAD2DEG(frame_angle) < angle_threshold)
            continue;

        std::cout << "use frame id " << ii << " for triangulation, current frame is " << frames.size() - 1 << "\n";
        return ref_frame;
    }

    std::cout << "use frame id 0 for triangulation\n";
    return frames[0];
}

void LGraph::new_landmarks_from_matched(const std::shared_ptr<Frame> ref_frame,
        const std::shared_ptr<Frame> frame)
{
    std::vector<std::pair<uint32_t, uint32_t>> matches =
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);

    matches = features::radius_distance_filter_matches(matches, ref_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);

    std::vector<std::pair<uint32_t, uint32_t>> new_matches;

    // filter matches for already existing landmarks
    for (const auto& m : matches)
    {
        // find if the feature id already exists in teh landmark lookup
        const auto it = std::find_if(ref_frame->feature_landmark_lookup.begin(), ref_frame->feature_landmark_lookup.end(),
            [&m](const std::pair<uint32_t, uint32_t>& feature_landmark) {
                return feature_landmark.first == m.first;
            });

        // the feature was not found --> create new landmarks
        if (it == ref_frame->feature_landmark_lookup.end())
            new_matches.push_back(m);
    }

    // std::cout << "new landmarks: " << new_matches.size() << "\n";

    // std::cout << ref_frame->position.transpose() << ", " << frame->position.transpose() << "\n";

    create_landmarks_from_matches(ref_frame, frame, new_matches);
}

#ifdef USE_OPEN3D
void LGraph::visualize_camera_tracks(const bool visualize_landmarks, bool generate_mesh) const
{
    std::vector<std::shared_ptr<const open3d::geometry::Geometry>> debug_cameras;

    for (int ii = 0; ii < frames.size(); ii++)
    {
        std::cout << "frame " << ii << ": " << frames[ii]->position.transpose() << "\n";

        std::shared_ptr<open3d::geometry::TriangleMesh> camera_mesh = std::make_shared<open3d::geometry::TriangleMesh>(open3d::geometry::TriangleMesh());
        open3d::io::ReadTriangleMeshFromOBJ("../assets/debug_camera_mesh.obj", *camera_mesh, false);

        camera_mesh->Transform(frames[ii]->transformation);
        debug_cameras.push_back(camera_mesh);
    }

    if (visualize_landmarks)
    {
        std::vector<Eigen::Vector3d> landmark_points;
        std::vector<Eigen::Vector3d> landmark_colors;
        std::vector<Eigen::Vector3d> landmark_normals;

        for (const auto& lm : landmarks)
        {
            landmark_points.push_back(Eigen::Vector3d(lm.location.x, lm.location.y, lm.location.z));
            landmark_colors.push_back(lm.color);
            landmark_normals.push_back(lm.normal);
        }

        auto lms_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(landmark_points));
        lms_cloud->colors_ = landmark_colors;
        lms_cloud->normals_ = landmark_normals;

        if (generate_mesh)
        {
            auto mesh_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(extra_3d_points));
            mesh_cloud->colors_ = extra_3d_points_colors;
            mesh_cloud->normals_ = extra_3d_points_normals;

            *mesh_cloud += *lms_cloud;

            // mesh_cloud->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.5, 16));

            auto [new_mesh, trash] = open3d::geometry::TriangleMesh::CreateFromPointCloudPoisson(*mesh_cloud, MESH_POISSON_DEPTH);
            new_mesh = new_mesh->FilterSmoothLaplacian(LAPLACIAN_ITERATIONS, LAPLACIAN_LAMBDA);

            // new_mesh->Translate(Eigen::Vector3d(0.0, 20.0, 0.0));

            debug_cameras.push_back(new_mesh);
            // debug_cameras.push_back(mesh_cloud);
        }
        else
        {
            debug_cameras.push_back(lms_cloud);

            auto extra_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(extra_3d_points));
            extra_cloud->colors_ = extra_3d_points_colors;
            extra_cloud->normals_ = extra_3d_points_normals;
            *extra_cloud += *lms_cloud;

            // open3d::io::WritePointCloudToPLY("export_cloud.ply", *extra_cloud);

            // debug_cameras.push_back(extra_cloud);
        }
    }

    open3d::visualization::DrawGeometries(debug_cameras, "track visualization", 1920, 1080);
}
#endif

void LGraph::print_camera_tracks() const
{
    for (int ii = 0; ii < frames.size(); ii++)
    {
        std::cout << "frame " << ii << ": " << frames[ii]->position.transpose() << "\n";
    }
}


/*
 * self_similarity.cpp
 *
 *  Created on: Jan 25, 2012
 *      Author: lbossard
 */

#include "self_similarity.hpp"

#include <boost/math/constants/constants.hpp>
#include <boost/foreach.hpp>

namespace vision
{
namespace features
{

SelfSimilarityExtractor::SelfSimilarityExtractor(
        bool color_ssd,
        unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{
    color_ssd_ = color_ssd;
    this->setSelfSimilarityParameters(
            patch_size,
            window_radius,
            radius_bin_count,
            angle_bin_count,
            var_noise,
            auto_var_radius);
}

void SelfSimilarityExtractor::setSelfSimilarityParameters(
        unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{

    self_similarity_.setParameters(patch_size,
            window_radius,
            radius_bin_count,
            angle_bin_count,
            var_noise,
            auto_var_radius);

}

/*virtual*/ SelfSimilarityExtractor::~SelfSimilarityExtractor()
{

}

/*virtual*/ cv::Mat_<float> SelfSimilarityExtractor::denseExtract(
        const cv::Mat& input_image,
        std::vector<cv::Point>& locations,
        const cv::Mat_<uchar>& mask
) const
{
    const unsigned int radius = 5;

    // generate locations
    locations.clear();
    LowLevelFeatureExtractor::generateGridLocations(
            input_image.size(),
            cv::Size(radius, radius),
            radius,
            radius,
            locations,
            mask);
    if (locations.size() == 0)
    {
        return cv::Mat_<float>();
    }

    // convert to gray if necessary
    cv::Mat image = input_image;
    if (!color_ssd_ && image.type() != CV_8U)
    {
        cv::cvtColor(image, image, CV_BGR2GRAY);
    }

    // allocate memroy
    cv::Mat_<float> features(locations.size(), descriptorLength());

    // loop over locations and compute descriptors
    std::size_t point_idx = 0;
    BOOST_FOREACH(const cv::Point& p, locations)
    {
        cv::Mat_<float> descriptor = features.row(point_idx);
        if (self_similarity_.extract(image, p, descriptor))
        {
            ++point_idx;
        }
    }
    if (point_idx == 0)
    {
        return cv::Mat_<float>();
    }
    features.resize(point_idx);
    return features;
}

/*virtual*/ unsigned int SelfSimilarityExtractor::descriptorLength() const
{
    return self_similarity_.descriptorLength();
}
////////////////////////////////////////////////////////////////////////////////
/**
 *
 * @param patch_size       odd
 * @param window_radius    even
 * @param radius_bin_count
 * @param angle_bin_count
 * @param var_noise
 * @param auto_var_radius
 */
SelfSimilarity::SelfSimilarity(
        unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{
    setParameters(patch_size,
            window_radius,
            radius_bin_count,
            angle_bin_count,
            var_noise,
            auto_var_radius);
}
/*virtual*/ SelfSimilarity::~SelfSimilarity()
{

}

void SelfSimilarity::setParameters(unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{
    // set parameters
    patch_size_ = patch_size;
    window_radius_ = window_radius;
    radius_bin_count_ = radius_bin_count;
    angle_bin_count_ = angle_bin_count;
    var_noise_ = var_noise;

    // precompute binning and autovar indices
    const int window_size = 2 * window_radius_ + 1;
    bin_map_= -cv::Mat_<int>::ones(window_size, window_size);  // init to -1
    autovar_indices_.clear();
    autovar_indices_.reserve((2*auto_var_radius + 1)*(2*auto_var_radius + 1)); // upper bound
    double angle = 0.f;
    double radius = 0.f;
    int angle_bin = 0;
    int radius_bin = 0;
    int bin_id = 0;
    for (int y = -window_radius_; y < window_size; ++y)
    {
        for (int x = -window_radius_; x < window_size; ++x)
        {
            /*
             * scale log(y) to (bin_count - 1)...0
             */
            radius = std::sqrt(static_cast<float>(x * x + y * y));
            if (radius > window_radius_ || radius == 0.)
            {
                continue;
            }
            radius_bin = static_cast<int>(
                    ( std::log(radius)) / std::log(window_radius_+.5)
                    * radius_bin_count_);

            /*
             * \alpha \in [ -\pi ... +\pi ]
             * bin_id \in [0 ... (bin_count - 1) ]
             * bin_id = floor( (\alpha + \pi) / ( 2 \pi) * bin_count )
             *        = floor( \alpha bin_count / ( 2 \pi) + bin_count/2 )
             */
            angle = std::atan2(y, x); // [-pi ... pi]
            angle_bin = static_cast<int>(
                   ((angle * angle_bin_count_)
                    / ( boost::math::constants::two_pi<double>()))
                    + angle_bin_count_/2.) % angle_bin_count; // atan2 gives [-pi ... pi] and not (-pi ... pi] that's why we mod

            bin_id = angle_bin * radius_bin_count + radius_bin;
            bin_map_(y + window_radius_, x + window_radius_) = bin_id;

            if (radius <= auto_var_radius && radius > 0)
            {
                autovar_indices_.push_back(cv::Point(y + window_radius_, x + window_radius_));
            }

        }
    }
}

bool SelfSimilarity::extract(const cv::Mat& image, const cv::Point& location, cv::Mat_<float>& descriptor) const
{
    // compute the distance surface
    cv::Mat_<float> distance_surface;
    compute_distance_surface(image, location, distance_surface);

    if (distance_surface.rows == 0 || distance_surface.cols == 0)
    {
        return false;
    }

    // put it into the logpolar descriptor
    compute_descriptor(distance_surface, descriptor);
    return true;
}

//void showmat(const std::string titile, const cv::Mat& mat, int size)
//{
//    cv::Mat scaled;
//    cv::resize(mat, scaled, cv::Size(mat.rows * size, mat.cols * size), 0, 0, cv::INTER_AREA);
//    cv::imshow(titile, scaled);
//}

void SelfSimilarity::compute_distance_surface(const cv::Mat& image, const cv::Point& loaction, cv::Mat_<float>& distance_surface) const
{
    const int patch_radius = patch_size_ / 2;
    const int window_size  = window_radius_ * 2 + 1;

    // check boundaries
    if (	   (loaction.x - (int)window_radius_ - patch_radius) < 0
            || (loaction.y - (int)window_radius_ - patch_radius) < 0
            || (loaction.x + (int)window_radius_ + patch_size_) >= image.cols
            || (loaction.y + (int)window_radius_ + patch_size_) >= image.rows)
    {
        distance_surface = cv::Mat_<float>();
        return;
    }

    // allocate memory and precompute some structures
    distance_surface.create(window_size, window_size);

    const cv::Mat inner_patch = image(
            cv::Range(loaction.y - patch_radius, loaction.y + patch_radius + 1),
            cv::Range(loaction.x - patch_radius, loaction.x + patch_radius + 1));

//    assert(distance_surface.rows == window_size);
//    assert(distance_surface.cols == window_size);
    for (unsigned int r = 0; r < window_size; ++r)
    {
        const int y_window_patch_start = loaction.y - window_radius_ + r - patch_radius ;
        const cv::Range y_window_range(
                y_window_patch_start,
                y_window_patch_start + patch_size_);
        for (unsigned int c = 0; c < window_size; ++c)
        {
//            const int bin_idx = bin_map_(r,c);
//            if (bin_idx < 0)
//            {
//                continue;
//            }
            const int x_window_patch_start = loaction.x - window_radius_  + c - patch_radius;
            const cv::Range x_window_range(
                    x_window_patch_start,
                    x_window_patch_start + patch_size_);

            const cv::Mat window_patch = image(y_window_range, x_window_range);
            if (image.channels() == 3)
            {
                distance_surface(r, c) = ssd3(inner_patch, window_patch);
            }
            else
            {
                distance_surface(r, c) = ssd1(inner_patch, window_patch);
            }

//            showmat("inner", inner_patch, 100);
//            showmat("window_patch", window_patch, 100);
//            cv::Mat d = image.clone();
//            // window_patch
//            cv::rectangle(d,
//                    cv::Point(x_window_range.start, y_window_range.start),
//                    cv::Point(x_window_range.end-1,   y_window_range.end-1),
//                    CV_RGB(0,255,255)
//                    );
//            // center_patch
//            cv::rectangle(d,
//                    cv::Point(loaction.x - patch_radius, loaction.y - patch_radius),
//                    cv::Point(loaction.x + patch_radius, loaction.y + patch_radius),
//                    CV_RGB(255,0,255)
//                    );
//            // whole window
//            cv::rectangle(d,
//                    cv::Point(
//                            loaction.x - window_radius_,
//                            loaction.y - window_radius_),
//                    cv::Point(
//                            loaction.x + window_radius_,
//                            loaction.y + window_radius_),
//                    CV_RGB(255,0,255)
//                    );
//            showmat("all", d, 5);
//            cv::waitKey();
        }
    }
}

void SelfSimilarity::compute_descriptor(const cv::Mat_<float>& distance_surface, cv::Mat_<float>& descriptor) const
{
    // allocate log-polar descriptor. we init it with -1 as the e^-x is
    // always > 0 for x < \inf
    const int descriptor_length = radius_bin_count_*angle_bin_count_;
    descriptor.create(1, descriptor_length);
    descriptor = -1;

    // compute auto noise (like original paper matlab implementation):
    // find max ssd at the autovar_indices (<- places with within radius 1)
    float variance = var_noise_;
    for (unsigned int i = 0; i < autovar_indices_.size(); ++i)
    {
        const cv::Point& p = autovar_indices_[i];
        variance = std::max(variance, distance_surface(p.y, p.x));
    }

    // compute finally the descriptor
    float min_correlation = std::numeric_limits<float>::max();
    float max_correlation = std::numeric_limits<float>::min();
    for (int r = 0; r < distance_surface.rows; ++r)
    {
        for (int c = 0; c < distance_surface.cols; ++c)
        {
            const int bin_idx = bin_map_(r,c);
            if (bin_idx < 0)
            {
                continue;
            }
            // compute S_q(x,y)
            float correlation = std::exp(-distance_surface(r, c) / variance);
            // we store only the max ssd
            const float current_value = descriptor(bin_idx);
            if (current_value < correlation)
            {
                descriptor(bin_idx) = correlation;
                min_correlation = std::min(correlation, min_correlation);
                max_correlation = std::max(correlation, max_correlation);
            }

        }
    }
    // finally: normalize to [0 ... 1]
    //XXX: Note: in the original implementation, there would be only stretching to max without substracting min
    // however, the paper states "normalized by linearly streticht its values to the range [0...1]"
//    descriptor = (descriptor - min_correlation) / (max_correlation - min_correlation);
    //XXX: strange enough: "correct" normalization gives a segfault while clustering
    descriptor = descriptor / max_correlation;  // normalisation from the paper implementation

}

} /* namespace features */
} /* namespace vision */


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  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 institute 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 OWNER 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.
 *
 * Author: Christoph Rösmann, Otniel Rinaldo
 *********************************************************************/

#include <costmap_converter/costmap_to_polygons.h>
#include <costmap_converter/misc.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)

namespace
{

/**
 * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
 *
 * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
 *
 * @param begin iterator pointing to the begin of the range of points
 * @param end interator pointing to the end of the range of points
 * @param epsilon distance criteria for removing points if it is closer to the line segment than this
 * @param result the simplified polygon
 */
std::vector<geometry_msgs::Point32> douglasPeucker(std::vector<geometry_msgs::Point32>::iterator begin,
  std::vector<geometry_msgs::Point32>::iterator end, double epsilon)
{
  if (std::distance(begin, end) <= 2)
  {
    return std::vector<geometry_msgs::Point32>(begin, end);
  }

  // Find the point with the maximum distance from the line [begin, end)
  double dmax = std::numeric_limits<double>::lowest();
  std::vector<geometry_msgs::Point32>::iterator max_dist_it;
  std::vector<geometry_msgs::Point32>::iterator last = std::prev(end);
  for (auto it = std::next(begin); it != last; ++it)
  {
    double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
    if (d > dmax)
    {
      max_dist_it = it;
      dmax = d;
    }
  }

  if (dmax < epsilon * epsilon)
  { // termination criterion reached, line is good enough
    std::vector<geometry_msgs::Point32> result;
    result.push_back(*begin);
    result.push_back(*last);
    return result;
  }

  // Recursive calls for the two splitted parts
  auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
  auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);

  // Combine the two lines into one line and return the merged line.
  // Note that we have to skip the first point of the second line, as it is duplicated above.
  firstLineSimplified.insert(firstLineSimplified.end(),
    std::make_move_iterator(std::next(secondLineSimplified.begin())),
    std::make_move_iterator(secondLineSimplified.end()));
  return firstLineSimplified;
}

} // end namespace

namespace costmap_converter
{

CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
{
  costmap_ = NULL;
  dynamic_recfg_ = NULL;
  neighbor_size_x_ = neighbor_size_y_ = -1;
  offset_x_ = offset_y_ = 0.;
}

CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH()
{
  if (dynamic_recfg_ != NULL)
    delete dynamic_recfg_;
}

void CostmapToPolygonsDBSMCCH::initialize(ros::NodeHandle nh)
{
    costmap_ = NULL;

    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);

    parameter_buffered_ = parameter_;

    // setup dynamic reconfigure
    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
}

void CostmapToPolygonsDBSMCCH::compute()
{
    std::vector< std::vector<KeyPoint> > clusters;
    dbScan(clusters);

    // Create new polygon container
    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());

    // add convex hulls to polygon container
    for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
    {
      polygons->push_back( geometry_msgs::Polygon() );
      convexHull2(clusters[i], polygons->back() );
    }

    // add our non-cluster points to the polygon container (as single points)
    if (!clusters.empty())
    {
      for (std::size_t i=0; i < clusters.front().size(); ++i)
      {
        polygons->push_back( geometry_msgs::Polygon() );
        convertPointToPolygon(clusters.front()[i], polygons->back());
      }
    }

    // replace shared polygon container
    updatePolygonContainer(polygons);
}

void CostmapToPolygonsDBSMCCH::setCostmap2D(costmap_2d::Costmap2D *costmap)
{
    if (!costmap)
      return;

    costmap_ = costmap;

    updateCostmap2D();
}

void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
      occupied_cells_.clear();

      if (!costmap_->getMutex())
      {
        ROS_ERROR("Cannot update costmap since the mutex pointer is null");
        return;
      }

      { // get a copy of our parameters from dynamic reconfigure
        boost::mutex::scoped_lock lock(parameter_mutex_);
        parameter_ = parameter_buffered_;
      }

      costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());

      // allocate neighbor lookup
      int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
      int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;

      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {
        neighbor_size_x_ = cells_x;
        neighbor_size_y_ = cells_y;
        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
      }
      offset_x_ = costmap_->getOriginX();
      offset_y_ = costmap_->getOriginY();
      for (auto& n : neighbor_lookup_)
        n.clear();

      // get indices of obstacle cells
      for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
      {
        for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
        {
          int value = costmap_->getCost(i,j);
          if(value >= costmap_2d::LETHAL_OBSTACLE)
          {
            double x, y;
            costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
            addPoint(x, y);
          }
        }
      }
}

void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)
{
  std::vector<bool> visited(occupied_cells_.size(), false);

  clusters.clear();

  //DB Scan Algorithm
  int cluster_id = 0; // current cluster_id
  clusters.push_back(std::vector<KeyPoint>());
  for(int i = 0; i< (int)occupied_cells_.size(); i++)
  {
    if(!visited[i]) //keypoint has not been visited before
    {
      visited[i] = true; // mark as visited
      std::vector<int> neighbors;
      regionQuery(i, neighbors); //Find neighbors around the keypoint
      if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise
      {
        clusters[0].push_back(occupied_cells_[i]);
      }
      else
      {
        ++cluster_id; // increment current cluster_id
        clusters.push_back(std::vector<KeyPoint>());

        // Expand the cluster
        clusters[cluster_id].push_back(occupied_cells_[i]);
        for(int j = 0; j<(int)neighbors.size(); j++)
        {
          if ((int)clusters[cluster_id].size() == parameter_.max_pts_)
            break;

          if(!visited[neighbors[j]]) //keypoint has not been visited before
          {
            visited[neighbors[j]] = true;  // mark as visited
            std::vector<int> further_neighbors;
            regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
//             if(further_neighbors.size() < min_pts_)
//             {
//               clusters[0].push_back(occupied_cells[neighbors[j]]);
//             }
//             else
            if ((int)further_neighbors.size() >= parameter_.min_pts_)
            {
              // neighbors found
              neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
              clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
            }
          }
        }
      }
    }
  }
}

void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)
{
    neighbors.clear();

    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
    const KeyPoint& kp = occupied_cells_[curr_index];
    int cx, cy;
    pointToNeighborCells(kp, cx,cy);

    // loop over the neighboring cells for looking up the points
    const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},
                               {-1,  0}, {0,  0}, {1,  0},
                               {-1,  1}, {0,  1}, {1,  1}};
    for (int i = 0; i < 9; ++i)
    {
      int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);
      if (idx < 0 || idx >= int(neighbor_lookup_.size()))
        continue;
      const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];
      for (int point_idx : pointIndicesToCheck) {
        if (point_idx == curr_index) // point is not a neighbor to itself
          continue;
        const KeyPoint& other = occupied_cells_[point_idx];
        double dx = other.x - kp.x;
        double dy = other.y - kp.y;
        double dist_sqr = dx*dx + dy*dy;
        if (dist_sqr <= dist_sqr_threshold)
          neighbors.push_back(point_idx);
      }
    }
}

bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
{
  return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
}

void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
{
    //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull

    int k = 0;
    int n = cluster.size();

    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);

    polygon.points.resize(2*n);

    // lower hull
    for (int i = 0; i < n; ++i)
    {
      while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
      {
        --k;
      }
      cluster[i].toPointMsg(polygon.points[k]);
      ++k;
    }

    // upper hull
    for (int i = n-2, t = k+1; i >= 0; --i)
    {
      while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
      {
        --k;
      }
      cluster[i].toPointMsg(polygon.points[k]);
      ++k;
    }

    polygon.points.resize(k); // original
    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost

    simplifyPolygon(polygon);
}

void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
{
    std::vector<KeyPoint>& P = cluster;
    std::vector<geometry_msgs::Point32>& points = polygon.points;

    // Sort P by x and y
    std::sort(P.begin(), P.end(), isXCoordinateSmaller);

    // the output array H[] will be used as the stack
    int i;                 // array scan index

    // Get the indices of points with min x-coord and min|max y-coord
    int minmin = 0, minmax;
    double xmin = P[0].x;
    for (i = 1; i < (int)P.size(); i++)
        if (P[i].x != xmin) break;
    minmax = i - 1;
    if (minmax == (int)P.size() - 1)
    {   // degenerate case: all x-coords == xmin
        points.push_back(geometry_msgs::Point32());
        P[minmin].toPointMsg(points.back());
        if (P[minmax].y != P[minmin].y) // a  nontrivial segment
        {
            points.push_back(geometry_msgs::Point32());
            P[minmax].toPointMsg(points.back());
        }
        // add polygon endpoint
        points.push_back(geometry_msgs::Point32());
        P[minmin].toPointMsg(points.back());
        return;
    }

    // Get the indices of points with max x-coord and min|max y-coord
    int maxmin, maxmax = (int)P.size() - 1;
    double xmax = P.back().x;
    for (i = P.size() - 2; i >= 0; i--)
        if (P[i].x != xmax) break;
    maxmin = i+1;

    // Compute the lower hull on the stack H
    // push  minmin point onto stack
    points.push_back(geometry_msgs::Point32());
    P[minmin].toPointMsg(points.back());
    i = minmax;
    while (++i <= maxmin)
    {
        // the lower line joins P[minmin]  with P[maxmin]
        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
            continue;           // ignore P[i] above or on the lower line

        while (points.size() > 1)         // there are at least 2 points on the stack
        {
            // test if  P[i] is left of the line at the stack top
            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
                break;         // P[i] is a new hull  vertex
            points.pop_back();         // pop top point off  stack
        }
        // push P[i] onto stack
        points.push_back(geometry_msgs::Point32());
        P[i].toPointMsg(points.back());
    }

    // Next, compute the upper hull on the stack H above  the bottom hull
    if (maxmax != maxmin)      // if  distinct xmax points
    {
         // push maxmax point onto stack
         points.push_back(geometry_msgs::Point32());
         P[maxmax].toPointMsg(points.back());
    }
    int bot = (int)points.size();                  // the bottom point of the upper hull stack
    i = maxmin;
    while (--i >= minmax)
    {
        // the upper line joins P[maxmax]  with P[minmax]
        if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)
            continue;           // ignore P[i] below or on the upper line

        while ((int)points.size() > bot)     // at least 2 points on the upper stack
        {
            // test if  P[i] is left of the line at the stack top
            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
                break;         // P[i] is a new hull  vertex
            points.pop_back();         // pop top point off stack
        }
        // push P[i] onto stack
        points.push_back(geometry_msgs::Point32());
        P[i].toPointMsg(points.back());
    }
    if (minmax != minmin)
    {
        // push  joining endpoint onto stack
        points.push_back(geometry_msgs::Point32());
        P[minmin].toPointMsg(points.back());
    }

    simplifyPolygon(polygon);
}

void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::Polygon& polygon)
{
  size_t triangleThreshold = 3;
  // check if first and last point are the same. If yes, a triangle has 4 points
  if (polygon.points.size() > 1
      && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5
      && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
  {
    triangleThreshold = 4;
  }
  if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines
    return;
  // TODO Reason about better start conditions for splitting lines, e.g., by
  // https://en.wikipedia.org/wiki/Rotating_calipers
  polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;
}

void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
{
  boost::mutex::scoped_lock lock(mutex_);
  polygons_ = polygons;
}

PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
{
  boost::mutex::scoped_lock lock(mutex_);
  PolygonContainerConstPtr polygons = polygons_;
  return polygons;
}

void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
{
  boost::mutex::scoped_lock lock(parameter_mutex_);
  parameter_buffered_.max_distance_ = config.cluster_max_distance;
  parameter_buffered_.min_pts_ = config.cluster_min_pts;
  parameter_buffered_.max_pts_ = config.cluster_max_pts;
  parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
}

}//end namespace costmap_converter


#ifndef STAN_MATH_REV_CORE_VAR_HPP
#define STAN_MATH_REV_CORE_VAR_HPP

#include <stan/math/rev/core/vari.hpp>
#include <stan/math/rev/core/grad.hpp>
#include <stan/math/rev/core/chainable_alloc.hpp>
#include <boost/math/tools/config.hpp>
#include <ostream>
#include <vector>

namespace stan {
  namespace math {

    // forward declare
    static void grad(vari* vi);

    /**
     * Independent (input) and dependent (output) variables for gradients.
     *
     * This class acts as a smart pointer, with resources managed by
     * an agenda-based memory manager scoped to a single gradient
     * calculation.
     *
     * An var is constructed with a double and used like any
     * other scalar.  Arithmetical functions like negation, addition,
     * and subtraction, as well as a range of mathematical functions
     * like exponentiation and powers are overridden to operate on
     * var values objects.
     */
    class var {
    public:
      // FIXME: doc what this is for
      typedef double Scalar;

      /**
       * Pointer to the implementation of this variable.
       *
       * This value should not be modified, but may be accessed in
       * var operators to construct vari
       * instances.
       */
      vari * vi_;

      /**
       * Return true if this variable has been
       * declared, but not been defined.  Any attempt to use an
       * undefined variable's value or adjoint will result in a
       * segmentation fault.
       *
       * @return true if this variable does not yet have
       * a defined variable.
       */
      bool is_uninitialized() {
        return (vi_ == static_cast<vari*>(0U));
      }

      /**
       * Construct a variable for later assignment.
       *
       * This is implemented as a no-op, leaving the underlying implementation
       * dangling.  Before an assignment, the behavior is thus undefined just
       * as for a basic double.
       */
      var() : vi_(static_cast<vari*>(0U)) { }

      /**
       * Construct a variable from a pointer to a variable implementation.
       *
       * @param vi Variable implementation.
       */
      var(vari* vi) : vi_(vi) {  }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(float x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument as
       * a value and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(double x) : vi_(new vari(x)) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(long double x) : vi_(new vari(x)) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(bool x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(char x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(short x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(int x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(long x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(unsigned char x)  // NOLINT(runtime/explicit)
      : vi_(new vari(static_cast<double>(x))) { }

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      // NOLINTNEXTLINE
      var(unsigned short x) : vi_(new vari(static_cast<double>(x))) { }

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(unsigned int x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      // NOLINTNEXTLINE
      var(unsigned long x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

#ifdef _WIN64

      // these two ctors are for Win64 to enable 64-bit signed
      // and unsigned integers, because long and unsigned long
      // are still 32-bit

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(size_t x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(ptrdiff_t x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT
#endif

#ifdef BOOST_MATH_USE_FLOAT128

      // this ctor is for later GCCs that have the __float128
      // type enabled, because it gets enabled by boost

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(__float128 x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

#endif

      /**
       * Return the value of this variable.
       *
       * @return The value of this variable.
       */
      inline double val() const {
        return vi_->val_;
      }

      /**
       * Return the derivative of the root expression with
       * respect to this expression.  This method only works
       * after one of the grad() methods has been
       * called.
       *
       * @return Adjoint for this variable.
       */
      inline double adj() const {
        return vi_->adj_;
      }

      /**
       * Compute the gradient of this (dependent) variable with respect to
       * the specified vector of (independent) variables, assigning the
       * specified vector to the gradient.
       *
       * The grad() function does not recover memory.  In Stan
       * 2.4 and earlier, this function did recover memory.
       *
       * @param x Vector of independent variables.
       * @param g Gradient vector of partial derivatives of this
       * variable with respect to x.
       */
      void grad(std::vector<var>& x,
                std::vector<double>& g) {
        stan::math::grad(vi_);
        g.resize(x.size());
        for (size_t i = 0; i < x.size(); ++i)
          g[i] = x[i].vi_->adj_;
      }

      /**
       * Compute the gradient of this (dependent) variable with respect
       * to all (independent) variables.
       *
       * The grad() function does not recover memory.
       */
      void grad() {
        stan::math::grad(vi_);
      }

      // POINTER OVERRIDES

      /**
       * Return a reference to underlying implementation of this variable.
       *
       * If x is of type var, then applying
       * this operator, *x, has the same behavior as
       * *(x.vi_).
       *
       * Warning:  The returned reference does not track changes to
       * this variable.
       *
       * @return variable
       */
      inline vari& operator*() {
        return *vi_;
      }

      /**
       * Return a pointer to the underlying implementation of this variable.
       *
       * If x is of type var, then applying
       * this operator, x->, behaves the same way as
       * x.vi_->.
       *
       * Warning: The returned result does not track changes to
       * this variable.
       */
      inline vari* operator->() {
        return vi_;
      }

      // COMPOUND ASSIGNMENT OPERATORS

      /**
       * The compound add/assignment operator for variables (C++).
       *
       * If this variable is a and the argument is the variable b,
       * then (a += b) behaves exactly the same way as (a = a + b),
       * creating an intermediate variable representing (a + b).
       *
       * @param b The variable to add to this variable.
       * @return The result of adding the specified variable to this variable.
       */
      inline var& operator+=(const var& b);

      /**
       * The compound add/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a += b) behaves exactly the same way as (a = a + b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to add to this variable.
       * @return The result of adding the specified variable to this variable.
       */
      inline var& operator+=(double b);

      /**
       * The compound subtract/assignment operator for variables (C++).
       *
       * If this variable is a and the argument is the variable b,
       * then (a -= b) behaves exactly the same way as (a = a - b).
       * Note that the result is an assignable lvalue.
       *
       * @param b The variable to subtract from this variable.
       * @return The result of subtracting the specified variable from
       * this variable.
       */
      inline var& operator-=(const var& b);

      /**
       * The compound subtract/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a -= b) behaves exactly the same way as (a = a - b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to subtract from this variable.
       * @return The result of subtracting the specified variable from this
       * variable.
       */
      inline var& operator-=(double b);

      /**
       * The compound multiply/assignment operator for variables (C++).
       *
       * If this variable is a and the argument is the variable b,
       * then (a *= b) behaves exactly the same way as (a = a * b).
       * Note that the result is an assignable lvalue.
       *
       * @param b The variable to multiply this variable by.
       * @return The result of multiplying this variable by the
       * specified variable.
       */
      inline var& operator*=(const var& b);

      /**
       * The compound multiply/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a *= b) behaves exactly the same way as (a = a * b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to multiply this variable by.
       * @return The result of multplying this variable by the specified
       * variable.
       */
      inline var& operator*=(double b);

      /**
       * The compound divide/assignment operator for variables (C++).  If this
       * variable is a and the argument is the variable b, then (a /= b)
       * behaves exactly the same way as (a = a / b).  Note that the
       * result is an assignable lvalue.
       *
       * @param b The variable to divide this variable by.
       * @return The result of dividing this variable by the
       * specified variable.
       */
      inline var& operator/=(const var& b);

      /**
       * The compound divide/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a /= b) behaves exactly the same way as (a = a / b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to divide this variable by.
       * @return The result of dividing this variable by the specified
       * variable.
       */
      inline var& operator/=(double b);

      /**
       * Write the value of this auto-dif variable and its adjoint to
       * the specified output stream.
       *
       * @param os Output stream to which to write.
       * @param v Variable to write.
       * @return Reference to the specified output stream.
       */
      friend std::ostream& operator<<(std::ostream& os, const var& v) {
        if (v.vi_ == 0)
          return os << "uninitialized";
        return os << v.val();
      }
    };

  }
}
#endif


/* Copyright (c) 2019 Kjetil Olsen Lye, ETH Zurich
 * MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <fbm/fbm.hpp>
namespace fbmpy {

//! Adapter function for fbm::generate_fractional_brownian_bridge
inline boost::python::numpy::ndarray fractional_brownian_bridge_3d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_3d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_bridge
inline boost::python::numpy::ndarray fractional_brownian_bridge_2d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_2d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_bridge
inline boost::python::numpy::ndarray fractional_brownian_bridge_1d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_1d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_motion
inline boost::python::numpy::ndarray fractional_brownian_motion_1d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_motion_1d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_motion
inline boost::python::numpy::ndarray fractional_brownian_motion_2d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_2d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()),
        false);

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_motion
inline boost::python::numpy::ndarray fractional_brownian_motion_3d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_3d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()),
        false);

    return bridge_data;

}
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include <cstdlib>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include "ublas_trace.hpp"
#include "ublas_elementwise.hpp"
#include "gauss_distribution.hpp"
#include "gaussian_process_normal.hpp"

namespace bayesopt
{

  namespace ublas = boost::numeric::ublas;

  GaussianProcessNormal::GaussianProcessNormal(size_t dim,
					       Parameters params,
					       const Dataset& data,
					       MeanModel& mean,
					       randEngine& eng):
    HierarchicalGaussianProcess(dim,params,data,mean,eng),
    mW0(params.mean.coef_mean.size()), mInvVarW(params.mean.coef_mean.size()),
    mD(params.mean.coef_mean.size(),params.mean.coef_mean.size())
  {
    mSigma = params.sigma_s;
    mW0 = params.mean.coef_mean;
    for (size_t ii = 0; ii < params.mean.coef_std.size(); ++ii)
      {
	double varii = params.mean.coef_std[ii] * params.mean.coef_std[ii];
	mInvVarW(ii) = 1/varii;
      }
     d_ = new GaussianDistribution(eng);
  }  // Constructor

  GaussianProcessNormal::~GaussianProcessNormal()
  {
    delete d_;
  } // Default destructor

  ProbabilityDistribution*
  GaussianProcessNormal::prediction(const vectord &query)
  {
    const double kq = computeSelfCorrelation(query);
    const vectord phi = mMean.getFeatures(query);

    vectord v = computeCrossCorrelation(query);

    inplace_solve(mL,v,ublas::lower_tag());

    vectord rq = phi - prod(v,mKF);

    vectord rho(rq);
    inplace_solve(mD,rho,ublas::lower_tag());

    double yPred = inner_prod(phi,mWMap) + inner_prod(v,mVf);
    double sPred = sqrt( mSigma * (kq - inner_prod(v,v)
			        + inner_prod(rho,rho)));

    if ((boost::math::isnan(yPred)) || (boost::math::isnan(sPred)))
      {
	throw std::runtime_error("Error in prediction. NaN found.");
      }

    d_->setMeanAndStd(yPred,sPred);
    return d_;
  }

  double GaussianProcessNormal::negativeLogLikelihood()
  {
    matrixd KK = computeCorrMatrix();
    const size_t n = KK.size1();
    const size_t p = mMean.nFeatures();

    vectord v0 = mData.mY - prod(trans(mMean.mFeatM),mW0);
    matrixd WW = zmatrixd(p,p);  //TODO: diagonal matrix
    utils::add_to_diagonal(WW,mInvVarW);
    matrixd FW = prod(trans(mMean.mFeatM),WW);
    KK += prod(FW,mMean.mFeatM);
    matrixd BB(n,n);
    utils::cholesky_decompose(KK,BB);
    inplace_solve(BB,v0,ublas::lower_tag());
    double zz = inner_prod(v0,v0);

    double lik = 1/(2*mSigma) * zz;
    lik += utils::log_trace(BB);
    return lik;
  }

  void GaussianProcessNormal::precomputePrediction()
  {
    const size_t p = mMean.nFeatures();

    mKF = trans(mMean.mFeatM);
    inplace_solve(mL,mKF,ublas::lower_tag());
    //TODO: make one line
    matrixd DD(p,p);
    DD = prod(trans(mKF),mKF);
    utils::add_to_diagonal(DD,mInvVarW);
    utils::cholesky_decompose(DD,mD);

    vectord vn = mData.mY;
    inplace_solve(mL,vn,ublas::lower_tag());
    mWMap = prod(mMean.mFeatM,vn) + utils::ublas_elementwise_prod(mInvVarW,mW0);
    utils::cholesky_solve(mD,mWMap,ublas::lower());

    mVf = mData.mY - prod(trans(mMean.mFeatM),mWMap);
    inplace_solve(mL,mVf,ublas::lower_tag());

    if (boost::math::isnan(mWMap(0)))
      {
	throw std::runtime_error("Error in precomputed prediction. NaN found.");
      }
  }

} //namespace bayesopt


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * 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 Politecnico di Milano 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 AMA_TENSOR_TENSOR_HPP
#define AMA_TENSOR_TENSOR_HPP 1

#include <ama/tensor/config.hpp>
#include <ama/tensor/copy.hpp>
#include <ama/tensor/detail/tensor_base.hpp>
#include <ama/tensor/iexp/iexp.hpp>
#include <ama/tensor/iexp/iexp_calculator.hpp>
#include <ama/tensor/iexp/iexp_factory.hpp>
#include <ama/tensor/iexp/indices.hpp>
#include <ama/common/size_t.hpp>
#include <ama/multi_array/multi_array.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/size_t.hpp>

namespace ama
{

  template <typename S, /* scalar value used to represent scalar value */
            size_t D,   /* the dimension of vector space               */
            size_t CT,  /* the dimension of controvariant part         */
            size_t CO>  /* the dimension of covariant part             */
  class tensor;

  /* tensor traits specialization */
  namespace tensor_
  {

    template <typename S, size_t D, size_t CT, size_t CO>
    struct tensor_traits< tensor<S,D,CT,CO> >
    {
      typedef S value_type;

      typedef ::boost::mpl::size_t<D> dimension_type;

      typedef ::boost::mpl::size_t<CT> controvariant_type;
      typedef ::boost::mpl::size_t<CO> covariant_type;

      typedef ::boost::mpl::true_ is_assignable;
      typedef ::boost::mpl::false_ is_temporary;
    };

  }

  /* class declaration */
  template <typename S, size_t D, size_t CT, size_t CO>
  class tensor:
        public tensor_::tensor_base< tensor<S,D,CT,CO> >
      , public multi_array<S,D,CT+CO>
#ifdef AMA_MULTI_ARRAY_USE_LINEAR_ACCESS
      , public tensor_::iexp< tensor<S,D,CT,CO>, CT+CO >
#endif /* AMA_MULTI_ARRAY_USE_LINEAR_ACCESS */
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::dimension_type dimension_type;

    /* needed */
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::controvariant_type controvariant_type;
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::covariant_type covariant_type;

  public:
    /* default constructor */
    tensor():
        tensor_::tensor_base< tensor<S,D,CT,CO> >(),
        multi_array<S,D,CT+CO>() { }

    /* copy constructor */
    tensor(tensor const & t):
        tensor_::tensor_base< tensor<S,D,CT,CO> >(),
        multi_array<S,D,CT+CO>(t) { }

    template <typename DERIVED>
    explicit
    tensor(tensor_::tensor_base<DERIVED> const & t):
        tensor_::tensor_base< tensor<S,D,CT,CO> >(),
        multi_array<S,D,CT+CO>()
    {
      ama::copy(t.derived(), *this);
    }

  public:
    /* copy operator */
    template <typename DERIVED>
    tensor & operator=(tensor const & t)
    {
      ama::copy(t.derived(), *this);
      return *this;
    }

    template <typename DERIVED>
    tensor & operator=(tensor_::tensor_base<DERIVED> const & t)
    {
      ama::copy(t.derived(), *this);
      return *this;
    }

#ifdef AMA_MULTI_ARRAY_USE_LINEAR_ACCESS
  public:
    using multi_array<S,D,CT+CO>::operator();
    using tensor_::iexp< tensor<S,D,CT,CO> , CT+CO >::operator();
#endif /* AMA_MULTI_ARRAY_USE_LINEAR_ACCESS */

/* ============================ INDEX EXPRESSION ============================ */
  public:
    /* index expression contruction */
    template <typename ILIST>
    typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::false_>::type
    idx()
    {
      namespace mpl = ::boost::mpl;

      typedef mpl::equal_to<
            mpl::size<typename tensor_::controvariant<tensor<S,D,CT,CO>, ILIST>::type>
          , controvariant_type
          > ct;
      typedef mpl::equal_to<
            mpl::size<typename tensor_::covariant<tensor<S,D,CT,CO>, ILIST>::type>
          , covariant_type
          > co;

      BOOST_MPL_ASSERT_MSG(
            (ct::value && co::value)
          , THE_LENGTH_OF_LIST_OF_INDICES_MUST_BE_EQUAL_TO_THE_TENSOR_ORDER
          , (ILIST));

      typedef typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::false_>::type what;

      return tensor_::iexp_factory<what>::template apply<ILIST>(*this);
    }

    template <typename ILIST>
    typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::true_>::type
    idx() const
    {
      namespace mpl = ::boost::mpl;

      typedef mpl::equal_to<
            mpl::size<typename tensor_::controvariant<tensor<S,D,CT,CO>, ILIST>::type>
          , controvariant_type
          > ct;
      typedef mpl::equal_to<
            mpl::size<typename tensor_::covariant<tensor<S,D,CT,CO>, ILIST>::type>
          , covariant_type
          > co;

      BOOST_MPL_ASSERT_MSG(
            (ct::value && co::value)
          , THE_LENGTH_OF_LIST_OF_INDICES_MUST_BE_EQUAL_TO_THE_TENSOR_ORDER
          , (ILIST));

      typedef typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::true_>::type what;

      return tensor_::iexp_factory<what>::template apply<ILIST>(*this);
    }
/* ============================ INDEX EXPRESSION ============================ */
  };

  /* partial specialization for 0-order tensor */
  template <typename S, size_t D>
  class tensor<S,D,0,0>:
      public tensor_::tensor_base< tensor<S,D,0,0> >,
      public multi_array<S,D,0>
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,D,0,0> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,D,0,0> >::dimension_type dimension_type;

  public:
    /* default constructor */
    explicit
    tensor(S const & value = S()):
        tensor_::tensor_base< tensor<S,D,0,0> >(),
        multi_array<S,D,0>(value) { }

    /* copy constructor */
    tensor(tensor const & t):
        tensor_::tensor_base< tensor<S,D,0,0> >(),
        multi_array<S,D,0>(t) { }

    template <typename DERIVED>
    explicit
    tensor(tensor_::tensor_base<DERIVED> const & t):
        tensor_::tensor_base< tensor<S,D,0,0> >(),
        multi_array<S,D,0>()
    {
      ama::copy(t.derived(), *this);
    }

  public:
    /* copy operator */
    template <typename DERIVED>
    tensor & operator=(tensor_::tensor_base<DERIVED> const & t)
    {
      ama::copy(t.derived(), *this);
      return *this;
    }

  public:
    /* cast operator */
    operator value_type() { return this->template at<void>(); }
    operator value_type() const { return this->template at<void>(); }
  };

  /* partial specialization for 0-dimension tensor */
  template <typename S, size_t CT, size_t CO>
  class tensor<S,0,CT,CO>:
      public tensor_::tensor_base< tensor<S,0,CT,CO> >,
      public multi_array<S,0,CT+CO>
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,0,CT,CO> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,0,CT,CO> >::dimension_type dimension_type;
  };

  /* partial specialization for 0-dimension tensor */
  template <typename S>
  class tensor<S,0,0,0>:
      public tensor_::tensor_base< tensor<S,0,0,0> >,
      public multi_array<S,0,0>
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,0,0,0> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,0,0,0> >::dimension_type dimension_type;
  };

}

#endif /* AMA_TENSOR_TENSOR_HPP */