// ======================================================================================
// Copyright 2017 State Key Laboratory of Remote Sensing Science, 
// Institute of Remote Sensing Science and Engineering, Beijing Normal University

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ======================================================================================

// #######################################################################################
// #                                                                                     #
// #            CSF: Airborne LiDAR filtering based on Cloth Simulation                  #
// #                                                                                     #
// #  Please cite the following paper, If you use this software in your work.            #
// #                                                                                     #
// #  Zhang W, Qi J, Wan P, Wang H, Xie D, Wang X, Yan G. An Easy-to-Use Airborne LiDAR  #
// #  Data Filtering Method Based on Cloth Simulation. Remote Sensing. 2016; 8(6):501.   #
// #                               (http://ramm.bnu.edu.cn/)                             #
// #                                                                                     #
// #                      Wuming Zhang; Jianbo Qi; Peng Wan; Hongtao Wang                #
// #                                                                                     #
// #                      contact us: 2009zwm@gmail.com; wpqjbzwm@126.com                #
// #                                                                                     #
// #######################################################################################


// cloth simulation filter for airborne lidar filtering
#ifndef _CSF_SAGA_H_
#define _CSF_SAGA_H_
#include <vector>
#include <string>

//#include "point_cloud.h"
#include <saga_api/saga_api.h>

// #######################################################################################
//#include "Cloth.h"
#ifdef _WIN32
# include <windows.h>
#endif // ifdef _WIN32
#include <math.h>
#include <iostream>
#include <sstream>
#include <list>
#include <cmath>
#include <queue>

// #######################################################################################
// #include "Vec3.h"
#include <iomanip>


// a minimal vector class of 3 doubles and overloaded math operators
class Vec3 {
public:

    double f[3];

    Vec3(double x, double y, double z) {
        f[0] = x;
        f[1] = y;
        f[2] = z;
    }

    Vec3() {}

    double length() {
        return sqrt(f[0] * f[0] + f[1] * f[1] + f[2] * f[2]);
    }

    Vec3 normalized() {
        double l = length();

        return Vec3(f[0] / l, f[1] / l, f[2] / l);
    }

    void operator+=(const Vec3& v) {
        f[0] += v.f[0];
        f[1] += v.f[1];
        f[2] += v.f[2];
    }

    Vec3 operator/(const double& a) {
        return Vec3(f[0] / a, f[1] / a, f[2] / a);
    }

    Vec3 operator-(const Vec3& v) {
        return Vec3(f[0] - v.f[0], f[1] - v.f[1], f[2] - v.f[2]);
    }

    Vec3 operator+(const Vec3& v) {
        return Vec3(f[0] + v.f[0], f[1] + v.f[1], f[2] + v.f[2]);
    }

    Vec3 operator*(const double& a) {
        return Vec3(f[0] * a, f[1] * a, f[2] * a);
    }

    Vec3 operator-() {
        return Vec3(-f[0], -f[1], -f[2]);
    }

    Vec3 cross(const Vec3& v) {
        return Vec3(
            f[1] * v.f[2] - f[2] * v.f[1],
            f[2] * v.f[0] - f[0] * v.f[2],
            f[0] * v.f[1] - f[1] * v.f[0]
        );
    }

    double dot(const Vec3& v) {
        return f[0] * v.f[0] + f[1] * v.f[1] + f[2] * v.f[2];
    }
};

// #include "Vec3.h"
// #######################################################################################

// #######################################################################################
// #include "Particle.h"
#include <vector>
/* Some physics constants */
#define DAMPING    0.01 // how much to damp the cloth simulation each frame
#define MAX_INF    9999999999
#define MIN_INF    -9999999999

