/*!
 * \file driver.cc
 *
 * \author Geoffrey F. Bomarito
 * \date
 *
 * This file contains the main function for BingoCpp.
 *
 * Notices
 * -------
 * Copyright 2018 United States Government as represented by the Administrator of
 * the National Aeronautics and Space Administration. No copyright is claimed in
 * the United States under Title 17, U.S. Code. All Other Rights Reserved.
 *
 *
 * Disclaimers
 * -----------
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY
 * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR
 * FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE ERROR
 * FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO THE
 * SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE, IF
 * PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."

 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST THE
 * UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL AS ANY
 * PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS IN ANY
 * LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH USE,
 * INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM, RECIPIENT'S USE
 * OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD HARMLESS THE UNITED
 * STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL AS ANY PRIOR
 * RECIPIENT, TO THE EXTENT PERMITTED BY LAW.  RECIPIENT'S SOLE REMEDY FOR ANY
 * SUCH MATTER SHALL BE THE IMMEDIATE, UNILATERAL TERMINATION OF THIS AGREEMENT.
 */

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <iostream>
#include <chrono>

#include <Eigen/Dense>

#include "BingoCpp/version.h"
#include "BingoCpp/backend.h"

int test_eig() {
  Eigen::MatrixXd m(2, 2);
  m(0, 0) = 3;
  m(1, 0) = 2.5;
  m(0, 1) = -1;
  m(1, 1) = m(1, 0) + m(0, 1);
  std::cout << m << std::endl;
}

void TestAcyclicGraph(int num_loops, int num_evals) {
  Eigen::ArrayX3i stack(12, 3);
  Eigen::ArrayX3i simple_stack(8, 3);
  Eigen::ArrayXXd x(60, 3);
  Eigen::VectorXd constants(2);
  // y = x_0 * ( C_0 + C_1/x_1 ) - x_0
  stack << 0, 0, 0,
        0, 1, 1,
        1, 0, 0,
        1, 1, 1,
        5, 3, 1,
        5, 3, 1,
        2, 4, 2,
        2, 4, 2,
        4, 6, 0,
        4, 5, 6,
        3, 7, 6,
        3, 8, 0;
  simple_stack << 0, 0, 0,
               0, 1, 1,
               1, 0, 0,
               1, 1, 1,
               5, 3, 1,
               2, 4, 2,
               4, 5, 0,
               3, 6, 0;
  x << 1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.,
  1., 2., 3., 4., 5., 6., 7., 8., 9., 1., 2., 3., 4., 5., 6., 7., 8., 9.;
  constants << 3.14, 10.0;
  //PrintStack(stack);
  Eigen::ArrayXXd y;
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy;
  double avg_time_per_eval = 0.;
  double avg_time_per_seval = 0.;
  double avg_time_per_deval = 0.;
  double avg_time_per_sdeval = 0.;

  for (int i = 0; i < num_loops; ++i) {
    std::chrono::high_resolution_clock::time_point t1;
    std::chrono::high_resolution_clock::time_point t2;
    std::chrono::duration<double, std::micro> duration;
    t1 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < num_evals; ++i) {
      y = bingo::backend::Evaluate(stack, x, constants);
    }

    t2 = std::chrono::high_resolution_clock::now();
    duration = t2 - t1;
    avg_time_per_eval += duration.count() / num_evals;
    t1 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < num_evals; ++i) {
      y = bingo::backend::Evaluate(simple_stack, x, constants);
    }

    t2 = std::chrono::high_resolution_clock::now();
    duration = t2 - t1;
    avg_time_per_seval += duration.count() / num_evals;
    t1 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < num_evals; ++i) {
      y_and_dy = bingo::backend::EvaluateWithDerivative(stack, x, constants);
    }

    t2 = std::chrono::high_resolution_clock::now();
    duration = t2 - t1;
    avg_time_per_deval += duration.count() / num_evals;
    t1 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < num_evals; ++i) {
      y_and_dy = bingo::backend::EvaluateWithDerivative(simple_stack, x, constants);
    }

    t2 = std::chrono::high_resolution_clock::now();
    duration = t2 - t1;
    avg_time_per_sdeval += duration.count() / num_evals;
  }

  avg_time_per_eval /= num_loops;
  avg_time_per_seval /= num_loops;
  avg_time_per_deval /= num_loops;
  avg_time_per_sdeval /= num_loops;
  std::cout << "Eval:              " << avg_time_per_eval << " microseconds\n";
  std::cout << "Simple Eval:       " << avg_time_per_seval << " microseconds\n";
  std::cout << "Eval Deriv:        " << avg_time_per_deval << " microseconds\n";
  std::cout << "Simple Eval Deriv: " << avg_time_per_sdeval << " microseconds\n";
}

int main(int argc, char *argv[]) {
  srand (time(NULL));

  if (argc < 3) {
    fprintf(stdout, "%s Version %d.%d.%d.%d\n", argv[0], PROJECT_VERSION_MAJOR,
            PROJECT_VERSION_MINOR, PROJECT_VERSION_PATCH, PROJECT_VERSION_TWEAK);
    fprintf(stdout, "Usage: %s number number\n", argv[0]);
    return 1;
  }

  TestAcyclicGraph(std::atol(argv[1]), std::atol(argv[2]));
  return 0;
}


/* This file is part of libtrevisan, a modular implementation of
   Trevisan's randomness extraction construction.

   Copyright (C) 2011-2012, Wolfgang Mauerer <wm@linux-kernel.net>

   This program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public License
   as published by the Free Software Foundation; either version 2
   of the License, or (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with libtrevisan. If not, see <http://www.gnu.org/licenses/>. */

// Generate a list of precomputed irreducible polynomials

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/tools.h>
#include <iostream>
#include <sys/time.h>

NTL_CLIENT

unsigned int field_sizes[] = {8192, 16382, 32768, 65536, 131072, 262144, 524288};

// returns time in seconds
double cpuSecond()
{
    struct timeval tp;
    gettimeofday(&tp, NULL);
    return ((double)tp.tv_sec + (double)tp.tv_usec * 1.e-6);
}

void gen_irreps()
{
	for( unsigned n = 0; n <= sizeof(field_sizes)/sizeof(unsigned int); n++ )
	{
		GF2X P;

		double iStart, iElaps;
		iStart = cpuSecond();

		clog << "calculating irreducible polynomial of size " << std::to_string(field_sizes[n]) << " Bit takes ... ";

		BuildSparseIrred(P, field_sizes[n]);

		iElaps = cpuSecond() - iStart;
		clog << std::to_string(iElaps) << "s" << std::endl;
	}
}

