//
// Created by zx on 2021/8/26.
//

#include "offset_solver.h"

//
// Created by zx on 2021/7/1.
//
#include <cppad/ipopt/solve.hpp>
#include <glog/logging.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_visualizer.h>


using CppAD::AD;
double CPPDA_PA=30.0;
double CPPDA_PB=10.0;
class affine_cppda_cost
{
 public:
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
    double m_ox;
    double m_oy;
    affine_cppda_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double ox,double oy)
            :m_cloud(cloud),m_ox(ox),m_oy(oy)
    {}
    typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;
    void operator()(ADvector& fg, const ADvector& x)
    {
        assert( fg.size() == 1 );   //误差
        assert( x.size()  == 3);

        AD<double> cx = x[1];
        AD<double> cy = x[2];
        AD<double> ca = x[0];

        fg[0]=0;
        for(int i=0;i<m_cloud->size();++i)
        {
            pcl::PointXYZ p=m_cloud->points[i];

            double x=p.x-m_ox;
            double y=p.y-m_oy;

            AD<double> nx=CppAD::cos(ca)*x-CppAD::sin(ca)*y +cx;
            AD<double> ny=CppAD::sin(ca)*x+CppAD::cos(ca)*y +cy;


            fg[0]+=CppAD::pow(1.0/(1.0+CppAD::exp(CPPDA_PA*(nx+1.0)))
                                      +1.0/(1.0+CppAD::exp(-CPPDA_PA*(nx-1.0))),2);

            fg[0]+=CppAD::pow(1.0/(1.0+CppAD::exp(CPPDA_PB*(ny+2.4)))
                                      +1.0/(1.0+CppAD::exp(-CPPDA_PB*(ny-2.4))),2);

        }


    }
};

offset_solver::offset_solver(double minx,double maxx,double mina,double maxa)
    :m_minx(minx),m_maxx(maxx),m_mina(mina),m_maxa(maxa)
{

}

Error_manager offset_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                    pcl::PointXYZ rotate_center,float& cx,float& cy,float& theta)
{

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid);

    LOG(INFO)<<"  cloud size:"<<cloud->size()<<" , center :"<<-centroid[0]<<"  ,"<<-centroid[1];
    double variable[] = {theta,cx,cy};

    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR( double ) Dvector;

    // number of independent variables (domain dimension for f and g)
    size_t n_vars = 3;
    // number of constraints (range dimension for g)
    //size_t ng = 1;
    // initial value of the independent variables
    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars[i] = 0.0;
    }

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    // Sets lower and upper limits for variables.
    // Set all non-actuators upper and lowerlimits
    // to the max negative and positive values.
    vars_lowerbound[0]=m_mina;
    vars_upperbound[0]=m_maxa;

    vars_lowerbound[1] = m_minx;
    vars_upperbound[1] = m_maxa;
    vars_lowerbound[2] = -1.0e19;
    vars_upperbound[2] = 1.0e19;


    affine_cppda_cost affine_cost(cloud,rotate_center.x,rotate_center.y);
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";

    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    options += "Numeric max_cpu_time          0.5\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, affine_cppda_cost>(
            options, vars, vars_lowerbound, vars_upperbound, Dvector(),
            Dvector(), affine_cost, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    // Cost
    if(ok)
    {
        auto cost = solution.obj_value;

        theta = solution.x[0];
        cx = solution.x[1];
        cy = solution.x[2];

        printf(" cppad solver  theta:%.3f , x;%.5f , y:%.5f\n",
               theta * 180.0 / M_PI, cx, cy);

        return SUCCESS;
    }
    return FAILED;
}

Error_manager offset_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                   pcl::PointXYZ rotate_center,float& cx,float& cy,float& theta,
                                   pcl::visualization::PCLVisualizer& viewer)
{
    Error_manager code=solve(cloud,rotate_center,cx,cy,theta);
    if(code!=SUCCESS)
    {
        viewer.addText(code.get_error_description(),50,50,1,0,0,"offset_solve_error_code");
        return code;
    }

    //先平移点云，后旋转,再平移
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    transform_2.translation() << -rotate_center.x, -rotate_center.y, 0;
    pcl::transformPointCloud(*cloud, *transformed_cloud, transform_2);

    Eigen::Affine3f transform_r = Eigen::Affine3f::Identity();
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud1(new pcl::PointCloud<pcl::PointXYZ>());
    transform_r.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
    pcl::transformPointCloud(*transformed_cloud, *transformed_cloud1, transform_r);

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud2(new pcl::PointCloud<pcl::PointXYZ>());
    Eigen::Affine3f transform_t = Eigen::Affine3f::Identity();
    transform_t.translation() << cx, 0, 0;
    pcl::transformPointCloud(*transformed_cloud1, *transformed_cloud2, transform_t);

    //创建坐标函数点云，用于显示
    pcl::PointCloud<pcl::PointXYZ>::Ptr viewer_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (float x = -2; x < 2; x += 0.01)
    {
        float y = 4.0 * (1.0 / (1.0 + exp(CPPDA_PA * (x + 1.0)))
                + 1.0 / (1.0 + exp(-CPPDA_PA * (x - 1.0))) - 1) + 1;
        viewer_cloud->push_back(pcl::PointXYZ(x, y, 0));

        float zx = 4.0 * (1.0 / (1.0 + exp(CPPDA_PB * (3 * x + 2.4)))
                + 1.0 / (1.0 + exp(-CPPDA_PB * (3 * x - 2.4))) - 1) + 2.5;
        viewer_cloud->push_back(pcl::PointXYZ(zx, 3 * x, 0));

    }


    viewer.removeAllPointClouds();
    viewer.addPointCloud(transformed_cloud2, "cloud_");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); // green
    viewer.addPointCloud(viewer_cloud, single_color, "cloud_axis");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_axis");
    viewer.spinOnce();

    return code;
}