#include <pcl/common/time.h>
#include <pcl/common/transforms.h> // for transformPointCloud
#include <pcl/simulation/range_likelihood.h>
#include <pcl/pcl_config.h>

#include <boost/math/distributions/normal.hpp>

#include <GL/glew.h>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#endif

#include <random>

// For adding noise:
static std::minstd_rand rng(std::random_device{}());

//#define SIMULATION_DEBUG 1
#define DO_TIMING_PROFILE 0

// 301 values, 0.0 uniform  1.0 normal. properly truncated/normalized
float normal_sigma0x5_normal1x0_range0to3_step0x01[] = {
    1.59576912f, 1.59545000f, 1.59449302f, 1.59289932f, 1.59067083f, 1.58781019f,
    1.58432085f, 1.58020697f, 1.57547345f, 1.57012594f, 1.56417078f, 1.55761504f,
    1.55046646f, 1.54273348f, 1.53442517f, 1.52555126f, 1.51612211f, 1.50614865f,
    1.49564242f, 1.48461552f, 1.47308056f, 1.46105069f, 1.44853953f, 1.43556117f,
    1.42213012f, 1.40826131f, 1.39397005f, 1.37927201f, 1.36418316f, 1.34871978f,
    1.33289841f, 1.31673585f, 1.30024906f, 1.28345522f, 1.26637163f, 1.24901574f,
    1.23140504f, 1.21355714f, 1.19548963f, 1.17722012f, 1.15876621f, 1.14014544f,
    1.12137525f, 1.10247299f, 1.08345589f, 1.06434100f, 1.04514521f, 1.02588518f,
    1.00657737f, 0.98723796f, 0.96788290f, 0.94852781f, 0.92918802f, 0.90987853f,
    0.89061400f, 0.87140871f, 0.85227659f, 0.83323116f, 0.81428555f, 0.79545248f,
    0.77674422f, 0.75817263f, 0.73974913f, 0.72148465f, 0.70338972f, 0.68547437f,
    0.66774817f, 0.65022022f, 0.63289916f, 0.61579315f, 0.59890986f, 0.58225652f,
    0.56583986f, 0.54966616f, 0.53374122f, 0.51807038f, 0.50265855f, 0.48751015f,
    0.47262918f, 0.45801920f, 0.44368334f, 0.42962430f, 0.41584438f, 0.40234547f,
    0.38912908f, 0.37619631f, 0.36354792f, 0.35118428f, 0.33910545f, 0.32731110f,
    0.31580063f, 0.30457310f, 0.29362725f, 0.28296157f, 0.27257426f, 0.26246326f,
    0.25262625f, 0.24306068f, 0.23376378f, 0.22473257f, 0.21596387f, 0.20745431f,
    0.19920035f, 0.19119830f, 0.18344431f, 0.17593438f, 0.16866443f, 0.16163022f,
    0.15482742f, 0.14825164f, 0.14189837f, 0.13576305f, 0.12984106f, 0.12412773f,
    0.11861834f, 0.11330815f, 0.10819240f, 0.10326630f, 0.09852508f, 0.09396394f,
    0.08957812f, 0.08536286f, 0.08131342f, 0.07742511f, 0.07369324f, 0.07011320f,
    0.06668040f, 0.06339032f, 0.06023847f, 0.05722044f, 0.05433188f, 0.05156850f,
    0.04892611f, 0.04640054f, 0.04398775f, 0.04168374f, 0.03948462f, 0.03738655f,
    0.03538582f, 0.03347876f, 0.03166181f, 0.02993149f, 0.02828442f, 0.02671730f,
    0.02522691f, 0.02381013f, 0.02246393f, 0.02118538f, 0.01997160f, 0.01881983f,
    0.01772739f, 0.01669169f, 0.01571021f, 0.01478053f, 0.01390031f, 0.01306728f,
    0.01227925f, 0.01153414f, 0.01082990f, 0.01016460f, 0.00953635f, 0.00894336f,
    0.00838388f, 0.00785626f, 0.00735890f, 0.00689028f, 0.00644891f, 0.00603340f,
    0.00564241f, 0.00527464f, 0.00492888f, 0.00460393f, 0.00429869f, 0.00401209f,
    0.00374309f, 0.00349073f, 0.00325408f, 0.00303227f, 0.00282444f, 0.00262981f,
    0.00244761f, 0.00227712f, 0.00211766f, 0.00196858f, 0.00182926f, 0.00169912f,
    0.00157761f, 0.00146420f, 0.00135840f, 0.00125975f, 0.00116779f, 0.00108211f,
    0.00100231f, 0.00092803f, 0.00085891f, 0.00079462f, 0.00073485f, 0.00067930f,
    0.00062770f, 0.00057979f, 0.00053532f, 0.00049406f, 0.00045581f, 0.00042034f,
    0.00038748f, 0.00035705f, 0.00032887f, 0.00030280f, 0.00027868f, 0.00025638f,
    0.00023577f, 0.00021673f, 0.00019915f, 0.00018292f, 0.00016795f, 0.00015414f,
    0.00014141f, 0.00012968f, 0.00011887f, 0.00010893f, 0.00009977f, 0.00009135f,
    0.00008360f, 0.00007648f, 0.00006994f, 0.00006393f, 0.00005842f, 0.00005336f,
    0.00004872f, 0.00004446f, 0.00004056f, 0.00003699f, 0.00003372f, 0.00003072f,
    0.00002798f, 0.00002548f, 0.00002319f, 0.00002110f, 0.00001918f, 0.00001744f,
    0.00001585f, 0.00001439f, 0.00001307f, 0.00001186f, 0.00001076f, 0.00000976f,
    0.00000884f, 0.00000801f, 0.00000726f, 0.00000657f, 0.00000595f, 0.00000538f,
    0.00000486f, 0.00000440f, 0.00000397f, 0.00000359f, 0.00000324f, 0.00000292f,
    0.00000264f, 0.00000238f, 0.00000214f, 0.00000193f, 0.00000174f, 0.00000157f,
    0.00000141f, 0.00000127f, 0.00000114f, 0.00000103f, 0.00000092f, 0.00000083f,
    0.00000074f, 0.00000067f, 0.00000060f, 0.00000054f, 0.00000048f, 0.00000043f,
    0.00000039f, 0.00000035f, 0.00000031f, 0.00000028f, 0.00000025f, 0.00000022f,
    0.00000020f, 0.00000018f, 0.00000016f, 0.00000014f, 0.00000013f, 0.00000011f,
    0.00000010f, 0.00000009f, 0.00000008f, 0.00000007f, 0.00000006f, 0.00000006f,
    0.00000005f, 0.00000004f, 0.00000004f, 0.00000003f, 0.00000003f, 0.00000003f,
    0.00000002f};

