#include "iostream"
#include "fstream"
#include "sstream"
#include "iomanip"
#include "vector"
#include "filesystem"
#include "toml.hpp"

using namespace std;

namespace dji_kmz
{
    int get_droneEnumValue(std::string name)
    {
        if (name == "Matrice 350 RTK") return 89;
        if (name == "Matrice 300 RTK") return 60;
        if (name == "Matrice 30") return 67;
        if (name == "Matrice 30T") return 67;
        if (name == "Mavic 3E") return 77;
        if (name == "Mavic 3T") return 77;
        if (name == "Matrice 3D") return 91;
        if (name == "Matrice 3TD") return 91;
        if (name == "Matrice 4D") return 100;
        if (name == "Matrice 4TD") return 100;
        if (name == "Matrice 4E") return 99;
        if (name == "Matrice 4T") return 99;
        throw std::runtime_error("dji_kmz: Invalid drone type.");
    }

    std::string get_flyToWaylineMode(std::string mode)
    {
        if (mode != "safely" && mode != "pointToPoint")
            throw std::runtime_error("dji_kmz: Invalid fly to wayline mode.");

        return mode;
    }

    std::string get_finishAction(std::string action)
    {
        if (action != "goHome" && action != "noAction" && 
            action != "autoLand" && action != "gotoFirstWaypoint")
            throw std::runtime_error("dji_kmz: Invalid finish action.");

        return action;
    }

    std::string get_exitOnRCLost(std::string action)
    {
        if (action != "goContinue" && action != "executeLostAction")
            throw std::runtime_error("dji_kmz: Invalid exit mode on RC lost.");

        return action;
    }

    std::string get_executeRCLostAction(std::string action)
    {
        if (action != "goBack" && action != "landing" && action != "hover")
            throw std::runtime_error("dji_kmz: Invalid action on RC lost.");

        return action;
    }

    std::string get_templateType(std::string type)
    {
        if (type != "waypoint" && type != "mapping2d" && 
            type != "mapping3d" && type != "mappingStrip")
            throw std::runtime_error("dji_kmz: Invalid template type.");

        return type;
    }

    int is_caliFlightEnable(std::string type)
    {
        if (type == "no") return 0;
        if (type == "yes") return 1;
        throw std::runtime_error("dji_kmz: Invalid caliberate flight mode.");
    }

    int is_globalUseStraightLine(std::string type)
    {
        if (type == "no") return 0;
        if (type == "yes") return 1;
        throw std::runtime_error("dji_kmz: Invalid global use straight line mode.");
    }

    std::string get_gimbalPitchMode(std::string type)
    {
        if (type != "manual" && type != "usePointSetting")
            throw std::runtime_error("dji_kmz: Invalid gimbal pitch mode.");

        return type;
    }

    std::string get_globalWaypointTurnMode(std::string mode)
    {
        if (mode != "coordinateTurn" && 
            mode != "toPointAndStopWithDiscontinuityCurvature" && 
            mode != "toPointAndStopWithContinuityCurvature" &&
            mode != "toPointAndPassWithContinuityCurvature")
            throw std::runtime_error("dji_kmz: Invalid global waypoint turn mode.");

        return mode;
    }

    std::string get_wayline_coordinateMode(std::string mode)
    {
        if (mode != "WGS84")
            throw std::runtime_error("dji_kmz: Invalid wayline coordinate mode.");

        return mode;
    }

    std::string get_wayline_heightMode(std::string mode)
    {
        if (mode != "EGM96" && mode != "relativeToStartPoint" &&
            mode != "aboveGroundLevel" && mode != "realTimeFollowSurface")
            throw std::runtime_error("dji_kmz: Invalid wayline height mode.");

        return mode;
    }

    std::string get_wayline_positioningType(std::string type)
    {
        if (type != "GPS" && type != "RTKBaseStation" &&
            type != "QianXun" && type != "Custom")
            throw std::runtime_error("dji_kmz: Invalid wayline positioning type.");

        return type;
    }

