/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2023  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. 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 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#ifndef _BOXSORT_SPH_H
#define _BOXSORT_SPH_H

#include "../core.h"
#include "heap_sort.h"

namespace gctl
{
    template <typename A>
    struct cargo_sph
    {
        double lon, lat;
        A* item;
    };

    template <typename B>
    struct box_sort_pair_sph
    {
        int id;
        double near_deg;
        std::vector<cargo_sph<B> > *box_ptr;
    };

    /**
     * @brief       This class implements the template class boxes_sph, which preforms the spherical degree based sorting of objects.
     *
     * @tparam     T     template type
     */
    template <typename T>
    class boxes_sph
    {
    public:
        boxes_sph();
        boxes_sph(const array<double> &lons, const array<double> &lats, const array<T> &items, size_t lon_bnum, size_t lat_bnum);
        virtual ~boxes_sph();

        void init(const array<double> &lons, const array<double> &lats, const array<T> &items, size_t lon_bnum, size_t lat_bnum);
        void get_by_angle(double inlon, double inlat, double indeg, std::vector<T*> &cargo_list, bool on_boundary = false);
        void get_by_number(double inlon, double inlat, size_t target_num, std::vector<T*> &cargo_list);
        double spherical_angle(double lon1, double lat1, double lon2, double lat2);

    protected:
        int lon_bsize_, lat_bsize_, item_size_;
        double lon_min_, lon_max_, lat_min_, lat_max_, dlon_, dlat_;
        array<std::vector<cargo_sph<T> > > boxes_;
        bool initialized_;

        // 以下为get_by_number函数所需变量
        array<box_sort_pair_sph<T> > box_pairs_;
        heap_sort<box_sort_pair_sph<T> > boxes_sorter_;
        array<double> LocalDeg_;
        bool box_pairs_initalized_;
    };

    template <typename T>
    boxes_sph<T>::boxes_sph()
    {
        initialized_ = false;
        box_pairs_initalized_ = false;
    }

    template <typename T>
    boxes_sph<T>::boxes_sph(const array<double> &lons, const array<double> &lats, const array<T> &items, size_t lon_bnum, size_t lat_bnum) : boxes_sph()
    {
        init(lons, lats, items, lon_bnum, lat_bnum);
    }

    template <typename T>
    boxes_sph<T>::~boxes_sph()
    {
        if (!boxes_.empty())
        {
            for (int i = 0; i < boxes_.size(); ++i)
            {
                boxes_[i].clear();
                std::vector<cargo_sph<T>>().swap(boxes_[i]);
            }
        }
    }

    template <typename T>
    void boxes_sph<T>::init(const array<double> &lons, const array<double> &lats, const array<T> &items, size_t lon_bnum, size_t lat_bnum)
    {
        if (lons.empty() || lats.empty() || items.empty())
        {
            throw domain_error("Empty inputs. From boxes_sph<T>::init(...)");
        }

        if (lons.size() != lats.size() || lons.size() != items.size())
        {
            throw invalid_argument("Unequal array sizes. From boxes_sph<T>::init(...)");
        }

        if (lon_bnum == 0 || lat_bnum == 0)
        {
            throw invalid_argument("Invalid box dimensions. From boxes_sph<T>::init(...)");
        }

        lon_bsize_ = lon_bnum;
        lat_bsize_ = lat_bnum;
        item_size_ = items.size();
        boxes_.resize(lon_bsize_*lat_bsize_);

        lon_min_ = GCTL_BDL_MAX; lon_max_ = GCTL_BDL_MIN;
        lat_min_ = GCTL_BDL_MAX; lat_max_ = GCTL_BDL_MIN;
        for (size_t i = 0; i < item_size_; i++)
        {
            lon_min_ = std::min(lon_min_, lons[i]);
            lon_max_ = std::max(lon_max_, lons[i]);
            lat_min_ = std::min(lat_min_, lats[i]);
            lat_max_ = std::max(lat_max_, lats[i]);
        }

        lon_min_ = std::max(lon_min_, -180.0);
        lon_max_ = std::min(lon_max_,  180.0);
        lat_min_ = std::max(lat_min_,  -90.0);
        lat_max_ = std::min(lat_max_,   90.0);

        dlon_ = (lon_max_ - lon_min_)/lon_bsize_;
        dlat_ = (lat_max_ - lat_min_)/lat_bsize_;

        int M, N;
        double lon, lat;
        cargo_sph<T> tmp_cargo;
        for (size_t i = 0; i < item_size_; i++)
        {
            lon = std::max(-180.0, std::min(lons[i], 180.0));
            lat = std::max(-90.0, std::min(lats[i], 90.0));

            N = floor((lon - lon_min_)/dlon_);
            M = floor((lat - lat_min_)/dlat_);

            if (N == lon_bsize_) N--;
            if (M == lat_bsize_) M--;

            if (M >= 0 && M < lat_bsize_ && N >= 0 && N < lon_bsize_)
            {
                tmp_cargo.lon = lon;
                tmp_cargo.lat = lat;
                tmp_cargo.item = items.get(i);
                boxes_[N + M*lon_bsize_].push_back(tmp_cargo);
            }
        }

        initialized_ = true;
        return;
    }