// Where the above if lhoodf, this a hard coded/optimized version:
// ratio = 0.99; r_min =0;  	r_max = 3;
// lhood = ratio/(r_max -r_min)  + (1-ratio)*lhood ;
// hard_coded_log_lhood=std::log(lhood)
float hard_coded_log_lhood[] = {
    -1.0614388f, -1.0614480f, -1.0614757f, -1.0615217f, -1.0615862f, -1.0616689f,
    -1.0617698f, -1.0618887f, -1.0620256f, -1.0621803f, -1.0623526f, -1.0625423f,
    -1.0627491f, -1.0629730f, -1.0632135f, -1.0634705f, -1.0637437f, -1.0640327f,
    -1.0643372f, -1.0646569f, -1.0649914f, -1.0653405f, -1.0657036f, -1.0660804f,
    -1.0664705f, -1.0668735f, -1.0672889f, -1.0677164f, -1.0681554f, -1.0686054f,
    -1.0690662f, -1.0695370f, -1.0700176f, -1.0705073f, -1.0710057f, -1.0715124f,
    -1.0720267f, -1.0725482f, -1.0730764f, -1.0736108f, -1.0741509f, -1.0746962f,
    -1.0752462f, -1.0758003f, -1.0763581f, -1.0769191f, -1.0774827f, -1.0780486f,
    -1.0786162f, -1.0791851f, -1.0797547f, -1.0803247f, -1.0808945f, -1.0814638f,
    -1.0820321f, -1.0825989f, -1.0831639f, -1.0837267f, -1.0842868f, -1.0848439f,
    -1.0853977f, -1.0859476f, -1.0864935f, -1.0870350f, -1.0875718f, -1.0881035f,
    -1.0886298f, -1.0891506f, -1.0896655f, -1.0901742f, -1.0906766f, -1.0911723f,
    -1.0916613f, -1.0921433f, -1.0926181f, -1.0930855f, -1.0935454f, -1.0939976f,
    -1.0944421f, -1.0948787f, -1.0953073f, -1.0957277f, -1.0961400f, -1.0965441f,
    -1.0969398f, -1.0973272f, -1.0977063f, -1.0980769f, -1.0984391f, -1.0987930f,
    -1.0991384f, -1.0994755f, -1.0998042f, -1.1001246f, -1.1004367f, -1.1007407f,
    -1.1010364f, -1.1013241f, -1.1016038f, -1.1018756f, -1.1021396f, -1.1023958f,
    -1.1026444f, -1.1028855f, -1.1031191f, -1.1033454f, -1.1035646f, -1.1037767f,
    -1.1039819f, -1.1041802f, -1.1043719f, -1.1045570f, -1.1047358f, -1.1049082f,
    -1.1050746f, -1.1052349f, -1.1053894f, -1.1055382f, -1.1056815f, -1.1058193f,
    -1.1059518f, -1.1060792f, -1.1062016f, -1.1063192f, -1.1064320f, -1.1065402f,
    -1.1066440f, -1.1067435f, -1.1068389f, -1.1069302f, -1.1070176f, -1.1071012f,
    -1.1071811f, -1.1072575f, -1.1073306f, -1.1074003f, -1.1074668f, -1.1075303f,
    -1.1075909f, -1.1076486f, -1.1077036f, -1.1077560f, -1.1078059f, -1.1078533f,
    -1.1078985f, -1.1079414f, -1.1079821f, -1.1080208f, -1.1080576f, -1.1080925f,
    -1.1081256f, -1.1081569f, -1.1081867f, -1.1082148f, -1.1082415f, -1.1082667f,
    -1.1082906f, -1.1083132f, -1.1083345f, -1.1083547f, -1.1083737f, -1.1083917f,
    -1.1084086f, -1.1084246f, -1.1084397f, -1.1084538f, -1.1084672f, -1.1084798f,
    -1.1084917f, -1.1085028f, -1.1085133f, -1.1085231f, -1.1085324f, -1.1085411f,
    -1.1085492f, -1.1085569f, -1.1085640f, -1.1085707f, -1.1085770f, -1.1085829f,
    -1.1085885f, -1.1085936f, -1.1085985f, -1.1086030f, -1.1086072f, -1.1086111f,
    -1.1086148f, -1.1086183f, -1.1086215f, -1.1086245f, -1.1086272f, -1.1086298f,
    -1.1086323f, -1.1086345f, -1.1086366f, -1.1086385f, -1.1086404f, -1.1086420f,
    -1.1086436f, -1.1086451f, -1.1086464f, -1.1086477f, -1.1086488f, -1.1086499f,
    -1.1086509f, -1.1086518f, -1.1086527f, -1.1086534f, -1.1086542f, -1.1086549f,
    -1.1086555f, -1.1086561f, -1.1086566f, -1.1086571f, -1.1086575f, -1.1086580f,
    -1.1086583f, -1.1086587f, -1.1086590f, -1.1086593f, -1.1086596f, -1.1086599f,
    -1.1086601f, -1.1086603f, -1.1086605f, -1.1086607f, -1.1086609f, -1.1086610f,
    -1.1086611f, -1.1086613f, -1.1086614f, -1.1086615f, -1.1086616f, -1.1086617f,
    -1.1086618f, -1.1086619f, -1.1086619f, -1.1086620f, -1.1086620f, -1.1086621f,
    -1.1086621f, -1.1086622f, -1.1086622f, -1.1086623f, -1.1086623f, -1.1086623f,
    -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086625f,
    -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f,
    -1.1086625f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
    -1.1086626f};