    std::string get_waypoint_HeadingMode(std::string mode)
    {
        if (mode != "followWayline" && mode != "manually" &&
            mode != "fixed" && mode != "smoothTransition" &&
            mode != "towardPOI")
            throw std::runtime_error("dji_kmz: Invalid waypoint heading mode.");

        return mode;
    }

    int is_placemark_useGlobalHeight(std::string type)
    {
        if (type == "no") return 0;
        if (type == "yes") return 1;
        throw std::runtime_error("dji_kmz: Invalid placemark use global height mode.");
    }

    int is_placemark_useGlobalSpeed(std::string type)
    {
        if (type == "no") return 0;
        if (type == "yes") return 1;
        throw std::runtime_error("dji_kmz: Invalid placemark use global speed mode.");
    }

    int is_placemark_useGlobalHeadingParam(std::string type)
    {
        if (type == "no") return 0;
        if (type == "yes") return 1;
        throw std::runtime_error("dji_kmz: Invalid placemark use global heading-param mode.");
    }

    int is_placemark_useGlobalTurnParam(std::string type)
    {
        if (type == "no") return 0;
        if (type == "yes") return 1;
        throw std::runtime_error("dji_kmz: Invalid placemark use global turn-param mode.");
    }

    void get_googleEarth_coorpoints(string google_kml, vector<double> &plon, vector<double> &plat)
    {
        ifstream ifile(google_kml);
        if (!ifile) throw std::runtime_error("dji_kmz: Fail to open the google earth's .kml file.");

        plon.clear();
        plat.clear();

        double lon, lat;
        string line, cor_str;
        stringstream line_ss, cor_ss;
        while (getline(ifile, line))
        {
            line.erase(0, line.find_first_not_of(" \t"));
            line.erase(line.find_last_not_of(" \t") + 1);

            if (line == "<coordinates>")
            {
                getline(ifile, line);
                line.erase(0, line.find_first_not_of(" \t"));
                line.erase(line.find_last_not_of(" \t") + 1);

                line_ss.clear();
                line_ss.str(line);
                while (line_ss >> cor_str)
                {
                    replace(cor_str.begin(), cor_str.end(), ',', ' ');

                    cor_ss.clear();
                    cor_ss.str(cor_str);
                    cor_ss >> lon >> lat;
                    plon.push_back(lon);
                    plat.push_back(lat);
                }
                break;
            }
        }
        ifile.close();
    }

    void read_PointFile(string filename, vector<double> &lons, vector<double> &lats, vector<double> &hgts)
    {
        std::ifstream infile(filename);
        if (!infile.is_open()) throw std::runtime_error("dji_kmz: Fail to open point file.");

        lons.clear();
        lats.clear();
        hgts.clear();

        string line;
        stringstream ss_line;
        double lon, lat, hgt;
        while (getline(infile, line))
        {
            replace(line.begin(), line.end(), ',', ' ');
            ss_line.clear();
            ss_line.str(line);
            ss_line >> lon >> lat >> hgt;

            lons.push_back(lon);
            lats.push_back(lat);
            hgts.push_back(hgt);
        }
        infile.close();
        return;
    }

    /**
     * @brief 在多边形区域内按一定间隔生成航线点
     * 
     * @param plon 多边形顶点经度数组
     * @param plat 多边形顶点纬度数组
     * @param lon1 航线参考方向起点经度
     * @param lat1 航线参考方向起点纬度
     * @param lon2 航线参考方向终点经度
     * @param lat2 航线参考方向终点纬度
     * @param mini_len 最短航线长度 以公里表示
     * @param line_space 航线之间的间距 以公里表示
     * @param wlon 返回的航线经度数组
     * @param wlat 返回的航线纬度数组
     */
    void polygon2waypoints(const vector<double> &plon, const vector<double> &plat, double lon1, double lat1, 
        double lon2, double lat2, double mini_len, double line_space, vector<double> &wlon, vector<double> &wlat)
    {

    }