/*
Initially, we have to make modifications of particle positions for each constraint(constraintTimes = rigidness), However, to save computation time, we 
precomputed the total displacement of a particle for all constraintTimes.
For singleMove1, which means one of the two particles is unmovable, then we move the other one only:
if constraintTimes = 0: singleMove1 = 0
if constraintTimes = 1: singleMove1 = 0.3, i.e., each time we move 0.3 (scale factor of the total distance) for a particle towards the other one
if constraintTimes = 2: singleMove1 = (1-0.3)*0.3+0.3 = 0.51
if constraintTimes = 3: singleMove1 = (1-0.51)*0.3+0.51 = 0.657
...

For doubleMove1, we move both of the two particles towards each other.
if constraintTimes = 0: singleMove2 = 0
if constraintTimes = 1: singleMove2 = 0.3, i.e., each time we move 0.3 (scale factor of the total distance) for the two particles towards each other
if constraintTimes = 2: singleMove2 = (1-0.3*2)*0.3+0.3 = 0.42
if constraintTimes = 3: singleMove2 = (1-0.42*2)*0.3+0.42 = 0.468
...

*/
const double singleMove1[15] = { 0, 0.3, 0.51, 0.657, 0.7599, 0.83193, 0.88235, 0.91765, 0.94235, 0.95965, 0.97175, 0.98023, 0.98616, 0.99031, 0.99322 };
const double doubleMove1[15] = { 0, 0.3, 0.42, 0.468, 0.4872, 0.4949, 0.498, 0.4992, 0.4997, 0.4999, 0.4999, 0.5, 0.5, 0.5, 0.5 };

/* The particle class represents a particle of mass that can move
* around in 3D space*/
class Particle {
private:

    bool movable;            // can the particle move or not ? used to pin parts of the cloth
    double mass;             // the mass of the particle (is always 1 in this example)
    Vec3 acceleration;       // a vector representing the current acceleration of the particle
    Vec3 accumulated_normal; // an accumulated normal (i.e. non normalized), used for OpenGL soft shading
    double time_step2;

public:

    // These two memebers are used in the process of edge smoothing after
    // the cloth simulation step.
    Vec3 pos;     // the current position of the particle in 3D space
    // the position of the particle in the previous time step, used as
    // part of the verlet numerical integration scheme
    Vec3 old_pos;
    bool isVisited;
    int neibor_count;
    int pos_x; // position in the cloth grid
    int pos_y;
    int c_pos;

    std::vector<Particle *> neighborsList;

    std::vector<int> correspondingLidarPointList;
    std::size_t nearestPointIndex;
    double nearestPointHeight;
    double tmpDist;
    void satisfyConstraintSelf(int constraintTimes)
    {
        Particle *p1 = this;

        for (std::size_t i = 0; i < neighborsList.size(); i++) {
            Particle *p2 = neighborsList[i];
            Vec3 correctionVector(0, p2->pos.f[1] - p1->pos.f[1], 0);

            if (p1->isMovable() && p2->isMovable()) {
                // Lets make it half that length, so that we can move BOTH p1 and p2.
                Vec3 correctionVectorHalf = correctionVector * (
                    constraintTimes > 14 ? 0.5 : doubleMove1[constraintTimes]
                    );
                p1->offsetPos(correctionVectorHalf);
                p2->offsetPos(-correctionVectorHalf);
            } else if (p1->isMovable() && !p2->isMovable()) {
                Vec3 correctionVectorHalf = correctionVector * (
                    constraintTimes > 14 ? 1 : singleMove1[constraintTimes]
                    );
                p1->offsetPos(correctionVectorHalf);
            } else if (!p1->isMovable() && p2->isMovable()) {
                Vec3 correctionVectorHalf = correctionVector * (
                    constraintTimes > 14 ? 1 : singleMove1[constraintTimes]
                    );
                p2->offsetPos(-correctionVectorHalf);
            }
        }
    }

public:

    Particle(Vec3 pos, double time_step) :
        movable(true),
        mass(1),
        acceleration(Vec3(0, 0, 0)),
        accumulated_normal(Vec3(0, 0, 0)),
        time_step2(time_step),
        pos(pos),
        old_pos(pos) {
        isVisited          = false;
        neibor_count       = 0;
        pos_x              = 0;
        pos_y              = 0;
        c_pos              = 0;
        nearestPointHeight = MIN_INF;
        tmpDist            = MAX_INF;
    }

    Particle() :
        movable(true),
        mass(1),
        acceleration(Vec3(0, 0, 0)),
        accumulated_normal(Vec3(0, 0, 0)),
        pos(Vec3(0, 0, 0)),
        old_pos(Vec3(0, 0, 0)) {
        isVisited          = false;
        neibor_count       = 0;
        pos_x              = 0;
        pos_y              = 0;
        c_pos              = 0;
        nearestPointHeight = MIN_INF;
        tmpDist            = MAX_INF;
    }


    bool isMovable() {
        return movable;
    }

    void addForce(Vec3 f) {
        acceleration += f / mass;
    }