// Disparity:
// sigma 0.025
double top_lookup[] = {
    15.9577,     15.8165,     15.4003,     14.7308,     13.8422,     12.7779,
    11.5877,     10.3231,     9.0345,      7.7674,      6.5604,      5.4433,
    4.4368,      3.5527,      2.7947,      2.1596,      1.6395,      1.2227,
    0.89578,     0.64471,     0.45584,     0.31662,     0.21604,     0.14482,
    0.095364,    0.061691,    0.039205,    0.024476,    0.015011,    0.0090444,
    0.0053532,   0.0031126,   0.001778,    0.0009977,   0.00054999,  0.00029784,
    0.00015845,  8.2811e-05,  4.2517e-05,  2.1444e-05,  1.0625e-05,  5.1718e-06,
    2.473e-06,   1.1617e-06,  5.361e-07,   2.4304e-07,  1.0824e-07,  4.7354e-08,
    2.0353e-08,  8.5933e-09,  3.5644e-09,  1.4524e-09,  5.8138e-10,  2.2862e-10,
    8.832e-11,   3.3518e-11,  1.2496e-11,  4.5766e-12,  1.6466e-12,  5.8201e-13,
    2.0209e-13,  6.8935e-14,  2.31e-14,    7.6043e-15,  2.4592e-15,  7.8127e-16,
    2.4383e-16,  7.4758e-17,  2.2517e-17,  6.6624e-18,  1.9366e-18,  5.5299e-19,
    1.5512e-19,  4.2749e-20,  1.1573e-20,  3.0778e-21,  8.0413e-22,  2.0639e-22,
    5.2038e-23,  1.289e-23,   3.1365e-24,  7.4975e-25,  1.7606e-25,  4.0617e-26,
    9.2049e-27,  2.0493e-27,  4.4821e-28,  9.6302e-29,  2.0327e-29,  4.2148e-30,
    8.5855e-31,  1.718e-31,   3.3774e-32,  6.5224e-33,  1.2374e-33,  2.3062e-34,
    4.2225e-35,  7.5947e-36,  1.3419e-36,  2.3294e-37,  3.9721e-38,  6.6539e-39,
    1.095e-39,   1.7703e-40,  2.8115e-41,  4.3864e-42,  6.7231e-43,  1.0123e-43,
    1.4973e-44,  2.1758e-45,  3.1059e-46,  4.3555e-47,  6.0003e-48,  8.1205e-49,
    1.0796e-49,  1.4101e-50,  1.8092e-51,  2.2804e-52,  2.8237e-53,  3.4349e-54,
    4.1047e-55,  4.8186e-56,  5.5571e-57,  6.2958e-58,  7.007e-59,   7.6611e-60,
    8.2287e-61,  8.6827e-62,  9.0002e-63,  9.165e-64,   9.1683e-65,  9.01e-66,
    8.6984e-67,  8.2497e-68,  7.6862e-69,  7.035e-70,   6.3255e-71,  5.5874e-72,
    4.8484e-73,  4.133e-74,   3.4611e-75,  2.8474e-76,  2.3012e-77,  1.827e-78,
    1.425e-79,   1.0918e-80,  8.2183e-82,  6.077e-83,   4.4144e-84,  3.1502e-85,
    2.2084e-86,  1.5209e-87,  1.029e-88,   6.8387e-90,  4.4651e-91,  2.864e-92,
    1.8046e-93,  1.1171e-94,  6.793e-96,   4.058e-97,   2.3815e-98,  1.373e-99,
    7.7759e-101, 4.3264e-102, 2.3647e-103, 1.2697e-104, 6.6975e-106, 3.4706e-107,
    1.7667e-108, 8.8352e-110, 4.3405e-111, 2.0948e-112, 9.9319e-114, 4.6259e-115,
    2.1166e-116, 9.514e-118,  4.2011e-119, 1.8224e-120, 7.7661e-122, 3.2512e-123,
    1.3371e-124, 5.402e-126,  2.144e-127,  8.3597e-129, 3.202e-130,  1.2049e-131,
    4.4538e-133, 1.6173e-134, 5.7697e-136, 2.022e-137,  6.9614e-139, 2.3544e-140,
    7.8227e-142, 2.5533e-143, 8.1871e-145, 2.5789e-146, 7.9803e-148, 2.426e-149,
    7.2448e-151, 2.1255e-152, 6.1257e-154, 1.7343e-155, 4.8239e-157, 1.3181e-158,
    3.538e-160,  9.3294e-162, 2.4167e-163, 6.1502e-165, 1.5375e-166, 3.7761e-168,
    9.1103e-170, 2.1593e-171, 5.0276e-173, 1.15e-174,   2.5841e-176, 5.7042e-178,
    1.237e-179,  2.6352e-181, 5.5149e-183, 1.1338e-184, 2.29e-186,   4.5436e-188,
    8.8561e-190, 1.6958e-191, 3.1899e-193, 5.8946e-195, 1.0701e-196, 1.9083e-198,
    3.3433e-200, 5.7541e-202, 9.7287e-204, 1.6159e-205, 2.6366e-207, 4.2263e-209,
    6.6552e-211, 1.0295e-212, 1.5645e-214, 2.3357e-216, 3.4256e-218, 4.9354e-220,
    6.9855e-222, 9.7128e-224, 1.3267e-225, 1.7803e-227, 2.3468e-229, 3.039e-231,
    3.8662e-233, 4.8318e-235, 5.9321e-237, 7.1548e-239, 8.4773e-241, 9.8673e-243,
    1.1283e-244, 1.2674e-246, 1.3986e-248, 1.5162e-250, 1.6147e-252, 1.6893e-254,
    1.7362e-256, 1.753e-258,  1.7388e-260, 1.6942e-262, 1.6218e-264, 1.525e-266,
    1.4088e-268, 1.2785e-270, 1.1398e-272, 9.9826e-275, 8.5888e-277, 7.2594e-279,
    6.0276e-281, 4.9167e-283, 3.9398e-285, 3.1014e-287, 2.3984e-289, 1.8221e-291,
    1.3598e-293, 9.9699e-296, 7.1808e-298, 5.0808e-300, 3.5316e-302, 2.4115e-304,
    1.6177e-306, 1.066e-308,  6.9011e-311, 4.3889e-313, 2.742e-315,  1.6829e-317,
    1.0147e-319, 6.324e-322,  0.0,         0.0,         0.0,         0.0,
    0.0,         0.0,         0.0,         0.0,         0.0,         0.0,
    0.0};
float bottom_lookup[] = {
    0.5f,     0.55304f, 0.60514f, 0.65542f, 0.7031f,  0.74751f, 0.78814f, 0.82468f,
    0.85694f, 0.88493f, 0.90879f, 0.92877f, 0.9452f,  0.95848f, 0.96903f, 0.97725f,
    0.98355f, 0.98829f, 0.9918f,  0.99435f, 0.99617f, 0.99744f, 0.99832f, 0.99892f,
    0.99931f, 0.99957f, 0.99974f, 0.99984f, 0.99991f, 0.99994f, 0.99997f, 0.99998f,
    0.99999f, 0.99999f, 1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
    1.0f,     1.0f,     1.0f,     0.99999f, 0.99999f, 0.99998f, 0.99997f, 0.99994f,
    0.99991f, 0.99984f, 0.99974f, 0.99957f, 0.99931f, 0.99892f, 0.99832f, 0.99744f,
    0.99617f, 0.99435f, 0.9918f,  0.98829f, 0.98355f, 0.97725f, 0.96903f, 0.95848f,
    0.9452f,  0.92877f, 0.90879f, 0.88493f, 0.85694f, 0.82468f, 0.78814f, 0.74751f,
    0.7031f,  0.65542f, 0.60514f, 0.55304f, 0.5f};

using namespace pcl::simulation;

// Finds the maximum level n so a and b are still
// divisible by 2^n
int
max_level(int a, int b)
{
  int level = 0;
  while (true) {
    if (a % 2 || b % 2)
      return level;
    a /= 2;
    b /= 2;
    level++;
  }
}