    void template_placemark(ofstream &ofile, double lon, double lat, int hgt, int idx, 
        int global_height, int global_speed, int global_heading, int global_turn)
    {
        ofile << "      <Placemark>\n";
        ofile << "        <Point>\n";
        ofile << "          <coordinates>\n";
        ofile << "            " << setprecision(16) << lon << "," << lat << "\n";
        ofile << "          </coordinates>\n";
        ofile << "        </Point>\n";
        ofile << "        <wpml:index>" << idx << "</wpml:index>\n";
        ofile << "        <wpml:ellipsoidHeight>" << hgt << "</wpml:ellipsoidHeight>\n";
        ofile << "        <wpml:height>" << hgt << "</wpml:height>\n";
        ofile << "        <wpml:useGlobalHeight>" << global_height << "</wpml:useGlobalHeight>\n";
        ofile << "        <wpml:useGlobalSpeed>" << global_speed << "</wpml:useGlobalSpeed>\n";
        ofile << "        <wpml:useGlobalHeadingParam>" << global_heading << "</wpml:useGlobalHeadingParam>\n";
        ofile << "        <wpml:useGlobalTurnParam>" << global_turn << "</wpml:useGlobalTurnParam>\n";
        ofile << "        <wpml:isRisky>0</wpml:isRisky>\n";
        ofile << "      </Placemark>\n";
        return;
    }
};