int main(int argc, char **argv) {
	gen_irreps();
	return 0;
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

/* Test_IO.cpp - Testing the I/O of the important classes of the library
 * (context, keys, ciphertexts).
 */
#include <fstream>
#include <unistd.h>

#include <NTL/ZZX.h>
#include <NTL/vector.h>

#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

#define N_TESTS 3
static long ms[N_TESTS][10] = {
  //nSlots  m   phi(m) ord(2)
  //  {   2,    7,    6,    3,   0,0,0,0,0,0},
  {   6,   31,   30,    5,   0,0,0,0,0,0},
  {   6,  127,  126,    7,  127,  1,  108,  24,   6,-3}, // gens=108(6), 24(!3)
  {  60, 1023,  600,   10,   11, 93,  838, 584,  10, 6}, // gens=838(10),584(6)
  //  {  378,  5461,  5292, 14}, // gens=3(126),509(3)
  //  {  630,  8191,  8190, 13}, // gens=39(630)
  //  {  600, 13981, 12000, 20}, // gens=10(30),23(10),3(!2)
  //  {  682, 15709, 15004, 22} // gens=5(682)
};

void checkCiphertext(const Ctxt& ctxt, const ZZX& ptxt, const FHESecKey& sk);

// Testing the I/O of the important classes of the library
// (context, keys, ciphertexts).
int main(int argc, char *argv[])
{
  ArgMapping amap;

  long r=1;
  long p=2;
  long c = 2;
  long w = 64;
  long L = 5;
  long mm=0;
  amap.arg("p", p, "plaintext base");
  amap.arg("r", r,  "lifting");
  amap.arg("c", c, "number of columns in the key-switching matrices");
  amap.arg("m", mm, "cyclotomic index","{31,127,1023}");
  amap.parse(argc, argv);

  bool useTable = (mm==0 && p==2);
  long ptxtSpace = power_long(p,r);
  long numTests = useTable? N_TESTS : 1;

  std::unique_ptr<FHEcontext> contexts[numTests];
  std::unique_ptr<FHESecKey> sKeys[numTests];
  std::unique_ptr<Ctxt> ctxts[numTests];
  std::unique_ptr<EncryptedArray> eas[numTests];
  vector<ZZX> ptxts[numTests];

  // first loop: generate stuff and write it to cout

  // open file for writing
  {fstream keyFile("iotest.txt", fstream::out|fstream::trunc);
   assert(keyFile.is_open());
  for (long i=0; i<numTests; i++) {
    long m = (mm==0)? ms[i][1] : mm;

    cout << "Testing IO: m="<<m<<", p^r="<<p<<"^"<<r<<endl;

    Vec<long> mvec(INIT_SIZE,2);
    mvec[0] = ms[i][4];  mvec[1] = ms[i][5];
    vector<long> gens(2);
    gens[0] = ms[i][6];  gens[1] = ms[i][7];
    vector<long> ords(2);
    ords[0] = ms[i][8];  ords[1] = ms[i][9];

    if (useTable && gens[0]>0)
      contexts[i].reset(new FHEcontext(m, p, r, gens, ords));
    else
      contexts[i].reset(new FHEcontext(m, p, r));
    contexts[i]->zMStar.printout();

    buildModChain(*contexts[i], L, c);  // Set the modulus chain
    if (mm==0 && m==1023) contexts[i]->makeBootstrappable(mvec);

    // Output the FHEcontext to file
    writeContextBase(keyFile, *contexts[i]);
    writeContextBase(cout, *contexts[i]);
    keyFile << *contexts[i] << endl;

    sKeys[i].reset(new FHESecKey(*contexts[i]));
    const FHEPubKey& publicKey = *sKeys[i];
    sKeys[i]->GenSecKey(w,ptxtSpace); // A Hamming-weight-w secret key
    addSome1DMatrices(*sKeys[i]);// compute key-switching matrices that we need
    eas[i].reset(new EncryptedArray(*contexts[i]));

    long nslots = eas[i]->size();

    // Output the secret key to file, twice. Below we will have two copies
    // of most things.
    keyFile << *sKeys[i] << endl;;
    keyFile << *sKeys[i] << endl;;

    vector<ZZX> b;
    long p2r = eas[i]->getContext().alMod.getPPowR();
    ZZX poly = RandPoly(0,to_ZZ(p2r)); // choose a random constant polynomial
    eas[i]->decode(ptxts[i], poly);

    ctxts[i].reset(new Ctxt(publicKey));
    eas[i]->encrypt(*ctxts[i], publicKey, ptxts[i]);
    eas[i]->decrypt(*ctxts[i], *sKeys[i], b);
    assert(ptxts[i].size() == b.size());
    for (long j = 0; j < nslots; j++) assert (ptxts[i][j] == b[j]);

    // output the plaintext
    keyFile << "[ ";
    for (long j = 0; j < nslots; j++) keyFile << ptxts[i][j] << " ";
    keyFile << "]\n";

    eas[i]->encode(poly,ptxts[i]);
    keyFile << poly << endl;

    // Output the ciphertext to file
    keyFile << *ctxts[i] << endl;
    keyFile << *ctxts[i] << endl;
    cerr << "okay " << i << endl<< endl;
  }
  keyFile.close();}
  cerr << "so far, so good\n\n";

  // second loop: read from input and repeat the computation

  // open file for read
  {fstream keyFile("iotest.txt", fstream::in);
  for (long i=0; i<numTests; i++) {

    // Read context from file
    unsigned long m1, p1, r1;
    vector<long> gens, ords;
    readContextBase(keyFile, m1, p1, r1, gens, ords);
    FHEcontext tmpContext(m1, p1, r1, gens, ords);
    keyFile >> tmpContext;
    assert (*contexts[i] == tmpContext);
    cerr << i << ": context matches input\n";

    // We define some things below wrt *contexts[i], not tmpContext.
    // This is because the various operator== methods check equality of
    // references, not equality of the referenced FHEcontext objects.
    FHEcontext& context = *contexts[i];
    FHESecKey secretKey(context);
    FHESecKey secretKey2(tmpContext);
    const FHEPubKey& publicKey = secretKey;
    const FHEPubKey& publicKey2 = secretKey2;

    keyFile >> secretKey;
    keyFile >> secretKey2;
    assert(secretKey == *sKeys[i]);
    cerr << "   secret key matches input\n";

    EncryptedArray ea(context);
    EncryptedArray ea2(tmpContext);

    long nslots = ea.size();

    // Read the plaintext from file
    vector<ZZX> a;
    a.resize(nslots);
    assert(nslots == (long)ptxts[i].size());
    seekPastChar(keyFile, '['); // defined in NumbTh.cpp
    for (long j = 0; j < nslots; j++) {
      keyFile >> a[j];
      assert(a[j] == ptxts[i][j]);
    }
    seekPastChar(keyFile, ']');
    cerr << "   ptxt matches input\n";

    // Read the encoded plaintext from file
    ZZX poly1, poly2;
    keyFile >> poly1;
    eas[i]->encode(poly2,a);
    assert(poly1 == poly2);
    cerr << "   eas[i].encode(a)==poly1 okay\n";

    ea.encode(poly2,a);
    assert(poly1 == poly2);
    cerr << "   ea.encode(a)==poly1 okay\n";

    ea2.encode(poly2,a);
    assert(poly1 == poly2);
    cerr << "   ea2.encode(a)==poly1 okay\n";

    eas[i]->decode(a,poly1);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   eas[i].decode(poly1)==ptxts[i] okay\n";

    ea.decode(a,poly1);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea.decode(poly1)==ptxts[i] okay\n";

    ea2.decode(a,poly1);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea2.decode(poly1)==ptxts[i] okay\n";

    // Read ciperhtext from file
    Ctxt ctxt(publicKey);
    Ctxt ctxt2(publicKey2);
    keyFile >> ctxt;
    keyFile >> ctxt2;
    assert(ctxts[i]->equalsTo(ctxt,/*comparePkeys=*/false));
    cerr << "   ctxt matches input\n";

    sKeys[i]->Decrypt(poly2,*ctxts[i]);
    assert(poly1 == poly2);
    cerr << "   sKeys[i]->decrypt(*ctxts[i]) == poly1 okay\n";

    secretKey.Decrypt(poly2,*ctxts[i]);
    assert(poly1 == poly2);
    cerr << "   secretKey.decrypt(*ctxts[i]) == poly1 okay\n";

    secretKey.Decrypt(poly2,ctxt);
    assert(poly1 == poly2);
    cerr << "   secretKey.decrypt(ctxt) == poly1 okay\n";

    secretKey2.Decrypt(poly2,ctxt2);
    assert(poly1 == poly2);
    cerr << "   secretKey2.decrypt(ctxt2) == poly1 okay\n";

    eas[i]->decrypt(ctxt, *sKeys[i], a);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   eas[i].decrypt(ctxt, *sKeys[i])==ptxts[i] okay\n";

    ea.decrypt(ctxt, secretKey, a);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea.decrypt(ctxt, secretKey)==ptxts[i] okay\n";

    ea2.decrypt(ctxt2, secretKey2, a);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea2.decrypt(ctxt2, secretKey2)==ptxts[i] okay\n";

    cerr << "test "<<i<<" okay\n\n";
  }}
  unlink("iotest.txt"); // clean up before exiting
}


#ifndef z_perm_hpp
#define z_perm_hpp

#include <Eigen/Dense>

class ZorderPermute {
  typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> PMatrix;
public:
  // three constructors
  //  (1) defalut constructor
  //  (2) takes the grid information and compute the permutation matrix
  //  (3) takes the permutation matrix
  ZorderPermute();
  ZorderPermute(int nx, int ny, int level);
  explicit ZorderPermute(const PMatrix&);

  // the inverse (transpose) permutation
  ZorderPermute inverse();
  ZorderPermute transpose();

  // operator overloading functions
  //  so the object can be used like a permutation matrix
  friend Eigen::MatrixXd operator*(const ZorderPermute&,
				   const Eigen::MatrixXd&);
  friend Eigen::MatrixXd operator*(const Eigen::MatrixXd&,
				   const ZorderPermute&);
private:
  void BuildMapOnQuadrant
  (int* map, int& index, int curLevel, int thisXSize, int thisYSize);

  // private variable
  int nx_;
  int ny_;
  int numLevel_;
  PMatrix P;
};

#endif


#include <gsa/agent.hh>

#include <boost/test/unit_test.hpp>
#include <random>

BOOST_AUTO_TEST_SUITE( ts_gsa_agent )

BOOST_AUTO_TEST_CASE( basic )  {
  auto f = [](common::vector_type v) {
             return v.squaredNorm();
           };

  std::vector<gsa::agent> pop;

  common::vector_type vel(2);
  common::vector_type pos(2);

  pos << 3, 4;
  vel << 0, 0;
  pop.emplace_back(gsa::agent(f, pos, vel, false));

  pos << -2, 3;
  vel <<  2, 2;
  pop.emplace_back(gsa::agent(f, pos, vel, false));

  pos <<  1, 2;
  vel << -1, 1;
  pop.emplace_back(gsa::agent(f, pos, vel, false));

  BOOST_CHECK_CLOSE(25.0, pop[0].fitness(), 1e-10);
  BOOST_CHECK_CLOSE(13.0, pop[1].fitness(), 1e-10);
  BOOST_CHECK_CLOSE( 5.0, pop[2].fitness(), 1e-10);

  for(auto &a : pop) {
    a.update_mass_pass1(5.0, 25.0);
  }

  BOOST_CHECK_SMALL(     pop[0].mass(), 1e-10);
  BOOST_CHECK_CLOSE(0.6, pop[1].mass(), 1e-10);
  BOOST_CHECK_CLOSE(1.0, pop[2].mass(), 1e-10);

  for(auto &a : pop) {
    a.update_mass_pass2(1.6);
  }

  BOOST_CHECK_SMALL(           pop[0].mass(), 1e-10);
  BOOST_CHECK_CLOSE(0.6 / 1.6, pop[1].mass(), 1e-10);
  BOOST_CHECK_CLOSE(1.0 / 1.6, pop[2].mass(), 1e-10);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * @Description:  使用正态分布变换进行配准
 * https://www.cnblogs.com/li-yao7758258/p/6554582.html
 * http://robot.czxy.com/docs/pcl/chapter03/registration/#ndt
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-22 19:20:43
 * @LastEditTime: 2020-10-22 19:26:04
 * @FilePath: /pcl-learning/14registration配准/4正态分布变换配准(NDT)/normal_distributions_transform.cpp
 */
/*
使用正态分布变换进行配准的实验 。其中room_scan1.pcd  room_scan2.pcd这些点云包含同一房间360不同视角的扫描数据
*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  （使用体素网格过滤器处理的效果比较好）

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int
main (int argc, char** argv)
{
  // 加载房间的第一次扫描点云数据作为目标
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../room_scan1.pcd", *target_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;

  // 加载从新视角得到的第二次扫描点云数据作为源点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../room_scan2.pcd", *input_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
  //以上的代码加载了两个PCD文件得到共享指针，后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，得到第二组点云变换到第一组点云坐标系下的变换矩阵
  // 将输入的扫描点云数据过滤到原始尺寸的10%以提高匹配的速度，只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理
  //因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setInputCloud (input_cloud);
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from room_scan2.pcd" << std::endl;

  // 初始化正态分布(NDT)对象
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // 根据输入数据的尺度设置NDT相关参数

  ndt.setTransformationEpsilon (0.01);   //为终止条件设置最小转换差异

  ndt.setStepSize (0.1);    //为more-thuente线搜索设置最大步长

  ndt.setResolution (1.0);   //设置NDT网格网格结构的分辨率（voxelgridcovariance）
  //以上参数在使用房间尺寸比例下运算比较好，但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体，需要对参数进行很大程度的缩小

  //设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon变换阀值下终止
  //添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  ndt.setMaximumIterations (35);

  ndt.setInputSource (filtered_cloud);  //源点云
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);  //目标点云

  // 设置使用机器人测距法得到的粗略初始变换矩阵结果
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  // 计算需要的刚体变换以便将输入的源点云匹配到目标点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt.align (*output_cloud, init_guess);
   //这个地方的output_cloud不能作为最终的源点云变换，因为上面对点云进行了滤波处理
  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // 使用创建的变换对为过滤的输入点云进行变换
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // 保存转换后的源点云作为最终的变换输出
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // 初始化点云可视化对象
  boost::shared_ptr<pcl::visualization::PCLVisualizer>
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色

  // 对目标点云着色可视化 (red).
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  target_color (target_cloud, 255, 0, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "target cloud");

  // 对转换后的源点云着色 (green)可视化.
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  output_color (output_cloud, 0, 255, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "output cloud");

  // 启动可视化
  viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
  viewer_final->initCameraParameters ();   //初始化摄像头参数

  // 等待直到可视化窗口关闭
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}


// Boost.GIL (Generic Image Library)
//
// Copyright (c) 2015, Oracle and/or its affiliates.
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
//
// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html
//
// Source: Boost.Geometry (aka GGL, Generic Geometry Library)
// Modifications: adapted for Boost.GIL
//  - Rename namespace boost::geometry to boost::gil
//  - Rename define BOOST_GEOMETRY_TEST_DEBUG to BOOST_GIL_TEST_DEBUG
//  - Remove use of macro BOOST_GEOMETRY_CONDITION
//  - Remove support for boost::multiprecision types
//  - Remove support for 128-bit integer types
//  - Update and sort includes
//  - Add explicit conversions to avoid warnings due to implicit integral promotions
//
#include <boost/gil/promote_integral.hpp>

#include <climits>
#include <cstddef>
#include <algorithm>
#include <limits>
#include <iostream>
#include <string>
#include <sstream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_promote_integral
#endif
#include "unit_test.hpp"

// Uncomment to enable debugging output
//#define BOOST_GIL_TEST_DEBUG 1

namespace bg = boost::gil;

template
<
    typename T,
    bool Signed = std::is_fundamental<T>::type::value && !std::is_unsigned<T>::type::value
>
struct absolute_value
{
    static inline T apply(T const& t)
    {
        return static_cast<T>(t < 0 ? -t : t);
    }
};

template <typename T>
struct absolute_value<T, false>
{
    static inline T apply(T const& t)
    {
        return t;
    }
};

template
    <
        typename Integral,
        typename Promoted,
        bool Signed = !std::is_unsigned<Promoted>::type::value
    >
struct test_max_values
{
    static inline void apply()
    {
        // Use >= where value is guaranteed to never be greater than comparator
        // but to avoid warning: comparing floating point with == is unsafe.

        Promoted min_value = (std::numeric_limits<Integral>::min)();
        // Explicit casts to avoid warning: conversion to short int from int may alter its value
        min_value = static_cast<Promoted>(min_value * min_value);
        BOOST_CHECK(absolute_value<Promoted>::apply(min_value) >= min_value);
        BOOST_CHECK(absolute_value<Promoted>::apply(min_value) <= min_value);
        Promoted max_value = (std::numeric_limits<Integral>::max)();
        max_value = static_cast<Promoted>(max_value * max_value);
        BOOST_CHECK(absolute_value<Promoted>::apply(max_value) >= max_value);
        BOOST_CHECK(absolute_value<Promoted>::apply(max_value) <= max_value);

#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "integral min_value^2: " << min_value << std::endl;
        std::cout << "promoted max_value:   "
                  << (std::numeric_limits<Promoted>::max)() << std::endl;
#endif
    }
};

template <typename Integral, typename Promoted>
struct test_max_values<Integral, Promoted, false>
{
    static inline void apply()
    {
        Promoted max_value = (std::numeric_limits<Integral>::max)();
        Promoted max_value_sqr = static_cast<Promoted>(max_value * max_value);
        BOOST_CHECK(max_value_sqr < (std::numeric_limits<Promoted>::max)()
                    &&
                    max_value_sqr > max_value);

#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "integral max_value^2: " << max_value_sqr << std::endl;
        std::cout << "promoted max_value:   "
                  << (std::numeric_limits<Promoted>::max)() << std::endl;
#endif
    }
};

// helper function that returns the bit size of a type
template
    <
        typename T,
        bool IsFundamental = std::is_fundamental<T>::type::value
    >
struct bit_size_impl : std::integral_constant<std::size_t, 0>
{};

template <typename T>
struct bit_size_impl<T, true> : bg::detail::promote_integral::bit_size<T>::type
{};

template <typename T>
std::size_t bit_size()
{
    return bit_size_impl<T>::type::value;
}

template <bool PromoteUnsignedToUnsigned>
struct test_promote_integral
{
    template <typename Type, typename ExpectedPromotedType>
    static inline void apply(std::string const& case_id)
    {
        using promoted_integral_type = typename bg::promote_integral
            <
                Type, PromoteUnsignedToUnsigned
            >::type;

        bool const same_types = std::is_same
            <
                promoted_integral_type, ExpectedPromotedType
            >::type::value;

        BOOST_CHECK_MESSAGE(same_types,
                            "case ID: " << case_id
                                        << "input type: " << typeid(Type).name()
                                        << "; detected: "
                                        << typeid(promoted_integral_type).name()
                                        << "; expected: "
                                        << typeid(ExpectedPromotedType).name());

        if (!std::is_same<Type, promoted_integral_type>::type::value)
        {
            test_max_values<Type, promoted_integral_type>::apply();
        }

#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "case ID: " << case_id << std::endl
                  << "type : " << typeid(Type).name()
                  << ", sizeof (bits): " << bit_size<Type>()
                  << ", min value: "
                  << (std::numeric_limits<Type>::min)()
                  << ", max value: "
                  << (std::numeric_limits<Type>::max)()
                  << std::endl;
        std::cout << "detected promoted type : "
                  << typeid(promoted_integral_type).name()
                  << ", sizeof (bits): " << bit_size<promoted_integral_type>()
                  << ", min value: "
                  << (std::numeric_limits<promoted_integral_type>::min)()
                  << ", max value: "
                  << (std::numeric_limits<promoted_integral_type>::max)()
                  << std::endl;
        std::cout << "expected promoted type : "
                  << typeid(ExpectedPromotedType).name()
                  << ", sizeof (bits): " << bit_size<ExpectedPromotedType>()
                  << ", min value: "
                  << (std::numeric_limits<ExpectedPromotedType>::min)()
                  << ", max value: "
                  << (std::numeric_limits<ExpectedPromotedType>::max)()
                  << std::endl;
        std::cout << std::endl;
#endif
    }
};

template
        <
                typename T,
                bool PromoteUnsignedToUnsigned = false,
                bool IsSigned = !std::is_unsigned<T>::type::value
        >
struct test_promotion
{
    static inline void apply(std::string case_id)
    {
#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "*** "
                  << (IsSigned ? "signed" : "unsigned")
                  << " -> signed ***" << std::endl;
#endif

        using tester = test_promote_integral<PromoteUnsignedToUnsigned>;

        case_id += (PromoteUnsignedToUnsigned ? "-t" : "-f");

        std::size_t min_size = 2 * bit_size<T>() - 1;
        if (!IsSigned)
        {
            min_size += 2;
        }

#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "min size: " << min_size << std::endl;
#endif

        if (bit_size<short>() >= min_size)
        {
            tester::template apply<T, short>(case_id);
        }
        else if (bit_size<int>() >= min_size)
        {
            tester::template apply<T, int>(case_id);
        }
        else if (bit_size<long>() >= min_size)
        {
            tester::template apply<T, long>(case_id);
        }
#if defined(BOOST_HAS_LONG_LONG)
        else if (bit_size<boost::long_long_type>() >= min_size)
        {
            tester::template apply<T, boost::long_long_type>(case_id);
        }
#endif
        else
        {
            tester::template apply<T, T>(case_id);
        }
    }
};

template <typename T>
struct test_promotion<T, true, false>
{
    static inline void apply(std::string case_id)
    {
#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "*** unsigned -> unsigned ***" << std::endl;
#endif
        case_id += "-t";

        using tester = test_promote_integral<true>;

        std::size_t min_size = 2 * bit_size<T>();

#ifdef BOOST_GIL_TEST_DEBUG
        std::cout << "min size: " << min_size << std::endl;
#endif

        if (bit_size<unsigned short>() >= min_size)
        {
            tester::apply<T, unsigned short>(case_id);
        }
        else if (bit_size<unsigned int>() >= min_size)
        {
            tester::apply<T, unsigned int>(case_id);
        }
        else if (bit_size<unsigned long>() >= min_size)
        {
            tester::apply<T, unsigned long>(case_id);
        }
        else if (bit_size<std::size_t>() >= min_size)
        {
            tester::apply<T, std::size_t>(case_id);
        }
#if defined(BOOST_HAS_LONG_LONG)
        else if (bit_size<boost::ulong_long_type>() >= min_size)
        {
            tester::template apply<T, boost::ulong_long_type>(case_id);
        }
#endif
        else
        {
            tester::apply<T, T>(case_id);
        }
    }
};

BOOST_AUTO_TEST_CASE( test_char )
{
    test_promotion<char>::apply("char");
    test_promotion<char, true>::apply("char");
    test_promotion<signed char>::apply("schar");
    test_promotion<signed char, true>::apply("schar");
    test_promotion<unsigned char>::apply("uchar");
    test_promotion<unsigned char, true>::apply("uchar");
}

BOOST_AUTO_TEST_CASE( test_short )
{
    test_promotion<short>::apply("short");
    test_promotion<short, true>::apply("short");
    test_promotion<unsigned short>::apply("ushort");
    test_promotion<unsigned short, true>::apply("ushort");
}

BOOST_AUTO_TEST_CASE( test_int )
{
    test_promotion<int>::apply("int");
    test_promotion<int, true>::apply("int");
    test_promotion<unsigned int>::apply("uint");
    test_promotion<unsigned int, true>::apply("uint");
}

BOOST_AUTO_TEST_CASE( test_long )
{
    test_promotion<long>::apply("long");
    test_promotion<long, true>::apply("long");
    test_promotion<unsigned long>::apply("ulong");
    test_promotion<unsigned long, true>::apply("ulong");
}

BOOST_AUTO_TEST_CASE( test_std_size_t )
{
    test_promotion<std::size_t>::apply("size_t");
    test_promotion<std::size_t, true>::apply("size_t");
}

#ifdef BOOST_HAS_LONG_LONG
BOOST_AUTO_TEST_CASE( test_long_long )
{
    test_promotion<boost::long_long_type>::apply("long long");
    test_promotion<boost::long_long_type, true>::apply("long long");
    test_promotion<boost::ulong_long_type>::apply("ulong long");
    test_promotion<boost::ulong_long_type, true>::apply("ulong long");
}
#endif

BOOST_AUTO_TEST_CASE( test_floating_point )
{
    using tester1 = test_promote_integral<true>;
    using tester2 = test_promote_integral<false>;

    // for floating-point types we do not do any promotion
    tester1::apply<float, float>("fp-f");
    tester1::apply<double, double>("fp-d");
    tester1::apply<long double, long double>("fp-ld");

    tester2::apply<float, float>("fp-f");
    tester2::apply<double, double>("fp-d");
    tester2::apply<long double, long double>("fp-ld");
}


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <graphblas/graphblas.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE mxm_test_suite

#include <boost/test/included/unit_test.hpp>

using namespace grb;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************

namespace
{
    static std::vector<std::vector<double> > mA_dense_3x3 =
            {{12, 7, 3},
             {4,  5, 6},
             {7,  8, 9}};

    static std::vector<std::vector<double> > mB_dense_3x4 =
            {{5, 8, 1, 2},
             {6, 7, 3, 0.},
             {4, 5, 9, 1}};

    static std::vector<std::vector<double> > mAnswer_dense =
            {{114, 160, 60,  27},
             {74,  97,  73,  14},
             {119, 157, 112, 23}};

    static std::vector<std::vector<double> > mA_sparse_3x3 =
            {{12.3, 7.5,  0},
             {0,    -5.2, 0},
             {7.0,  0,    9.0}};

    static std::vector<std::vector<double> > mA_sparse_1337zero_3x3 =
            {{12.3, 7.5,  1337},
             {1337, -5.2, 1337},
             {7.0,  1337, 9.0}};

    static std::vector<std::vector<double> > mB_sparse_3x4 =
            {{5.0, 8.5, 0,   -2.1},
             {0.0, -7,  3.8, 0.0},
             {4.0, 0,   0,   1.3}};

    static std::vector<std::vector<double> > mAnswer_sparse =
            {{61.5, 52.05, 28.5,   -25.83},
             {0.0,  36.4,  -19.76, 0.0},
             {71.0, 59.5,  0.0,    -3.0}};

    //static Matrix<double, DirectedMatrixTag> mAns(mAns_dense);
    grb::IndexArrayType i_all3x4 = {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2};
    grb::IndexArrayType j_all3x4 = {0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};

    static std::vector<std::vector<double> > mOnes_4x4 =
            {{1, 1, 1, 1},
             {1, 1, 1, 1},
             {1, 1, 1, 1},
             {1, 1, 1, 1}};

    static std::vector<std::vector<double> > mOnes_3x4 =
            {{1, 1, 1, 1},
             {1, 1, 1, 1},
             {1, 1, 1, 1}};

    static std::vector<std::vector<double> > mOnes_3x3 =
            {{1, 1, 1},
             {1, 1, 1},
             {1, 1, 1}};

    static std::vector<std::vector<double> > mIdentity_3x3 =
            {{1, 0, 0},
             {0, 1, 0},
             {0, 0, 1}};

    static std::vector<std::vector<double> > mLowerMask_3x4 =
            {{1, 0,    0,   0},
             {1, 0.5,  0,   0},
             {1, -1.0, 1.5, 0}};

    static std::vector<std::vector<bool> > mLowerBoolMask_3x4 =
            {{true, false, false, false},
             {true, true,  false, false},
             {true, true,  true,  false}};

}

//****************************************************************************
BOOST_AUTO_TEST_CASE(mxm_test_template_specialization_compilation)
{
    IndexArrayType i_A      = { 0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_A      = { 0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_A = {12, 7, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double> A(3, 3);
    A.build(i_A, j_A, v_A);

    IndexArrayType i_B      = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_B      = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_B = {5, 8, 1, 6, 7, 3, 4, 5, 9};
    Matrix<double> B(3, 3);
    B.build(i_B, j_B, v_B);

    IndexArrayType i_M    = {0, 0, 1, 1, 2, 2};
    IndexArrayType j_M    = {0, 2, 0, 2, 0, 2};
    std::vector<bool> v_M = {true, true, true, true, true, true};
    Matrix<bool> M(3, 3);
    M.build(i_M, j_M, v_M);

    Matrix<double> C(3, 3);
    ArithmeticSemiring<double> sr;
    Plus<double> accum;

    // Make sure all combinations compile

    // A*B
    mxm(C, NoMask(),      NoAccumulate(), sr, A, B);
    mxm(C, NoMask(),      accum,          sr, A, B);
    mxm(C, M,             NoAccumulate(), sr, A, B, REPLACE);
    mxm(C, M,             NoAccumulate(), sr, A, B, MERGE);
    mxm(C, complement(M), NoAccumulate(), sr, A, B, REPLACE);
    mxm(C, complement(M), NoAccumulate(), sr, A, B, MERGE);
    mxm(C, M,             accum,          sr, A, B, REPLACE);
    mxm(C, M,             accum,          sr, A, B, MERGE);
    mxm(C, complement(M), accum,          sr, A, B, REPLACE);
    mxm(C, complement(M), accum,          sr, A, B, MERGE);

    // A*B'
    mxm(C, NoMask(),      NoAccumulate(), sr, A, transpose(B));
    mxm(C, NoMask(),      accum,          sr, A, transpose(B));
    mxm(C, M,             NoAccumulate(), sr, A, transpose(B), REPLACE);
    mxm(C, M,             NoAccumulate(), sr, A, transpose(B), MERGE);
    mxm(C, complement(M), NoAccumulate(), sr, A, transpose(B), REPLACE);
    mxm(C, complement(M), NoAccumulate(), sr, A, transpose(B), MERGE);
    mxm(C, M,             accum,          sr, A, transpose(B), REPLACE);
    mxm(C, M,             accum,          sr, A, transpose(B), MERGE);
    mxm(C, complement(M), accum,          sr, A, transpose(B), REPLACE);
    mxm(C, complement(M), accum,          sr, A, transpose(B), MERGE);

    // A'B
    mxm(C, NoMask(),      NoAccumulate(), sr, transpose(A), B);
    mxm(C, NoMask(),      accum,          sr, transpose(A), B);
    mxm(C, M,             NoAccumulate(), sr, transpose(A), B, REPLACE);
    mxm(C, M,             NoAccumulate(), sr, transpose(A), B, MERGE);
    mxm(C, complement(M), NoAccumulate(), sr, transpose(A), B, REPLACE);
    mxm(C, complement(M), NoAccumulate(), sr, transpose(A), B, MERGE);
    mxm(C, M,             accum,          sr, transpose(A), B, REPLACE);
    mxm(C, M,             accum,          sr, transpose(A), B, MERGE);
    mxm(C, complement(M), accum,          sr, transpose(A), B, REPLACE);
    mxm(C, complement(M), accum,          sr, transpose(A), B, MERGE);

    // A'B'
    mxm(C, NoMask(),      NoAccumulate(), sr, transpose(A), transpose(B));
    mxm(C, NoMask(),      accum,          sr, transpose(A), transpose(B));
    mxm(C, M,             NoAccumulate(), sr, transpose(A), transpose(B), REPLACE);
    mxm(C, M,             NoAccumulate(), sr, transpose(A), transpose(B), MERGE);
    mxm(C, complement(M), NoAccumulate(), sr, transpose(A), transpose(B), REPLACE);
    mxm(C, complement(M), NoAccumulate(), sr, transpose(A), transpose(B), MERGE);
    mxm(C, M,             accum,          sr, transpose(A), transpose(B), REPLACE);
    mxm(C, M,             accum,          sr, transpose(A), transpose(B), MERGE);
    mxm(C, complement(M), accum,          sr, transpose(A), transpose(B), REPLACE);
    mxm(C, complement(M), accum,          sr, transpose(A), transpose(B), MERGE);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(mxm_reg)
{
    IndexArrayType i_mA    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_mA = {12, 7, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 3);
    mA.build(i_mA, j_mA, v_mA);

    IndexArrayType i_mB    = {0, 0, 0, 0, 1, 1, 1, 2, 2, 2, 2};
    IndexArrayType j_mB    = {0, 1, 2, 3, 0, 1, 2, 0, 1, 2, 3};
    std::vector<double> v_mB = {5, 8, 1, 2, 6, 7, 3, 4, 5, 9, 1};
    Matrix<double, DirectedMatrixTag> mB(3, 4);
    mB.build(i_mB, j_mB, v_mB);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    IndexArrayType i_answer = {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};
    std::vector<double> v_answer = {114, 160, 60, 27, 74, 97,
                                    73, 14, 119, 157, 112, 23};
    Matrix<double, DirectedMatrixTag> answer(3, 4);
    answer.build(i_answer, j_answer, v_answer);

    std::cout << "Test AxB" << std::endl;
    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_a_transpose)
{
    IndexArrayType i_mA    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_mA = {12, 7, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 3);
    mA.build(i_mA, j_mA, v_mA);

    IndexArrayType i_mB    = {0, 0, 0, 0, 1, 1, 1, 2, 2, 2, 2};
    IndexArrayType j_mB    = {0, 1, 2, 3, 0, 1, 2, 0, 1, 2, 3};
    std::vector<double> v_mB = {5, 8, 1, 2, 6, 7, 3, 4, 5, 9, 1};
    Matrix<double, DirectedMatrixTag> mB(3, 4);
    mB.build(i_mB, j_mB, v_mB);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    IndexArrayType i_answer = {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};
    std::vector<double> v_answer = {112, 159, 87, 31, 97, 131,
                                    94, 22, 87, 111, 102, 15};
    Matrix<double, DirectedMatrixTag> answer(3, 4);
    answer.build(i_answer, j_answer, v_answer);

    std::cout << "Test ATxB" << std::endl;
    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        transpose(mA), mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_b_transpose)
{
    IndexArrayType i_mA    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_mA = {12, 7, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 3);
    mA.build(i_mA, j_mA, v_mA);

    IndexArrayType i_mB    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mB    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_mB = {5, 8, 1, 2, 6, 7, 3, 4, 5};
    Matrix<double, DirectedMatrixTag> mB(3, 3);
    mB.build(i_mB, j_mB, v_mB);

    Matrix<double, DirectedMatrixTag> result(3, 3);

    IndexArrayType i_answer = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_answer = {119, 87, 79, 66, 80, 62, 108, 125, 98};
    Matrix<double, DirectedMatrixTag> answer(3, 3);
    answer.build(i_answer, j_answer, v_answer);

    std::cout << "Test AxBT" << std::endl;
    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, transpose(mB));

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_a_and_b_transpose)
{
    IndexArrayType i_mA    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mA    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_mA = {12, 7, 3, 4, 5, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> mA(3, 3);
    mA.build(i_mA, j_mA, v_mA);

    IndexArrayType i_mB    = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_mB    = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_mB = {5, 8, 1, 2, 6, 7, 3, 4, 5};
    Matrix<double, DirectedMatrixTag> mB(3, 3);
    mB.build(i_mB, j_mB, v_mB);

    Matrix<double, DirectedMatrixTag> result(3, 3);

    IndexArrayType i_answer = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_answer = {99, 97, 87, 83, 100, 81, 72, 105, 78};
    Matrix<double, DirectedMatrixTag> answer(3, 3);
    answer.build(i_answer, j_answer, v_answer);

    std::cout << "Test ATxBT" << std::endl;
    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        transpose(mA), transpose(mB));

    BOOST_CHECK_EQUAL(result, answer);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <time.h>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <algorithm>
#include <functional>
#include <map>
#include <wx/filename.h>
#include <wx/stopwatch.h>
#include "../DataViewer/TableInterface.h"
#include "../ShapeOperations/Randik.h"
#include "../ShapeOperations/WeightsManState.h"
#include "../VarCalc/WeightsManInterface.h"
#include "../logger.h"
#include "../Project.h"
#include "GetisOrdMapNewView.h"
#include "GStatCoordinator.h"

/*
 Example data: x = {2, 3, 3.2, 5, 8, 7}
 locations = {(10, 10), (20, 10), (40, 10), (15, 20), (30, 20), (30, 30)}
 distances:
	 0      1      2      3      4      5
 0  0.00  10.00  30.00  11.18  22.36  28.28
 1 10.00   0.00  20.00  11.18  14.14  22.36
 2 30.00  20.00   0.00  26.93  14.14  22.36
 3 11.18  11.18  26.93   0.00  15.00  18.03
 4 22.36  14.14  14.14  15.00   0.00  10.00
 5 28.28  22.36  22.46  18.03  10.00   0.00

 W for threshold distance = 15
 0: 1 3
 1: 0 3 4
 2: 4
 3: 0 1 4
 4: 1 2 3 5
 5: 4

 W_i = [1, 1, 1, 1, 1]

 G_i = (lag_i)/(x_star-x_i)
  = {0.152671756, 0.198412698, 0.32, 0.186781609, 0.225247525, 0.377358491}

 n = 6
 ExG = E[G*_i] = 1/(n-1) = 0.2
 ExGstar = E[G_i] = 1/n = 1.6666666
 x_star = 2 + 3 + 3.2 + 5 + 8 + 7 = 28.2
 x_sstar = 2^2 + 3^2 + (3.2)^2 + 5^2 + 8^2 + 7^2 = 161.24
 x_hat = x_star/n = 28.2/6 = 4.7
 s2 = x_sstar/n - (x_hat)^2 = 4.783333
 s = sqrt(s^2) = 2.18708329

 x_hat_i = {5.24, 5.04, 5, 4.64, 4.04, 4.24}
 s2_i = (x_sstar-x_i^2)/(n-1) - (x_hat_i)^2
   = {3.9904, 5.0464, 5.2, 5.7184, 3.1264, 4.4704}
 Var[G_i] = (1/(n-1)^2) * (s_i/x_hat_i)^2
  = {0.00581318105, 0.00794658604, 0.00832, 0.0106242568, 0.00766199392,
      0.00994660021  }
 z_i = (G_i - E[G_i]) / sqrt(Var[G_i])
  = {-0.620745338, -0.0178061189, 1.31558703,
     -0.128241714, 0.288434967, 1.77833941}

 */

GStatWorkerThread::GStatWorkerThread(const GalElement* W_,
                                     const std::vector<bool>& undefs_,
                                     int obs_start_s, int obs_end_s,
									 uint64_t seed_start_s,
									 GStatCoordinator* gstat_coord_s,
									 wxMutex* worker_list_mutex_s,
									 wxCondition* worker_list_empty_cond_s,
									 std::list<wxThread*> *worker_list_s,
									 int thread_id_s)
: wxThread(),
W(W_),
undefs(undefs_),
obs_start(obs_start_s), obs_end(obs_end_s), seed_start(seed_start_s),
gstat_coord(gstat_coord_s),
worker_list_mutex(worker_list_mutex_s),
worker_list_empty_cond(worker_list_empty_cond_s),
worker_list(worker_list_s),
thread_id(thread_id_s)
{
}

GStatWorkerThread::~GStatWorkerThread()
{
}

wxThread::ExitCode GStatWorkerThread::Entry()
{
	LOG_MSG(wxString::Format("GStatWorkerThread %d started", thread_id));

	// call work for assigned range of observations
	gstat_coord->CalcPseudoP_range(W, undefs, obs_start, obs_end, seed_start);

	wxMutexLocker lock(*worker_list_mutex);
	// remove ourself from the list
	worker_list->remove(this);
	// if empty, signal on empty condition since only main thread
	// should be waiting on this condition
	if (worker_list->empty()) {
		worker_list_empty_cond->Signal();
	}

	return NULL;
}

GStatCoordinator::
GStatCoordinator(boost::uuids::uuid weights_id,
                 Project* project,
                 const std::vector<GdaVarTools::VarInfo>& var_info_s,
                 const std::vector<int>& col_ids,
                 bool row_standardize_weights)
: w_man_state(project->GetWManState()),
w_man_int(project->GetWManInt()),
w_id(weights_id),
num_obs(project->GetNumRecords()),
row_standardize(row_standardize_weights),
permutations(999),
var_info(var_info_s),
data(var_info_s.size()),
data_undef(var_info_s.size()),
last_seed_used(0), reuse_last_seed(false)
{
    reuse_last_seed = GdaConst::use_gda_user_seed;
    if ( GdaConst::use_gda_user_seed) {
        last_seed_used = GdaConst::gda_user_seed;
    }
	TableInterface* table_int = project->GetTableInt();
	for (int i=0; i<var_info.size(); i++) {
		table_int->GetColData(col_ids[i], data[i]);
        table_int->GetColUndefined(col_ids[i], data_undef[i]);
	}

	weight_name = w_man_int->GetLongDispName(w_id);
	SetSignificanceFilter(1);

	InitFromVarInfo();

	maps.resize(8);
	for (int i=0, iend=maps.size(); i<iend; i++) {
		maps[i] = (GetisOrdMapFrame*) 0;
	}
	w_man_state->registerObserver(this);
}

GStatCoordinator::~GStatCoordinator()
{
	LOG_MSG("In GStatCoordinator::~GStatCoordinator");
	w_man_state->removeObserver(this);
	DeallocateVectors();
}

void GStatCoordinator::DeallocateVectors()
{
	for (int i=0; i<G_vecs.size(); i++) if (G_vecs[i]) delete [] G_vecs[i];
	G_vecs.clear();

	for (int i=0; i<G_defined_vecs.size(); i++) {
		if (G_defined_vecs[i]) delete [] G_defined_vecs[i];
	}
	G_defined_vecs.clear();

	for (int i=0; i<G_star_vecs.size(); i++) {
		if (G_star_vecs[i]) delete [] G_star_vecs[i];
	}
	G_star_vecs.clear();

	for (int i=0; i<z_vecs.size(); i++) if (z_vecs[i]) delete [] z_vecs[i];
	z_vecs.clear();

	for (int i=0; i<p_vecs.size(); i++) if (p_vecs[i]) delete [] p_vecs[i];
	p_vecs.clear();

	for (int i=0; i<z_star_vecs.size(); i++) {
		if (z_star_vecs[i]) delete [] z_star_vecs[i];
	}
	z_star_vecs.clear();

	for (int i=0; i<p_star_vecs.size(); i++) {
		if (p_star_vecs[i]) delete [] p_star_vecs[i];
	}
	p_star_vecs.clear();

	for (int i=0; i<pseudo_p_vecs.size(); i++) {
		if (pseudo_p_vecs[i]) delete [] pseudo_p_vecs[i];
	}
	pseudo_p_vecs.clear();

	for (int i=0; i<pseudo_p_star_vecs.size(); i++) {
		if (pseudo_p_star_vecs[i]) delete [] pseudo_p_star_vecs[i];
	}
	pseudo_p_star_vecs.clear();

	for (int i=0; i<x_vecs.size(); i++) if (x_vecs[i]) delete [] x_vecs[i];
	x_vecs.clear();

    // clear W_vecs
    for (size_t i=0; i<has_undefined.size(); i++) {
        if (has_undefined[i]) {
            delete Gal_vecs[i];
        }
    }
    Gal_vecs.clear();
    x_undefs.clear();
}

/** allocate based on var_info and num_time_vals **/
void GStatCoordinator::AllocateVectors()
{
	int tms = num_time_vals;
	G_vecs.resize(tms);
	G_defined_vecs.resize(tms);
	G_star_vecs.resize(tms);
	z_vecs.resize(tms);
	p_vecs.resize(tms);
	z_star_vecs.resize(tms);
	p_star_vecs.resize(tms);
	pseudo_p_vecs.resize(tms);
	pseudo_p_star_vecs.resize(tms);
	x_vecs.resize(tms);

    x_undefs.resize(tms);
    Gal_vecs.resize(tms);
    Gal_vecs_orig.resize(tms);

	n.resize(tms, 0);
	x_star.resize(tms, 0);
	x_sstar.resize(tms, 0);
	ExG.resize(tms);
	ExGstar.resize(tms);
	mean_x.resize(tms);
	var_x.resize(tms);
	VarGstar.resize(tms);
	sdGstar.resize(tms);

	map_valid.resize(tms);
	map_error_message.resize(tms);
	has_isolates.resize(tms);
	has_undefined.resize(tms);

	for (int i=0; i<tms; i++) {
		G_vecs[i] = new double[num_obs];
		G_defined_vecs[i] = new bool[num_obs];
		for (int j=0; j<num_obs; j++) G_defined_vecs[i][j] = true;
		G_star_vecs[i] = new double[num_obs];
		z_vecs[i] = new double[num_obs];
		p_vecs[i] = new double[num_obs];
		z_star_vecs[i] = new double[num_obs];
		p_star_vecs[i] = new double[num_obs];
		pseudo_p_vecs[i] = new double[num_obs];
		pseudo_p_star_vecs[i] = new double[num_obs];
		x_vecs[i] = new double[num_obs];

		map_valid[i] = true;
		map_error_message[i] = wxEmptyString;

        Gal_vecs[i] = NULL;
	}
}

/** We assume only that var_info is initialized correctly.
 ref_var_index, is_any_time_variant, is_any_sync_with_global_time and
 num_time_vals are first updated based on var_info */
void GStatCoordinator::InitFromVarInfo()
{
	DeallocateVectors();

	num_time_vals = 1;
	is_any_time_variant = var_info[0].is_time_variant;
	is_any_sync_with_global_time = false;
	ref_var_index = -1;
	if (var_info[0].is_time_variant &&
		var_info[0].sync_with_global_time) {
		num_time_vals = (var_info[0].time_max - var_info[0].time_min) + 1;
		is_any_sync_with_global_time = true;
		ref_var_index = 0;
	}

	AllocateVectors();

    bool has_undef = false;

	for (int t=var_info[0].time_min; t<=var_info[0].time_max; t++) {
		int d_t = t - var_info[0].time_min;
        vector<bool> undefs(num_obs);
        for (int i=0; i<num_obs; i++) {
            x_vecs[d_t][i] = data[0][t][i];
            undefs[i] = data_undef[0][t][i];
            G_defined_vecs[d_t][i] = !undefs[i];
            if (undefs[i]) {
                has_undef = true;
            }
        }
        x_undefs[d_t] = undefs;
        has_undefined[d_t] = has_undef;
	}

	for (int t=0; t<num_time_vals; t++) {
        GalElement* W  = NULL;
        if (Gal_vecs.empty() || Gal_vecs[t] == NULL) {
            // local weights copy
            GalWeight* gw = NULL;
            if ( has_undefined[t] ) {
                gw = new GalWeight(*w_man_int->GetGal(w_id));
                gw->Update(x_undefs[t]);
            } else {
                gw = w_man_int->GetGal(w_id);
            }
            W = gw->gal ;
            Gal_vecs[t] = gw;
            Gal_vecs_orig[t] = w_man_int->GetGal(w_id);
        }

		x = x_vecs[t];
		for (int i=0; i<num_obs; i++) {
			if ( W[i].Size() > 0 ) {
				n[t]++;
				x_star[t] += x[i];
				x_sstar[t] += x[i] * x[i];
			}
		}
		ExG[t] = 1.0/(n[t]-1); // same for all i when W is row-standardized
		ExGstar[t] = 1.0/n[t]; // same for all i when W is row-standardized
		mean_x[t] = x_star[t] / n[t]; // x hat (overall)
		var_x[t] = x_sstar[t]/n[t] - mean_x[t]*mean_x[t]; // s^2 overall

		// when W is row-standardized, VarGstar same for all i
		// same as s^2 / (n^2 mean_x ^2)
		VarGstar[t] = var_x[t] / (n[t]*n[t] * mean_x[t]*mean_x[t]);
		// when W is row-standardized, sdGstar same for all i
		sdGstar[t] = sqrt(VarGstar[t]);
	}

	CalcGs();
	CalcPseudoP();
}

/** Update Secondary Attributes based on Primary Attributes.
 Update num_time_vals and ref_var_index based on Secondary Attributes. */
void GStatCoordinator::VarInfoAttributeChange()
{
	GdaVarTools::UpdateVarInfoSecondaryAttribs(var_info);

	is_any_time_variant = false;
	is_any_sync_with_global_time = false;
	for (int i=0; i<var_info.size(); i++) {
		if (var_info[i].is_time_variant) is_any_time_variant = true;
		if (var_info[i].sync_with_global_time) {
			is_any_sync_with_global_time = true;
		}
	}
	ref_var_index = -1;
	num_time_vals = 1;
	for (int i=0; i<var_info.size() && ref_var_index == -1; i++) {
		if (var_info[i].is_ref_variable) ref_var_index = i;
	}
	if (ref_var_index != -1) {
		num_time_vals = (var_info[ref_var_index].time_max -
						 var_info[ref_var_index].time_min) + 1;
	}
	//GdaVarTools::PrintVarInfoVector(var_info);
}

/** The category vector c_val will be filled based on the current
 significance filter and significance values corresponding to specified
 canvas_time.  */
void GStatCoordinator::FillClusterCats(int canvas_time,
									   bool is_gi,
                                       bool is_perm,
									   std::vector<wxInt64>& c_val)
{
	int t = canvas_time;
	double* p_val = 0;
	if (is_gi && is_perm) p_val = pseudo_p_vecs[t];
	if (is_gi && !is_perm) p_val = p_vecs[t];
	if (!is_gi && is_perm) p_val = pseudo_p_star_vecs[t];
	if (!is_gi && !is_perm) p_val = p_star_vecs[t];
	double* z_val = is_gi ? z_vecs[t] : z_star_vecs[t];

    const GalElement* W = Gal_vecs[t]->gal;

	c_val.resize(num_obs);
	for (int i=0; i<num_obs; i++) {
        if (!G_defined_vecs[t][i]) {
            c_val[i] = 4; // undefined

        } else if (W[i].Size() == 0) {
			c_val[i] = 3; // isolate

		} else if (p_val[i] <= significance_cutoff) {
			c_val[i] = z_val[i] > 0 ? 1 : 2; // high = 1, low = 2

		} else {
			c_val[i] = 0; // not significant
		}
	}
}

/** Initialize Gi and Gi_star.  We handle either binary or row-standardized
 binary weights.  Weights with self-neighbors are handled correctly. */
void GStatCoordinator::CalcGs()
{
	using boost::math::normal; // typedef provides default type is double.
	// Construct a standard normal distribution std_norm_dist
	normal std_norm_dist; // default mean = zero, and s.d. = unity

	for (int t=0; t<num_time_vals; t++) {
		G = G_vecs[t];
		G_defined = G_defined_vecs[t];
		G_star = G_star_vecs[t];
		z = z_vecs[t];
		p = p_vecs[t];
		z_star = z_star_vecs[t];
		p_star = p_star_vecs[t];
		pseudo_p = pseudo_p_vecs[t];
		pseudo_p_star = pseudo_p_star_vecs[t];
		x = x_vecs[t];

		has_undefined[t] = false;
		has_isolates[t] = false;

        const GalElement* W = Gal_vecs[t]->gal;

		double n_expr = sqrt((n[t]-1)*(n[t]-1)*(n[t]-2));
		for (long i=0; i<num_obs; i++) {
            if (x_undefs[t][i]) {
                G_defined[i] = false;
                has_undefined[t] = true;
                continue;
            }

			const GalElement& elm_i = W[i];
			if ( elm_i.Size() > 0 ) {
				double lag = 0;
				bool self_neighbor = false;
				for (size_t j=0, sz=W[i].Size(); j<sz; j++) {
					if (elm_i[j] != i) {
						lag += x[elm_i[j]];
					} else {
						self_neighbor = true;
					}
				}
				double Wi = self_neighbor ? W[i].Size()-1 : W[i].Size();
				if (row_standardize) {
					lag /= elm_i.Size();
					Wi /= elm_i.Size();
				}
				double xd_i = x_star[t] - x[i];
				if (xd_i != 0) {
					G[i] = lag / xd_i;
				} else {
					G_defined[i] = false;
				}
				double x_hat_i = xd_i * ExG[t]; // (x_star - x[i])/(n-1)

				double ExGi = Wi/(n[t]-1);
				// location-specific variance
				double ss_i = ((x_sstar[t] - x[i]*x[i])/(n[t]-1)
							   - x_hat_i*x_hat_i);
				double sdG_i = sqrt(Wi*(n[t]-1-Wi)*ss_i)/(n_expr * x_hat_i);

				// compute z and one-sided p-val from standard-normal table
				if (G_defined[i]) {
					z[i] = (G[i] - ExGi)/sdG_i;
					if (z[i] >= 0) {
						p[i] = 1.0-cdf(std_norm_dist, z[i]);
					} else {
						p[i] = cdf(std_norm_dist, z[i]);
					}
				} else {
					has_undefined[t] = true;
				}
			} else {
				has_isolates[t] = true;
			}
		}

		if (x_star[t] == 0) {
            for (long i=0; i<num_obs; i++) {
                G_defined[i] = false;
            }
			has_undefined[t] = true;
			break;
		}

		if (row_standardize) {
			for (long i=0; i<num_obs; i++) {
                if (x_undefs[t][i]) {
                    G_star[i] = 0;
                    z_star[i] = 0;
                    continue;
                }
				const GalElement& elm_i = W[i];
				double lag = 0;
				bool self_neighbor = false;
				int sz_i=W[i].Size();
				for (int j=0; j<sz_i; j++) {
                    if (elm_i[j] == i) {
                        self_neighbor = true;
                    }
					lag += x[elm_i[j]];
				}
				G_star[i] = self_neighbor ? lag / (sz_i * x_star[t]) :
					(lag+x[i]) / ((sz_i+1) * x_star[t]);
				z_star[i] = (G_star[i] - ExGstar[t])/sdGstar[t];
			}

		} else { // binary weights

			double n_expr_mean_x = n[t] * sqrt(n[t]-1) * mean_x[t];

			for (long i=0; i<num_obs; i++) {
                if (x_undefs[t][i]) {
                    G_star[i] = 0;
                    z_star[i] = 0;
                    continue;
                }
				const GalElement& elm_i = W[i];
				double lag = 0;
				bool self_neighbor = false;
				for (int j=0, sz=elm_i.Size(); j<sz; j++) {
                    if (elm_i[j] == i) {
                        self_neighbor = true;
                    }
					lag += x[elm_i[j]];
				}
                if (!self_neighbor) {
                    lag += x[i];
                }
				G_star[i] = lag / x_star[t];
				double Wi = self_neighbor ? W[i].Size() : W[i].Size()+1;
				// location-specific mean
				double ExGi_star = Wi/n[t];
				// location-specific variance
				double sdG_i_star = sqrt(Wi*(n[t]-Wi)*var_x[t])/n_expr_mean_x;
				z_star[i] = (G_star[i] - ExGi_star)/sdG_i_star;
			}
		}

		for (long i=0; i<num_obs; i++) {
			// compute z and one-sided p-val from standard-normal table
			if (z_star[i] >= 0) {
				p_star[i] = 1.0-cdf(std_norm_dist, z_star[i]);
			} else {
				p_star[i] = cdf(std_norm_dist, z_star[i]);
			}
		}
	}
}

void GStatCoordinator::CalcPseudoP()
{
	LOG_MSG("Entering GStatCoordinator::CalcPseudoP");
	wxStopWatch sw;
	int nCPUs = wxThread::GetCPUCount();

	// To ensure thread safety, only work on one time slice of data
	// at a time.  For each time period t:
	// 1. copy data for time period t into data1 and data2 arrays
	// 2. Perform multi-threaded computation
	// 3. copy results into results array

	for (int t=0; t<num_time_vals; t++) {

        std::vector<bool>& undefs = x_undefs[t];

		G = G_vecs[t];
		G_defined = G_defined_vecs[t];
		G_star = G_star_vecs[t];
		z = z_vecs[t];
		p = p_vecs[t];
		z_star = z_star_vecs[t];
		p_star = p_star_vecs[t];
		pseudo_p = pseudo_p_vecs[t];
		pseudo_p_star = pseudo_p_star_vecs[t];
		x = x_vecs[t];
		x_star_t = x_star[t];

		if (nCPUs <= 1) {
			if (!reuse_last_seed) last_seed_used = time(0);
			CalcPseudoP_range(Gal_vecs[t]->gal, undefs, 0, num_obs-1, last_seed_used);
		} else {
			CalcPseudoP_threaded(Gal_vecs[t]->gal, undefs);
		}
	}
	/*
		wxString m;
		m << "GStat on " << num_obs << " obs with " << permutations;
		m << " perms over " << num_time_vals << " time periods took ";
		m << sw.Time() << " ms. Last seed used: " << last_seed_used;
		LOG_MSG(m);
	*/
	LOG_MSG("Exiting GStatCoordinator::CalcPseudoP");
}

void GStatCoordinator::CalcPseudoP_threaded(const GalElement* W,
                                            const std::vector<bool>& undefs)
{
	LOG_MSG("Entering GStatCoordinator::CalcPseudoP_threaded");
	int nCPUs = wxThread::GetCPUCount();

	// mutext protects access to the worker_list
    wxMutex worker_list_mutex;
	// signals that worker_list is empty
	wxCondition worker_list_empty_cond(worker_list_mutex);
	worker_list_mutex.Lock(); // mutex should be initially locked

    // List of all the threads currently alive.  As soon as the thread
	// terminates, it removes itself from the list.
	std::list<wxThread*> worker_list;

	// divide up work according to number of observations
	// and number of CPUs
	int work_chunk = num_obs / nCPUs;
	int obs_start = 0;
	int obs_end = obs_start + work_chunk;

	bool is_thread_error = false;
	int quotient = num_obs / nCPUs;
	int remainder = num_obs % nCPUs;
	int tot_threads = (quotient > 0) ? nCPUs : remainder;

	if (!reuse_last_seed) last_seed_used = time(0);
	for (int i=0; i<tot_threads && !is_thread_error; i++) {
		int a=0;
		int b=0;
		if (i < remainder) {
			a = i*(quotient+1);
			b = a+quotient;
		} else {
			a = remainder*(quotient+1) + (i-remainder)*quotient;
			b = a+quotient-1;
		}
		uint64_t seed_start = last_seed_used+a;
		uint64_t seed_end = seed_start + ((uint64_t) (b-a));
		int thread_id = i+1;
		wxString msg;
		msg << "thread " << thread_id << ": " << a << "->" << b;
		msg << ", seed: " << seed_start << "->" << seed_end;

		GStatWorkerThread* thread =
			new GStatWorkerThread(W, undefs, a, b, seed_start, this,
								  &worker_list_mutex,
								  &worker_list_empty_cond,
								  &worker_list, thread_id);
		if ( thread->Create() != wxTHREAD_NO_ERROR ) {
			delete thread;
			is_thread_error = true;
		} else {
			worker_list.push_front(thread);
		}
	}
	if (is_thread_error) {
		// fall back to single thread calculation mode
		CalcPseudoP_range(W, undefs, 0, num_obs-1, last_seed_used);
	} else {
		std::list<wxThread*>::iterator it;
		for (it = worker_list.begin(); it != worker_list.end(); it++) {
			(*it)->Run();
		}

		while (!worker_list.empty()) {
			// wait until thread_list might be empty
			worker_list_empty_cond.Wait();
			// We have been woken up. If this was not a false
			// alarm (spurious signal), the loop will exit.
		}
	}

	LOG_MSG("Exiting GStatCoordinator::CalcPseudoP_threaded");
}

/** In the code that computes Gi and Gi*, we specifically checked for
 self-neighbors and handled the situation appropriately.  For the
 permutation code, we will disallow self-neighbors. */
void GStatCoordinator::CalcPseudoP_range(const GalElement* W,
                                         const std::vector<bool>& undefs,
                                         int obs_start, int obs_end,
										 uint64_t seed_start)
{
	GeoDaSet workPermutation(num_obs);

	int max_rand = num_obs-1;

	for (long i=obs_start; i<=obs_end; i++) {

        if (undefs[i])
            continue;

		const int numNeighsI = W[i].Size();
		const double numNeighsD = W[i].Size();

        //only compute for non-isolates
		if ( numNeighsI > 0 && G_defined[i]) {
            // know != 0 since G_defined[i] true
			double xd_i = x_star_t - x[i];

			int countGLarger = 0;
			int countGStarLarger = 0;
			double permutedG = 0;
			double permutedGStar = 0;

			for (int perm=0; perm < permutations; perm++) {
				int rand = 0;
				while (rand < numNeighsI) {
					// computing 'perfect' permutation of given size
                    double rng_val = Gda::ThomasWangHashDouble(seed_start++) * max_rand;
                    // round is needed to fix issue
                    //https://github.com/GeoDaCenter/geoda/issues/488
                    int newRandom = (int) (rng_val < 0.0 ? ceil(rng_val - 0.5) : floor(rng_val + 0.5));

					if (newRandom != i &&
                        !workPermutation.Belongs(newRandom) &&
                        undefs[newRandom] == false)
					{
						workPermutation.Push(newRandom);
						rand++;
					}
				}

				double lag_i=0;
				// use permutation to compute the lags
				for (int j=0; j<numNeighsI; j++) {
					lag_i += x[workPermutation.Pop()];
				}

				if (row_standardize) {
					permutedG = lag_i / (numNeighsD * xd_i);
					permutedGStar = (lag_i+x[i]) / ((numNeighsD+1)*x_star_t);
				} else { // binary weights
					// Wi = numNeighsD // assume no self-neighbors
					permutedG = lag_i / xd_i;
					permutedGStar = (lag_i+x[i]) / x_star_t;
				}

				if (permutedG >= G[i]) countGLarger++;
				if (permutedGStar >= G_star[i]) countGStarLarger++;
			}
			// pick the smallest
			if (permutations-countGLarger < countGLarger) {
				countGLarger=permutations-countGLarger;
			}
			pseudo_p[i] = (countGLarger + 1.0)/(permutations+1.0);
			//if (i == DBGI) LOG(pseudo_p[i]);

			if (permutations-countGStarLarger < countGStarLarger) {
				countGStarLarger=permutations-countGStarLarger;
			}
			pseudo_p_star[i] = (countGStarLarger + 1.0)/(permutations+1.0);
		}
	}
}

void GStatCoordinator::SetSignificanceFilter(int filter_id)
{
	// 0: >0.05 1: 0.05, 2: 0.01, 3: 0.001, 4: 0.0001
	if (filter_id < 1 || filter_id > 4) return;
	significance_filter = filter_id;
	if (filter_id == 1) significance_cutoff = 0.05;
	if (filter_id == 2) significance_cutoff = 0.01;
	if (filter_id == 3) significance_cutoff = 0.001;
	if (filter_id == 4) significance_cutoff = 0.0001;
}

void GStatCoordinator::update(WeightsManState* o)
{
	weight_name = w_man_int->GetLongDispName(w_id);
}

int GStatCoordinator::numMustCloseToRemove(boost::uuids::uuid id) const
{
	int n=0;
	if (id == w_id) {
		for (std::vector<GetisOrdMapFrame*>::const_iterator i=maps.begin();
			 i != maps.end(); ++i) {
			if (*i != 0) ++n;
		}
	}
	return n;
}

void GStatCoordinator::closeObserver(boost::uuids::uuid id)
{
	if (numMustCloseToRemove(id) == 0) return;
	std::vector<GetisOrdMapFrame*> maps_cpy = maps;
	for (std::vector<GetisOrdMapFrame*>::iterator i=maps_cpy.begin();
		 i != maps_cpy.end(); ++i) {
		if (*i != 0) {
			(*i)->closeObserver(this);
		}
	}
}

void GStatCoordinator::registerObserver(GetisOrdMapFrame* o)
{
	maps[o->map_type] = o;
}

void GStatCoordinator::removeObserver(GetisOrdMapFrame* o)
{
	LOG_MSG("Entering GStatCoordinator::removeObserver");
	maps[o->map_type] = 0;
	int num_observers=0;
	for (int i=0, iend=maps.size(); i<iend; i++) if (maps[i]) num_observers++;
	if (num_observers == 0) {
		delete this;
	}
	LOG_MSG("Exiting GStatCoordinator::removeObserver");
}

void GStatCoordinator::notifyObservers()
{
	for (int i=0, iend=maps.size(); i<iend; i++) {
		if (maps[i]) maps[i]->update(this);
	}
}


#include <Functions/FunctionFactory.h>
#include <Functions/FunctionBinaryArithmetic.h>
#include <boost/integer/common_factor.hpp>

namespace DB
{

template <typename A, typename B>
struct GCDImpl
{
    using ResultType = typename NumberTraits::ResultOfAdditionMultiplication<A, B>::Type;

    template <typename Result = ResultType>
    static inline Result apply(A a, B b)
    {
        throwIfDivisionLeadsToFPE(typename NumberTraits::ToInteger<A>::Type(a), typename NumberTraits::ToInteger<B>::Type(b));
        throwIfDivisionLeadsToFPE(typename NumberTraits::ToInteger<B>::Type(b), typename NumberTraits::ToInteger<A>::Type(a));
        return boost::integer::gcd(
            typename NumberTraits::ToInteger<Result>::Type(a),
            typename NumberTraits::ToInteger<Result>::Type(b));
    }

#if USE_EMBEDDED_COMPILER
    static constexpr bool compilable = false; /// exceptions (and a non-trivial algorithm)
#endif
};

struct NameGCD { static constexpr auto name = "gcd"; };
using FunctionGCD = FunctionBinaryArithmetic<GCDImpl, NameGCD, false>;

void registerFunctionGCD(FunctionFactory & factory)
{
    factory.registerFunction<FunctionGCD>();
}

}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/GPFCorrection.h>

#include <Eigen/Cholesky>

#include <exception>

using namespace bfl;
using namespace Eigen;

GPFCorrection::GPFCorrection
(
    std::unique_ptr<GaussianCorrection> gauss_corr,
    std::unique_ptr<LikelihoodModel> lik_model,
    std::unique_ptr<StateModel> state_model
) noexcept :
    GPFCorrection(std::move(gauss_corr), std::move(lik_model), std::move(state_model), 1)
{ }

GPFCorrection::GPFCorrection
(
    std::unique_ptr<GaussianCorrection> gauss_corr,
    std::unique_ptr<LikelihoodModel> lik_model,
    std::unique_ptr<StateModel> state_model,
    unsigned int seed
) noexcept :
    gaussian_correction_(std::move(gauss_corr)),
    likelihood_model_(std::move(lik_model)),
    state_model_(std::move(state_model)),
    generator_(std::mt19937_64(seed)),
    distribution_(std::normal_distribution<double>(0.0, 1.0)),
    gaussian_random_sample_([&] { return (distribution_)(generator_); })
{ }

GPFCorrection::GPFCorrection(GPFCorrection&& gpf_correction) noexcept :
    PFCorrection(std::move(gpf_correction)),
    gaussian_correction_(std::move(gpf_correction.gaussian_correction_)),
    likelihood_model_(std::move(gpf_correction.likelihood_model_)),
    state_model_(std::move(gpf_correction.state_model_)),
    generator_(std::move(gpf_correction.generator_)),
    distribution_(std::move(gpf_correction.distribution_)),
    gaussian_random_sample_(std::move(gpf_correction.gaussian_random_sample_)) { }

void GPFCorrection::setLikelihoodModel(std::unique_ptr<LikelihoodModel> likelihood_model)
{
    throw std::runtime_error("ERROR::GPFCORRECTION::SETLIKELIHOODMODEL\nERROR:\n\tCall to unimplemented base class method.");
}

void GPFCorrection::setMeasurementModel(std::unique_ptr<MeasurementModel> measurement_model)
{
    throw std::runtime_error("ERROR::GPFCORRECTION::SETMEASUREMENTMODEL\nERROR:\n\tCall to unimplemented base class method.");
}

MeasurementModel& GPFCorrection::getMeasurementModel()
{
    return gaussian_correction_->getMeasurementModel();
}

LikelihoodModel& GPFCorrection::getLikelihoodModel()
{
    return *likelihood_model_;
}

std::pair<bool, Eigen::VectorXd> GPFCorrection::getLikelihood()
{
    throw std::runtime_error("ERROR::GPFCORRECTION::GETLIKELIHOOD\nERROR:\n\tCall to unimplemented base class method.");
}

void GPFCorrection::correctStep(const bfl::ParticleSet& pred_particles, bfl::ParticleSet& corr_particles)
{
    /* Propagate Gaussian belief associated to each particle. */
    gaussian_correction_->correctStep(pred_particles, corr_particles);

    /* Sample from the proposal distribution. */
    for (std::size_t i = 0; i < pred_particles.components; i++)
    {
        corr_particles.state(i) = sampleFromProposal(corr_particles.mean(i), corr_particles.covariance(i));
    }

    /* Evaluate the likelihood. */
    bool valid_likelihood;
    VectorXd likelihood;
    std::tie(valid_likelihood, likelihood) = likelihood_model_->likelihood(getMeasurementModel(), corr_particles.state());

    if (!valid_likelihood)
    {
        corr_particles = pred_particles;

        return;
    }

    /* Evaluate the transition probability. */
    VectorXd transition_probability = state_model_->getTransitionProbability(pred_particles.state(), corr_particles.state());

    /* Update weights in the log space.
     w_{k} = w_{k-1} + log(likelihood) + log(transition_probability) - log(proposal_distribution) */
    double eps = std::numeric_limits<double>::min();
    for (std::size_t i = 0; i < pred_particles.components; i++)
    {
        corr_particles.weight(i) = pred_particles.weight(i) + log(likelihood(i) + eps) + log(transition_probability(i) + eps) - log(evaluateProposal(corr_particles.state(i), corr_particles.mean(i), corr_particles.covariance(i)) + eps);
    }
}

Eigen::VectorXd GPFCorrection::sampleFromProposal(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance)
{
    /* Evaluate the square root of the state covariance matrix using the LDL' decomposition
       (it can be used even if the covariance matrix is positive semidefinite). */
    LDLT<MatrixXd> chol_ldlt(covariance);
    MatrixXd sqrt_P = (chol_ldlt.transpositionsP() * MatrixXd::Identity(mean.size(), mean.size())).transpose() *
                       chol_ldlt.matrixL() *
                       chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();

    /* Sample i.i.d standard normal univariates. */
    VectorXd rand_vectors(mean.size());
    for (int i = 0; i < rand_vectors.size(); i++)
        rand_vectors(i) = gaussian_random_sample_();

    /* Return a sample from a normal multivariate having mean `mean` and covariance 'covariance'. */
    return mean + sqrt_P * rand_vectors;
}

double GPFCorrection::evaluateProposal
(
    const Eigen::VectorXd& state,
    const Eigen::VectorXd& mean,
    const Eigen::MatrixXd& covariance
)
{
    /* Evaluate the proposal distribution, a Gaussian centered in 'mean' and having
       covariance 'covariance', in the state 'state'. */
    VectorXd difference = state - mean;

    return (-0.5 * static_cast<double>(difference.size()) * log(2.0 * M_PI) -0.5 * log(covariance.determinant()) -0.5 * (difference.transpose() * covariance.inverse() * difference).array()).exp().coeff(0);
}


#include "3dUnitDriver.hpp"
#include "encoder_driver.hpp"

#include <boost/log/expressions.hpp>
#include <fstream>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#define M_PI 3.14159265358979323846
#include <logger.hpp>
#include "3dUnitTypeSerialization.hpp"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

using namespace m3d;
SET_DEBUGLVL(_PRINTOUT_INFO);

void m3d::transferPc(m3d::rawPointcloud &raw, m3d::pointcloud &normalPc, m3d::_3dUnitConfig &cfg)
{

	for (int i=0; i< raw.angles.size(); i++)
	{

		float ang = raw.angles[i];

		if (ang !=-1)
		{
			glm::mat4 affine3Dunit = glm::rotate(glm::mat4(1.0f),cfg.angularOffsetUnitLaser+ang, glm::vec3(0.0f, 0.0f, 1.0f));

			std::vector<lms_measurement>::iterator profile = raw.ranges.begin()+i;
			for (int i =0; i <profile->echoes[0].data.size(); i++)
			{

				//float lasAng = float(1.0*i *(lit->profile.echoes[0].angStepWidth)  -135.0f);
				//float lasAng = float(1.0*i *(profile->echoes[0].angStepWidth)  - profile->echoes[0].startAngle);
				float lasAng = float(1.0*i *(profile->echoes[0].angStepWidth)+cfg.angularOffsetRotLaser);

				float d = profile->echoes[0].data[i];
				glm::vec4 in (d, 0.0, 0.0f, 1.0f);
				glm::mat4 affineLaser = glm::rotate(glm::mat4(1.0f),  glm::radians(lasAng),glm::vec3(0.0f, 0.0f, 1.0f));
				glm::mat4 calib = cfg.calibMatrix;
				glm::mat4 cor = glm::rotate(glm::mat4(1.0f), glm::radians(-90.0f),glm::vec3(0.0f, 1.0f, 0.0f));

				//glm::mat4 dAffine = glm::matrixCompMult(affineLaser, affine);
				glm::vec4 out =affine3Dunit* cor* calib* affineLaser * in;
				normalPc.data.push_back(glm::vec3(out.x, out.y,out.z));
				if (profile->rssis.size()>0)normalPc.intensity.push_back(profile->rssis[0].data[i]);
			}
		}
	}
}

bool _3dUnitConfig::readConfigFromXML(std::string fileName)
{
	LOG_INFO("config file       :" << fileName);
	using boost::property_tree::ptree;
	ptree pt;
	try {
		read_xml(fileName,pt);
	}
	catch(std::exception const&  e)
	{
		LOG_FATAL("cannot parse file "<< e.what());
		exit(-1);
	}

    try {

		maximumAngleOfScan = 1.0f*M_PI;
		angularOffsetRotLaser =0.0f;
		angularOffsetUnitLaser=0.0f;
		boost::optional<float> op_maximumAngleOfScan = pt.get_optional<float>("m3dUnitDriver.angles.maximum");
		boost::optional<float> op_angularOffsetRotLaser = pt.get_optional<float> ("m3dUnitDriver.angles.laserOffset");
		boost::optional<float> op_angularOffsetUnitLaser = pt.get_optional<float> ("m3dUnitDriver.angles.unitOffset");

		boost::optional<std::string> op_outputPath = pt.get_optional<std::string> ("m3dUnitDriver.outputFolder");
		outputPath ="";
		if (op_outputPath) outputPath = *op_outputPath;

		if(op_maximumAngleOfScan) maximumAngleOfScan = *op_maximumAngleOfScan;
		if(op_angularOffsetRotLaser) angularOffsetRotLaser =*op_angularOffsetRotLaser;
		if(op_angularOffsetUnitLaser) angularOffsetUnitLaser =*op_angularOffsetUnitLaser;

		rotLaserIp = pt.get<std::string>("m3dUnitDriver.adresses.rotLaser");
        unitIp= pt.get<std::string> ("m3dUnitDriver.adresses.unit");
		ptree locpt = pt.get_child("m3dUnitDriver");
        m3d::typeSerialization::deserialize(locpt, calibMatrix, "calibration");

		frontLaserIp = pt.get<std::string>("m3dUnitDriver.adresses.frontLaser");

    }
    catch (...)
    {

    }
    LOG_INFO("Configuration readed");
    LOG_INFO("calibration matrix");
    LOG_INFO("\t"<<calibMatrix[0][0]<<"\t"<<calibMatrix[0][1]<<"\t"<<calibMatrix[0][2]<<"\t"<<calibMatrix[0][3]<<"\t");
    LOG_INFO("\t"<<calibMatrix[1][0]<<"\t"<<calibMatrix[1][1]<<"\t"<<calibMatrix[1][2]<<"\t"<<calibMatrix[1][3]<<"\t");
    LOG_INFO("\t"<<calibMatrix[2][0]<<"\t"<<calibMatrix[2][1]<<"\t"<<calibMatrix[2][2]<<"\t"<<calibMatrix[2][3]<<"\t");
    LOG_INFO("\t"<<calibMatrix[3][0]<<"\t"<<calibMatrix[3][1]<<"\t"<<calibMatrix[3][2]<<"\t"<<calibMatrix[3][3]<<"\t");

	LOG_INFO("front laser IP     :" << frontLaserIp);
	LOG_INFO("roatation laser IP : "<< rotLaserIp);
	LOG_INFO("unit IP            :" << unitIp);
	LOG_INFO("output folder      :" << outputPath);

	return true;

}

void _3dUnitDriver::initializeEncoderOnly()
{
	encThread  =  boost::thread(boost::bind(&_3dUnitDriver::encoderWorker, this));
	applyPriority(&encThread, ABOVE_NORMAL);
	boost::this_thread::sleep(boost::posix_time::milliseconds(500));
}

void _3dUnitDriver::startOver()
{
	LOG_INFO ("Starting over pointcloud");
	LOG_DEBUG("get angle :"<<angleCollection);
	angleCollection = 0.0f;

	pointcloudLock.lock();
	{
		progress = 0;
		collectingPointCloud.data.clear();
		collectingPointCloud.intensity.clear();
		collectingRawpointcloud.angles.clear();
		collectingRawpointcloud.ranges.clear();

	}
	pointcloudLock.unlock();
}
	void _3dUnitDriver::initialize()
{
	newSpeed =-1.0f;
	is_initialized = true;
	_done = false;
	LOG_INFO("initializing _3dUnitDriver");
	lmsThread  =  boost::thread(boost::bind(&_3dUnitDriver::laserThreadWorker, this));
	encThread  =  boost::thread(boost::bind(&_3dUnitDriver::encoderWorker, this));
	collectorThread =  boost::thread(boost::bind(&_3dUnitDriver::combineThread, this));
	applyPriority(&lmsThread, ABOVE_NORMAL);
	applyPriority(&encThread, ABOVE_NORMAL);
	boost::this_thread::sleep(boost::posix_time::milliseconds(500));
}

void _3dUnitDriver::deinitialize()
{
	_done = true;
	is_initialized = false;
	LOG_INFO("waiting for joining threads");
	lmsThread.join();
	encThread.join();
	collectorThread.join();

}

_3dUnitDriver::_3dUnitDriver(_3dUnitConfig config)
{
	is_initialized  = false;
	//LMS.debug();
	lmsIp =config.rotLaserIp;
	unitIp=config.unitIp;
	newSpeed =-1.0f;
    calib = config.calibMatrix;

    LOG_INFO("calibration matrix");
    LOG_INFO("\t"<<calib[0][0]<<"\t"<<calib[0][1]<<"\t"<<calib[0][2]<<"\t"<<calib[0][3]<<"\t");
    LOG_INFO("\t"<<calib[1][0]<<"\t"<<calib[1][1]<<"\t"<<calib[1][2]<<"\t"<<calib[1][3]<<"\t");
    LOG_INFO("\t"<<calib[2][0]<<"\t"<<calib[2][1]<<"\t"<<calib[2][2]<<"\t"<<calib[2][3]<<"\t");
    LOG_INFO("\t"<<calib[3][0]<<"\t"<<calib[3][1]<<"\t"<<calib[3][2]<<"\t"<<calib[3][3]<<"\t");
	maximumScanAngle = config.maximumAngleOfScan;
	angularOffsetRotLaser = config.angularOffsetRotLaser;
	angularOffsetUnitLaser = config.angularOffsetUnitLaser;
	collectingPointcloud=false;
	_newPointCloud=false;
}

void _3dUnitDriver::encoderWorker()
{
	LOG_INFO("enc thread started");
	ENC.connect_to_m3d(unitIp);
	ENC.setSpeed(30);
	while (!_done)
	{
		if (newSpeed !=-1.0f)
		{
			if (ENC.setSpeed((int)newSpeed))	newSpeed =-1;
		}
		bool isAngle =false;
		double angle = ENC.getAngle(isAngle);
		if (isAngle)
		{
			encoderMeasurment m;

			boost::posix_time::ptime now = boost::posix_time::microsec_clock::local_time();
			m.first = now;
			m.second =static_cast<float>(angle);
			LOG_DEBUG("angle:"<< m.second);
			encMeasurmentLock.lock();
			curentAngle = m.second;
			encMeasurmentBuffer.push_back(m);
			if(encMeasurmentBuffer.size()> 10) encMeasurmentBuffer.pop_back();
			encMeasurmentLock.unlock();

		}

		boost::this_thread::sleep(boost::posix_time::millisec(3));
	}
	ENC.setSpeed(0);
	boost::this_thread::sleep(boost::posix_time::millisec(300));
	LOG_INFO("enc thread ended");
}

void _3dUnitDriver::laserThreadWorker()
{
	LOG_INFO("lms thread started");

	LMS.connectToLaser(lmsIp);
	LMS.requestContinousScan();

	while(!_done)
	{
		bool isMeasurment = false;
		LMS.readData(isMeasurment);
		if (isMeasurment &&!LMS.currentMessage.echoes.empty())
		{
			boost::posix_time::ptime now = boost::posix_time::microsec_clock::local_time();

            profileWithAngle m;
            m.encoder = fabs(curentAngle);
			m.profile =LMS.currentMessage;
			laserMeasurmentsLock.lock();
			readyData.push_back(m);
			laserMeasurmentsLock.unlock();
            boost::this_thread::sleep(boost::posix_time::millisec(5));
		}
	}
	LMS.disconnet();
	LOG_INFO("lms thread ended");
}

void _3dUnitDriver::getPointCloud(pointcloud &pc)
{
	pointcloudLock.lock();
	pc.data = lastPointCloud.data;
	pc.intensity = lastPointCloud.intensity;
	pointcloudLock.unlock();
}

void _3dUnitDriver::getRawPointCloud(rawPointcloud &pc)
{
	pointcloudLock.lock();
	pc.angles = lastRawPointCloud.angles;
	pc.ranges = lastRawPointCloud.ranges;
	pointcloudLock.unlock();
}
void _3dUnitDriver::combineThread()
{

	while (!_done)
	{
		if (readyData.size()< 50)
		{
			boost::this_thread::sleep(boost::posix_time::millisec(100));
			continue;
		}

		std::vector<glm::vec4> pointcloud;
		std::vector<profileWithAngle> copyProfiles;

		laserMeasurmentsLock.lock();
		copyProfiles = readyData;
		readyData.clear();
		laserMeasurmentsLock.unlock();

		for (std::vector<profileWithAngle>::iterator lit = copyProfiles.begin(); lit != copyProfiles.end(); lit++)
		{

			float ang = lit->encoder;

			if (ang !=-1)
			{
				glm::mat4 affine3Dunit = glm::rotate(glm::mat4(1.0f), angularOffsetUnitLaser+ang, glm::vec3(0.0f, 0.0f, 1.0f));

				for (int i =0; i < lit->profile.echoes[0].data.size(); i++)
				{

					//float lasAng = float(1.0*i *(lit->profile.echoes[0].angStepWidth)  -135.0f);
					//float lasAng = float(1.0*i *(lit->profile.echoes[0].angStepWidth)  -lit->profile.echoes[0].startAngle);
					float lasAng = float(1.0*i *(lit->profile.echoes[0].angStepWidth)+angularOffsetRotLaser);

					float d = lit->profile.echoes[0].data[i];
					glm::vec4 in (d, 0.0, 0.0f, 1.0f);
					glm::mat4 affineLaser = glm::rotate(glm::mat4(1.0f), glm::radians(lasAng),glm::vec3(0.0f, 0.0f, 1.0f));
                    //glm::mat4 calib = glm::translate(glm::mat4(1.0f),glm::vec3(0.0f, 0.0f, -50.0f));
					glm::mat4 cor = glm::rotate(glm::mat4(1.0f), glm::radians(-90.0f),glm::vec3(0.0f, 1.0f, 0.0f));

					//glm::mat4 dAffine = glm::matrixCompMult(affineLaser, affine);
					glm::vec4 out =affine3Dunit* cor* calib* affineLaser * in;
					collectingPointCloud.data.push_back(glm::vec3(out.x, out.y,out.z));
					if (lit->profile.rssis.size()>0)collectingPointCloud.intensity.push_back( lit->profile.rssis[0].data[i]);
				}

				collectingRawpointcloud.angles.push_back(lit->encoder);
				collectingRawpointcloud.ranges.push_back(lit->profile);

                float dAngle = fabs(lit->encoder-lastAngleCollection);
				//if greater than M_PI or NAN then dangle
				if (dAngle > 0.3*M_PI || (dAngle!=dAngle)) dAngle=0;

				angleCollection=angleCollection+dAngle;

				angleCollection = angleCollection+dAngle;
				progress =-1.0f;
				if (angleCollection !=0.f)
				{
					progress = angleCollection /( 2.2* maximumScanAngle);
				}
				if (fabs(angleCollection) > 2.2* maximumScanAngle)
				{
					LOG_DEBUG("get angle :"<<angleCollection);
					angleCollection = 0.0f;

					pointcloudLock.lock();

					{
						lastPointCloud.data = collectingPointCloud.data;
						lastPointCloud.intensity = collectingPointCloud.intensity;

						lastRawPointCloud.ranges = collectingRawpointcloud.ranges;
						lastRawPointCloud.angles = collectingRawpointcloud.angles;

						collectingPointCloud.data.clear();
						collectingPointCloud.intensity.clear();
						collectingRawpointcloud.angles.clear();
						collectingRawpointcloud.ranges.clear();

					}
					pointcloudLock.unlock();
					_newPointCloud = true;
					if(!scanCallback.empty()) scanCallback();
				}

				lastAngleCollection =  lit->encoder;
			}
		}
	}
	LOG_INFO("combine thread ended");

}
void m3d::applyPriority(boost::thread* m_pThread,  threadPriority priority)
{
	// some native WINAPI ugh!
#ifdef _WIN32
	if (!m_pThread)
		return;

	BOOL res;
	HANDLE th = m_pThread->native_handle();
	switch (priority)
	{
	case REALTIME		: res = SetThreadPriority(th, THREAD_PRIORITY_TIME_CRITICAL);	break;
	case HIGH			: res = SetThreadPriority(th, THREAD_PRIORITY_HIGHEST);			break;
	case ABOVE_NORMAL	: res = SetThreadPriority(th, THREAD_PRIORITY_ABOVE_NORMAL);	break;
	case NORMAL			: res = SetThreadPriority(th, THREAD_PRIORITY_NORMAL);			break;
	case BELOW_NORMAL	: res = SetThreadPriority(th, THREAD_PRIORITY_BELOW_NORMAL);	break;
	case IDLE			: res = SetThreadPriority(th, THREAD_PRIORITY_LOWEST);			break;
	}
#else
	std::cerr<<"CANNOT SET THREAD PRIORITY ON THIS PLATFORM \n";
#endif
}

float _3dUnitDriver::getNearestAngle(boost::posix_time::ptime timeStamp)
{
	///@todo vector is nearly sorted - maybe optimalisation!!
	///@todo maybe better is map for measurments;
	float angle = -1;
	long long int maxTimeD =10;//msec
	long long int timeAcc =2; //msec
	long long int minimal_dTime =maxTimeD;
	for (std::vector<encoderMeasurment>::iterator it = encMeasurmentBuffer.begin(); it!= encMeasurmentBuffer.end(); it++)
	{
		long long int timeD = (it->first-timeStamp).total_milliseconds();
		if (abs(timeD) < minimal_dTime)
		{
			angle = it->second;
			minimal_dTime= abs(timeD);

		}
	}
	return angle;
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "EvolutionComposite.h"
#include "Mode.h"
#include "MLJC.h"

#include <boost/fusion/sequence/intrinsic/at.hpp>

using namespace std;

const int NL=8;

using multilevel::result_of::make_vector;
using multilevel::Pump; using multilevel::Decay;

typedef multilevel::RealLevelsMF<NL>::type Levels;

typedef make_vector<Pump    <1,2>,Pump    <0,3>/*,Pump<4,2>,Pump<6,2>,Pump<5,3>,Pump<7,3>*/ >::type Pumps          ;
typedef make_vector<Coupling<1,2>,Coupling<0,3>                                             >::type PhononCouplings;

typedef make_vector<Decay<0,2>,Decay<1,2>,Decay<0,3>,Decay<1,3>,Decay<4,2>,Decay<5,2>,Decay<6,2>,Decay<5,3>,Decay<6,3>,Decay<7,3> >::type Decays;

typedef make_vector<Coupling<4,2>,Coupling<5,2>,Coupling<6,2>,Coupling<5,3>,Coupling<6,3>,Coupling<7,3> >::type Couplings;

typedef quantumdata::StateVector<3> StateVector;

using boost::fusion::at_c;

int main(int argc, char* argv[])
{
  // ****** Parameters of the Problem

  // Clebsches:
  const double sqrt13=sqrt(1./3), sqrt23=sqrt(2./3), sqrt12=sqrt(1./2), sqrt16=sqrt(1./6);

  ParameterTable pDummy, pActual;

  multilevel::ParsPumpedLossy<NL,Pumps,Decays> pml  (pDummy);
  mode::      Pars                             pphon(pDummy,"P");
  mode::      ParsPumpedLossy                  pplm (pDummy,"C");

  mljc::Pars<PhononCouplings> pPhononCouplings(pDummy,"P");
  mljc::Pars<      Couplings> pmljc           (pDummy,"C");

  {
    // These can be hardcoded here:

    const double gammaPS=10.48, gammaPD=.73;

    at_c<0>(pml.gammas).set(sqrt13*gammaPS);
    at_c<1>(pml.gammas).set(sqrt23*gammaPS);
    at_c<2>(pml.gammas).set(sqrt23*gammaPS);
    at_c<3>(pml.gammas).set(sqrt13*gammaPS);

    at_c<4>(pml.gammas).set(sqrt12*gammaPD);
    at_c<5>(pml.gammas).set(sqrt13*gammaPD);
    at_c<6>(pml.gammas).set(sqrt16*gammaPD);
    at_c<7>(pml.gammas).set(sqrt16*gammaPD);
    at_c<8>(pml.gammas).set(sqrt13*gammaPD);
    at_c<9>(pml.gammas).set(sqrt12*gammaPD);
  }

  dcomp
    &eta =pActual.add("eta","Pumping laser Rabi frequency",dcomp(30,0)),
    &etaC=pActual.add("etaC","Cavity pump Rabi frequency",dcomp(0,0));

  double
    &phononCouplingFactor=pActual.add("phononCouplingFactor","Phonon coupling strength: -i*eta*phononCouplingFactor",.12),
    &g                   =pActual.add("g","Atom-cavity coupling: Clebsches*g",1.4),
    &deltaEta            =pActual.add("deltaEta","Pump detuning",-318.803),
    &deltaG              =pActual.add("deltaG","Cavity detuning",-315.12 ),
    &omegaPhonon         =pActual.add("omegaPhonon","Phonon frequency",1.);

  unsigned
    &phononCutoff=pActual.add("phononCutoff","Phonon mode cutoff",20u),
    &cavityCutoff=pActual.add("cavityCutoff","Cavity mode cutoff",3u);

  evolution::Pars<> pe(pActual); // Driver Parameters

  // Parameter finalization
  update(pActual,argc,argv,"--");

  {
    // Enforcing constraints:

    at_c<0>(pml.etas).set(eta);
    at_c<1>(pml.etas).set(eta);

    at_c<0>(pPhononCouplings.gs).set(-DCOMP_I*phononCouplingFactor*at_c<0>(pml.etas).get());
    at_c<1>(pPhononCouplings.gs).set(-DCOMP_I*phononCouplingFactor*at_c<1>(pml.etas).get());

    at_c<0>(pmljc.gs).set(sqrt12*g);
    at_c<1>(pmljc.gs).set(sqrt13*g);
    at_c<2>(pmljc.gs).set(sqrt16*g);
    at_c<3>(pmljc.gs).set(sqrt16*g);
    at_c<4>(pmljc.gs).set(sqrt13*g);
    at_c<5>(pmljc.gs).set(sqrt12*g);

    pml.deltas(0)=0;

    pplm.kappa=.055;
    pplm.delta=deltaG-deltaEta;

    pphon.cutoff=phononCutoff;
    pplm .cutoff=cavityCutoff;

    pphon.delta=-omegaPhonon;

    pplm.eta=etaC;
  }

  // ****** ****** ****** ****** ****** ******

  multilevel::StateVector psiML(NL); psiML(1)=1;

  StateVector psi(psiML*mode::init(pphon)*mode::init(pplm));

  // The free components

  MultiLevelBase<NL>::Ptr atomInner(multilevel::makePumpedLossySch(pml.deltas,pml.etas,pml.gammas));

  Mode<> phonon(pphon);

  mode::Ptr cavityMode(mode::make(pplm,QMP_IP));

  // The interaction components

  MLJC<NL,PhononCouplings> ia01(atomInner,phonon    ,pPhononCouplings);

  MLJC<NL,      Couplings> ia02(atomInner,cavityMode,pmljc           );

  evolve<0>
    (psi,
     composite::make(_<0,1>(ia01),_<0,2>(ia02)),
     pe);

}