// display_tic_toc: a helper function which accepts a set of
// timestamps and displays the elapsed time between them as
// a fraction and time used [for profiling]
void
display_tic_toc(std::vector<double>& tic_toc, const std::string& fun_name)
{
  std::size_t tic_toc_size = tic_toc.size();

  double percent_tic_toc_last = 0;
  double dtime = tic_toc[tic_toc_size - 1] - tic_toc[0];
  std::cout << "fraction_" << fun_name << ",";
  for (std::size_t i = 0; i < tic_toc_size; i++) {
    double percent_tic_toc =
        (tic_toc[i] - tic_toc[0]) / (tic_toc[tic_toc_size - 1] - tic_toc[0]);
    std::cout << percent_tic_toc - percent_tic_toc_last << ", ";
    percent_tic_toc_last = percent_tic_toc;
  }
  std::cout << "\ntime_" << fun_name << ",";
  double time_tic_toc_last = 0;
  for (std::size_t i = 0; i < tic_toc_size; i++) {
    double percent_tic_toc =
        (tic_toc[i] - tic_toc[0]) / (tic_toc[tic_toc_size - 1] - tic_toc[0]);
    std::cout << percent_tic_toc * dtime - time_tic_toc_last << ", ";
    time_tic_toc_last = percent_tic_toc * dtime;
  }
  std::cout << "\ntotal_time_" << fun_name << " " << dtime << "\n";
}

pcl::simulation::RangeLikelihood::RangeLikelihood(
    int rows, int cols, int row_height, int col_width, Scene::Ptr scene)
: scene_(scene)
, rows_(rows)
, cols_(cols)
, row_height_(row_height)
, col_width_(col_width)
, sum_reduce_(cols * col_width, rows * row_height, max_level(col_width, row_height))
{
  height_ = rows_ * row_height;
  width_ = cols_ * col_width;

  depth_buffer_ = new float[width_ * height_];
  color_buffer_ = new std::uint8_t[width_ * height_ * 3];

  // Set Default Camera Intrinstic Parameters. techquad
  // Correspond closely to those stated here:
  // http://www.ros.org/wiki/kinect_calibration/technical
  camera_width_ = 640;
  camera_height_ = 480;
  camera_fx_ = 576.09757860f;
  camera_fy_ = 576.09757860f;
  camera_cx_ = 321.06398107f;
  camera_cy_ = 242.97676897f;

  z_near_ = 0.7f;
  z_far_ = 20.0f;

  which_cost_function_ = 2; // default to commonly used meter based function

  // default lhood parameters - these should always be set by the user
  // so might want to add to constructor eventually:
  sigma_ = 0.1;
  floor_proportion_ = 0.9;

  int height = rows * row_height;
  int width = cols * col_width;

  // For now we only support a limited size texture
  assert(height > 0 && height <= 8192 && width > 0 && width <= 8192);
  // throw std::runtime_error "

  // Allocate framebuffer
  glGenRenderbuffers(1, &depth_render_buffer_);
  glBindRenderbuffer(GL_RENDERBUFFER, depth_render_buffer_);
  glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT32, width, height);
  glBindRenderbuffer(GL_RENDERBUFFER, 0);

  glGenRenderbuffers(1, &color_render_buffer_);
  glBindRenderbuffer(GL_RENDERBUFFER, color_render_buffer_);
  glRenderbufferStorage(GL_RENDERBUFFER, GL_RGB8UI, width, height);
  glBindRenderbuffer(GL_RENDERBUFFER, 0);

  // Setup texture to store depth image
  glGenTextures(1, &depth_texture_);
  glBindTexture(GL_TEXTURE_2D, depth_texture_);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
  glTexImage2D(GL_TEXTURE_2D,
               0,
               GL_DEPTH_COMPONENT32,
               width,
               height,
               0,
               GL_DEPTH_COMPONENT,
               GL_FLOAT,
               nullptr);
  glBindTexture(GL_TEXTURE_2D, 0);

  glGenTextures(1, &color_texture_);
  glBindTexture(GL_TEXTURE_2D, color_texture_);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
  glTexImage2D(
      GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
  glBindTexture(GL_TEXTURE_2D, 0);

  // Setup texture for incoming image
  glGenTextures(1, &sensor_texture_);
  glBindTexture(GL_TEXTURE_2D, sensor_texture_);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
  glTexImage2D(
      GL_TEXTURE_2D, 0, GL_R32F, col_width, row_height, 0, GL_RED, GL_FLOAT, nullptr);
  glBindTexture(GL_TEXTURE_2D, 0);

  // Texture for to score on each pixel
  glGenTextures(1, &score_texture_);
  glBindTexture(GL_TEXTURE_2D, score_texture_);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
  glTexImage2D(
      GL_TEXTURE_2D, 0, GL_R32F, width_, height_, 0, GL_RED, GL_FLOAT, nullptr);
  glBindTexture(GL_TEXTURE_2D, 0);

  // Setup texture for likelihood function
  // size of likelihood texture
  int likelihood_size = 301;
  glActiveTexture(GL_TEXTURE2);
  glGenTextures(1, &likelihood_texture_);
  glBindTexture(GL_TEXTURE_2D, likelihood_texture_);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_RED);
  glTexImage2D(GL_TEXTURE_2D,
               0,
               GL_R32F,
               likelihood_size,
               1,
               0,
               GL_RED,
               GL_FLOAT,
               normal_sigma0x5_normal1x0_range0to3_step0x01);
  glBindTexture(GL_TEXTURE_2D, 0);

  // Setup the framebuffer object for rendering
  glGenFramebuffers(1, &fbo_);
  glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
  glFramebufferTexture2D(
      GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_texture_, 0);
  glFramebufferTexture2D(
      GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, color_texture_, 0);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  if (gllib::getGLError() != GL_NO_ERROR) {
    std::cerr << "RangeLikelihoodGLSL::RangeLikelihoodGLSL: Failed initializing OpenGL "
                 "buffers"
              << std::endl;
    exit(-1);
  }

  glGenFramebuffers(1, &score_fbo_);
  glBindFramebuffer(GL_FRAMEBUFFER, score_fbo_);
  glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, 0);
  glFramebufferTexture2D(
      GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, score_texture_, 0);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  // Load shader
  likelihood_program_ = gllib::Program::Ptr(new gllib::Program());
  // TODO: to remove file dependency include the shader source in the binary
  if (!likelihood_program_->addShaderFile("compute_score.vert", gllib::VERTEX)) {
    std::cout << "Failed loading vertex shader" << std::endl;
    exit(-1);
  }

  if (!likelihood_program_->addShaderFile("compute_score.frag", gllib::FRAGMENT)) {
    std::cout << "Failed loading fragment shader" << std::endl;
    exit(-1);
  }

  likelihood_program_->link();

  vertices_.emplace_back(-1.0, 1.0, 0.0);
  vertices_.emplace_back(1.0, 1.0, 0.0);
  vertices_.emplace_back(1.0, -1.0, 0.0);
  vertices_.emplace_back(-1.0, -1.0, 0.0);

  glGenBuffers(1, &quad_vbo_);
  glBindBuffer(GL_ARRAY_BUFFER, quad_vbo_);
  glBufferData(GL_ARRAY_BUFFER,
               sizeof(Eigen::Vector3f) * vertices_.size(),
               vertices_.data(),
               GL_STATIC_DRAW);
  glBindBuffer(GL_ARRAY_BUFFER, 0);

  gllib::getGLError();

  // Go back to the default pipeline
  glUseProgram(0);

  score_buffer_ = new float[width_ * height_];
}

