#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

constexpr double deg_to_rad(double deg) {
	return PI * deg / 180.0;
}

constexpr double rad_to_deg(double rad) {
	return rad * 180 / PI;
}

// 二次元ベクトルクラス
class Vec2d {
	double _x;
	double _y;

	public:
	// 原点の位置ベクトル
	static Vec2d origin() { return Vec2d(0.0, 0.0); }

	// ゼロベクトル
	static Vec2d zero() { return Vec2d::origin(); }

	Vec2d(double a, double b) : _x(a), _y(b) {}

	double x() const { return this->_x; }
	double y() const { return this->_y; }

	// 外積
	double det(const Vec2d& rhs) const {
		return this->x() * rhs.y() - this->y() * rhs.x();
	}

	// 内積
	double dot(const Vec2d& rhs) const {
		return this->x() * rhs.x() + this->y() * rhs.y();
	}

	// 長さ
	double length() const { return this->distance(Vec2d::origin()); }

	// 2つの位置ベクトル間のユークリッド距離
	double distance(const Vec2d& rhs) const {
		return std::sqrt(square(this->x() - rhs.x()) +
						 square(this->y() - rhs.y()));
	}

	// 2つの位置ベクトル間のマンハッタン距離
	double manhattan_distance(const Vec2d& rhs) const {
		return std::abs(this->x() - rhs.x()) + std::abs(this->y() - rhs.y());
	}

	// ベクトルとx軸のなす角
	double argument() const { return std::atan2(this->y(), this->x()); }

	// 反時計回りに回転したベクトル
	Vec2d rotate(double rad) const {
		double xx = this->x();
		double yy = this->y();
		return Vec2d(xx * cos(rad) - yy * sin(rad),
					 xx * sin(rad) + yy * cos(rad));
	}

	// 単位ベクトル
	Vec2d unit() const {
		double len = this->length();
		return Vec2d(this->x() / len, this->y() / len);
	}

	// 法線ベクトル
	Vec2d normal() const {
		double len = this->length();
		return Vec2d(this->y() / len, -this->x() / len);
	}

	// 平行判定
	bool is_parallel(const Vec2d& rhs) const { return false; }

	// ベクトル和
	Vec2d operator+() const { return *this; }
	Vec2d operator+(const Vec2d& rhs) const {
		return Vec2d(this->x() + rhs.x(), this->y() + rhs.y());
	}

	// ベクトル差
	Vec2d operator-() const { return Vec2d(-this->x(), -this->y()); }
	Vec2d operator-(const Vec2d& rhs) const {
		return Vec2d(this->x() - rhs.x(), this->y() - rhs.y());
	}

	// スカラー積
	Vec2d operator*(const double rhs) const {
		return Vec2d(this->x() * rhs, this->y() * rhs);
	}
	Vec2d operator/(const double rhs) const {
		return Vec2d(this->x() / rhs, this->y() / rhs);
	}

	bool operator<(const Vec2d& rhs) const {
		return sign(this->x()) ? this->y() < rhs.y() : this->x() < rhs.x();
	}

	private:
};