int main(int argc, char const *argv[])
{
    if (argc < 2)
    {
        cout << "Usage: dji_kmz [-h] <config.toml>\n";
        return -1;
    }
    else if (argc >= 2 && !strcmp(argv[1], "-h"))
    {
        cout << "Convert Google Earth's kml file or plain locations to DJI's wayline file. \
This open-source software is provided “as is” without warranty of any kind, and the \
author/contributors shall not be liable for any damages arising from its use.\n\n";
        cout << "Author: Dr. Yi Zhang\n";
        cout << "Contact: School of Earth Science, Zhejiang University\n";
        cout << "E-mail: yizhang-geo@zju.edu.cn\n\n";
        cout << "Usage: dji_kmz [-h] <config.toml>\n\n";
        cout << "Toml's Options:\n";
        cout << "----------------------------------\n";
        cout << "author = \"name\"                   # Author of output .kmz file\n";
        cout << "kmzFile = \"out.kmz\"               # Output DJI's .kmz file\n";
        cout << "\n";
        cout << "[missionConfig]\n";
        cout << "droneType = \"Matrice 350 RTK\"     # Type of drone used for the mission\n";
        cout << "                                    # Possible values:\n";
        cout << "                                    #   - Matrice 350 RTK\n";
        cout << "                                    #   - Matrice 300 RTK\n";
        cout << "                                    #   - Matrice 30\n";
        cout << "                                    #   - Matrice 30T\n";
        cout << "                                    #   - Mavic 3E\n";
        cout << "                                    #   - Mavic 3T\n";
        cout << "                                    #   - Matrice 3D\n";
        cout << "                                    #   - Matrice 3TD\n";
        cout << "                                    #   - Matrice 4D\n";
        cout << "                                    #   - Matrice 4TD\n";
        cout << "                                    #   - Matrice 4E\n";
        cout << "                                    #   - Matrice 4T\n";
        cout << "flyToWaylineMode = \"safely\"       # Mode to fly to wayline (safely or pointToPoint)\n";
        cout << "finishAction = \"goHome\"           # Action to take on mission finish (goHome, noAction, autoLand, gotoFirstWaypoint)\n";
        cout << "exitOnRCLost = \"goContinue\"       # Action on RC signal lost (goContinue, executeLostAction)\n";
        cout << "takeOffSecurityHeight = 50.0        # Height to ascend to before starting mission\n";
        cout << "globalTransitionalSpeed = 10.0      # Speed during transitional phases\n";
        cout << "globalRTHHeight = 50.0              # Height for Return to Home (RTH)\n";
        cout << "\n";
        cout << "[wayline]\n";
        cout << "templateType = \"waypoint\"         # Type of wayline template (waypoint, mapping2d, mapping3d, mappingStrip)\n";
        cout << "templateId = 0                      # Unique identifier for the wayline template\n";
        cout << "autoFlightSpeed = 5.0               # Speed during automatic flight\n";
        cout << "globalHeight = 30.0                 # Default height for waypoints\n";
        cout << "caliFlightEnable = \"no\"           # Enable calibration flight (yes/no)\n";
        cout << "gimbalPitchMode = \"manual\"        # Gimbal pitch control mode (manual, usePointSetting)\n";
        cout << "globalWaypointTurnMode = \"coordinateTurn\"  # Turn mode at waypoints (coordinateTurn, toPointAndStopWithDiscontinuityCurvature, toPointAndStopWithContinuityCurvature, toPointAndPassWithContinuityCurvature)\n";
        cout << "globalUseStraightLine = \"yes\"     # Use straight line for waypoints (yes/no)\n";
        cout << "\n";
        cout << "[wayline.waylineCoordinateSysParam]\n";
        cout << "coordinateMode = \"WGS84\"          # Coordinate system mode (WGS84)\n";
        cout << "heightMode = \"relativeToStartPoint\"  # Height reference mode (EGM96, relativeToStartPoint, aboveGroundLevel, realTimeFollowSurface)\n";
        cout << "positioningType = \"GPS\"           # Positioning type (GPS, RTKBaseStation, QianXun, Custom)\n";
        cout << "\n";
        cout << "[wayline.globalWaypointHeadingParam]\n";
        cout << "waypointHeadingMode = \"followWayline\"  # Heading mode at waypoints (followWayline, manually, fixed, smoothTransition, towardPOI)\n";
        cout << "waypointHeadingAngle = 0.0          # Heading angle for waypoints\n";
        cout << "waypointPoiPoint = [0.0, 0.0, 0.0]  # Point of interest coordinates for heading (x, y, z)\n";
        cout << "waypointHeadingPoiIndex = 0         # Index of the point of interest for heading\n";
        cout << "\n";
        cout << "[placemark]\n";
        cout << "PointFile = \"example.csv\"         # File containing waypoint coordinates\n";
        cout << "useGlobalHeight = \"yes\"           # Use global height settings for waypoints (yes/no)\n";
        cout << "useGlobalSpeed = \"yes\"            # Use global speed settings for waypoints (yes/no)\n";
        cout << "useGlobalHeadingParam = \"yes\"     # Use global heading parameters for waypoints (yes/no)\n";
        cout << "useGlobalTurnParam = \"yes\"        # Use global turn parameters for waypoints (yes/no)\n\n";
        cout << R"(Point File Format:
----------------------------------
This program can read coordinate marker files saved by Google Earth (in KML format) or ordinary text data files, where each line contains the longitude, latitude, and flight altitude of a waypoint (a negative value indicates the use of the global flight altitude defined by the globalHeight parameter).
)";
        return -1;
    }

    // Read configuration
    toml::value toml_data = toml::parse(argv[1]);

    // Basic options
    std::string author = toml::find<string>(toml_data, "author");
    std::string kmz_file = toml::find<string>(toml_data, "kmzFile");
    std::string exten_name = kmz_file.substr(kmz_file.find_last_of('.'));
    if (exten_name != ".kmz") kmz_file += ".kmz";

    // mission options
    int drone_enum = dji_kmz::get_droneEnumValue(toml::find<string>(toml_data, "missionConfig", "droneType"));
    string fly_to_wayline = dji_kmz::get_flyToWaylineMode(toml::find<string>(toml_data, "missionConfig", "flyToWaylineMode"));
    string finish_action = dji_kmz::get_finishAction(toml::find<string>(toml_data, "missionConfig", "finishAction"));
    string on_rc_lost = dji_kmz::get_exitOnRCLost(toml::find<string>(toml_data, "missionConfig", "exitOnRCLost"));
    float security_height = toml::find<float>(toml_data, "missionConfig", "takeOffSecurityHeight");
    float trans_speed = toml::find<float>(toml_data, "missionConfig", "globalTransitionalSpeed");
    float rth_height = toml::find<float>(toml_data, "missionConfig", "globalRTHHeight");

    // wayline options
    int use_straight_line = dji_kmz::is_globalUseStraightLine(toml::find<string>(toml_data, "wayline", "globalUseStraightLine"));
    int template_id = toml::find<int>(toml_data, "wayline", "templateId");
    int cali_flight = dji_kmz::is_caliFlightEnable(toml::find<string>(toml_data, "wayline", "caliFlightEnable"));
    float auto_speed = toml::find<float>(toml_data, "wayline", "autoFlightSpeed");
    float global_hgt = toml::find<float>(toml_data, "wayline", "globalHeight");
    string template_type = dji_kmz::get_templateType(toml::find<string>(toml_data, "wayline", "templateType"));
    string pitch_mode = dji_kmz::get_gimbalPitchMode(toml::find<string>(toml_data, "wayline", "gimbalPitchMode"));
    string global_trun_mode = dji_kmz::get_globalWaypointTurnMode(toml::find<string>(toml_data, "wayline", "globalWaypointTurnMode"));
    // wayline waylineCoordinateSysParam
    string coor_mode = dji_kmz::get_wayline_coordinateMode(toml::find<string>(toml_data, "wayline", "waylineCoordinateSysParam", "coordinateMode"));
    string hgt_mode = dji_kmz::get_wayline_heightMode(toml::find<string>(toml_data, "wayline", "waylineCoordinateSysParam", "heightMode"));
    string posi_type = dji_kmz::get_wayline_positioningType(toml::find<string>(toml_data, "wayline", "waylineCoordinateSysParam", "positioningType"));
    // wayline globalWaypointHeadingParam
    int poi_indx = toml::find<int>(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingPoiIndex");
    float heading_angle = toml::find<float>(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingAngle");
    string heading_mode = dji_kmz::get_waypoint_HeadingMode(toml::find<string>(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingMode"));
    std::vector<float> poi_point = toml::find<std::vector<float> >(toml_data, "wayline", "globalWaypointHeadingParam", "waypointPoiPoint");
    if (poi_point.size() != 3) throw std::runtime_error("dji_kmz: Invalid poi-points.");

    // Placemark options
    std::string point_file = toml::find<string>(toml_data, "placemark", "PointFile");
    int global_height = dji_kmz::is_placemark_useGlobalHeight(toml::find<string>(toml_data, "placemark", "useGlobalHeight"));
    int global_speed = dji_kmz::is_placemark_useGlobalSpeed(toml::find<string>(toml_data, "placemark", "useGlobalSpeed"));
    int global_heading = dji_kmz::is_placemark_useGlobalHeadingParam(toml::find<string>(toml_data, "placemark", "useGlobalHeadingParam"));
    int global_turn = dji_kmz::is_placemark_useGlobalTurnParam(toml::find<string>(toml_data, "placemark", "useGlobalTurnParam"));

    // Get waypoints
    vector<double> lons, lats, hgts;
    exten_name = point_file.substr(point_file.find_last_of('.'));
    if (exten_name == ".kml")
    {
        dji_kmz::get_googleEarth_coorpoints(point_file, lons, lats);
        hgts.resize(lons.size(), -1.0);
    }
    else dji_kmz::read_PointFile(point_file, lons, lats, hgts);

    // Create a template folder
    // The following may diff between different OS-systems, improve later
    int ret = system("mkdir wpmz");
    if (ret == -1) throw std::runtime_error("dji_kmz: Fail to create working directory.");

    // Write template.kml
    ofstream ofile("wpmz/template.kml");
    if (!ofile.is_open()) throw std::runtime_error("dji_kmz: Fail to create working file.");

    ofile << R"(<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2" xmlns:wpml="http://www.dji.com/wpmz/1.0.5">
  <Document>
)";
    ofile << "    <wpml:author>" << author << "</wpml:author>\n";
    ofile << "    <wpml:missionConfig>\n";
    ofile << "      <wpml:flyToWaylineMode>" << fly_to_wayline << "</wpml:flyToWaylineMode>\n";
    ofile << "      <wpml:finishAction>" << finish_action << "</wpml:finishAction>\n";
    ofile << "      <wpml:exitOnRCLost>" << on_rc_lost << "</wpml:exitOnRCLost>\n";
    ofile << "      <wpml:takeOffSecurityHeight>" << security_height << "</wpml:takeOffSecurityHeight>\n";
    ofile << "      <wpml:globalTransitionalSpeed>" << trans_speed << "</wpml:globalTransitionalSpeed>\n";
    ofile << "      <wpml:globalRTHHeight>" << rth_height << "</wpml:globalRTHHeight>\n";
    ofile << "      <wpml:droneInfo>\n";
    ofile << "        <wpml:droneEnumValue>" << drone_enum << "</wpml:droneEnumValue>\n";
    ofile << "        <wpml:droneSubEnumValue>0</wpml:droneSubEnumValue>\n";
    ofile << "      </wpml:droneInfo>\n";
    ofile << "    </wpml:missionConfig>\n";
    ofile << "    <Folder>\n";
    ofile << "      <wpml:templateType>" << template_type << "</wpml:templateType>\n";
    ofile << "      <wpml:templateId>" << template_id << "</wpml:templateId>\n";
    ofile << "      <wpml:waylineCoordinateSysParam>\n";
    ofile << "        <wpml:coordinateMode>" << coor_mode << "</wpml:coordinateMode>\n";
    ofile << "        <wpml:heightMode>" << hgt_mode << "</wpml:heightMode>\n";
    ofile << "        <wpml:positioningType>" << posi_type << "</wpml:positioningType>\n";
    ofile << "      </wpml:waylineCoordinateSysParam>\n";
    ofile << "      <wpml:autoFlightSpeed>" << auto_speed << "</wpml:autoFlightSpeed>\n";
    ofile << "      <wpml:globalHeight>" << global_hgt << "</wpml:globalHeight>\n";
    ofile << "      <wpml:caliFlightEnable>" << cali_flight << "</wpml:caliFlightEnable>\n";
    ofile << "      <wpml:gimbalPitchMode>" << pitch_mode << "</wpml:gimbalPitchMode>\n";
    ofile << "      <wpml:globalWaypointHeadingParam>\n";
    ofile << "        <wpml:waypointHeadingMode>" << heading_mode << "</wpml:waypointHeadingMode>\n";
    ofile << "        <wpml:waypointHeadingAngle>" << heading_angle << "</wpml:waypointHeadingAngle>\n";
    ofile << "        <wpml:waypointPoiPoint>" << poi_point[0] << "," << poi_point[1] << "," << poi_point[2] << "</wpml:waypointPoiPoint>\n";
    ofile << "        <wpml:waypointHeadingPoiIndex>" << poi_indx << "</wpml:waypointHeadingPoiIndex>\n";
    ofile << "      </wpml:globalWaypointHeadingParam>\n";
    ofile << "      <wpml:globalWaypointTurnMode>" << global_trun_mode << "</wpml:globalWaypointTurnMode>\n";
    ofile << "      <wpml:globalUseStraightLine>" << use_straight_line << "</wpml:globalUseStraightLine>\n";

    int hgt;
    for (size_t i = 0; i < lons.size(); i++)
    {
        if (hgts[i] < 0) hgt = global_hgt;
        else hgt = hgts[i];

        dji_kmz::template_placemark(ofile, lons[i], lats[i], hgt, i, global_height, global_speed, global_heading, global_turn);
    }
    
    ofile << R"(    </Folder>
  </Document>
</kml>)";
    ofile.close();

    // Zip
    string cmd = "zip -q -r " + kmz_file + " wpmz && rm -rf wpmz";
    ret = system(cmd.c_str());
    if (ret != 0) throw std::runtime_error("gji_kmz: Fail to compress the .kmz file.");
    return 0;
}