pcl::simulation::RangeLikelihood::~RangeLikelihood()
{
  glDeleteBuffers(1, &quad_vbo_);
  glDeleteTextures(1, &depth_texture_);
  glDeleteTextures(1, &color_texture_);
  glDeleteTextures(1, &score_texture_);
  glDeleteTextures(1, &score_summarized_texture_);
  glDeleteTextures(1, &sensor_texture_);
  glDeleteTextures(1, &likelihood_texture_);
  glDeleteFramebuffers(1, &fbo_);
  glDeleteFramebuffers(1, &score_fbo_);
  glDeleteRenderbuffers(1, &depth_render_buffer_);
  glDeleteRenderbuffers(1, &color_render_buffer_);

  delete[] depth_buffer_;
  delete[] color_buffer_;
  delete[] score_buffer_;
}

double
pcl::simulation::RangeLikelihood::sampleNormal(double sigma)
{
  std::normal_distribution<> dist(0.0, sigma);
  return (dist(rng));
}

void
pcl::simulation::RangeLikelihood::setupProjectionMatrix() const
{
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  // Prepare scaled simulated camera projection matrix
  float sx = static_cast<float>(camera_width_) / static_cast<float>(col_width_);
  float sy = static_cast<float>(camera_height_) / static_cast<float>(row_height_);
  float width = static_cast<float>(col_width_);
  float height = static_cast<float>(row_height_);

  float fx = camera_fx_ / sx;
  float fy = camera_fy_ / sy;
  float cx = camera_cx_ / sx;
  float cy = camera_cy_ / sy;
  float z_nf = (z_near_ - z_far_);

  // clang-format off
  float m[16] = {2.0f * fx / width       , 0                        , 0                              , 0     ,
                 0                       , 2.0f * fy / height       , 0                              , 0     ,
                 1.0f - (2 * cx / width) , 1.0f - (2 * cy / height) , (z_far_ + z_near_) / z_nf      , -1.0f ,
                 0                       , 0                        , 2.0f * z_near_ * z_far_ / z_nf , 0};
  // clang-format on

  glMultMatrixf(m);
}

void
pcl::simulation::RangeLikelihood::applyCameraTransform(const Eigen::Isometry3d& pose)
{
  Eigen::Matrix4f m = (pose.matrix().inverse()).cast<float>();
  // clang-format off
  float T[16] = {
      m(0, 0), m(1, 0), m(2, 0), m(3, 0),
      m(0, 1), m(1, 1), m(2, 1), m(3, 1),
      m(0, 2), m(1, 2), m(2, 2), m(3, 2),
      m(0, 3), m(1, 3), m(2, 3), m(3, 3),
  };
  // clang-format on
  glMultMatrixf(T);
}

void
pcl::simulation::RangeLikelihood::drawParticles(
    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses)
{
  int n = 0;
  for (int i = 0; i < rows_; ++i) {
    for (int j = 0; j < cols_; ++j) {
      glMatrixMode(GL_MODELVIEW);
      glLoadIdentity();

      glViewport(j * col_width_, i * row_height_, col_width_, row_height_);

      // Go from Z-up, X-forward coordinate frame
      // to OpenGL Z-out,Y-up [both Right Handed]
      // clang-format off
      float T[16] = {
           0, 0, -1, 0,
          -1, 0,  0, 0,
           0, 1,  0, 0,
           0, 0,  0, 1,
      };
      // clang-format on
      glMultMatrixf(T);

      // Apply camera transformation
      applyCameraTransform(poses[n++]);

      // Draw the planes in each location:
      scene_->draw();
    }
  }
}

/////////////////////////////////////////////////////////////////
// Below are 4 previously used cost functions:
// 0 original scoring method
float
costFunction0(float ref_val, float depth_val)
{
  return (sqr(ref_val - depth_val));
}

// 1st working cost function:
// Empirical reverse mapping between depthbuffer and true depth:
// Version 0: [25 aug 2011]
// TRUEDEPTH =  1/(1.33 -(DEPTHBUFFER)*1.29)
// float cost = sqr(ref[col%col_width] - 1/(1.33 -(*depth)*1.29));
// Version 1: [29 aug 2011] Exact version using correct mappings:
float
costFunction1(float ref_val, float depth_val)
{
  float cost = sqr(ref_val - 1 / (1.4285f - (depth_val)*1.3788f));
  // std::cout << " [" << ref_val << "," << 1/(1.4285 -(depth_val)*1.3788) << "] ";
  if (ref_val < 0) { // all images pixels with no range
    cost = 1;
  }
  if (cost > 10) {
    // required to lessen the effect of modelpixel with no range (ie holes in the model)
    cost = 10;
  }
  // return std::log (cost);
  return (cost);
}

// 1st working likelihood function (by far most commonly used)
float
costFunction2(float ref_val, float depth_val)
{
  float min_dist = std::abs(ref_val - 1 / (1.4285f - (depth_val)*1.3788f));
  int lup = static_cast<int>(std::ceil(min_dist * 100)); // has resolution of 0.01m

  if (lup > 300) { // implicitly this caps the cost if there is a hole in the model
    lup = 300;
  }

  double lhood = 1;
  if (std::isnan(depth_val)) { // pixels with nan depth - for openNI null points
    lhood = 1;                 // std::log(1) = 0 ---> has no effect
  }
  else if (ref_val < 0) { // all RGB pixels with no depth - for freenect null points
    lhood = 1;            // std::log(1) = 0 ---> has no effect
  }
  else {
    lhood = normal_sigma0x5_normal1x0_range0to3_step0x01[lup];
    // add a ground floor:
    // increasing this will mean that the likelihood is less peaked
    // but you need more particles to do this...
    // with ~90particles user 0.999, for example in the quad dataset
    // ratio of uniform to	normal
    double ratio = 0.99; // was always 0.99;
    double r_min = 0;    // metres
    double r_max = 3;    // metres
    lhood = ratio / (r_max - r_min) + (1 - ratio) * lhood;
  }
  return static_cast<float>(std::log(lhood));
}

float
costFunction3(float ref_val, float depth_val)
{
  float log_lhood = 0;
  // std::log(1) = 0 ---> has no effect
  if (ref_val < 0) {
    // all images pixels with no range
  }
  else if (ref_val > 7) {
    // ignore long ranges... for now
  }
  else { // working range
    float min_dist = std::abs(ref_val - 0.7253f / (1.0360f - (depth_val)));

    int lup = static_cast<int>(std::ceil(min_dist * 100)); // has resolution of 0.01m
    if (lup > 300) { // implicitly this caps the cost if there is a hole in the model
      lup = 300;
    }
    log_lhood = hard_coded_log_lhood[lup];
  }
  return log_lhood;
}