int main() {
	int n, x0, y0, xharf, yharf;
	cin >> n;
	cin >> x0 >> y0;
	cin >> xharf >> yharf;

	Vec2d p0 = {x0, y0};
	Vec2d ph = {xharf, yharf};

	Vec2d center = (p0 + ph) / 2;
	Vec2d c_to_p0 = p0 - center;
	Vec2d c_to_pr = c_to_p0.rotate(PI * 2 / n);
	Vec2d result = center + c_to_pr;

	cout << PRECISION(15) << result.x() << " " << result.y() << endl;
	return 0;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/at.hpp>
#include <boost/hana/back.hpp>
#include <boost/hana/core/to.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/sort.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/zip.hpp>

#include <type_traits>
namespace hana = boost::hana;
using namespace hana::literals;

auto indexed_sort = [](auto list, auto predicate) {
    auto indexed_list = hana::zip(
        list,
        hana::to_tuple(hana::make_range(0_c, hana::length(list)))
    );
    auto sorted = hana::sort.by(predicate ^hana::on^ hana::front, indexed_list);
    return hana::make_pair(hana::transform(sorted, hana::front),
                           hana::transform(sorted, hana::back));
};

int main() {
    auto types = hana::tuple_t<char[4], char[2], char[1], char[5], char[3]>;
    auto sorted = indexed_sort(types, [](auto t, auto u) {
        return hana::sizeof_(t) < hana::sizeof_(u);
    });
    using Tup = decltype(
        hana::unpack(hana::first(sorted), hana::template_<hana::tuple>)
    )::type;
    auto indices = hana::second(indexed_sort(hana::second(sorted), hana::less));

    // When accessed through the indices sequence, the tuple appears to be
    // ordered as the `types` above. However, as can be seen in the
    // static_assert below, the tuple is actually ordered differently.
    Tup tup;
    char const(&a)[4] = tup[indices[0_c]];
    char const(&b)[2] = tup[indices[1_c]];
    char const(&c)[1] = tup[indices[2_c]];
    char const(&d)[5] = tup[indices[3_c]];
    char const(&e)[3] = tup[indices[4_c]];

    static_assert(std::is_same<
        Tup,
        hana::tuple<char[1], char[2], char[3], char[4], char[5]>
    >{}, "");

    (void)a; (void)b; (void)c; (void)d; (void)e;
}


#include "MapsMesh.h"

#include <igl/gaussian_curvature.h>
#include <poly2tri.h>
#include <tbb/tbb.h>

#include <Eigen/Dense>
#include <algorithm>
#include <limits>
#include <progressbar/progressbar.hpp>

namespace Maps {

template <class Mesh>
static void OpenMesh2IGL(const Mesh *mesh, Eigen::MatrixX3d &V, Eigen::MatrixX3i &F) {

    assert(mesh->is_trimesh());

    auto verticesCount = std::distance(mesh->vertices_sbegin(), mesh->vertices_end());
    auto facesCount = std::distance(mesh->faces_sbegin(), mesh->faces_end());
    V.resize(verticesCount, 3);
    F.resize(facesCount, 3);

    std::map<typename Mesh::VertexHandle, int> newVertexHandleMap;
    int j = 0;
    for (auto i = mesh->vertices_sbegin(); i != mesh->vertices_end(); i++, j++) {
        auto p = mesh->point(*i);
        V(j, 0) = p[0];
        V(j, 1) = p[1];
        V(j, 2) = p[2];
        newVertexHandleMap[*i] = j;
    }

    j = 0;
    for (auto i = mesh->faces_sbegin(); i != mesh->faces_end(); i++, j++) {
        auto fh = *i;
        int vi = 0;
        for (typename Mesh::ConstFaceVertexCCWIter fvi = mesh->cfv_ccwbegin(fh);
             vi < 3 && fvi != mesh->cfv_ccwend(fh); ++fvi) {
            F(j, vi) = newVertexHandleMap[*fvi];
            vi++;
        }
    }
}

int MapMesh::CDTTrangle(const Coordinate2DPair &coordinates,
                        std::vector<std::array<VertexHandle, 3>> &faces,
                        BaryCoor &barycentricCoordinates) {

    faces.clear();

    std::vector<p2t::Point *> points(coordinates.size());
    for (int i = 0; i < coordinates.size(); i++) {
        double x = coordinates[i].second[0];
        double y = coordinates[i].second[1];

        points[i] = new p2t::Point(x, y);
    }

    p2t::CDT cdt(points);

    try {
        cdt.Triangulate();
    } catch (const std::runtime_error &exception) {
        std::cout << exception.what() << std::endl;
        throw exception;
    }

    auto triangles = cdt.GetTriangles();
    if (!p2t::IsDelaunay(triangles)) {
        return -1;
    }

    int trangleIdx = -1;
    for (int j = 0; j < triangles.size(); j++) {
        auto &triangle = triangles[j];
        std::array<Point2D, 3> triangle2D;
        triangle2D[0] = Point2D(triangle->GetPoint(0)->x, triangle->GetPoint(0)->y);
        triangle2D[1] = Point2D(triangle->GetPoint(1)->x, triangle->GetPoint(1)->y);
        triangle2D[2] = Point2D(triangle->GetPoint(2)->x, triangle->GetPoint(2)->y);

        std::array<VertexHandle, 3> face;
        for (int i = 0; i < 3; i++) {
            auto position = std::find(points.begin(), points.end(), triangle->GetPoint(i));
            if (position == points.end()) return -1;
            face[i] = coordinates[position - points.begin()].first;
        }
        faces.push_back(face);

        /**
         * 计算原点的重心坐标
         */

        if (!IsInTriangle(Point2D(0, 0), triangle2D)) continue;

        auto [alpha, beta, gamma] = CalculateBaryCoor(Point2D(0, 0), triangle2D);
        barycentricCoordinates[0] = std::make_pair(face[0], alpha);
        barycentricCoordinates[1] = std::make_pair(face[1], beta);
        barycentricCoordinates[2] = std::make_pair(face[2], gamma);
        trangleIdx = j;
    }

    for (auto &point : points) {
        delete point;
    }

    return trangleIdx;
}

int MapMesh::MVTTrangle(const Coordinate2DPair &coordinates, int startIdx,
                        std::vector<std::array<VertexHandle, 3>> &faces,
                        BaryCoor &barycentricCoordinates) {
    faces.clear();
    int trangleIdx = -1;
    for (size_t i = 0, ii = 1; i < coordinates.size(); i++, ii++, ii %= coordinates.size()) {
        if (i == startIdx || ii == startIdx) continue;

        std::array<VertexHandle, 3> face(
            {coordinates[i].first, coordinates[ii].first, coordinates[startIdx].first});

        faces.push_back(face);

        std::array<Point2D, 3> triangle;
        triangle[0] = coordinates[startIdx].second;
        triangle[1] = coordinates[i].second;
        triangle[2] = coordinates[ii].second;

        /**
         * 计算原点的重心坐标
         */

        if (!IsInTriangle(Point2D(0, 0), triangle)) continue;

        auto [alpha, beta, gamma] = CalculateBaryCoor(Point2D(0, 0), triangle);
        barycentricCoordinates[0] = std::make_pair(coordinates[startIdx].first, alpha);
        barycentricCoordinates[1] = std::make_pair(coordinates[i].first, beta);
        barycentricCoordinates[2] = std::make_pair(coordinates[ii].first, gamma);

        trangleIdx = static_cast<int>(faces.size() - 1);
    }
    return trangleIdx;
}

std::optional<MapMesh::Point2D> MapMesh::ReCalculate2DCoordinates(
    std::map<VertexHandle, Point2D> &originCoor, VertexHandle deleteVertex) const {
    if (!IsVertexDeleted(deleteVertex)) return std::nullopt;
    auto baryCoor = data(deleteVertex).baryCoor.value();

    return originCoor[baryCoor[0].first] * baryCoor[0].second +
           originCoor[baryCoor[1].first] * baryCoor[1].second +
           originCoor[baryCoor[2].first] * baryCoor[2].second;
}

void MapMesh::ReTrangleAndAddFace(const VertexHandle &deleteVertex) {
    // 记录1领域面中所有包含的点, 并更新这些点的参数
    // 假设1领域面f上记录了一个点v, v中记录了v在f上的重心坐标(a, b, c), 面f在经过2维映射变成了f'
    // 更新过程: 1. 根据f'和(a, b, c)计算v的二维映射点v'
    //           2. 重新计算包含v'的三角形p‘, 并根据p’重新计算坐标(a', b', c')
    //           3. 删除f, 添加面p, 更新v的重心坐标, 建立p与v的关联

    std::vector<FaceHandle> ringFaces(vf_begin(deleteVertex), vf_end(deleteVertex));
    Coordinate2DPair coordinates;
    Calculate2D(deleteVertex, coordinates);

    // 重新计算1领域面上所有包含的点的二维坐标
    std::map<VertexHandle, Point2D> currentPoint2DMap(coordinates.begin(), coordinates.end());
    std::map<VertexHandle, Point2D> deletePoint2DMap;
    for (auto &face : ringFaces) {
        for (auto &vertex : data(face).vertrices) {
            auto newCoor = ReCalculate2DCoordinates(currentPoint2DMap, vertex);
            if (!newCoor.has_value()) break;
            deletePoint2DMap[vertex] = newCoor.value();
        }
    }

    delete_vertex(deleteVertex, false);

    BaryCoor barycentricCoordinates;
    std::vector<std::array<VertexHandle, 3>> newFaces;

    int trangleIdx = CDTTrangle(coordinates, newFaces, barycentricCoordinates);
    std::vector<FaceHandle> facesHandle;
    if (trangleIdx != -1 && TryToAddFaces(newFaces, facesHandle)) {
        data(facesHandle[trangleIdx]).vertrices.push_back(deleteVertex);
        data(deleteVertex).baryCoor = barycentricCoordinates;

        // 判断被删除的点在那个三角形内部, 并计算重心坐标

        for (auto &[vertex, point2D] : deletePoint2DMap) {
            auto newParma = UpdateParam(newFaces, currentPoint2DMap, point2D);
            if (!newParma.has_value()) continue;
            auto [idx, alpha, beta, gamma] = newParma.value();
            data(facesHandle[idx]).vertrices.push_back(vertex);

            BaryCoor baryCoor;
            baryCoor[0].first = newFaces[idx][0], baryCoor[0].second = alpha;
            baryCoor[1].first = newFaces[idx][1], baryCoor[1].second = beta;
            baryCoor[2].first = newFaces[idx][2], baryCoor[2].second = gamma;
            data(vertex).baryCoor = baryCoor;
        }

        return;
    }
    for (int i = 0; i < coordinates.size(); i++) {
        newFaces.clear();
        trangleIdx = MVTTrangle(coordinates, i, newFaces, barycentricCoordinates);
        if (TryToAddFaces(newFaces, facesHandle)) {

            data(facesHandle[trangleIdx]).vertrices.push_back(deleteVertex);
            data(deleteVertex).baryCoor = barycentricCoordinates;

            for (auto &[vertex, point2D] : deletePoint2DMap) {
                auto newParma = UpdateParam(newFaces, currentPoint2DMap, point2D);
                if (!newParma.has_value()) continue;
                auto [idx, alpha, beta, gamma] = newParma.value();
                data(facesHandle[idx]).vertrices.push_back(vertex);

                BaryCoor baryCoor;
                baryCoor[0].first = newFaces[idx][0], baryCoor[0].second = alpha;
                baryCoor[1].first = newFaces[idx][1], baryCoor[1].second = beta;
                baryCoor[2].first = newFaces[idx][2], baryCoor[2].second = gamma;
                data(vertex).baryCoor = baryCoor;
            }
            return;
        }
    }
    throw std::runtime_error("can't add face");
}

void MapMesh::Initialize() {
    originFaces = std::vector<FaceHandle>(faces_sbegin(), faces_end());
    for (const auto &face : originFaces) {
        originFaceVertices[face] = std::vector<VertexHandle>(fv_begin(face), fv_end(face));
    }
}

void MapMesh::CalculateCurvature() {
    Eigen::MatrixX3d V;
    Eigen::MatrixX3i F;
    Eigen::VectorXd K;

    OpenMesh2IGL(this, V, F);
    igl::gaussian_curvature(V, F, K);

    int j = 0;
    maxCurvature = (std::numeric_limits<double>::min)();
    for (auto i = vertices_sbegin(); i != vertices_end(); i++, j++) {
        data(*i).curvature = K[j];
        if (K[j] > maxCurvature) {
            maxCurvature = K[j];
        }
    }
}

void MapMesh::CalculateWeight(double lambda) {
    while (!curvatureQueue.empty()) {
        curvatureQueue.pop();
    }

    for (auto i = vertices_sbegin(); i != vertices_end(); i++) {
        data(*i).weight = lambda * data(*i).curvature / maxCurvature +
                          (1 - lambda) * data(*i).ringArea / maxRingArea;
        curvatureQueue.push(*i);
    }
}

void MapMesh::CalculateAreas() {
    maxRingArea = (std::numeric_limits<double>::min)();
    for (auto faceIter = faces_sbegin(); faceIter != faces_end(); faceIter++) {
        double area = CalculateArea(*faceIter);
        data(*faceIter).area = area;
    }

    for (auto vertexIter = vertices_sbegin(); vertexIter != vertices_end(); vertexIter++) {
        double ringArea = 0;
        for (auto faceIter = vf_begin(*vertexIter); faceIter != vf_end(*vertexIter); faceIter++) {
            ringArea += data(*faceIter).area;
        }
        if (ringArea > maxRingArea) {
            maxRingArea = ringArea;
        }
        data(*vertexIter).ringArea = ringArea;
    }
}

void MapMesh::MapFaceFromOriginMesh(const FaceHandle &face, std::array<Point, 3> &mapFace) const {
    if (originFaceVertices.find(face) == originFaceVertices.end()) return;

    const std::vector<VertexHandle> &vertices = originFaceVertices.at(face);
    assert(vertices.size() == 3);
    for (int i = 0; i < 3; i++) {
        auto vertexHandle = vertices[i];
        if (!IsVertexDeleted(vertexHandle)) {
            mapFace[i] = point(vertexHandle);
            continue;
        }

        auto baryCoor = data(vertexHandle).baryCoor.value();
        mapFace[i] = point(baryCoor[0].first) * baryCoor[0].second +
                     point(baryCoor[1].first) * baryCoor[1].second +
                     point(baryCoor[2].first) * baryCoor[2].second;
    }
}

void MapMesh::FaceSubDivision() {
    std::vector<FaceHandle> originFaceHandle(faces_sbegin(), faces_end());

    for (const auto &faceHandle : originFaceHandle) {
        std::vector<VertexHandle> vertices(fv_begin(faceHandle), fv_end(faceHandle));
        assert(vertices.size() == 3);

        for (int i = 0, ii = 1; i < 3; i++, ii++, ii %= 3) {
            auto newPoint = (point(vertices[i]) + point(vertices[ii])) / 2.0;
            auto newPointHandle = add_vertex(newPoint);
            data(newPointHandle).isNew = true;
            vertices.push_back(newPointHandle);
        }
        delete_face(faceHandle);
        add_face({vertices[0], vertices[3], vertices[5]});
        add_face({vertices[3], vertices[1], vertices[4]});
        add_face({vertices[5], vertices[4], vertices[2]});
        add_face({vertices[3], vertices[4], vertices[5]});
    }
}

void MapMesh::DownSampling() {
    request_face_status();
    request_vertex_status();
    request_edge_status();

    for (auto vertexIter = vertices_sbegin(); vertexIter != vertices_end(); vertexIter++) {
        data(*vertexIter).canBeDeleted = true;
    }

    CalculateCurvature();
    CalculateAreas();
    CalculateWeight(0.5);
    while (!curvatureQueue.empty()) {
        auto vertexHandle = curvatureQueue.top();
        curvatureQueue.pop();
        if (!data(vertexHandle).canBeDeleted) continue;

        for (auto ringVertex = vv_begin(vertexHandle); ringVertex != vv_end(vertexHandle);
             ringVertex++) {
            data(*ringVertex).canBeDeleted = false;
        }

        ReTrangleAndAddFace(vertexHandle);
    }
    SaveBaseLevelMesh();
}

void MapMesh::Remesh() {

    int N = static_cast<int>(std::distance(vertices_sbegin(), vertices_end()));
    progressbar bar(N);

    std::mutex _mutex;

    auto IsMapedInASingleFace = [&](const FaceHandle &faceHandle) -> bool {
        std::set<VertexHandle> verticesHandle;
        for (const auto &vertexHandle : originFaceVertices[faceHandle]) {
            if (data(vertexHandle).baryCoor.has_value()) {
                auto baryCoor = data(vertexHandle).baryCoor.value();
                verticesHandle.insert(baryCoor[0].first);
                verticesHandle.insert(baryCoor[1].first);
                verticesHandle.insert(baryCoor[2].first);
            } else {
                verticesHandle.insert(vertexHandle);
            }
        }

        return verticesHandle.size() == 3;
    };

    auto findMaxCommonVertex = [&](const std::vector<FaceHandle> &facesHandle) {
        std::multiset<VertexHandle> verticesHandle;
        for (const auto &faceHandle : facesHandle) {
            for (const auto &vertexHandle : baseLevelMesh->fv_range(faceHandle)) {
                verticesHandle.insert(vertexHandle);
            }
        }

        VertexHandle maxCommonVertex;
        int maxCount = -1;
        for (const auto &vertex : verticesHandle) {
            int count = static_cast<int>(verticesHandle.count(vertex));
            if (count > maxCount) {
                maxCount = static_cast<int>(verticesHandle.count(vertex));
                maxCommonVertex = vertex;
            }
        }

        return maxCommonVertex;
    };

    auto isRingFace = [&](const VertexHandle &vertexHandle, const FaceHandle &faceHandle) {
        for (const auto &face : baseLevelMesh->vf_range(vertexHandle)) {
            if (face == faceHandle) return true;
        }
        return false;
    };

    auto isRingPoint =
        [&](const VertexHandle &vertexHandle, const std::vector<VertexHandle> &ringVertices) {
        for (const auto &ringVertex : ringVertices) {
            bool found = false;
            for (const auto &realRingVertex : baseLevelMesh->vv_range(vertexHandle)) {
                if (realRingVertex == ringVertex) {
                    found = true;
                }
            }
            if (!found) return false;
        }
        return true;
    };

    oneapi::tbb::parallel_for_each(vertices_sbegin(), vertices_end(),
                                   [&](const VertexHandle &vertex) {
        if (!data(vertex).isNew) {
            _mutex.lock();
            bar.update();
            _mutex.unlock();
            /* continue; */
            return;
        }

        // 在基面上找到公共顶点，并展开到2维平面计算重心坐标
        auto vertexFaceOption = baseLevelMesh->FindFace(point(vertex));
        if (!vertexFaceOption.has_value()) {
            _mutex.lock();
            bar.update();
            _mutex.unlock();
            /* continue; */
            return;
        }
        auto vertexFace = vertexFaceOption.value();

        for (const auto &face : originFaces) {
            std::array<Point, 3> mapFace;
            const std::vector<VertexHandle> &fv = originFaceVertices[face];

            if (IsMapedInASingleFace(face)) {  // 原面能够完全映射到基面上
                MapFaceFromOriginMesh(face, mapFace);
                if (IsInTriangle(point(vertex), mapFace)) {
                    auto [alpha, beta, gamma] = CalculateBaryCoor(point(vertex), mapFace);
                    Point newPoint =
                        alpha * point(fv[0]) + beta * point(fv[1]) + gamma * point(fv[2]);
                    point(vertex) = newPoint;
                    break;
                }
            } else {

                std::vector<FaceHandle> baseFaces;
                std::vector<VertexHandle> fixVertex;
                for (const auto &vertexHandle : fv) {
                    if (!IsVertexDeleted(vertexHandle)) {
                        fixVertex.push_back(vertexHandle);
                        continue;
                    }

                    auto baryCoor = data(vertexHandle).baryCoor.value();
                    auto face = baseLevelMesh->FindFace(
                        {baryCoor[0].first, baryCoor[1].first, baryCoor[2].first});

                    if (face.has_value()) {
                        baseFaces.push_back(face.value());
                    }
                }

                VertexHandle commonVertex;
                if (baseFaces.size() == 3) {
                    if (std::find(baseFaces.begin(), baseFaces.end(), vertexFace) ==
                        baseFaces.end()) {
                        continue;
                    }
                    commonVertex = findMaxCommonVertex(baseFaces);
                } else {
                    bool foundCommonVertex = false;
                    for (const auto &baseFace : baseFaces) {
                        for (const auto &baseVert : baseLevelMesh->fv_range(baseFace)) {
                            if (isRingFace(baseVert, vertexFace) &&
                                isRingPoint(baseVert, fixVertex)) {
                                commonVertex = baseVert;
                                foundCommonVertex = true;
                                break;
                            }
                        }
                        if (foundCommonVertex) break;
                    }
                    if (!foundCommonVertex) continue;
                }

                Coordinate2DPair coorPair;
                baseLevelMesh->Calculate2D(commonVertex, coorPair);

                Coordinate2DMap coorMap(coorPair.begin(), coorPair.end());
                Point2D vertex2D(0, 0);
                auto vertex2DBaryCoor = baseLevelMesh->CalculateBaryCoor(point(vertex), vertexFace);
                for (const auto &vertex2DBaryCoorItem : vertex2DBaryCoor) {
                    if (coorMap.find(vertex2DBaryCoorItem.first) == coorMap.end()) {
                        continue;
                    }
                    vertex2D +=
                        coorMap.at(vertex2DBaryCoorItem.first) * vertex2DBaryCoorItem.second;
                }

                std::array<std::pair<VertexHandle, Point2D>, 3> face2D;
                bool canMapTo2D = true;
                for (int i = 0; i < 3; i++) {
                    face2D[i].first = fv[i];
                    face2D[i].second = Point2D(0, 0);
                    if (!IsVertexDeleted(fv[i])) {
                        face2D[i].second = coorMap[fv[i]];
                        continue;
                    }

                    auto baryCoor = data(fv[i]).baryCoor.value();
                    for (const auto &baryCoorItem : baryCoor) {
                        if (baryCoorItem.first == commonVertex) continue;
                        if (coorMap.find(baryCoorItem.first) == coorMap.end()) {
                            canMapTo2D = false;
                            break;
                            /* throw std::runtime_error("can't found item"); */
                        }
                        face2D[i].second += coorMap.at(baryCoorItem.first) * baryCoorItem.second;
                    }
                }
                if (!canMapTo2D) continue;

                std::array<Point2D, 3> triangle{face2D[0].second, face2D[1].second,
                                                face2D[2].second};

                if (!IsInTriangle(vertex2D, triangle)) continue;

                auto [alpha, beta, gamma] = CalculateBaryCoor(vertex2D, triangle);

                point(vertex) = alpha * point(face2D[0].first) + beta * point(face2D[1].first) +
                                gamma * point(face2D[2].first);
                break;
            }
        }
        _mutex.lock();
        bar.update();
        _mutex.unlock();
    });
}

std::optional<std::tuple<int, double, double, double>> MapMesh::UpdateParam(
    const std::vector<std::array<VertexHandle, 3>> &faces,
    const std::map<VertexHandle, Point2D> &point2DMap, const Point2D &point) {

    for (int i = 0; i < faces.size(); i++) {
        const auto &face = faces[i];
        std::array<Point2D, 3> triangle;
        triangle[0] = point2DMap.at(face[0]);
        triangle[1] = point2DMap.at(face[1]);
        triangle[2] = point2DMap.at(face[2]);
        if (!IsInTriangle(point, triangle)) continue;

        auto [alpha, beta, gamma] = CalculateBaryCoor(point, triangle);
        return std::make_tuple(i, alpha, beta, gamma);
    }

    return std::nullopt;
}

void MapMesh::Calculate2D(VertexHandle vertex, Coordinate2DPair &coordinates) const {
    coordinates.clear();
    auto vertex3D = point(vertex);

    double K_i = 0;
    for (const auto &face : vf_range(vertex)) {
        K_i += CalculateAngle(vertex, face);
    }
    double a = 2 * M_PI / K_i;

    K_i = 0;

    VertexHandle lastVertex;
    int i = 0;
    for (const auto &ringPoint : vv_range(vertex)) {
        if (i != 0) {
            auto Vec1 = point(lastVertex) - point(vertex);
            auto Vec2 = point(ringPoint) - point(vertex);

            K_i += std::acos(Vec1.normalized().dot(Vec2.normalized()));
        }
        auto point3D = point(ringPoint);
        double r_k = (point3D - vertex3D).norm();

        Point2D point2D{std::pow(r_k, a) * std::cos(K_i * a), std::pow(r_k, a) * std::sin(K_i * a)};

        coordinates.emplace_back(ringPoint, point2D);
        lastVertex = ringPoint;

        i++;
    }
}

}  // namespace Maps


//==============================================================================
//          Copyright 2015 J.T.  Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_SWAPBYTES_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_SWAPBYTES_HPP_INCLUDED

#include <boost/simd/bitwise/functions/swapbytes.hpp>
#include <boost/simd/include/functions/shuffle.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/sdk/simd/native.hpp>

namespace boost { namespace simd
{
  namespace details
  {
    template<std::size_t N> struct swap_bytes_helper
    {
      template<typename I, typename C>
      struct apply : boost::mpl::int_<N*(I::value/N)+N-1-I::value%N>
      {};
    };
  }
} }

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT( swapbytes_,  tag::simd_
                          , (A0)(X)
                          , ((simd_<arithmetic_<A0>, X>))
                          )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      using s0        = typename meta::scalar_of<A0>::type;
      using i8        = typename meta::vector_of<uint8_t, sizeof(A0)>::type;
      using pattern_t = details::swap_bytes_helper<sizeof(s0)>;

      return bitwise_cast<A0>(shuffle<pattern_t>(bitwise_cast<i8>(a0)));
    }
  };
} } }