    /* This is one of the important methods, where the time is
    * progressed a single step size (TIME_STEPSIZE) The method is
    * called by Cloth.time_step()*/
    void  timeStep()
    {
        if (movable) {
            Vec3 temp = pos;
            pos = pos + (pos - old_pos) * (1.0 - DAMPING) + acceleration * time_step2;
            old_pos = temp;
        }
    }

    Vec3& getPos() {
        return pos;
    }

    Vec3 getPosCopy() {
        return pos;
    }

    void resetAcceleration() {
        acceleration = Vec3(0, 0, 0);
    }

    void offsetPos(const Vec3 v) {
        if (movable) pos += v;
    }

    void makeUnmovable() {
        movable = false;
    }

    void addToNormal(Vec3 normal) {
        accumulated_normal += normal.normalized();
    }

    Vec3& getNormal() {
        // note: The normal is not unit length
        return accumulated_normal;
    }

    void resetNormal() {
        accumulated_normal = Vec3(0, 0, 0);
    }

    void printself(std::string s = "") {
        std::cout << s << ": " << this->getPos().f[0] << " movable:  " << this->movable << std::endl;
    }
};

// #include "Particle.h"
// #######################################################################################

// #include <boost/progress.hpp>
// post processing is only for connected component which is large than 50
#define MAX_PARTICLE_FOR_POSTPROCESSIN    50

struct XY {
    XY(int x1, int y1) {
        x = x1; y = y1;
    }

    int x;
    int y;
};

class Cloth {
private:

    // total number of particles is num_particles_width * num_particles_height
    int constraint_iterations;

    int rigidness;
    double time_step;

    std::vector<Particle> particles; // all particles that are part of this cloth

    double smoothThreshold;
    double heightThreshold;

public:

    Vec3 origin_pos;
    double step_x, step_y;
    std::vector<double> heightvals; // height values
    int num_particles_width;   // number of particles in width direction
    int num_particles_height;  // number of particles in height direction

    Particle* getParticle(int x, int y) {
        return &particles[y * num_particles_width + x];
    }

    void makeConstraint(Particle *p1, Particle *p2) {
        p1->neighborsList.push_back(p2);
        p2->neighborsList.push_back(p1);
    }

public:

    int getSize() {
        return num_particles_width * num_particles_height;
    }

    std::size_t get1DIndex(int x, int y) {
        return y * num_particles_width + x;
    }

    inline std::vector<double>& getHeightvals() {
        return heightvals;
    }

    Particle* getParticle1d(int index) {
        return &particles[index];
    }

public:

    /* This is a important constructor for the entire system of
    * particles and constraints */
    Cloth(const Vec3& origin_pos,
        int         num_particles_width,
        int         num_particles_height,
        double      step_x,
        double      step_y,
        double      smoothThreshold,
        double      heightThreshold,
        int         rigidness,
        double      time_step);

    /* this is an important methods where the time is progressed one
    * time step for the entire cloth.  This includes calling
    * satisfyConstraint() for every constraint, and calling
    * timeStep() for all particles
    */
    double timeStep();

    /* used to add gravity (or any other arbitrary vector) to all
    * particles */
    void addForce(const Vec3 direction);

    void terrCollision();

    void movableFilter();

    std::vector<int> findUnmovablePoint(std::vector<XY> connected);

    void        handle_slop_connected(std::vector<int>          edgePoints,
        std::vector<XY>           connected,
        std::vector<std::vector<int> > neibors);

    std::vector<double> toVector();

};

struct Params
{
    // refer to the website:http://ramm.bnu.edu.cn/projects/CSF/ for the setting of these paramters
    bool    bSlopeSmooth;
    double  time_step, class_threshold, cloth_resolution, height_threshold;
    int     rigidness, iterations;
};

class CSF
{
public:

    CSF();
    CSF(int index);

    // PointCloud set pointcloud
    void setPointCloud(CSG_PointCloud *_pPoints) { pPoints = _pPoints; };

    // The results are index of ground points in the original
    // pointcloud and write the cloth particles coordinates
    void do_filtering(std::vector<int>& groundIndexes, std::vector<int>& offGroundIndexes);//, bool exportCloth=true);
   

    std::vector<double> do_cloth_export();

private:

    // Do the filtering and return the Cloth object
    Cloth do_cloth();
        
    CSG_PointCloud *pPoints = NULL;

public:

    Params params;
    int index;
};

#endif // ifndef _CSF_SAGA_H_
