#include "geo/private/coord/ct_convert.h"

namespace ns
{
namespace geo
{

Eigen::Vector2d convert_pt(const PointD& pt, const GaussConverter& gc)
{
    PointD gauss = gc.wgs842Gauss(pt);
    return {gauss.x, gauss.y};
}

Eigen::Vector2d convert_pt(const PointDST& pt, const GaussConverter& gc)
{
    PointD gauss = gc.wgs842Gauss(pt.p);
    return {gauss.x, gauss.y};
}

Eigen::Matrix2Xd convert_pl(const std::vector<PointD>& pl, const GaussConverter& gc)
{
    Eigen::Matrix2Xd mat = Eigen::Matrix2Xd::Zero(2, pl.size());
    for (size_t i = 0; i < pl.size(); i++)
    {
        mat.col(i) = convert_pt(pl[i], gc);
    }
    return mat;
}

Eigen::Matrix2Xd convert_pl(const std::vector<PointDST>& pl, const GaussConverter& gc)
{
    Eigen::Matrix2Xd mat = Eigen::Matrix2Xd::Zero(2, pl.size());
    for (size_t i = 0; i < pl.size(); i++)
    {
        mat.col(i) = convert_pt(pl[i], gc);
    }
    return mat;
}

RectD convert_bb(const RectD& bb, const GaussConverter& gc)
{
    double wgs_min_x = bb.minX, wgs_min_y = bb.minY, wgs_max_x = bb.maxX, wgs_max_y = bb.maxY;
    PointD min_pt = gc.wgs842Gauss({wgs_min_x, wgs_min_y});
    PointD max_pt = gc.wgs842Gauss({wgs_max_x, wgs_max_y});
    return RectD(min_pt.x, max_pt.x, min_pt.y, max_pt.y);
}

PointD convert_pt_reverse(const Eigen::Vector2d& pt, const GaussConverter& gc)
{
    return gc.gauss2Wgs84({pt(0), pt(1)});
}

std::vector<PointD> convert_pl_reverse(const Eigen::Matrix2Xd& pl, const GaussConverter& gc)
{
    std::vector<PointD> pts;

    Eigen::Index n = pl.cols();
    for (Eigen::Index i = 0; i < n; ++i)
    {
        pts.emplace_back(convert_pt_reverse(pl.col(i), gc));
    }
    return pts;
}

} // namespace geo
} // namespace ns