#endif


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

#include <algorithm>
#include <string>

#include <image/image_util_impl.hpp>
#include <sframe/sframe_iterators.hpp>
#include <sframe/sframe.hpp>
#include <unity/lib/unity_sframe.hpp>
#include <fileio/sanitize_url.hpp>
#include <sframe_query_engine/util/aggregates.hpp>
#include <unity/lib/toolkit_function_macros.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem/path.hpp>

namespace turi{

namespace image_util{

  /**
  * Return flex_vec flexible type that is sum of all images with data in vector form.
  */

  flexible_type sum(std::shared_ptr<unity_sarray> unity_data){
    log_func_entry();

    if (unity_data->size() > 0){
      bool failure = false;
      size_t reference_size;
      size_t failure_size;
      auto reductionfn =
        [&failure, &reference_size, &failure_size]
        (const flexible_type& in, std::pair<bool, flexible_type>& sum)->bool {
          if (in.get_type() != flex_type_enum::UNDEFINED) {
            flexible_type tmp_img = in;
            image_util_detail::decode_image_impl(tmp_img.mutable_get<flex_image>());
            flexible_type f(flex_type_enum::VECTOR);
            f.soft_assign(tmp_img);
            if (sum.first == false) {
              // initial val
              sum.first = true;
              sum.second = f;
            } else if (sum.second.size() == f.size()) {
              // accumulation
              sum.second += f;
            } else {
              // length mismatch. fail
              failure = true;
              reference_size = sum.second.size();
              failure_size = f.size();
              return false;
            }
          }
          return true;
        };

      auto combinefn =
        [&failure, &reference_size, &failure_size]
        (const std::pair<bool, flexible_type>& f,
         std::pair<bool, flexible_type>& sum)->bool {
          if (sum.first == false) {
            // initial state
            sum = f;
          } else if (f.first == false) {
            // there is no f to add.
            return true;
          } else if (sum.second.size() == f.second.size()) {
            // accumulation
            sum.second += f.second;
          } else {
            // length mismatch
            failure = true;
            reference_size = sum.second.size();
            failure_size = f.second.size();
            return false;
          }
          return true;
        };
      std::pair<bool, flexible_type> start_val{false, flex_vec()};
      std::pair<bool, flexible_type> sum_val =
        query_eval::reduce<std::pair<bool, flexible_type>>(unity_data->get_planner_node(),
                                                           reductionfn,
                                                           combinefn,
                                                           start_val);

      if(failure){
        std::string error =
            std::string("Cannot perform sum or average over images of different sizes. ")
            + std::string("Found images of total size (ie. width * height * channels) ")
            + std::string(" of both ") + std::to_string(reference_size)
            + std::string( "and ") + std::to_string(failure_size)
            + std::string(". Please use graplab.image_analysis.resize() to make images a uniform")
            + std::string(" size.");
        log_and_throw(error);
      }
      return sum_val.second;
    }

    log_and_throw("Input image sarray is empty");
    __builtin_unreachable();
  }

/**
 * Construct an image of mean pixel values from a pointer to a unity_sarray.
 */

flexible_type generate_mean(std::shared_ptr<unity_sarray> unity_data) {
  log_func_entry();

  std::vector<flexible_type> sample_img = unity_data->_head(1);
  flex_image meta_img;
  meta_img = sample_img[0];
  size_t channels = meta_img.m_channels;
  size_t height = meta_img.m_height;
  size_t width = meta_img.m_width;
  size_t image_size = width * height * channels;
  size_t num_images = unity_data->size();

  //Perform sum
  flexible_type mean = sum(unity_data);

  //Divide for mean images.
  mean /= num_images;

  flexible_type ret;
  flex_image img;
  char* data_bytes = new char[image_size];
  for(size_t i = 0; i < image_size; ++i){
    data_bytes[i] = static_cast<unsigned char>(mean[i]);
  }
  img.m_image_data_size = image_size;
  img.m_channels = channels;
  img.m_height = height;
  img.m_width = width;
  img.m_image_data.reset(data_bytes);
  img.m_version = IMAGE_TYPE_CURRENT_VERSION;
  img.m_format = Format::RAW_ARRAY;
  ret = img;
  return ret;
}

/**
 * Construct a single image from url, and format hint.
 */
flexible_type load_image(const std::string& url, const std::string format) {
  flexible_type ret(flex_type_enum::IMAGE);
  ret = read_image(url, format);
  return ret;
};

namespace {

size_t load_images_impl(std::vector<std::string>& all_files,
                        sarray<flexible_type>::iterator& image_iter,
                        sarray<flexible_type>::iterator& path_iter,
                        const std::string& format,
                        bool with_path, bool ignore_failure, size_t thread_id) {
  timer mytimer;

  atomic<size_t> cnt = 0;
  double previous_time = 0;
  int previous_cnt = 0;
  bool cancel = false;

  auto iter = all_files.begin();
  auto end = all_files.end();
  while(iter < end && !cancel) {
    flexible_type img(flex_type_enum::IMAGE);
    // read a single image
    try {
      img = read_image(*iter, format);
      *image_iter = img;
      ++image_iter;
      if (with_path) {
        *path_iter = *iter;
        ++path_iter;
      }
      ++cnt;
    } catch (std::string error) {
      logprogress_stream << error << "\t" << " file: " << sanitize_url(*iter) << std::endl;
      if (!ignore_failure)
        throw;
    } catch (const char* error) {
      logprogress_stream << error << "\t" << " file: " << sanitize_url(*iter) << std::endl;
      if (!ignore_failure)
        throw;
    } catch (...) {
      logprogress_stream << "Unknown error reading image \t file: " << sanitize_url(*iter) << std::endl;
      if (!ignore_failure)
        throw;
    }
    ++iter;
    // output progress
    if (thread_id == 0) {
      double current_time = mytimer.current_time();
      size_t current_cnt = cnt;
      if (current_time - previous_time > 5) {
        logprogress_stream << "Read " << current_cnt << " images in " << current_time << " secs\t"
          << "speed: " << double(current_cnt - previous_cnt) / (current_time - previous_time)
          << " file/sec" << std::endl;
        previous_time = current_time;
        previous_cnt = current_cnt;
      }
    }
    // check for user interrupt ctrl-C
    if (cppipc::must_cancel()) {
      cancel = true;
    }
  } // end of while

  if (cancel)  {
    log_and_throw("Cancelled by user");
  }
  return cnt;
}

std::vector<std::string> get_directory_files(std::string url, bool recursive) {
  typedef std::vector<std::pair<std::string, fileio::file_status>> path_status_vec_t;
  path_status_vec_t path_status_vec = fileio::get_directory_listing(url);
  std::vector<std::string> ret;
  for (const auto& path_status : path_status_vec) {
    if (recursive && path_status.second == fileio::file_status::DIRECTORY) {
      auto tmp = get_directory_files(path_status.first, recursive);
      ret.insert(ret.end(), tmp.begin(), tmp.end());
    } else if (path_status.second == fileio::file_status::REGULAR_FILE){
      ret.push_back(path_status.first);
    }
  }
  return ret;
}

bool lacks_image_extension(const std::string& url) {

  // Return true unless the url ends with any of these strings.
  const std::initializer_list<const char*> extensions =
      {".jpg", ".jpeg", ".png"};

  // Define a predicate (over extensions) that performs case-insensitive
  // matching against the url.
  auto ends_url = [&url](const char* extension) {
    return boost::algorithm::iends_with(url, extension);
  };

  return std::none_of(extensions.begin(), extensions.end(), ends_url);
}

}  // namespace

/**
 * Construct an sframe of flex_images, with url pointing to directory where images reside.
 */
std::shared_ptr<unity_sframe> load_images(std::string url, std::string format, bool with_path, bool recursive,
                                          bool ignore_failure, bool random_order) {
    log_func_entry();

    std::vector<std::string> all_files;

    // See what's at the user-provided location.
    switch (fileio::get_file_status(url)) {

    case fileio::file_status::MISSING:
      log_and_throw_io_failure(sanitize_url(url) + " not found.");

    case fileio::file_status::REGULAR_FILE:
      all_files.push_back(url);
      break;

    case fileio::file_status::DIRECTORY:
      all_files = get_directory_files(url, recursive);
      if (format != "JPG" && format != "PNG") {
        // We will deduce file formats from file extensions. Prune the list of
        // files to those supported.
        auto first_to_remove = std::remove_if(
            all_files.begin(), all_files.end(), lacks_image_extension);
        if (first_to_remove == all_files.begin() && !all_files.empty()) {
          logprogress_stream << "Directory " << sanitize_url(url)
                             << " does not contain any files with supported"
                             << " image extensions: .jpg .jpeg .png"
                             << std::endl;
        }
        all_files.erase(first_to_remove, all_files.end());
      }
      break;

    case fileio::file_status::FS_UNAVAILABLE:
      log_and_throw_io_failure("Error getting file system status for "
                               + sanitize_url(url));
    }

    std::vector<std::string> column_names;
    std::vector<flex_type_enum> column_types;

    sframe image_sframe;
    auto path_sarray = std::make_shared<sarray<flexible_type>>();
    auto image_sarray = std::make_shared<sarray<flexible_type>>();

    // Parallel read dioes not seems to help, and it slow IO down when there is only one disk.
    // We can expose this option in the future for parallel disk IO or RAID.
    // size_t num_threads = thread_pool::get_instance().size();
    size_t num_threads = 1;
    column_names = {"path","image"};
    path_sarray->open_for_write(num_threads + 1); // open one more segment for appending recursive results
    image_sarray->open_for_write(num_threads + 1); // ditto
    path_sarray->set_type(flex_type_enum::STRING);
    image_sarray->set_type(flex_type_enum::IMAGE);

    if (random_order) {
      std::random_shuffle(all_files.begin(), all_files.end());
    } else {
      std::sort(all_files.begin(), all_files.end());
    }

    size_t files_per_thread = all_files.size() / num_threads;
    parallel_for(0, num_threads, [&](size_t thread_id) {
      auto path_iter = path_sarray->get_output_iterator(thread_id);
      auto image_iter = image_sarray->get_output_iterator(thread_id);

      size_t begin = files_per_thread * thread_id;
      size_t end = (thread_id + 1) == num_threads ? all_files.size() : begin + files_per_thread;

      std::vector<std::string> subset(all_files.begin() + begin, all_files.begin() + end);
      load_images_impl(subset, image_iter, path_iter, format, with_path, ignore_failure, thread_id);
    });

    image_sarray->close();
    path_sarray->close();

    if (with_path){
      std::vector<std::shared_ptr<sarray<flexible_type> > > sframe_columns {path_sarray, image_sarray};
      image_sframe = sframe(sframe_columns, {"path","image"});
    } else {
      std::vector<std::shared_ptr<sarray<flexible_type> > > sframe_columns {image_sarray};
      image_sframe = sframe(sframe_columns, {"image"});
    }

    std::shared_ptr<unity_sframe> image_unity_sframe(new unity_sframe());
    image_unity_sframe->construct_from_sframe(image_sframe);

    return image_unity_sframe;
}

/**
 * Decode the image into raw pixels
 */
flexible_type decode_image(const flexible_type& image) {
  if (image.get<flex_image>().is_decoded()) {
    return image;
  }
  flexible_type ret = image;
  flex_image& img = ret.mutable_get<flex_image>();
  turi::decode_image_inplace(img);
  return ret;
};

/**
 * Encode the image into compressed format.(losslessly)
 */
flexible_type encode_image(const flexible_type& image) {
  if (!image.get<flex_image>().is_decoded()) {
    return image;
  }
  flexible_type ret = image;
  flex_image& img = ret.mutable_get<flex_image>();
  turi::encode_image_inplace(img);
  return ret;
};

/**
 * Decode an sarray of flex_images into raw pixels
 */
std::shared_ptr<unity_sarray> decode_image_sarray(std::shared_ptr<unity_sarray> image_sarray) {
  auto fn = [](const flexible_type& f)->flexible_type {
              return decode_image(f);
            };
  auto ret = image_sarray->transform_lambda(fn, flex_type_enum::IMAGE, true, 0);
  return std::static_pointer_cast<unity_sarray>(ret);
};

/**
 * Reisze an sarray of flex_images with the new size.
 */
flexible_type resize_image(const flexible_type& input, size_t resized_width,
			   size_t resized_height, size_t resized_channels,
			   bool decode, int resample_method) {
  if (input.get_type() != flex_type_enum::IMAGE){
    std::string error = "Cannot resize non-image type";
    log_and_throw(error);
  }
  flex_image image = input.get<flex_image>();
  auto has_desired_size = [&] {
    return image.m_width == resized_width && image.m_height == resized_height && image.m_channels == resized_channels;
  };

  // Is this resize a no-op?
  if (has_desired_size() && image.is_decoded() == decode) {
    return input;
  }

  // Decode if necessary.
  if (!image.is_decoded()) {
    image_util_detail::decode_image_impl(image);
  }

  // Resize if necessary.
  if (!has_desired_size()) {
    char* resized_data;
    image_util_detail::resize_image_impl(
        reinterpret_cast<const char*>(image.get_image_data()),
	image.m_width, image.m_height, image.m_channels,
	resized_width, resized_height, resized_channels,
	&resized_data, resample_method);
    image.m_width = resized_width;
    image.m_height = resized_height;
    image.m_channels = resized_channels;
    image.m_format = Format::RAW_ARRAY;
    image.m_image_data_size = resized_height * resized_width * resized_channels;
    image.m_image_data.reset(resized_data);
  }

  // Encode if necessary.
  if (!decode) {
    image_util_detail::encode_image_impl(image);
  }

  return image;
};

/**
 * Resize an sarray of flex_image with the new size.
 */
std::shared_ptr<unity_sarray> resize_image_sarray(
    std::shared_ptr<unity_sarray> image_sarray,
    size_t resized_width,
    size_t resized_height,
    size_t resized_channels,
    bool decode,
    int resample_method) {
  log_func_entry();
  auto fn = [=](const flexible_type& f)->flexible_type {
      return flexible_type(resize_image(f, resized_width, resized_height, resized_channels, decode, resample_method));
    };
  auto ret = image_sarray->transform_lambda(fn, flex_type_enum::IMAGE, true, 0);
  return std::static_pointer_cast<unity_sarray>(ret);
};

/**
 * Convert sarray of image data to sarray of vector
 */
std::shared_ptr<unity_sarray> image_sarray_to_vector_sarray(
    std::shared_ptr<unity_sarray> image_sarray,
    bool undefined_on_failure) {
  // decoded_image_sarray
  log_func_entry();

  // transform the array with type casting
  auto fn = [=](const flexible_type& f)->flexible_type {
              flexible_type ret(flex_type_enum::VECTOR);
              flex_image tmp_img = f;
              image_util_detail::decode_image_impl(tmp_img);
              try {
                ret = tmp_img;
              } catch (...) {
                if (undefined_on_failure) {
                  ret = FLEX_UNDEFINED;
                } else {
                  throw;
                }
              }
              return ret;
            };
  auto ret = image_sarray->transform_lambda(fn, flex_type_enum::VECTOR, true, 0);
  return std::static_pointer_cast<unity_sarray>(ret);
};

/**
 * Convert sarray of vector to sarray of image
 */
std::shared_ptr<unity_sarray> vector_sarray_to_image_sarray(
    std::shared_ptr<unity_sarray> image_sarray,
    size_t width, size_t height, size_t channels,
    bool undefined_on_failure) {
  log_func_entry();
  size_t expected_array_size = height * width * channels;
  auto transformfn =
      [height,width,channels,
      expected_array_size,undefined_on_failure](const flex_vec& vec)->flexible_type {
        try {
          if (expected_array_size != vec.size()) {
            logprogress_stream << "Dimensions do not match vec size" << std::endl;
            log_and_throw("Dimensions do not match vec size");
          }
          flexible_type ret;
          flex_image img;
          size_t data_size = vec.size();
          char* data = new char[data_size];
          for (size_t i = 0; i < data_size; ++i){
            data[i] = static_cast<char>(vec[i]);
          }
          img.m_image_data_size = data_size;
          img.m_image_data.reset(data);
          img.m_height = height;
          img.m_width = width;
          img.m_channels = channels;
          img.m_format = Format::RAW_ARRAY;
          img.m_version = 0;
          ret = img;
          return ret;
        } catch (...) {
          if (undefined_on_failure) {
            return FLEX_UNDEFINED;
          } else {
            throw;
          }
        }
      };

  auto ret = image_sarray->transform_lambda(transformfn,
                                            flex_type_enum::IMAGE, true, 0);
  return std::static_pointer_cast<unity_sarray>(ret);
};

} //namespace image_util
} //namespace turi


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/bitwise/include/functions/popcnt.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/sdk/config.hpp>