    template <typename T>
    void boxes_sph<T>::get_by_angle(double inlon, double inlat, double indeg, std::vector<T*> &cargo_list, bool on_boundary)
    {
        if (!initialized_)
        {
            throw runtime_error("The boxes_sph object is not initialized_. From boxes_sph<T>::get_by_angle(...)");
        }

        if (!cargo_list.empty()) cargo_list.clear();

        // sort all boxes_ by the closest possible angle
        size_t idx;
        double cor_lon, cor_lat;
        if (!box_pairs_initalized_)
        {
            box_pairs_.resize(lon_bsize_*lat_bsize_);
            for (size_t i = 0; i < lat_bsize_; i++)
            {
                for (size_t j = 0; j < lon_bsize_; j++)
                {
                    idx = j+i*lon_bsize_;
                    box_pairs_[idx].id = idx;
                    box_pairs_[idx].near_deg = 180.0;
                    box_pairs_[idx].box_ptr = boxes_.get(idx);

                    cor_lon = lon_min_ + j*dlon_;
                    cor_lat = lat_min_ + i*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                    cor_lon = lon_min_ + (j+1)*dlon_;
                    cor_lat = lat_min_ + i*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                    cor_lon = lon_min_ + (j+1)*dlon_;
                    cor_lat = lat_min_ + (i+1)*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                    cor_lon = lon_min_ + j*dlon_;
                    cor_lat = lat_min_ + (i+1)*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));
                }
            }