float
costFunction4(float ref_val, float depth_val)
{
  float disparity_diff = std::abs((-0.7253f / ref_val + 1.0360f) - depth_val);

  int top_lup =
      static_cast<int>(std::ceil(disparity_diff * 300)); // has resolution of 0.001m
  if (top_lup > 300) {
    top_lup = 300;
  }
  float top =
      static_cast<float>(top_lookup[top_lup]); // round( std::abs(x-mu) *1000+1) );

  // bottom:
  // bottom = bottom_lookup(   round(mu*1000+1));
  int bottom_lup =
      static_cast<int>(std::ceil((depth_val)*300)); // has resolution of 0.001m
  if (bottom_lup > 300) {
    bottom_lup = 300;
  }
  float bottom = bottom_lookup[bottom_lup]; // round( std::abs(x-mu) *1000+1) );

  float proportion = 0.999f;
  float lhood = proportion + (1 - proportion) * (top / bottom);

  // safety fix that seems to be required due to opengl asynchronization
  // ask hordur about this
  if (bottom == 0) {
    lhood = proportion;
  }

  if (ref_val < 0) { // all images pixels with no range
    lhood = 1;       // std::log(1) = 0 ---> has no effect
  }
  return std::log(lhood);
}

// TODO: WHEN WE'RE HAPPY THIS SHOULD BE "THE" LIKELIHOOD FUNCTION
// add these global variables into the class
// abd use sigma and floor_proportion directly from class also
using boost::math::normal;   // typedef provides default type is double.
normal unit_norm_dist(0, 1); // (default mean = zero, and standard deviation = unity)
double
costFunction5(double measured_depth,
              double model_disp,
              double sigma,
              double floor_proportion)
{
  // NEED TO CONVERT MEASURED TO DISPARITY
  double measured_disp = (-0.7253 / measured_depth + 1.0360);

  // measured_depth = ref_val  [m]
  // model_disp = depth_val [0-1]
  // upper and lower bound on depth buffer:
  double lower_bound = 0;
  double upper_bound = 1;

  double gaussian_part =
      pdf(unit_norm_dist, (measured_disp - model_disp) / sigma) / sigma;
  double truncation = 1 / cdf(unit_norm_dist, (upper_bound - model_disp) / sigma) -
                      cdf(unit_norm_dist, (lower_bound - model_disp) / sigma);

  double trunc_gaussian_part = truncation * gaussian_part;

  double lhood = (floor_proportion / (upper_bound - lower_bound) +
                  (1 - floor_proportion) * trunc_gaussian_part);

  if (measured_depth < 0) { // all images pixels with no range
    lhood = 1;              // std::log(1) = 0 ---> has no effect
  }

  return std::log(lhood);
}

void
pcl::simulation::RangeLikelihood::computeScores(float* reference,
                                                std::vector<float>& scores)
{
  const float* depth = getDepthBuffer();
  // Mapping between disparity and range:
  // range or depth = 1/disparity
  //
  // the freenect produces a disparity <here we call this depth_buffer>
  // that is mapped between 0->1 to minimize quantization
  // near_range = n = 0.7m   | far_range = f = 20m
  // disparity can be found as a linear function of the depth_buffer (d = [0,1] )
  // disparity =  1/n   - (f-n)*d / (n*f)
  // Below We compare range-versus-range using this mapping
  //
  // TODO: remove usage of 'depth' and 'depth_buffer_' as variable names as it implies
  // that that is was held by these variables

  // ref[col%col_width] - z/depth value in metres,   0-> ~20
  // depth_val - contents of depth buffer [0->1]

  // for row across each image in a row of model images
  for (int row = 0; row < rows_ * row_height_; row++) {
    float* ref = reference + col_width_ * (row % row_height_);
    // for each column: across each image in a column of model images
    for (int col = 0; col < cols_ * col_width_; col++) {
      float depth_val =
          (*depth++); // added jan 2012 - check this is not a breaking fix later mfallon
      float score = 0;
      if (which_cost_function_ == 0) {
        score = costFunction0(ref[col % col_width_], depth_val);
      }
      else if (which_cost_function_ == 1) {
        score = costFunction1(ref[col % col_width_], depth_val);
      }
      else if (which_cost_function_ == 2) {
        score = costFunction2(ref[col % col_width_], depth_val);
      }
      else if (which_cost_function_ == 3) {
        score = costFunction3(ref[col % col_width_], depth_val);
      }
      else if (which_cost_function_ == 4) {
        score = costFunction4(ref[col % col_width_], depth_val);
      }
      else if (which_cost_function_ == 5) {
        // double sigma = 0.025;
        // double floor_proportion_ = 0.999;
        score = static_cast<float>(
            costFunction5(ref[col % col_width_], depth_val, sigma_, floor_proportion_));
      }
      scores[row / row_height_ * cols_ + col / col_width_] += score;
      // std::cout << "(" << scores[row/row_height_ * cols_ + col/col_width_] <<"," <<
      // score << "," << ref[col%col_width_] << "," << depth_val << ") ";
      score_buffer_[row * width_ + col] = score;
    }
  }
  score_buffer_dirty_ = false;
}