NT2_TEST_CASE_TPL ( popcnt_real,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::popcnt;
  using boost::simd::tag::popcnt_;
  typedef typename boost::dispatch::meta::call<popcnt_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(popcnt(boost::simd::Nan<T>()), sizeof(T)*8);
#endif
  NT2_TEST_EQUAL(popcnt(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( popcnt_integer,  BOOST_SIMD_INTEGRAL_TYPES)
{

  using boost::simd::popcnt;
  using boost::simd::tag::popcnt_;
  typedef typename boost::dispatch::meta::call<popcnt_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(popcnt(T(2)), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(popcnt(T(3)), boost::simd::Two<r_t>());
  NT2_TEST_EQUAL(popcnt(boost::simd::Mone<T>()), sizeof(T)*8);
  NT2_TEST_EQUAL(popcnt(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(popcnt(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
} // end of test for integer_


#include <cxxstd/iostream.h>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl; using mtl::iall;

//
// we feed the beast with a const reference
//
void
beast(const dense2D<double> &A)
{
    dense2D<double>   B = sub_matrix(A, 0, 5, 0, 5);
    B[1][1] = 666;
}

int main(int, char**)
{
    using namespace mtl; using mtl::iall;

    const int         m = 5, n = 5;
    dense2D<double>   A(n, n);

    for (int i=0; i<m; ++i) {
        for (int j=0; j<n; ++j) {
            A[i][j] = i+j;
        }
    }

    std::cout << "before: A is\n" << A << "\n";
//
//  calling the beast
//
    beast(A);
    std::cout << "after: A is\n" << A << "\n";

    return 0;
}


#include "SummaryElementMatrixCollector.hpp"
#include <boost/detail/limits.hpp>
#include "application/output/recordsets/ResultRecordset.hpp"
#include "domain/algorithms/messages/ModelStateUpdateMessage.hpp"
#include "domain/analyses/NumericalModel.hpp"
#include "domain/collections/ElementSet.hpp"

namespace aaocs = axis::application::output::collectors::summarizers;
namespace aaor = axis::application::output::recordsets;
namespace ada = axis::domain::analyses;
namespace adc = axis::domain::collections;
namespace adam = axis::domain::algorithms::messages;
namespace ade = axis::domain::elements;
namespace asmm = axis::services::messaging;
namespace afb = axis::foundation::blas;

aaocs::SummaryElementMatrixCollector::SummaryElementMatrixCollector(
  const axis::String& targetSetName, SummaryType summaryType, int rowCount,
  int colCount ) : targetSetName_(targetSetName), summaryType_(summaryType)
{
  rowCount_ = rowCount; colCount_ = colCount;
}

aaocs::SummaryElementMatrixCollector::~SummaryElementMatrixCollector( void )
{
  // nothing to do here
}

bool aaocs::SummaryElementMatrixCollector::IsOfInterest(
  const asmm::ResultMessage& message ) const
{
  return adam::ModelStateUpdateMessage::IsOfKind(message);
}

void aaocs::SummaryElementMatrixCollector::Collect(
  const asmm::ResultMessage& message, aaor::ResultRecordset& recordset,
  const ada::NumericalModel& numericalModel )
{
  adc::ElementSet& set = targetSetName_.empty()? numericalModel.Elements() :
    numericalModel.GetElementSet(targetSetName_);
  size_type count = set.Count();
  afb::DenseMatrix m(rowCount_, colCount_);

  StartCollect(m);
  for (size_type i = 0; i < count; i++)
  {
    const ade::FiniteElement& e = set.GetByPosition(i);
    switch (summaryType_)
    {
    case kAverage:
    case kSum:
      {
        auto& r = CollectMatrix(message, e);
        m += r;
      }
      break;
    case kMaximum:
      {
        real norm = CalculateMatrixNorm(message, e);
        if (norm > bestNormValue_)
        {
          targetIdToCollect_ = i;
          bestNormValue_ = norm;
        }
      }
      break;
    case kMinimum:
      {
        real norm = CalculateMatrixNorm(message, e);
        if (norm < bestNormValue_)
        {
          targetIdToCollect_ = i;
          bestNormValue_ = norm;
        }
      }
      break;
    }
  }

  switch (summaryType_)
  {
  case kAverage:
    m.Scale(1.0 / (real) count);
    break;
  case kMaximum:
  case kMinimum:
    ade::FiniteElement& e = set.GetByPosition(targetIdToCollect_);
    auto& r = CollectMatrix(message, e);
    m = r;
    break;
  }

  recordset.WriteData(m);
}

axis::String aaocs::SummaryElementMatrixCollector::GetFriendlyDescription(
  void ) const
{
  axis::String desc;
  switch (summaryType_)
  {
  case kSum:
    desc = _T("Total ");
    break;
  case kAverage:
    desc = _T("Average ");
    break;
  case kMaximum:
    desc = _T("Maximum ");
    break;
  case kMinimum:
    desc = _T("Minimum ");
    break;
  default:
    assert(!_T("Unexpected summary type!"));
    break;
  }
  desc += GetVariableName();
  if (targetSetName_.empty())
  {
    desc += _T(" on all elements");
  }
  else
  {
    desc += _T(" on element set '") + targetSetName_ + _T("'");
  }
  return desc;
}

void aaocs::SummaryElementMatrixCollector::StartCollect(
  afb::DenseMatrix& matrix )
{
  targetIdToCollect_ = -1;
  switch (summaryType_)
  {
  case kSum:
  case kAverage:
    matrix.ClearAll();
    break;
  case kMaximum:
    bestNormValue_ = std::numeric_limits<real>::min();
    break;
  case kMinimum:
    bestNormValue_ = std::numeric_limits<real>::max();
    break;
  }
}


/*
 * Copyright 2018, Gepetto team, LAAS-CNRS
 *
 * This file is part of sot-talos-balance.
 * sot-talos-balance is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-talos-balance 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "sot/talos_balance/simple-state-integrator.hh"

#include <cmath>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/core/stop-watch.hh>

#include <Eigen/Core>

#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
//#include "pinocchio/math/quaternion.hpp"

namespace dynamicgraph {
namespace sot {
namespace talos_balance {

namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;

// Size to be aligned                       "-------------------------------------------------------"
#define PROFILE_SIMPLE_STATE_INTEGRATOR_COMPUTATION \
  "SimpleStateIntegrator computation                                  "

#define INPUT_SIGNALS m_controlSIN

#define OUTPUT_SIGNALS m_stateSOUT << m_velocitySOUT

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef SimpleStateIntegrator EntityClassName;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleStateIntegrator, "SimpleStateIntegrator");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
SimpleStateIntegrator::SimpleStateIntegrator(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(control, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_OUT(state, dynamicgraph::Vector, INPUT_SIGNALS),
      CONSTRUCT_SIGNAL_OUT(velocity, dynamicgraph::Vector, NULL) {
  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);

  /* Commands. */

  std::string docstring;

  docstring =
      "\n"
      "    Set integration timestep value\n"
      "\n";
  addCommand("init",
             new command::Setter<SimpleStateIntegrator, double>(*this, &SimpleStateIntegrator::init, docstring));

  docstring =
      "\n"
      "    Set state vector value\n"
      "\n";
  addCommand("setState",
             new command::Setter<SimpleStateIntegrator, Vector>(*this, &SimpleStateIntegrator::setState, docstring));

  docstring =
      "\n"
      "    Set velocity vector value\n"
      "\n";
  addCommand("setVelocity", new command::Setter<SimpleStateIntegrator, Vector>(
                                *this, &SimpleStateIntegrator::setVelocity, docstring));
}

void SimpleStateIntegrator::init(const double& step) { timestep_ = step; }

void SimpleStateIntegrator::setState(const dg::Vector& st) { state_ = st; }

void SimpleStateIntegrator::setVelocity(const dg::Vector& vel) { velocity_ = vel; }

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void SimpleStateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control, double dt) {
  using Eigen::AngleAxisd;
  using Eigen::Matrix3d;
  using Eigen::QuaternionMapd;
  using Eigen::Vector3d;

  typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
  Eigen::Matrix<double, 7, 1> qin, qout;
  qin.head<3>() = state.head<3>();

  QuaternionMapd quat(qin.tail<4>().data());
  quat = AngleAxisd(state(5), Vector3d::UnitZ()) * AngleAxisd(state(4), Vector3d::UnitY()) *
         AngleAxisd(state(3), Vector3d::UnitX());

  SE3().integrate(qin, control.head<6>() * dt, qout);

  Matrix3d rotationMatrix = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
  // Create the Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]
  Vector3d rollPitchYaw;
  rotationMatrixToEuler(rotationMatrix, rollPitchYaw);
  // Update freeflyer state (pose)
  state.head<3>() = qout.head<3>();
  state.segment<3>(3) << rollPitchYaw;
}

void SimpleStateIntegrator::rotationMatrixToEuler(const Eigen::Matrix3d& rotationMatrix,
                                                  Eigen::Vector3d& rollPitchYaw) {
  double m = sqrt(rotationMatrix(2, 1) * rotationMatrix(2, 1) + rotationMatrix(2, 2) * rotationMatrix(2, 2));
  double p = atan2(-rotationMatrix(2, 0), m);
  double r, y;
  if (fabs(fabs(p) - M_PI / 2.) < 0.001) {
    r = 0;
    y = -atan2(rotationMatrix(0, 1), rotationMatrix(1, 1));
  } else {
    y = atan2(rotationMatrix(1, 0), rotationMatrix(0, 0));  // alpha
    r = atan2(rotationMatrix(2, 1), rotationMatrix(2, 2));  // gamma
  }
  rollPitchYaw << r, p, y;
}

DEFINE_SIGNAL_OUT_FUNCTION(state, dynamicgraph::Vector) {
  m_velocitySOUT.setConstant(velocity_);
  m_velocitySOUT.setTime(iter + 1);

  const dynamicgraph::Vector& control = m_controlSIN(iter);

  const size_t sz = control.size();
  if ((size_t)(s.size()) != sz) s.resize(sz);
  if ((size_t)(state_.size()) != sz) throw std::runtime_error("Mismatching state and control size");
  if ((size_t)(velocity_.size()) != sz) throw std::runtime_error("Mismatching velocity and control size");

  velocity_ = control;

  integrateRollPitchYaw(state_, control, timestep_);
  state_.tail(sz - 6) += control.tail(sz - 6) * timestep_;

  s = state_;

  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(velocity, dynamicgraph::Vector) {
  s = velocity_;
  return s;
}

/* --- COMMANDS ---------------------------------------------------------- */

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void SimpleStateIntegrator::display(std::ostream& os) const {
  os << "SimpleStateIntegrator " << getName();
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}

}  // namespace talos_balance
}  // namespace sot
}  // namespace dynamicgraph