            box_pairs_initalized_ = true;
        }
        else
        {
            int M, N;
            for (size_t i = 0; i < box_pairs_.size(); i++)
            {
                M = box_pairs_[i].id/lon_bsize_;
                N = box_pairs_[i].id%lon_bsize_;

                cor_lon = lon_min_ + N*dlon_;
                cor_lat = lat_min_ + M*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                cor_lon = lon_min_ + (N+1)*dlon_;
                cor_lat = lat_min_ + M*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                cor_lon = lon_min_ + (N+1)*dlon_;
                cor_lat = lat_min_ + (M+1)*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                cor_lon = lon_min_ + N*dlon_;
                cor_lat = lat_min_ + (M+1)*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));
            }
        }
        
        // Sort boxes_' pointers by the closest possible distance
        boxes_sorter_.execute(box_pairs_, [](array<box_sort_pair<T>> &a, int l_id, int r_id)->bool{
            if (a[l_id].near_deg < a[r_id].near_deg) return true;
            else return false;
        });

        double deg;
        for (size_t i = 0; i < box_pairs_.size(); i++)
        {
            if (box_pairs_[i].near_deg < indeg || (on_boundary && box_pairs_[i].near_deg == indeg))
            {
                for (size_t j = 0; j < box_pairs_[i].box_ptr->size(); j++)
                {
                    deg = spherical_angle(inlon, inlat, box_pairs_[i].box_ptr->at(j).lon, box_pairs_[i].box_ptr->at(j).lat);
                    if (deg < indeg || (on_boundary && deg == indeg))
                    {
                        cargo_list.push_back(box_pairs_[i].box_ptr->at(j).item);
                    }
                }
            }

            break; // break at the first failed inquire
        }
        return;
    }

    template <typename T>
    void boxes_sph<T>::get_by_number(double inlon, double inlat, size_t target_num, std::vector<T*> &cargo_list)
    {
        if (!initialized_)
        {
            throw runtime_error("The boxes_sph object is not initialized_. From boxes_sph<T>::get_by_number(...)");
        }

        if (target_num <= 0)
        {
            throw invalid_argument("Invalid target number. From boxes_sph<T>::get_by_number(...)");
        }

        if (!cargo_list.empty()) cargo_list.clear();
        cargo_list.resize(target_num, nullptr);

        int count = 0;
        if (target_num >= item_size_)
        {
            for (int i = 0; i < lon_bsize_*lat_bsize_; ++i)
            {
                for (int j = 0; j < boxes_[i].size(); ++j)
                {
                    cargo_list[count] = boxes_[i][j].item;
                    count++;
                }
            }
            return;
        }

        // sort all boxes_ by the closest possible angle
        size_t idx;
        double cor_lon, cor_lat;
        if (!box_pairs_initalized_)
        {
            box_pairs_.resize(lon_bsize_*lat_bsize_);
            for (size_t i = 0; i < lat_bsize_; i++)
            {
                for (size_t j = 0; j < lon_bsize_; j++)
                {
                    idx = j+i*lon_bsize_;
                    box_pairs_[idx].id = idx;
                    box_pairs_[idx].near_deg = 180.0;
                    box_pairs_[idx].box_ptr = boxes_.get(idx);

                    cor_lon = lon_min_ + j*dlon_;
                    cor_lat = lat_min_ + i*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                    cor_lon = lon_min_ + (j+1)*dlon_;
                    cor_lat = lat_min_ + i*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                    cor_lon = lon_min_ + (j+1)*dlon_;
                    cor_lat = lat_min_ + (i+1)*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                    cor_lon = lon_min_ + j*dlon_;
                    cor_lat = lat_min_ + (i+1)*dlat_;
                    box_pairs_[idx].near_deg = std::min(box_pairs_[idx].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));
                }
            }

            box_pairs_initalized_ = true;
        }
        else
        {
            int M, N;
            for (size_t i = 0; i < box_pairs_.size(); i++)
            {
                M = box_pairs_[i].id/lon_bsize_;
                N = box_pairs_[i].id%lon_bsize_;

                cor_lon = lon_min_ + N*dlon_;
                cor_lat = lat_min_ + M*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                cor_lon = lon_min_ + (N+1)*dlon_;
                cor_lat = lat_min_ + M*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                cor_lon = lon_min_ + (N+1)*dlon_;
                cor_lat = lat_min_ + (M+1)*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));

                cor_lon = lon_min_ + N*dlon_;
                cor_lat = lat_min_ + (M+1)*dlat_;
                box_pairs_[i].near_deg = std::min(box_pairs_[i].near_deg, spherical_angle(inlon, inlat, cor_lon, cor_lat));
            }
        }
        
        // Sort boxes_' pointers by the closest possible distance
        boxes_sorter_.execute(box_pairs_, [](array<box_sort_pair_sph<T>> &a, int l_id, int r_id)->bool{
            if (a[l_id].near_deg < a[r_id].near_deg) return true;
            else return false;
        });

        double deg, tmp_deg, maxi_deg = GCTL_BDL_MIN, maxi_record_deg = GCTL_BDL_MAX;
        T *tmp_item, *curr_item;

        LocalDeg_.resize(target_num);

        count = 0;
        for (size_t i = 0; i < box_pairs_.size(); i++)
        {
            if (!box_pairs_[i].box_ptr->empty() && box_pairs_[i].near_deg < maxi_record_deg)
            {
                for (size_t j = 0; j < box_pairs_[i].box_ptr->size(); j++)
                {
                    deg = spherical_angle(inlon, inlat, box_pairs_[i].box_ptr->at(j).lon, box_pairs_[i].box_ptr->at(j).lat);

                    curr_item = box_pairs_[i].box_ptr->at(j).item;
                    if (count < target_num)
                    {
                        LocalDeg_[count] = deg;
                        cargo_list[count] = curr_item;
                        count++;

                        maxi_deg = std::max(maxi_deg, deg);
                        continue;
                    }

                    maxi_record_deg = maxi_deg;

                    // 这比排序还要快
                    if (deg < maxi_record_deg)
                    {
                        for (size_t k = 0; k < target_num; k++)
                        {
                            if (deg < LocalDeg_[k])
                            {
                                tmp_deg = LocalDeg_[k]; LocalDeg_[k] = deg; deg = tmp_deg;
                                tmp_item = cargo_list[k]; cargo_list[k] = curr_item; curr_item = tmp_item;
                                break;
                            }
                        }
                        
                        maxi_deg = GCTL_BDL_MIN;
                        for (size_t k = 0; k < target_num; k++)
                        {
                            maxi_deg = std::max(maxi_deg, LocalDeg_[k]);
                        }
                    }
                }
            }
        }
        return;
    }

    template <typename T>
    double boxes_sph<T>::spherical_angle(double lon1, double lat1, double lon2, double lat2)
    {
        double x1, y1, z1, x2, y2, z2;
        x1 = cos(GCTL_Pi*lat1/180.0)*cos(GCTL_Pi*lon1/180.0);
		y1 = cos(GCTL_Pi*lat1/180.0)*sin(GCTL_Pi*lon1/180.0);
		z1 = sin(GCTL_Pi*lat1/180.0);

        x2 = cos(GCTL_Pi*lat2/180.0)*cos(GCTL_Pi*lon2/180.0);
		y2 = cos(GCTL_Pi*lat2/180.0)*sin(GCTL_Pi*lon2/180.0);
		z2 = sin(GCTL_Pi*lat2/180.0);

        return acos(std::max(-1.0, std::min(1.0, (x1*x2 + y1*y2 + z1*z2))))*180.0/GCTL_Pi;
    }
}

#endif // _BOXSORT_SPH_H