void
pcl::simulation::RangeLikelihood::getPointCloud(
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
    bool make_global,
    const Eigen::Isometry3d& pose,
    bool organized) const
{
  // TODO: check if this works for for rows/cols >1  and for width&height != 640x480
  // i.e. multiple tiled images
  pc->width = col_width_;
  pc->height = row_height_;
  // Was:
  // pc->width    = camera_width_;
  // pc->height   = camera_height_;

  pc->is_dense = true;
  pc->points.resize(pc->width * pc->height);

  int points_added = 0;

  float camera_fx_reciprocal_ = 1.0f / camera_fx_;
  float camera_fy_reciprocal_ = 1.0f / camera_fy_;
  float zn = z_near_;
  float zf = z_far_;

  const std::uint8_t* color_buffer = getColorBuffer();

  // TODO: support decimation
  // Copied the format of RangeImagePlanar::setDepthImage()
  // Use this as a template for decimation
  for (int y = 0; y < row_height_; ++y) // camera_height_
  {
    for (int x = 0; x < col_width_; ++x) // camera_width_
    {
      // Find XYZ from normalized 0->1 mapped disparity
      int idx;
      if (organized)
        idx = y * col_width_ + x;
      else
        idx = points_added; // y*camera_width_ + x;

      float d = depth_buffer_[y * camera_width_ + x];

      if (d < 1.0) // only add points with depth buffer less than max (20m) range
      {
        float z = zf * zn / ((zf - zn) * (d - zf / (zf - zn)));

        // TODO: add mode to ignore points with no return i.e. depth_buffer_ ==1
        // NB: OpenGL uses a Right Hand system with +X right, +Y up, +Z back out of the
        // screen, The Z-buffer is natively -1 (far) to 1 (near). But in this class we
        // invert this to be 0 (near, 0.7m) and 1 (far, 20m), so by negating y we get to
        // a right-hand computer vision system which is also used by PCL and OpenNi.
        (*pc)[idx].z = z;
        (*pc)[idx].x =
            (static_cast<float>(x) - camera_cx_) * z * (-camera_fx_reciprocal_);
        (*pc)[idx].y =
            (static_cast<float>(y) - camera_cy_) * z * (-camera_fy_reciprocal_);

        int rgb_idx = y * col_width_ + x;             // camera_width_
        (*pc)[idx].b = color_buffer[rgb_idx * 3 + 2]; // blue
        (*pc)[idx].g = color_buffer[rgb_idx * 3 + 1]; // green
        (*pc)[idx].r = color_buffer[rgb_idx * 3];     // red
        points_added++;
      }
      else if (organized) {
        pc->is_dense = false;
        (*pc)[idx].z = std::numeric_limits<float>::quiet_NaN();
        (*pc)[idx].x = std::numeric_limits<float>::quiet_NaN();
        (*pc)[idx].y = std::numeric_limits<float>::quiet_NaN();
        (*pc)[idx].rgba = 0;
      }
    }
  }

  if (!organized) {
    pc->height = 1;
    pc->width = points_added;
    pc->points.resize(points_added);
  }

  if (make_global) {
    // Go from OpenGL to (Z-up, X-forward, Y-left)
    Eigen::Matrix4f T;
    // clang-format off
    T <<  0, 0, -1, 0,
         -1, 0,  0, 0,
          0, 1,  0, 0,
          0, 0,  0, 1;
    // clang-format on
    Eigen::Matrix4f m = pose.matrix().cast<float>();
    m *= T;
    pcl::transformPointCloud(*pc, *pc, m);
  }
  else {
    // Go from OpenGL to Camera (Z-forward, X-right, Y-down)
    Eigen::Matrix4f T;
    // clang-format off
    T << 1,  0,  0, 0,
         0, -1,  0, 0,
         0,  0, -1, 0,
         0,  0,  0, 1;
    // clang-format on
    pcl::transformPointCloud(*pc, *pc, T);

    // Go from Camera to body (Z-up, X-forward, Y-left)
    Eigen::Matrix4f cam_to_body;
    // clang-format off
    cam_to_body <<  0,  0, 1, 0,
                   -1,  0, 0, 0,
                    0, -1, 0, 0,
                    0,  0, 0, 1;
    // clang-format on
    Eigen::Matrix4f camera = pose.matrix().cast<float>();
    camera *= cam_to_body;
    pc->sensor_origin_ = camera.rightCols(1);
    Eigen::Quaternion<float> quat(camera.block<3, 3>(0, 0));
    pc->sensor_orientation_ = quat;
  }
}

void
pcl::simulation::RangeLikelihood::getRangeImagePlanar(pcl::RangeImagePlanar& rip) const
{
  rip.setDepthImage(depth_buffer_,
                    camera_width_,
                    camera_height_,
                    camera_fx_,
                    camera_fy_,
                    camera_fx_,
                    camera_fy_);
}

void
pcl::simulation::RangeLikelihood::addNoise()
{
  // Other noises:
  // edge noise: look for edges in the range image and add a few pixels here and there
  // texture noise: look at the normals and

  // Add Gaussian Noise
  // TODO: make the variance a parameter
  // TODO: might want to add a range-based variance
  float variance = 0.0015f;
  for (int i = 0; i < camera_width_ * camera_height_; i++) {
    if (depth_buffer_[i] < 1) {
      depth_buffer_[i] += variance * static_cast<float>(sampleNormal());
      if (depth_buffer_[i] > 1) {
        depth_buffer_[i] = 1.0;
      }
      else if (depth_buffer_[i] < 0) {
        depth_buffer_[i] = 0.0;
      }
    }
  }

  // Add Kinect/Primesense Quantisation Noise:
  // TODO: better fit this:
  // 0.6m  = ~600 kinect return
  // 20m   = ~1070 kinect return - not not well calibrated
  // The fitted model stated here cannot work for long ranges:
  // http://www.ros.org/wiki/kinect_calibration/technical
  // TODO: make a parameter
  float bins = 470;
  for (int i = 0; i < camera_width_ * camera_height_; i++) {
    depth_buffer_[i] = std::ceil(depth_buffer_[i] * bins) / bins;
  }
  std::cout << "in add noise\n";
}

void
RangeLikelihood::computeLikelihoods(
    float* reference,
    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses,
    std::vector<float>& scores)
{
#if DO_TIMING_PROFILE
  std::vector<double> tic_toc;
  tic_toc.push_back(getTime());
#endif

  scores.resize(cols_ * rows_);
  std::fill(scores.begin(), scores.end(), 0);

  // Generate depth image for each particle
  render(poses);

#if DO_TIMING_PROFILE
  tic_toc.push_back(getTime());
#endif

#if DO_TIMING_PROFILE
  tic_toc.push_back(getTime());
#endif
  // The depth image is now in depth_texture_

  // Compute likelihoods
  if (compute_likelihood_on_cpu_) {
    computeScores(reference, scores);
  }
  else {
    computeScoresShader(reference);

    // Aggregate results (we do not use GPU to sum cpu scores)
    if (aggregate_on_cpu_) {
      const float* score_buffer = getScoreBuffer();

      for (int n = 0, row = 0; row < height_; ++row) {
        for (int col = 0; col < width_; ++col, ++n) {
          scores[row / row_height_ * cols_ + col / col_width_] += score_buffer[n];
        }
      }
    }
    else {
      int levels = max_level(row_height_, col_width_);
      int reduced_width = width_ >> levels;
      int reduced_height = height_ >> levels;
      int reduced_col_width = col_width_ >> levels;
      int reduced_row_height = row_height_ >> levels;

      float* score_sum = new float[reduced_width * reduced_height];
      sum_reduce_.sum(score_texture_, score_sum);
      for (int n = 0, row = 0; row < reduced_height; ++row) {
        for (int col = 0; col < reduced_width; ++col, ++n) {
          scores[row / reduced_row_height * cols_ + col / reduced_col_width] +=
              score_sum[n];
        }
      }
      delete[] score_sum;
    }
  }

#if DO_TIMING_PROFILE
  tic_toc.push_back(getTime());
  display_tic_toc(tic_toc, "range_likelihood");
#endif
}

// Computes the likelihood scores using a shader
void
pcl::simulation::RangeLikelihood::computeScoresShader(float* reference)
{
  if (gllib::getGLError() != GL_NO_ERROR) {
    std::cout << "GL error: RangeLikelihood::compute_scores_shader - enter"
              << std::endl;
  }

#ifdef SIMULATION_DEBUG
  std::cout << "DepthSampler location: "
            << likelihood_program_->getUniformLocation("DepthSampler") << std::endl;
  std::cout << "ReferenceSampler location: "
            << likelihood_program_->getUniformLocation("ReferenceSampler") << std::endl;
  std::cout << "CostSampler location: "
            << likelihood_program_->getUniformLocation("CostSampler") << std::endl;

  int depth_tex_id;
  int ref_tex_id;
  int cost_tex_id;

  glGetUniformiv(likelihood_program_->programId(),
                 likelihood_program_->getUniformLocation("DepthSampler"),
                 &depth_tex_id);
  glGetUniformiv(likelihood_program_->programId(),
                 likelihood_program_->getUniformLocation("ReferenceSampler"),
                 &ref_tex_id);
  glGetUniformiv(likelihood_program_->programId(),
                 likelihood_program_->getUniformLocation("CostSampler"),
                 &cost_tex_id);
  std::cout << "depth id: " << depth_tex_id << " " << GL_TEXTURE0 << std::endl;
  std::cout << "ref id: " << ref_tex_id << " " << GL_TEXTURE1 << std::endl;
  std::cout << "cost id: " << cost_tex_id << " " << GL_TEXTURE2 << std::endl;
#endif

  likelihood_program_->use();
  likelihood_program_->setUniform("DepthSampler", 0);
  likelihood_program_->setUniform("ReferenceSampler", 1);
  likelihood_program_->setUniform("CostSampler", 2);
  likelihood_program_->setUniform("cols", cols_);
  likelihood_program_->setUniform("rows", rows_);
  likelihood_program_->setUniform("near", z_near_);
  likelihood_program_->setUniform("far", z_far_);

  glBindFramebuffer(GL_FRAMEBUFFER, score_fbo_);
  glDrawBuffer(GL_COLOR_ATTACHMENT0);

  glReadBuffer(GL_NONE);
  GLboolean enable_depth_test;
  glGetBooleanv(GL_DEPTH_TEST, &enable_depth_test);
  glDisable(GL_DEPTH_TEST);
  glViewport(0, 0, width_, height_);

  // Setup textures
  glActiveTexture(GL_TEXTURE0);
  glBindTexture(GL_TEXTURE_2D, depth_texture_);

  glActiveTexture(GL_TEXTURE1);
  glBindTexture(GL_TEXTURE_2D, sensor_texture_);
  glTexImage2D(GL_TEXTURE_2D,
               0,
               GL_R32F,
               col_width_,
               row_height_,
               0,
               GL_RED,
               GL_FLOAT,
               reference);

  glActiveTexture(GL_TEXTURE2);
  glBindTexture(GL_TEXTURE_2D, likelihood_texture_);

  quad_.render();
  glUseProgram(0);

  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  // Unbind all textures that were used
  glActiveTexture(GL_TEXTURE0);
  glBindTexture(GL_TEXTURE_2D, 0);
  glActiveTexture(GL_TEXTURE1);
  glBindTexture(GL_TEXTURE_2D, 0);
  glActiveTexture(GL_TEXTURE2);
  glBindTexture(GL_TEXTURE_2D, 0);

  if (gllib::getGLError() != GL_NO_ERROR) {
    std::cout << "GL error: RangeLikelihood::compute_scores_shader - exit" << std::endl;
  }

  if (enable_depth_test == GL_TRUE)
    glEnable(GL_DEPTH_TEST);
}

void
RangeLikelihood::render(
    const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>&
        poses)
{
  if (gllib::getGLError() != GL_NO_ERROR) {
    std::cerr << "GL Error: RangeLikelihood::render - enter" << std::endl;
  }

  GLint old_matrix_mode;
  GLint old_draw_buffer;
  GLint old_read_buffer;
  glGetIntegerv(GL_DRAW_BUFFER, &old_draw_buffer);
  glGetIntegerv(GL_READ_BUFFER, &old_read_buffer);
  glGetIntegerv(GL_MATRIX_MODE, &old_matrix_mode);

  glMatrixMode(GL_PROJECTION);
  glPushMatrix();

  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();

  glBindFramebuffer(GL_FRAMEBUFFER, fbo_);

  if (use_color_) {
    glDrawBuffer(GL_COLOR_ATTACHMENT0);
  }
  else {
    glDrawBuffer(GL_NONE);
  }
  glReadBuffer(GL_NONE);

  GLenum status;
  status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
  switch (status) {
  case GL_FRAMEBUFFER_COMPLETE: {
    break;
  }
  default: {
    std::cout << "RangeLikelihood::render: Framebuffer failed" << std::endl;
    exit(-1);
  }
  }

  // Render
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glEnable(GL_COLOR_MATERIAL);
  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
  glClearDepth(1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  // Setup projection matrix
  setupProjectionMatrix();

  glEnable(GL_DEPTH_TEST);
  glDepthMask(GL_TRUE);
  glCullFace(GL_FRONT);
  drawParticles(poses);

  glPopAttrib();
  glFlush();

  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  // Restore OpenGL state
  glReadBuffer(old_read_buffer);
  glDrawBuffer(old_draw_buffer);

  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();

  glMatrixMode(GL_PROJECTION);
  glPopMatrix();

  glMatrixMode(old_matrix_mode);

  if (gllib::getGLError() != GL_NO_ERROR) {
    std::cerr << "GL Error: RangeLikelihood::render - exit" << std::endl;
  }

  color_buffer_dirty_ = true;
  depth_buffer_dirty_ = true;
  score_buffer_dirty_ = true;
}

const float*
RangeLikelihood::getDepthBuffer() const
{
  if (depth_buffer_dirty_) {
    // Read depth
    glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
    glReadPixels(0, 0, width_, height_, GL_DEPTH_COMPONENT, GL_FLOAT, depth_buffer_);
    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    if (gllib::getGLError() != GL_NO_ERROR) {
      std::cerr << "GL Error: RangeLikelihoodGLSL::getDepthBuffer" << std::endl;
    }

    depth_buffer_dirty_ = false;
  }
  return depth_buffer_;
}

const std::uint8_t*
RangeLikelihood::getColorBuffer() const
{
  // It's only possible to read the color buffer if it
  // was rendered in the first place.
  assert(use_color_);

  if (color_buffer_dirty_) {
    std::cout << "Read color buffer" << std::endl;

    // Read Color
    GLint old_read_buffer;
    glGetIntegerv(GL_READ_BUFFER, &old_read_buffer);

    glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
    glReadBuffer(GL_COLOR_ATTACHMENT0);
    glReadPixels(0, 0, width_, height_, GL_RGB, GL_UNSIGNED_BYTE, color_buffer_);
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    glReadBuffer(old_read_buffer);

    if (gllib::getGLError() != GL_NO_ERROR) {
      std::cerr << "GL Error: RangeLikelihood::getColorBuffer" << std::endl;
    }

    color_buffer_dirty_ = false;
  }
  return color_buffer_;
}

// The scores are in score_texture_
const float*
RangeLikelihood::getScoreBuffer() const
{
  if (score_buffer_dirty_ && !compute_likelihood_on_cpu_) {
    glActiveTexture(GL_TEXTURE0);
    glBindTexture(GL_TEXTURE_2D, score_texture_);
    glGetTexImage(GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, score_buffer_);
    glBindTexture(GL_TEXTURE_2D, 0);
    if (gllib::getGLError() != GL_NO_ERROR) {
      std::cerr << "GL Error: RangeLikelihood::getScoreBuffer" << std::endl;
    }
    score_buffer_dirty_ = false;
  }
  return score_buffer_;
}
