#include <iostream>
#include <NTL/RR.h>
#include "ring.h"
#include "she.h"
#include "util.h"
#include "params.h"

using namespace std;
using namespace NTL;

void encode_test (param& prm) {
  cout << "--- encode_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText x(prm);
    x.uniform();

    PlainText z(prm);
    encode_pt(prm, z, x);

    PlainText x1(prm);
    decode_pt(prm, x1, z);

    if (x != x1) {
      cout << "NG: encode_test: (" << cnt << ")" << endl;
      cout << " x = " << x << endl;
      cout << " z = " << z << endl;
      cout << "x1 = " << x1 << endl;
      exit(-1);
    }
  }

  cout << "OK: encode_test" << endl;
}

void dec_test (param& prm, const SKey& s) {
  cout << "--- dec_test for " << prm.label << " ---" << endl;

  for (long i = 0; i < 5; ++i) {
    PlainText m(prm), m1(prm);
    m.uniform();

    CipherText ct(prm);
    encrypt(prm, ct, s, m);

    if (dec_ng(prm, m1, s, ct, m)) {
      cout << "NG: dec" << endl;
      cout << "m  = " << m << endl;
      cout << "m1 = " << m1 << endl;
      exit(-1);
    }
  }

  cout << "OK: dec_test" << endl;
}

void div_by_2_test (param& prm, const SKey& s) {
  cout << "--- div_by_2_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m(prm), m1(prm), m2(prm);
    m.uniform();
    for (long i = 0; i < prm.rg.gR; ++i) m[i] = m[i] - (m[i] % 2);
    for (long i = 0; i < prm.rg.gR; ++i) m2[i] = m[i]/2;

    CipherText ct(prm);
    encrypt(prm, ct, s, m);

    CipherText dt(prm);
    div_by_2(dt, ct);

    ZZ v = decrypt(prm, m1, s, dt);
    for (long i = 0; i < dt.dim; ++i) {
      if (((m2[i] - m1[i]) % (ct.t/2)) != 0) {
	cout << "NG: div_by_2" << endl;
	cout << "be: " << VectorCopy(m2.data, 10) << endl;
	cout << "is: " << VectorCopy(m1.data, 10) << endl;
	exit(-1);
      }
    }
  }

  cout << "OK: div_by_2_test" << endl;
}

void add_test (param& prm, const SKey& s) {
  cout << "--- add_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    add(m3, m1, m2);

    CipherText ct1(prm), ct2(prm), ct3(prm);
    encrypt(prm, ct1, s, m1);
    encrypt(prm, ct2, s, m2);

    add_ct(ct3, ct1, ct2);
    if (dec_ng(prm, _m, s, ct3, m3)) {
      cout << "NG: add" << endl;
      cout << "  be: " << m3 << endl;
      cout << "  is: " << _m << endl;
      exit(-1);
    }
  }

  cout << "OK: add_test" << endl;
}

void plain_add_test (param& prm, const SKey& s) {
  cout << "--- plain_add_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    add(m3, m1, m2);

    CipherText ct2(prm), ct3(prm);
    encrypt(prm, ct2, s, m2);

    plain_add_ct(ct3, m1, ct2);
    if (dec_ng(prm, _m, s, ct3, m3)) {
      cout << "NG: plain_add_test" << endl;
      cout << "  be: " << VectorCopy(m3.data, 10) << endl;
      cout << "  is: " << VectorCopy(_m.data, 10)  << endl;
      exit(-1);
    }
  }

  cout << "OK: plain_add_test" << endl;
}

void plain_mult_test (param& prm, const SKey& s) {
  cout << "--- plain_mult_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    mult(m3, m1, m2);

    CipherText ct2(prm), ct3(prm);
    encrypt(prm, ct2, s, m2);

    plain_mult_ct(ct3, m1, ct2);
    if (dec_ng(prm, _m, s, ct3, m3)) {
      cout << "NG: plain_mult_test" << endl;
      cout << "  be: " << m3 << endl;
      cout << "  is: " << _m << endl;
      exit(-1);
    }
  }

  cout << "OK: plain_mult_test" << endl;
}

void hint_gen_test (param& prm, const SKey& s) {
  cout << "--- hint_gen_test for " << prm.label << " ---" << endl;
  ZZ_p::init(prm.qq);
  ZZ& sq = prm.sR;

  long test_num = 1;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    ZZ w(1);
    for (long j = 0; j < prm.lw; ++j) {
      cout << "-- " << j << " ------" << endl;
      vec_ZZ_p e;
      e.SetLength(prm.rg.gR);

      // e = H[j][0] + H[j][1] s.x
      mult(e, conv<vec_ZZ_p>(s.Hint.H[j][1]), conv<vec_ZZ_p>(s.x));
      add(e, conv<vec_ZZ_p>(s.Hint.H[j][0]), e);
      //prm.applyGammaInv(e, e);
      prm.applyOmegaInv(e, e);

      // f = sR s2 g^T
      vec_ZZ_p f =  conv<ZZ_p>(w) * conv<vec_ZZ_p>(s.s2);
      f = conv<ZZ_p>(sq) * f;

      e = e - f;

      vec_ZZ ee;
      ee.SetLength(prm.rg.gR);
      center_lift(ee, e);
      cout << "|e| = " << inf_norm(ee) << endl;

      w *= prm.w;
    }
  }

  //cout << "OK: hint_gen_test" << endl;
}

void hint_gen_test2 (param& prm, const SKey& s) {
  cout << "--- hint_gen_test2 for " << prm.label << " ---" << endl;
  ZZ& q0 = prm.qq;
  ZZ& q = prm.q;

  long test_num = 1;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    ZZ w(1);
    for (long j = 0; j < prm.lw; ++j) {
      cout << "-- " << j << " ------" << endl;

      ZZ_p::init(q0);

      vec_ZZ_p _e;
      _e.SetLength(prm.rg.gR);

      // e = H[j][0] + H[j][1] s.x
      mult(_e, conv<vec_ZZ_p>(s.Hint.H[j][1]), conv<vec_ZZ_p>(s.x));
      add(_e, conv<vec_ZZ_p>(s.Hint.H[j][0]), _e);
      //prm.applyGammaInv(_e, _e);
      prm.applyOmegaInv(_e, _e);

      vec_ZZ e;
      e.SetLength(prm.rg.gR);
      rescale(e, conv<vec_ZZ>(_e), q0, q);

      ZZ_p::init(q);

      // f = s2 g^T
      vec_ZZ_p f =  conv<ZZ_p>(w) * conv<vec_ZZ_p>(s.s2);

      vec_ZZ_p e1 = conv<vec_ZZ_p>(e) - f;
      vec_ZZ ee;
      ee.SetLength(prm.rg.gR);
      center_lift(ee, e1);
      cout << "|e| = " << inf_norm(ee) << endl;

      w *= prm.w;
    }
  }

  //cout << "OK: hint_gen_test2" << endl;
}

void decomp_test (param& prm) {
  cout << "--- decomp_test for " << prm.label << " ---" << endl;

  ZZ& q = prm.qq;
  ZZ_p::init(q);

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    vec_ZZ c;
    c.SetLength(prm.rg.gR);
    for (long i = 0; i < c.length(); ++i) c[i] = RandomBnd(q);

    Vec<vec_ZZ> d;
    d.SetLength(prm.lw);
    for (long l = 0; l < d.length(); ++l) d[l].SetLength(prm.rg.gR);
    Decomp(prm, d, c);

    vec_ZZ_p y;
    y.SetLength(prm.rg.gR);

    ZZ w(1);
    for (long l = 0; l < prm.lw; ++l) {
      vec_ZZ_p dd = conv<ZZ_p>(w) * conv<vec_ZZ_p>(d[l]);
      y += dd;
      w *= prm.w;
    }

    if (conv<vec_ZZ>(y) != c) {
      cout << "NG: decomp" << endl;
      cout << "  be: " << c[0] << endl;
      cout << "  is: " << (conv<vec_ZZ>(y))[0] << endl;
      exit(-1);
    }
  }

  cout << "OK: decomp_test" << endl;
}

// Typically, sin = s2x (s2=s^2), sout = sx
// -- H : encryption of sin
// -- d = apply_hint(H, c)  --> d(sout) ~ sin c
void hint_apply_test (param& prm, const hint Hint, const vec_ZZ& sin, const vec_ZZ& sout) {
  cout << "--- hint_apply_test for " << prm.label << " ---" << endl;
  long test_num = 3;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    ZZ& q = prm.q;
    ZZ_p::init(q);

    vec_ZZ a, b, c;
    c.SetLength(prm.rg.gR);
    for (long i = 0; i < c.length(); ++i) c[i] = RandomBnd(q);

    apply_hint(prm, a, b, Hint, c, q);

    vec_ZZ_p _a, _b, _e;
    _e.SetLength(prm.rg.gR);

    //prm.applyGamma(_a, conv<vec_ZZ_p>(a));
    prm.applyOmega(_a, conv<vec_ZZ_p>(a));
    prm.applyOmega(_b, conv<vec_ZZ_p>(b));
    mult(_e, _b, conv<vec_ZZ_p>(sout));
    _e = _a + _e;

    vec_ZZ_p _c;
    prm.applyOmega(_c, conv<vec_ZZ_p>(c));
    mult(_c, conv<vec_ZZ_p>(sin), _c);

    _c = _c - _e;
    //prm.applyGammaInv(_c, _c);
    prm.applyOmegaInv(_c, _c);

    center_lift(c, _c);
    cout << "inf_norm(noise) = " << inf_norm(c) << endl;
  }

  //cout << "OK: hint_apply_test" << endl;
}

void key_switch_test (param& prm, const SKey& s, const SKey& t, const hint& Hint) {
  cout << "--- key_switch_test for " << prm.label << " ---" << endl;

  long test_num = 3;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m(prm), m1(prm);
    m.uniform();
    CipherText ct(prm);
    encrypt(prm, ct, t, m);

    // ct/t --> dt/s
    CipherText dt(prm);
    key_switch(dt, Hint, ct);

    if (dec_ng(prm, m1, s, dt, m)) {
      cout << "NG: dec" << endl;
      cout << "be: " << VectorCopy(m.data, 10) << endl;
      cout << "is: " << VectorCopy(m1.data, 10) << endl;
      exit(-1);
    }
  }

  cout << "OK: key_switch_test" << endl;
}

void direct_mult_test (param& prm, const SKey& s) {
  cout << "--- direct_mult_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    mult(m3, m1, m2);

    CipherText ct1(prm), ct2(prm);
    encrypt(prm, ct1, s, m1);
    encrypt(prm, ct2, s, m2);

    CipherText2 dt(prm);
    direct_mult_ct(dt, ct1, ct2);

    if (dec_ng2(prm, _m, s, dt, m3)) {
      cout << "NG: direct_mult_test" << endl;
      cout << "  be: " << m3 << endl;
      cout << "  is: " << _m << endl;
      exit(-1);
    }
  }

  cout << "OK: direct_mult_test" << endl;
}

void mult_test (param& prm, const hint& Hint, const SKey& s) {
  cout << "--- mult_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    mult(m3, m1, m2);

    CipherText ct1(prm), ct2(prm);
    encrypt(prm, ct1, s, m1);
    encrypt(prm, ct2, s, m2);

    CipherText dt(prm);
    mult_ct(dt, Hint, ct1, ct2);

    if (dec_ng(prm, _m, s, dt, m3)) {
      cout << "NG: mult_test" << endl;
      cout << "  be: " << VectorCopy(m3.data, 10) << endl;
      cout << "  is: " << VectorCopy(_m.data, 10) << endl;
      exit(-1);
    }
  }

  cout << "OK: mult_test" << endl;
}

void square_test (param& prm, const hint& Hint, const SKey& s) {
  cout << "--- square_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m3(prm), _m(prm);
    m1.uniform();
    mult(m3, m1, m1);

    CipherText ct1(prm);
    encrypt(prm, ct1, s, m1);

    CipherText dt(prm);
    //square_ct(dt, Hint, ct1);
    square_ct_debug(dt, Hint, ct1, s);

    if (dec_ng(prm, _m, s, dt, m3)) {
      cout << "NG: square_test" << endl;
      cout << "  be: " << VectorCopy(m3.data, 10) << endl;
      cout << "  is: " << VectorCopy(_m.data, 10) << endl;
      exit(-1);
    }
  }

  cout << "OK: square_test" << endl;
}

void power_test (param& prm, const hint& Hint, const SKey& s, const long n) {
  cout << "--- power_test for " << prm.label << " ---" << endl;

  long test_num = 3;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    cout << "[" << cnt << "]-----------------------" << endl;

    Vec<PlainText> m;
    for (long i = 0; i < n + 1; ++i) {
      PlainText _m(prm);
      append(m, _m);
    }

    Vec<CipherText> ct;
    for (long i = 0; i < n + 1; ++i) {
      CipherText _ct(prm);
      append(ct, _ct);
    }

    m[0].uniform();
    encrypt(prm, ct[0], s, m[0]);

    PlainText _m(prm);
    if (dec_ng(prm, _m, s, ct[0], m[0])) {
      cout << "NG: power_test" << endl;
      cout << "  be: " << VectorCopy(m[0].data,10) << endl;
      cout << "  is: " << VectorCopy(_m.data,10) << endl;
      exit(-1);
    }

    for (long k = 1; k < n+1; ++k) {
      cout << "-- " << k << " --------------------" << endl;
      mult(m[k], m[k-1], m[k-1]);
      //cout << "m[" << k << "] = " << VectorCopy(m[k].data,10) << endl;

      //square_ct_debug(ct[k], Hint, ct[k-1], s);
      square_ct(ct[k], Hint, ct[k-1]);
      if (dec_ng(prm, _m, s, ct[k], m[k])) {
	cout << "NG: power_test" << endl;
	cout << "  be: " << VectorCopy(m[k].data,10) << endl;
	cout << "  is: " << VectorCopy(_m.data,10) << endl;
	exit(-1);
      }
    }
  }

  cout << "OK: power_test" << endl;
}

void hom_test (param& prm, const hint& Hint, const SKey& s, long L) {
  cout << "--- hom_test for " << prm.label << " --- : level = " << L << endl;

  long test_num = 1;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    cout << "[" << cnt << "]-----------------------" << endl;

    Vec<Vec<long>> op;
    op.SetLength(L+1);
    for (long j = 0; j < L + 1; ++j) op[j].SetLength(pow(2,L));

    // plain computation
    Vec<Vec<PlainText>> m;
    for (long j = 0; j < L + 1; ++j) {
      Vec<PlainText> _mm;
      for (long i = 0; i < pow(2,L); ++i) {
	PlainText _m(prm);
	append(_mm, _m);
      }
      append(m, _mm);
    }

    for (long i = 0; i < pow(2,L); ++i) m[0][i].uniform();

    for (long j = 1; j < L + 1; ++j) {
      for (long i = 0; i < pow(2,L-j); ++i) {
	op[j][i] = RandomBnd(2);
	if (op[j][i]) add(m[j][i], m[j-1][2*i], m[j-1][2*i+1]);
	else mult(m[j][i], m[j-1][2*i], m[j-1][2*i+1]);
      }
    }

    // homomorphic computation
    Vec<Vec<CipherText>> ct;
    for (long j = 0; j < L + 1; ++j) {
      Vec<CipherText> _cc;
      for (long i = 0; i < pow(2,L); ++i) {
	CipherText _ct(prm);
	append(_cc, _ct);
      }
      append(ct, _cc);
    }

    for (long i = 0; i < pow(2,L); ++i) encrypt(prm, ct[0][i], s, m[0][i]);

    for (long j = 1; j < L + 1; ++j) {
      cout << "-- level " << j << endl;
      for (long i = 0; i < pow(2,L-j); ++i) {
	if (op[j][i]) {
	  cout << "ct[" << j-1 << "][" << 2*i << "] + ct[" << j-1 << "][" << 2*i+1 << "]" << endl;
	  add_ct(ct[j][i], ct[j-1][2*i], ct[j-1][2*i+1]);
	}
	else {
	  cout << "ct[" << j-1 << "][" << 2*i << "] * ct[" << j-1 << "][" << 2*i+1 << "]" << endl;
	  mult_ct(ct[j][i], Hint, ct[j-1][2*i], ct[j-1][2*i+1]);
	}

	PlainText _m(prm);
	if (dec_ng(prm, _m, s, ct[j][i], m[j][i])) {
	  cout << "NG: hom_test" << endl;
	  cout << "  be: " << m[j][i] << endl;
	  cout << "  is: " << _m << endl;
	  exit(-1);
	}
      }
    }
  }

  cout << "OK: hom_test" << endl;
}

void she_test (param& prm) {
  cout << "--- she_test for " << prm << " ---" << endl;

  cout << "generating a secret key..." << flush;
  SKey s(prm);
  KeyGen(prm, s);
  cout << "done" << endl;

  encode_test(prm);
  dec_test(prm, s);

  div_by_2_test(prm, s);

  add_test(prm, s);
  plain_add_test(prm, s);

  if (prm.level > 1) {
    plain_mult_test(prm, s);

    hint_gen_test(prm, s);
    hint_gen_test2(prm, s);
    decomp_test(prm);

    hint_apply_test(prm, s.Hint, s.s2x, s.x);

    SKey t(prm);
    KeyGen(prm, t);

    hint Hint;
    Hint.hint_gen(prm, t.data, s.x);
    hint_apply_test(prm, Hint, t.x, s.x);
    key_switch_test(prm, s, t, Hint);

    direct_mult_test(prm, s);
    mult_test(prm, s.Hint, s);
    square_test(prm, s.Hint, s);

    //hom_test(prm, s.Hint, s, prm.level-1);
    power_test(prm, s.Hint, s, prm.level-1);
  }
}

void check_modulus_switch (param& prm, param& prm_tmp) {
  cout << "check_modulus_switch: " << flush;

  // the original secret key
  SKey s(prm);
  KeyGen(prm, s);

  // the corresponding secret key in prm_tmp
  SKey s_tmp(prm_tmp);
  KeyGen(prm_tmp, s_tmp, s.data);  // sx = s_tmp.x

  PlainText m(prm);
  CipherText ct(prm), dt(prm_tmp);

  m.uniform();
  encrypt(prm, ct, s, m);

  modulus_switch(dt, prm_tmp, prm_tmp.q, prm, ct);

  // check the result
  PlainText _m(prm_tmp);
  if (dec_ng(prm_tmp, _m, s_tmp, dt, m)) {
    cout << "NG: check_modulus_switch: " << endl;
    cout << "  be: " << VectorCopy(m.data,10) << endl;
    cout << "  is: " << VectorCopy(_m.data,10) << endl;
    exit(-1);
  }
  cout << "-- OK" << endl;
}

int main (int argc, char *argv[]) {
  /* Do Test */
  //long prec = 2*left_param_big.r - left_param_big.l;
  //long prec = 2*right_param_big.r - right_param_big.l;
  long prec = 2*composed_param_big.r - composed_param_big.l;

  // left_param.init(prec);
  // she_test(left_param);

  // left_param_tmp.init(prec);
  // she_test(left_param_tmp);

  // left_param_big.init(prec);
  // she_test(left_param_big);

  // right_param.init(prec);
  // she_test(right_param);

  // right_param_tmp.init(prec);
  // she_test(right_param_tmp);

  // right_param_big.init(prec);
  // she_test(right_param_big);

  composed_param.init(prec);
  she_test(composed_param);

  composed_param_tmp.init(prec);
  she_test(composed_param_tmp);

  composed_param_big.init(prec);
  she_test(composed_param_big);

  check_modulus_switch(composed_param, composed_param_tmp);
}


//****************************************************************************
// (c) 2012 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
#ifndef openOR_HistogramProbabilityAnalyser_hpp
#define openOR_HistogramProbabilityAnalyser_hpp

#include <openOR/Callable.hpp> //basic
#include <openOR/DataSettable.hpp> // basic
#include <openOR/Utility/Types.hpp> //openOR_core
#include <openOR/Math/vector.hpp> //openOR_core

#include <string>
#include <memory>

#include <openOR/Image/Image1DData.hpp>

#include <boost/shared_array.hpp>

#include <openOR/Defs/Image_Utility.hpp>

namespace openOR {
	namespace Image {

		class OPENOR_IMAGE_UTILITY_API HistogramProbabilityAnalyser : public Callable, public DataSettable {

		public:
			HistogramProbabilityAnalyser();
			virtual ~HistogramProbabilityAnalyser();

			void operator()() const;
			void setData(const AnyPtr& data, const std::string& name = "");
			void setHighDensityMaterialSearch(bool b){ m_bProbabilityBasedHighDensityMaterialSearch = b; };

		private:

			std::tr1::shared_ptr<Image1DData<Triple<size_t> > > m_pRegions;
			std::tr1::shared_ptr<Image1DData<Quad<double> > > m_pResult;
			std::tr1::shared_ptr<Image1DDataUI64> m_pHistogram;
			bool m_bProbabilityBasedHighDensityMaterialSearch;

			struct HistogramProbabilityAnalyserImpl {
				HistogramProbabilityAnalyserImpl() {};
				~HistogramProbabilityAnalyserImpl() {};

				std::vector <Math::Vector4d> getSplittedNormalDistributions(boost::shared_array<long long> Histogram, uint nHistogramDataSize,std::vector<Math::Vector3ui>& vecDensityIntervals,bool bProbabilityBasedHighDensityMaterialSearch = true,bool bLog10Normalization=false,uint nNumOfRefinements = 0);

			private:

				Math::Vector4d getSplittedNormalDistributionOfAHistogram(boost::shared_array<long long> vecData, uint m_nHistogramDataSize, uint nBeginIndex, uint nEndIndex, uint nPeak,bool bLog10Normalization);
				Math::Vector4d refineSplittedNormalDistributionOfAHistogram(boost::shared_array<long long> vecData,boost::shared_array<long long> vecDummyData,uint nHistogramDataSize, Math::Vector4d vecPre,Math::Vector4d vecCurrent,Math::Vector4d vecPost,bool bLog10Normalization);
				Math::Vector4d probabilityBasedHighDensityMaterialSearch(boost::shared_array<long long> pHistogram, uint nHistogramDataSize, uint nLeftBorder, uint nRightBorder, uint nPeak,bool bLog10Normalization,std::vector<Math::Vector3ui>& vecDensityIntervals,Math::Vector4d& vecCurrentLastMaterial);
				double computeNormalDistributedProbability(double mean,double stddev,double x);
				Math::Vector2d getNormalDistributionOfAHistogram(std::vector<long long>& vecData,bool bLog10Normalization);
			};
		};
	}
}
#endif //openOR_HistogramAnalyser_hpp


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_RECTIFICATION_HPP
#define PIC_COMPUTER_VISION_RECTIFICATION_HPP

#include <vector>
#include <cmath>
#include <stdlib.h>

#include "../base.hpp"

#include "../image.hpp"

#include "../image_vec.hpp"

#include "../filtering/filter_warp_2d.hpp"
#include "../filtering/filter_rotation.hpp"

#include "../computer_vision/camera_matrix.hpp"

#include "../util/eigen_util.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief computeImageRectificationWarp
 * @param img0 is the first image to rectify
 * @param img1 is the second image to rectify
 * @param T0 is the homography for img0
 * @param T1 is the homography for img0
 * @param out is the output as an ImageVec with two images; i.e., rectified versions of img0 and img1
 * @return
 */
PIC_INLINE ImageVec *computeImageRectificationWarp(Image *img0,
                                                   Image *img1,
                                                   Eigen::Matrix3d &T0,
                                                   Eigen::Matrix3d &T1,
                                                   ImageVec *out,
                                                   bool bPartial = true)
{
    if(img0 == NULL || img1 == NULL) {
        return out;
    }

    if(out == NULL) {
        out = new ImageVec();
    }

    auto H0 = MatrixConvert(T0);
    auto H1 = MatrixConvert(T1);
    FilterWarp2D warp0(H0);
    FilterWarp2D warp1(H1);

    int bmin0[2], bmin1[2], bmax0[2], bmax1[2];

    FilterWarp2D::computeBoundingBox(H0, warp0.getBCentroid(), img0->widthf, img0->heightf, bmin0, bmax0);
    FilterWarp2D::computeBoundingBox(H1, warp1.getBCentroid(), img1->widthf, img1->heightf, bmin1, bmax1);

    if(bPartial) {
        bmin0[1] = MIN(bmin0[1], bmin1[1]);
        bmax0[1] = MAX(bmax0[1], bmax1[1]);

        bmin1[1] = bmin0[1];
        bmax1[1] = bmax0[1];
    } else {
        for(int i = 0; i < 2; i++) {
            bmin0[i] = MIN(bmin0[i], bmin1[i]);
            bmin1[i] = bmin0[i];

            bmax0[i] = MAX(bmax0[i], bmax1[i]);
            bmax1[i] = bmax0[i];
        }
    }

    warp0.setBoundingBox(bmin0, bmax0);
    warp1.setBoundingBox(bmin1, bmax1);

    Image *img0_r = NULL;
    Image *img1_r = NULL;

    bool bTest = out->size() == 2;
    if(bTest) {
        img0_r = out->at(0);
        img1_r = out->at(1);
    }

    img0_r = warp0.Process(Single(img0), img0_r);
    img1_r = warp1.Process(Single(img1), img1_r);

    if(!bTest) {
        out->push_back(img0_r);
        out->push_back(img1_r);
    }

    return out;
}

/**
 * @brief computeImageRectification this function rectifies two images
 * @param img0 is the first image to rectify
 * @param img1 is the second image to rectify
 * @param M0 is the camera matrix (3x4) of img0
 * @param M1 is the camera matrix (3x4) of img1
 * @param out is the output as an ImageVec with two images; i.e., rectified versions of img0 and img1
 * @return is the output as an ImageVec with two images; i.e., rectified versions of img0 and img1

 */
PIC_INLINE ImageVec *computeImageRectification(Image *img0,
                                               Image *img1,
                                               Eigen::Matrix34d &M0,
                                               Eigen::Matrix34d &M1,
                                               ImageVec *out = NULL,
                                               bool bPartial = true)
{
    //NOTE: we should check that img0 and img1 are valid...
    if(img0 == NULL || img1 == NULL) {
        return out;
    }

    if(out == NULL) {
        out = new ImageVec();
    }

    Eigen::Matrix34d M0_r, M1_r;
    Eigen::Matrix3d T0, T1;

    cameraRectify(M0, M1, M0_r, M1_r, T0, T1);

    //check if the trasform is correct!
    Eigen::Vector3d corners[4], corners_T0[4];
    corners[0] = Eigen::Vector3d(0.0, 0.0, 1.0);
    corners[1] = Eigen::Vector3d(img0->widthf, 0.0, 1.0);
    corners[2] = Eigen::Vector3d(img0->widthf, img0->heightf, 1.0);
    corners[3] = Eigen::Vector3d(img0->heightf, 0.0, 1.0);

    for(int i = 0; i < 4; i ++) {
        corners_T0[i] = T0 * corners[i];
        corners_T0[i] /= corners_T0[i][2];
    }

    auto d_c   = corners[2] - corners[0];
    auto d_c_T0 = corners_T0[2] - corners_T0[0];

    bool b_x = std::signbit(d_c[0]) == std::signbit(d_c_T0[0]);
    bool b_y = std::signbit(d_c[1]) == std::signbit(d_c_T0[1]);

    double f_x = b_x ? 1.0 : -1.0;
    double f_y = b_y ? 1.0 : -1.0;

    auto H = DiagonalMatrix(Eigen::Vector3d(f_x, f_y, 1));
    T0 = H * T0;
    T1 = H * T1;

    out = computeImageRectificationWarp(img0, img1, T0, T1, out, bPartial);

    return out;
}

/**
 * @brief computeImageRectification this function rectifies two images
 * @param img0 is the first image to rectify
 * @param img1 is the second image to rectify
 * @param K0 is the intrisic matrix of img0
 * @param R0 is the rotation matrix of img0
 * @param t0 is the translation vector of img0
 * @param K1 is the intrisic matrix of im1
 * @param R1 is the rotation matrix of img1
 * @param t1 is the translation vector of img1
 * @param out is the output as an ImageVec with two images; i.e., rectified versions of img0 and img1
 * @return is the output as an ImageVec with two images; i.e., rectified versions of img0 and img1
 */
PIC_INLINE ImageVec *computeImageRectification(Image *img0,
                                               Image *img1,
                                               Eigen::Matrix3d &K0,
                                               Eigen::Matrix3d &R0,
                                               Eigen::Vector3d &t0,
                                               Eigen::Matrix3d &K1,
                                               Eigen::Matrix3d &R1,
                                               Eigen::Vector3d &t1,
                                               ImageVec *out = NULL,
                                               bool bPartial = true)
{
    //NOTE: we should check that img0 and img1 are valid...
    if(img0 == NULL || img1 == NULL) {
        return out;
    }

    if(out == NULL) {
        out = new ImageVec();
    }

    Eigen::Matrix34d M0_r, M1_r;
    Eigen::Matrix3d T0, T1;

    cameraRectify(K0, R0, t0, K1, R1, t1, M0_r, M1_r, T0, T1);

    out = computeImageRectificationWarp(img0, img1, T0, T1, out, bPartial);

    return out;
}

/**
 * @brief alignPanoramicLL
 * @param R0
 * @param t0
 * @param R1
 * @param t1
 * @param R01
 * @param t01
 */
PIC_INLINE void alignPanoramicLL(Eigen::Matrix3d &R0, Eigen::Vector3d &t0,
                                 Eigen::Matrix3d &R1, Eigen::Vector3d &t1,
                                 Eigen::Matrix3d &R01, Eigen::Vector3d &t01)
{
    t01 = t1 - t0;
    Eigen::Matrix3d R0_t = Eigen::Transpose< Eigen::Matrix3d >(R0);

    //R0 --> I
    //t0 --> 0

    R01 = R0_t * R1;
    t01 = R0_t * t01;
}

/**
 * @brief computeImageRectificationPanoramicLL
 * @param img0
 * @param img1
 * @param R01
 * @param t01
 * @param out
 * @return
 */
PIC_INLINE ImageVec *computeImageRectificationPanoramicLL(
                                               Image *img0,
                                               Image *img1,
                                               Eigen::Matrix3d &R01,
                                               Eigen::Vector3d &t01,
                                               ImageVec *out = NULL)
{
    //NOTE: we should check that img0 and img1 are valid...
    if(img0 == NULL || img1 == NULL) {
        return out;
    }

    if(out == NULL) {
        out = new ImageVec();
    }

    //rotation 1
    Eigen::Matrix3d R01_t = Eigen::Transpose< Eigen::Matrix3d >(R01);

    //rotation 2
    Eigen::Vector3d X(0.0, 1.0, 0.0);
    Eigen::Vector3d n;
    n = t01.cross(X);
    n.normalize();

    double alpha = std::acos(t01.dot(X));
    Eigen::Matrix3d rot, rotation1;

    rot = Eigen::AngleAxisd(alpha, n);
    rotation1 = rot * R01_t;

    Eigen::Matrix3f rotation0f, rotation1f;
    rotation0f = rot.cast<float>();
    rotation1f = rotation1.cast<float>();

    out->push_back(FilterRotation::execute(img0, NULL, rotation0f));
    out->push_back(FilterRotation::execute(img1, NULL, rotation1f));
    return out;
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_RECTIFICATION_HPP


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_MEANOF_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_MEANOF_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_finite.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
//no overflow average for floating numbers

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( meanof_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_type = A0;
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      A0 m = min(a0, a1);
      return if_else( bitwise_and(is_finite(a0), is_finite(a1)),//is_finite(a0) && is_finite(a1),
                      m + (max(a0, a1)-m)*Half<result_type>(),
                      average(a0, a1)
                    );
    }
  };
  BOOST_DISPATCH_OVERLOAD ( meanof_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::integer_<A0> >
                          , bd::generic_< bd::integer_<A0> >
                          )
  {
    using result_type = A0;
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return (a0 & a1) + ((a0 ^ a1) >> 1);
    }
  };
} } }

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/div.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" div real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(bs::nearbyint, T(), T()));

  STF_IEEE_EQUAL(div(bs::nearbyint, bs::Inf<T>(), bs::Inf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(div(bs::nearbyint, bs::Minf<T>(), bs::Minf<T>()), bs::Nan<r_t>());
  STF_EQUAL(div(bs::nearbyint, bs::Mone<T>(), bs::Mone<T>()), bs::One<r_t>());
  STF_IEEE_EQUAL(div(bs::nearbyint, bs::Nan<T>(), bs::Nan<T>()), bs::Nan<r_t>());
  STF_EQUAL(div(bs::nearbyint, bs::One<T>(), bs::One<T>()), bs::One<r_t>());
} // end of test for floating_

STF_CASE_TPL (" div unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::div;
  using r_t = decltype(div(bs::nearbyint, T(), T()));

  STF_EQUAL(div(bs::nearbyint, bs::One<T>(), bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(div(bs::nearbyint, T(5), T(2)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(7), T(2)), T(4));
  STF_EQUAL(div(bs::nearbyint, T(9), T(3)), T(3));
  STF_EQUAL(div(bs::nearbyint, T(10), T(3)), T(3));
  STF_EQUAL(div(bs::nearbyint, T(11), T(3)), T(4));
  STF_EQUAL(div(bs::nearbyint, T(12), T(3)), T(4));
  STF_EQUAL(div(bs::nearbyint, T(18), T(6)), T(3));
  STF_EQUAL(div(bs::nearbyint, T(20), T(6)), T(3));
  STF_EQUAL(div(bs::nearbyint, T(22), T(6)), T(4));
  STF_EQUAL(div(bs::nearbyint, T(24), T(6)), T(4));
  STF_EQUAL(div(bs::nearbyint, bs::Valmax<T>(),bs::Two<T>()), bs::Valmax<T>()/bs::Two<T>()+bs::One<T>());

} // end of test for unsigned_int_

STF_CASE_TPL (" div signed_int", STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));

  STF_EQUAL(div(bs::nearbyint, bs::Mone<T>(), bs::Mone<T>()), bs::One<r_t>());
  STF_EQUAL(div(bs::nearbyint, bs::One<T>(), bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(div(bs::nearbyint, T(5), T(2)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(7), T(2)), T(4));
  STF_EQUAL(div(bs::nearbyint, T(-5), T(2)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(-7), T(2)), T(-4));
  STF_EQUAL(div(bs::nearbyint, T(-4),T(0)), bs::Valmin<r_t>());
  STF_EQUAL(div(bs::nearbyint, T(4),T(0)), bs::Valmax<r_t>());
  STF_EQUAL(div(bs::nearbyint, T(4),T(3)), T(1));
  STF_EQUAL(div(bs::nearbyint, T(-4),T(-3)), T(1));
  STF_EQUAL(div(bs::nearbyint, T(4),T(-3)), T(-1));
  STF_EQUAL(div(bs::nearbyint, T(-4),T(3)), T(-1));
  STF_EQUAL(div(bs::nearbyint, T(5),T(3)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(-5),T(-3)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(5),T(-3)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(-5),T(3)), T(-2));

  STF_EQUAL(div(bs::nearbyint, T(5),T(4)), T(1));
  STF_EQUAL(div(bs::nearbyint, T(-5),T(-4)), T(1));
  STF_EQUAL(div(bs::nearbyint, T(5),T(-4)), T(-1));
  STF_EQUAL(div(bs::nearbyint, T(-5),T(4)), T(-1));
  STF_EQUAL(div(bs::nearbyint, T(6),T(4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(-6),T(-4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(6),T(-4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(-6),T(4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(8),T(4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(-8),T(-4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(8),T(-4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(-8),T(4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(9),T(4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(-9),T(-4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(9),T(-4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(-9),T(4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(10),T(4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(-10),T(-4)), T(2));
  STF_EQUAL(div(bs::nearbyint, T(10),T(-4)), T(-2));
  STF_EQUAL(div(bs::nearbyint, T(-10),T(4)), T(-2));

} // end of test for signed_int_


//
//  IntervalChecks.h
//  YRoots
//
//  Created by Erik Hales Parkinson on 7/7/20.
//  Copyright © 2020 Erik Hales Parkinson. All rights reserved.
//

#ifndef IntervalChecker_h
#define IntervalChecker_h

#include "Approximation/ChebyshevApproximation.hpp"
#include "SolutionTracking/IntervalTracker.hpp"
#include "Utilities/MultiPool.hpp"
#include "Utilities/ConcurrentStack.hpp"
#include "Utilities/Timer.hpp"
#include "Utilities/utilities.hpp"
#include "IntervalChecking/IntervalBounder.hpp"
#include <Eigen/Dense>

enum EvalSign {
    //If we & to signs together as a bool, we get true if we can throw out an interval
    Zero = 0,
    Positive = 1,
    Negative = 2
};

template <int Rank>
class IntervalChecker {
public:
    IntervalChecker(size_t _rank, IntervalTracker& _intervalTracker, size_t _threadNum, ConcurrentStack<SolveParameters>& _intervalsToRun, ObjectPool<SolveParameters>&        _solveParametersPool);

    bool runIntervalChecks(ChebyshevApproximation<Rank>& _approximation, Interval& _currentInterval);
    void runSubintervalChecks(std::vector<ChebyshevApproximation<Rank> >& _chebyshevApproximations, SolveParameters* _currentParameters, size_t _numGoodApproximations);

protected:
    bool runConstantTermCheck(ChebyshevApproximation<Rank>& _approximation);
    void runQuadraticCheck(ChebyshevApproximation<Rank>& _approximation);
    void pushIntervalToSolve(SolveParameters* _currentParameters, const Interval& _newInterval);

    inline EvalSign getEvalSign(double _eval, double _error) {
        if(_eval > _error) {
            return EvalSign::Positive;
        }
        else if (_eval < -_error) {
            return EvalSign::Negative;
        }
        else {
            return EvalSign::Zero;
        }
    }

protected:
    size_t                  m_rank;
    IntervalTracker&        m_intervalTracker;
    static constexpr double m_randomIntervalDivider = 0.5139303900908738;
    std::vector<Interval>   m_scaledSubIntervals;
    std::vector<bool>       m_intervalMask;
    std::vector<bool>       m_throwOutMask;
    Interval                m_tempInterval;
    std::vector<bool>       m_allowedToReduceDimension;

    //Multithreading objects
    size_t                              m_threadNum;
    ConcurrentStack<SolveParameters>&   m_intervalsToRun;
    ObjectPool<SolveParameters>&        m_solveParametersPool;

    //For Bounding Intervals
    IntervalBounder<Rank>               m_intervalBounder;

    //For Timing
    static size_t           m_timerBoundingIntervalIndex;
    static size_t           m_timerQuadraticCheckIndex;
    Timer&                  m_timer = Timer::getInstance();
};

template<int Rank>
size_t IntervalChecker<Rank>::m_timerBoundingIntervalIndex = -1;
template<int Rank>
size_t IntervalChecker<Rank>::m_timerQuadraticCheckIndex = -1;

#include "BoundingIntervalUtilities.hpp"
#include "IntervalChecker1D.ipp"
#include "IntervalChecker2D.ipp"
#include "IntervalChecker3D.ipp"
#include "IntervalCheckerND.ipp"

#endif /* IntervalChecker_h */


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE MixedMIAAssignTests
#include "MIAConfig.h"
#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "SparseMIA.h"
#include "DenseMIA.h"
#include "Index.h"

template<class _data_type>
void mult_work(size_t dim1, size_t dim2){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;

    LibMIA::DenseMIA<_data_type,4> original_dense_a(dim1,dim2,dim1,dim2);
    original_dense_a.randu(0,10);
    for(auto it=original_dense_a.data_begin();it<original_dense_a.data_end();++it){
        if(*it<7)
            *it=0;
    }
    LibMIA::DenseMIA<_data_type,4> dense_a=original_dense_a;
    LibMIA::DenseMIA<_data_type,4> dense_temp;
    LibMIA::SparseMIA<_data_type,4> original_sparse_a(dim1,dim2,dim1,dim2);
    LibMIA::SparseMIA<_data_type,4> sparse_a;
    LibMIA::SparseMIA<_data_type,4> sparse_temp;
    original_sparse_a.resize(original_sparse_a.dimensionality()/2);

    original_sparse_a.randu(-50,50);
    original_sparse_a.rand_indices();
    original_sparse_a.collect_duplicates();

    sparse_a=original_dense_a;

    BOOST_CHECK_MESSAGE(sparse_a==original_dense_a,std::string("Straight dense to sparse assign for ")+typeid(_data_type).name());

    sparse_a=original_sparse_a;
    dense_a=sparse_a;
    //sparse_a.print();
    //dense_a.print();
    BOOST_CHECK_MESSAGE(sparse_a==dense_a,std::string("Straight sparse to dense assign for ")+typeid(_data_type).name());

    //now try with indices
    sparse_a(i,j,k,l)=original_dense_a(i,j,k,l);

    BOOST_CHECK_MESSAGE(sparse_a==original_dense_a,std::string("Straight dense to sparse assign with indices for ")+typeid(_data_type).name());

    sparse_a=original_sparse_a;
    dense_a(i,j,k,l)=sparse_a(i,j,k,l);

    BOOST_CHECK_MESSAGE(sparse_a==dense_a,std::string("Straight sparse to dense assign with indices for ")+typeid(_data_type).name());

    sparse_temp(i,j,k,l)=original_dense_a(k,j,l,i);
    sparse_a(k,j,l,i)=sparse_temp(i,j,k,l);

    BOOST_CHECK_MESSAGE(sparse_a==original_dense_a,std::string("Dense to sparse assign with shuffled indices for ")+typeid(_data_type).name());

    dense_temp(i,j,k,l)=original_sparse_a(k,j,l,i);
    dense_a(k,j,l,i)=dense_temp(i,j,k,l);
//    original_sparse_a.print();
//    dense_a.print();

    BOOST_CHECK_MESSAGE(dense_a==original_sparse_a,std::string("Sparse to dense assign with shuffled indices for ")+typeid(_data_type).name());

}

BOOST_AUTO_TEST_CASE( MixedMIAAssignTests )
{

    //mult_work<double>(3,3);
    //mult_work<float>(3,3);
    //mult_work<int>(3,3);
    //mult_work<long>(3,3);
//
//

    mult_work<double>(8,5);
    mult_work<float>(8,5);
    mult_work<int>(8,5);
    mult_work<long>(8,5);
//
//
//
//    mult_work<double>(5,8);
//    mult_work<float>(5,8);
//    mult_work<int>(5,8);
//    mult_work<long>(5,8);

}


#pragma once

#include <Eigen/Dense>
#include <cmath>

/**
 * @brief shift zero frequency component to the center of the spectrum
 *
 * @param dst
 * @param src
 * @param dim (default both directions), 0: shift along rows, 1: shift along
 * columns
 */
template <typename DERIVED1, typename DERIVED2>
inline void
fftshift(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, int dim = -1)
{
  // nx, ny: position of the smallest negative frequency
  unsigned int nx = std::ceil(src.cols() / 2.);
  unsigned int ny = std::ceil(src.rows() / 2.);

  unsigned int Nx = src.cols();
  unsigned int Ny = src.rows();

  assert(dst.rows() == src.rows() && dst.cols() == src.cols());

  if (dim == -1 && dst.cols() > 1 && dst.rows() > 1) {
    // diagonal blocks
    dst.block(0, 0, Ny - ny, Nx - nx) = src.bottomRightCorner(Ny - ny, Nx - nx);
    dst.bottomRightCorner(ny, nx) = src.block(0, 0, ny, nx);

    // off-diagonal blocks
    dst.topRightCorner(Ny - ny, nx) = src.bottomLeftCorner(Ny - ny, nx);
    dst.bottomLeftCorner(ny, Nx - nx) = src.topRightCorner(ny, Nx - nx);
  } else if (dim == 0) {
    assert(Ny > 1);
    /*  shift along rows  */
    // 2d, shift along rows (y-dim)
    dst.topRows(Ny - ny) = src.bottomRows(Ny - ny);
    dst.bottomRows(ny) = src.topRows(ny);
  } else if (dim == 1) {
    assert(Nx > 1);
    /* shift along columns  */
    // 2d, shift along columns (x-dim)
    // negative frequencies
    dst.leftCols(Nx - nx) = src.rightCols(Nx - nx);
    // positive frequencies
    dst.rightCols(nx) = src.leftCols(nx);
  } else {
    assert(false);
  }
}

/**
 * @brief undoes the effect of fftshift
 *
 * @param dst
 * @param src
 * @param dim
 */
template <typename DERIVED1, typename DERIVED2>
inline void
ifftshift(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, int dim = -1)
{
  // nx, ny: position of the smallest negative frequency
  unsigned int nx = std::floor(src.cols() / 2.);
  unsigned int ny = std::floor(src.rows() / 2.);

  unsigned int Nx = src.cols();
  unsigned int Ny = src.rows();

  assert(dst.rows() == src.rows() && dst.cols() == src.cols());

  if (dim == -1 && dst.cols() > 1 && dst.rows() > 1) {
    // diagonal blocks
    dst.block(0, 0, Ny - ny, Nx - nx) = src.bottomRightCorner(Ny - ny, Nx - nx);
    dst.bottomRightCorner(ny, nx) = src.block(0, 0, ny, nx);

    // off-diagonal blocks
    dst.topRightCorner(Ny - ny, nx) = src.bottomLeftCorner(Ny - ny, nx);
    dst.bottomLeftCorner(ny, Nx - nx) = src.topRightCorner(ny, Nx - nx);
  } else if (dim == 0) {
    // 2d, shift along rows (y-dim)
    dst.topRows(Ny - ny) = src.bottomRows(Ny - ny);
    dst.bottomRows(ny) = src.topRows(ny);
  } else if (dim == 1) {
    // 2d, shift along columns (x-dim)
    // negative frequencies
    dst.leftCols(Nx - nx) = src.rightCols(Nx - nx);
    // positive frequencies
    dst.rightCols(nx) = src.leftCols(nx);
  }
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm convolution convolve
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/types.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_array.h"
#include "fern/algorithm/algebra/elementary/equal.h"
#include "fern/algorithm/statistic/count.h"
#include "fern/algorithm/convolution/neighborhood.h"
#include "fern/algorithm/convolution/convolve.h"
#include "fern/algorithm/convolution/replace_no_data_by_focal_average.h"
#include "fern/algorithm/core/test/test_utils.h"

namespace fa = fern::algorithm;

void compare_result1(
    fern::Array<double, 2> const& result)
{
    // Compare results.
    // Upper left corner. ------------------------------------------------------
    // 0, 0:
    // +----+----+----+
    // |  0 |  1 |  2 |
    // +----+----+----+
    // |  6 |  7 |  8 |
    // +----+----+----+
    // | 12 | 13 | 14 |
    // +----+----+----+
    BOOST_CHECK_CLOSE(result[0][0], 63.0 / 9.0, 1e-6);

    // 0, 1:
    // +----+----+----+----+
    // |  0 |  1 |  2 |  3 |
    // +----+----+----+----+
    // |  6 |  7 |  8 |  9 |
    // +----+----+----+----+
    // | 12 | 13 | 14 | 15 |
    // +----+----+----+----+
    BOOST_CHECK_CLOSE(result[0][1], 90.0 / 12.0, 1e-6);

    // 1, 1:
    // +----+----+----+----+
    // |  0 |  1 |  2 |  3 |
    // +----+----+----+----+
    // |  6 |  7 |  8 |  9 |
    // +----+----+----+----+
    // | 12 | 13 | 14 | 15 |
    // +----+----+----+----+
    // | 18 | 19 | 20 | 21 |
    // +----+----+----+----+
    BOOST_CHECK_CLOSE(result[1][1], 168.0 / 16.0, 1e-6);

    // Upper right corner. -----------------------------------------------------
    // 0, 4:
    // +----+----+----+----+
    // |  2 |  3 |  4 |  5 |
    // +----+----+----+----+
    // |  8 |  9 | 10 | 11 |
    // +----+----+----+----+
    // | 14 | 15 | 16 | 17 |
    // +----+----+----+----+
    BOOST_CHECK_CLOSE(result[0][4], 114.0 / 12.0, 1e-6);

    // Lower left corner. ------------------------------------------------------
    // 6, 1:
    // +----+----+----+----+
    // | 24 | 25 | 26 | 27 |
    // +----+----+----+----+
    // | 30 | 31 | 32 | 33 |
    // +----+----+----+----+
    // | 36 | 37 | 38 | 39 |
    // +----+----+----+----+
    BOOST_CHECK_CLOSE(result[6][1], 378 / 12.0, 1e-6);

    // Lower right corner. -----------------------------------------------------
    // 6, 5
    // +----+----+----+
    // | 27 | 28 | 29 |
    // +----+----+----+
    // | 33 | 34 | 35 |
    // +----+----+----+
    // | 39 | 40 | 41 |
    // +----+----+----+
    BOOST_CHECK_CLOSE(result[6][5], 306 / 9.0, 1e-6);

    // North side. -------------------------------------------------------------
    // 0, 2
    // +----+----+----+----+----+
    // |  0 |  1 |  2 |  3 |  4 |
    // +----+----+----+----+----+
    // |  6 |  7 |  8 |  9 | 10 |
    // +----+----+----+----+----+
    // | 12 | 13 | 14 | 15 | 16 |
    // +----+----+----+----+----+
    BOOST_CHECK_CLOSE(result[0][2], 120 / 15.0, 1e-6);

    // West side. --------------------------------------------------------------
    // 4, 0
    // +----+----+----+
    // | 12 | 13 | 14 |
    // +----+----+----+
    // | 18 | 19 | 20 |
    // +----+----+----+
    // | 24 | 25 | 26 |
    // +----+----+----+
    // | 30 | 31 | 32 |
    // +----+----+----+
    // | 36 | 37 | 38 |
    // +----+----+----+
    BOOST_CHECK_CLOSE(result[4][0], 375 / 15.0, 1e-6);

    // East side. --------------------------------------------------------------
    // 2, 4
    // +----+----+----+----+
    // |  2 |  3 |  4 |  5 |
    // +----+----+----+----+
    // |  8 |  9 | 10 | 11 |
    // +----+----+----+----+
    // | 14 | 15 | 16 | 17 |
    // +----+----+----+----+
    // | 20 | 21 | 22 | 23 |
    // +----+----+----+----+
    // | 26 | 27 | 28 | 29 |
    // +----+----+----+----+
    BOOST_CHECK_CLOSE(result[2][4], 310 / 20.0, 1e-6);

    // South side.
    // 6, 3
    // +----+----+----+----+----+
    // | 25 | 26 | 27 | 28 | 29 |
    // +----+----+----+----+----+
    // | 31 | 32 | 33 | 34 | 35 |
    // +----+----+----+----+----+
    // | 37 | 38 | 39 | 40 | 41 |
    // +----+----+----+----+----+
    BOOST_CHECK_CLOSE(result[6][3], 495 / 15.0, 1e-6);

    // Inner part.
    // 3, 2
    // +----+----+----+----+----+
    // |  6 |  7 |  8 |  9 | 10 |
    // +----+----+----+----+----+
    // | 12 | 13 | 14 | 15 | 16 |
    // +----+----+----+----+----+
    // | 18 | 19 | 20 | 21 | 22 |
    // +----+----+----+----+----+
    // | 24 | 25 | 26 | 27 | 28 |
    // +----+----+----+----+----+
    // | 30 | 31 | 32 | 33 | 34 |
    // +----+----+----+----+----+
    BOOST_CHECK_CLOSE(result[3][2], 500 / 25.0, 1e-6);

    // Make sure all cells in the result have a new value.
    BOOST_CHECK_EQUAL(std::count(result.data(), result.data() +
        result.num_elements(), 0), 0);
}

void compare_result2(
    fern::Array<double, 2> const& result)
{
    // Upper left corner. ----------------------------------------------
    BOOST_CHECK_CLOSE(result[0][0], (63.0 + 24.5) / (9.0 + 7.0), 1e-6);
    BOOST_CHECK_CLOSE(result[0][1], (90.0 + 27.5) / (12.0 + 8.0), 1e-6);
    BOOST_CHECK_CLOSE(result[1][1], (168.0 + 45.5) / (16.0 + 9.0),
        1e-6);

    // Upper right corner. ---------------------------------------------
    BOOST_CHECK_CLOSE(result[0][4], (114.0 + 54.5) / (12.0 + 8.0),
        1e-6);

    // Lower left corner. ----------------------------------------------
    // Out of image values:
    //     (18 + 24 + 30) / 3 ->  72 / 3
    //     (24 + 30 + 36) / 3 ->  90 / 3
    //     (30 + 36     ) / 2 ->  66 / 2
    //     (36          ) / 1 ->  36 / 1
    //     (36 + 37     ) / 2 ->  73 / 2
    //     (36 + 37 + 38) / 3 -> 111 / 3
    //     (37 + 38 + 39) / 3 -> 114 / 3
    //     (38 + 39 + 40) / 3 -> 117 / 3
    BOOST_CHECK_CLOSE(result[6][1], (378.0 + 273.5) / (12.0 + 8.0),
        1e-6);

    // Lower right corner. ---------------------------------------------
    // Out of image values:
    //     (38 + 39 + 40) / 3 -> 117 / 3
    //     (39 + 40 + 41) / 3 -> 120 / 3
    //     (40 + 41     ) / 2 ->  81 / 2
    //     (41          ) / 1 ->  41 / 1
    //     (41 + 35     ) / 2 ->  76 / 2
    //     (29 + 35 + 41) / 3 -> 105 / 3
    //     (23 + 29 + 35) / 3 ->  87 / 3
    BOOST_CHECK_CLOSE(result[6][5], (306.0 + 262.5) / (9.0 + 7.0),
        1e-6);

    // North side. -----------------------------------------------------
    // Out of image values:
    // (0 + 1    ) / 2
    // (0 + 1 + 2) / 3
    // (1 + 2 + 3) / 3
    // (2 + 3 + 4) / 3
    // (3 + 4 + 5) / 3
    BOOST_CHECK_CLOSE(result[0][2], (120.0 + 10.5) / (15.0 + 5.0),
        1e-6);

    // West side. ------------------------------------------------------
    // Out of image values:
    // ( 6 + 12 + 18) / 3 -> 36 / 3
    // (12 + 18 + 24) / 3 -> 54 / 3
    // (18 + 24 + 30) / 3 -> 72 / 3
    // (24 + 30 + 36) / 3 -> 90 / 3
    // (30 + 36     ) / 2 -> 66 / 2
    BOOST_CHECK_CLOSE(result[4][0], (375.0 + 117.0) / (15.0 + 5.0),
        1e-6);

    // East side. ------------------------------------------------------
    // Out of image values:
    // ( 5 + 11     ) / 2
    // ( 5 + 11 + 17) / 3
    // (11 + 17 + 23) / 3
    // (17 + 23 + 29) / 3
    // (23 + 29 + 35) / 3
    BOOST_CHECK_CLOSE(result[2][4], (310.0 + 88.0) / (20.0 + 5.0),
        1e-6);

    // South side. -----------------------------------------------------
    // Out of image values:
    // (36 + 37 + 38) / 3
    // (37 + 38 + 39) / 3
    // (38 + 39 + 40) / 3
    // (39 + 40 + 41) / 3
    // (40 + 41     ) / 2
    BOOST_CHECK_CLOSE(result[6][3], (495.0 + 194.5) / (15.0 + 5.0),
        1e-6);

    // Inner part. -----------------------------------------------------
    // No out of image values.
    BOOST_CHECK_CLOSE(result[3][2], 500 / 25.0, 1e-6);

    // Make sure all cells in the result have a new value.
    BOOST_CHECK_EQUAL(std::count(result.data(), result.data() +
        result.num_elements(), 0), 0);
}

void compare_result3(
    fern::Array<double, 2> const& result)
{
    // +----+----+
    // |  0 |  1 |
    // +----+----+
    // |  6 |  7 |
    // +----+----+
    BOOST_CHECK_CLOSE(result[0][0], (0 + 1 + 6) / 3.0, 1e-6);

    // +----+----+
    // | 34 | 35 |
    // +----+----+
    // | 40 | 41 |
    // +----+----+
    BOOST_CHECK_CLOSE(result[6][5], (35 + 40 + 41) / 3.0, 1e-6);

    // +----+----+----+
    // |  0 |  1 |  2 |
    // +----+----+----+
    // |  6 |  7 |  8 |
    // +----+----+----+
    // | 12 | 13 | 14 |
    // +----+----+----+
    BOOST_CHECK_CLOSE(result[1][1], (1 + 6 + 7 + 8 + 13) / 5.0, 1e-6);

    // +----+----+
    // | 18 | 19 |
    // +----+----+
    // | 24 | 25 |
    // +----+----+
    // | 30 | 31 |
    // +----+----+
    BOOST_CHECK_CLOSE(result[4][0], (18 + 24 + 25 + 30) / 4.0, 1e-6);

    // +----+----+----+
    // |  1 |  2 |  3 |
    // +----+----+----+
    // |  7 |  8 |  9 |
    // +----+----+----+
    // | 13 | 14 | 15 |
    // +----+----+----+
    BOOST_CHECK_CLOSE(result[1][2], (2 + 7 + 8 + 9 + 14) / 5.0, 1e-6);
}

void compare_result4(
    fern::MaskedArray<double, 2> const& result)
{
    // +----+----+----+
    // | 10 |  7 |  6 |
    // +----+----+----+
    // |  2 | 18 | -1 |
    // +----+----+----+
    // | 12 | -6 |  8 |
    // +----+----+----+
    BOOST_CHECK(!result.mask()[0][0]);
    BOOST_CHECK_CLOSE(result[0][0], 6 + 4, 1e-6);

    BOOST_CHECK(!result.mask()[0][1]);
    BOOST_CHECK_CLOSE(result[0][1], 8 + -2 + 1, 1e-6);

    BOOST_CHECK(!result.mask()[0][2]);
    BOOST_CHECK_CLOSE(result[0][2], 6 + 0, 1e-6);

    BOOST_CHECK(!result.mask()[1][0]);
    BOOST_CHECK_CLOSE(result[1][0], 8 + 1 + -7, 1e-6);

    BOOST_CHECK(!result.mask()[1][1]);
    BOOST_CHECK_CLOSE(result[1][1], 6 + 4 + 0 + 8, 1e-6);

    BOOST_CHECK(!result.mask()[1][2]);
    BOOST_CHECK_CLOSE(result[1][2], -2 + 1, 1e-6);

    BOOST_CHECK(!result.mask()[2][0]);
    BOOST_CHECK_CLOSE(result[2][0], 4 + 8, 1e-6);

    BOOST_CHECK(!result.mask()[2][1]);
    BOOST_CHECK_CLOSE(result[2][1], 1 + -7, 1e-6);

    BOOST_CHECK(!result.mask()[2][2]);
    BOOST_CHECK_CLOSE(result[2][2], 0 + 8, 1e-6);
}

void compare_result5(
    fern::MaskedArray<double, 2> const& result)
{
    // +----+----+----+----+-----+-----+
    // |  7 |  7 |  1 |  4 |  15 |  15 |
    // +----+----+----+----+-----+-----+
    // | 19 | 20 |  7 | 10 |  15 |  15 |
    // +----+----+----+----+-----+-----+
    // | 37 | 38 | 13 |  X |  32 |  34 |
    // +----+----+----+----+-----+-----+
    // | 55 | 56 | 19 | 22 |  51 |  51 |
    // +----+----+----+----+-----+-----+
    // | 73 | 74 | 25 | 28 |  85 |  86 |
    // +----+----+----+----+-----+-----+
    // | 91 | 92 | 31 | 34 | 103 | 104 |
    // +----+----+----+----+-----+-----+
    // | 67 | 67 | 37 | 40 |  75 |  75 |
    // +----+----+----+----+-----+-----+

    size_t const nr_cols = 6;

    size_t row_id = 0;
    std::vector<double> values = { 7, 7, 1, 4, 15, 15 };
    std::vector<bool> no_data = { false, false, false, false, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data() + row_id * nr_cols,
        result.data() + row_id * nr_cols + nr_cols);

    ++row_id;
    values = { 19, 20, 7, 10, 15, 15 };
    no_data = { false, false, false, false, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data() + row_id * nr_cols,
        result.data() + row_id * nr_cols + nr_cols);

    ++row_id;
    no_data = { false, false, false, true, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);

    BOOST_CHECK_CLOSE(result[2][0], 37, 1e-6);
    BOOST_CHECK_CLOSE(result[2][1], 38, 1e-6);
    BOOST_CHECK_CLOSE(result[2][2], 13, 1e-6);
    BOOST_CHECK_CLOSE(result[2][4], 32, 1e-6);
    BOOST_CHECK_CLOSE(result[2][5], 34, 1e-6);

    ++row_id;
    values = { 55, 56, 19, 22, 51, 51 };
    no_data = { false, false, false, false, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data() + row_id * nr_cols,
        result.data() + row_id * nr_cols + nr_cols);

    ++row_id;
    values = { 73, 74, 25, 28, 85, 86 };
    no_data = { false, false, false, false, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data() + row_id * nr_cols,
        result.data() + row_id * nr_cols + nr_cols);

    ++row_id;
    values = { 91, 92, 31, 34, 103, 104 };
    no_data = { false, false, false, false, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data() + row_id * nr_cols,
        result.data() + row_id * nr_cols + nr_cols);

    ++row_id;
    values = { 67, 67, 37, 40, 75, 75 };
    no_data = { false, false, false, false, false, false };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data() + row_id * nr_cols,
        result.data() + row_id * nr_cols + nr_cols);
}

void compare_result6(
    fern::MaskedArray<double, 2> const& result)
{
    // +----+----ٍ+-----+-----+----+----+
    // |  X |  X |   8 |   9 |  X |  X |
    // +----+----ٍ+-----+-----+----+----+
    // |  X | 21 |  23 |  23 | 25 |  X |
    // +----+----ٍ+-----+-----+----+----+
    // | 31 | 33 |  56 |  60 | 37 | 39 |
    // +----+----ٍ+-----+-----+----+----+
    // | 19 | 76 |  80 |  84 | 88 | 22 |
    // +----+----ٍ+-----+-----+----+----+
    // | 43 | 45 | 104 | 108 | 49 | 51 |
    // +----+----ٍ+-----+-----+----+----+
    // |  X | 57 |  97 |  59 | 61 |  X |
    // +----+----ٍ+-----+-----+----+----+
    // |  X | 38 |  32 |  71 |  X |  X |
    // +----+----ٍ+-----+-----+----+----+

    size_t const nr_cols = 6;
    size_t row_id = 0;
    std::vector<double> values;
    std::vector<bool> no_data;

    no_data = { true, true, false, false, true, true };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[0][2], 8.0, 1e-6);
    BOOST_CHECK_CLOSE(result[0][3], 9.0, 1e-6);

    ++row_id;
    no_data = { true, false, false, false, false, true };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[1][1], 21.0, 1e-6);
    BOOST_CHECK_CLOSE(result[1][2], 23.0, 1e-6);
    BOOST_CHECK_CLOSE(result[1][3], 23.0, 1e-6);
    BOOST_CHECK_CLOSE(result[1][4], 25.0, 1e-6);

    ++row_id;
    no_data = { false, false, false, false, false, false };
    values = { 31, 33,  56,  60, 37, 39 };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[2][0], 31.0, 1e-6);
    BOOST_CHECK_CLOSE(result[2][1], 33.0, 1e-6);
    BOOST_CHECK_CLOSE(result[2][2], 56.0, 1e-6);
    BOOST_CHECK_CLOSE(result[2][3], 60.0, 1e-6);
    BOOST_CHECK_CLOSE(result[2][4], 37.0, 1e-6);
    BOOST_CHECK_CLOSE(result[2][5], 39.0, 1e-6);

    ++row_id;
    no_data = { false, false, false, false, false, false };
    values = { 19, 76,  80,  84, 88, 22 };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[3][0], 19.0, 1e-6);
    BOOST_CHECK_CLOSE(result[3][1], 76.0, 1e-6);
    BOOST_CHECK_CLOSE(result[3][2], 80.0, 1e-6);
    BOOST_CHECK_CLOSE(result[3][3], 84.0, 1e-6);
    BOOST_CHECK_CLOSE(result[3][4], 88.0, 1e-6);
    BOOST_CHECK_CLOSE(result[3][5], 22.0, 1e-6);

    ++row_id;
    no_data = { false, false, false, false, false, false };
    values = { 43, 45, 104, 108, 49, 51 };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[4][0], 43.0, 1e-6);
    BOOST_CHECK_CLOSE(result[4][1], 45.0, 1e-6);
    BOOST_CHECK_CLOSE(result[4][2], 104.0, 1e-6);
    BOOST_CHECK_CLOSE(result[4][3], 108.0, 1e-6);
    BOOST_CHECK_CLOSE(result[4][4], 49.0, 1e-6);
    BOOST_CHECK_CLOSE(result[4][5], 51.0, 1e-6);

    ++row_id;
    no_data = { true, false, false, false, false, true };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[5][1], 57.0, 1e-6);
    BOOST_CHECK_CLOSE(result[5][2], 97.0, 1e-6);
    BOOST_CHECK_CLOSE(result[5][3], 59.0, 1e-6);
    BOOST_CHECK_CLOSE(result[5][4], 61.0, 1e-6);

    ++row_id;
    no_data = { true, false, false, false, true, true };
    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data() + row_id * nr_cols,
        result.mask().data() + row_id * nr_cols + nr_cols);
    BOOST_CHECK_CLOSE(result[6][1], 38.0, 1e-6);
    BOOST_CHECK_CLOSE(result[6][2], 32.0, 1e-6);
    BOOST_CHECK_CLOSE(result[6][3], 71.0, 1e-6);
}

BOOST_AUTO_TEST_CASE(convolve)
{
    using Weights = std::initializer_list<std::initializer_list<int>>;

    fa::ParallelExecutionPolicy parallel;
    fa::SequentialExecutionPolicy sequential;

    // Kernel with radius 2.
    {
        // Create input array:
        // +----+----+----+----+----+----+
        // |  0 |  1 |  2 |  3 |  4 |  5 |
        // +----+----+----+----+----+----+
        // |  6 |  7 |  8 |  9 | 10 | 11 |
        // +----+----+----+----+----+----+
        // | 12 | 13 | 14 | 15 | 16 | 17 |
        // +----+----+----+----+----+----+
        // | 18 | 19 | 20 | 21 | 22 | 23 |
        // +----+----+----+----+----+----+
        // | 24 | 25 | 26 | 27 | 28 | 29 |
        // +----+----+----+----+----+----+
        // | 30 | 31 | 32 | 33 | 34 | 35 |
        // +----+----+----+----+----+----+
        // | 36 | 37 | 38 | 39 | 40 | 41 |
        // +----+----+----+----+----+----+
        size_t const nr_rows = 7;
        size_t const nr_cols = 6;
        auto extents = fern::extents[nr_rows][nr_cols];
        fern::Array<double, 2> argument(extents);
        std::iota(
            argument.data(), argument.data() + argument.num_elements(), 0);

        // Calculate local average.
        // Define kernel shape and weights.

        Weights weights{
            {1, 1, 1, 1, 1},
            {1, 1, 1, 1, 1},
            {1, 1, 1, 1, 1},
            {1, 1, 1, 1, 1},
            {1, 1, 1, 1, 1}
        };

        fern::Square<int, 2> compile_time_kernel(weights);
        fern::Kernel<int> runtime_kernel(2, weights);

        // Convolute while skipping out-of-image cells.
        {
            // Sequential, compile-time kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve(
                    sequential, argument, compile_time_kernel, result);
                compare_result1(result);
            }

            // Sequential, runtime kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve(
                    sequential, argument, runtime_kernel, result);
                compare_result1(result);
            }

            // Parallel, compile-time kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve(
                    parallel, argument, compile_time_kernel, result);
                compare_result1(result);
            }

            // Parallel, runtime kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve(
                    parallel, argument, runtime_kernel, result);
                compare_result1(result);
            }
        }

        // Convolute while calculating values for out-of-image cells.
        {
            using AlternativeForNoDataPolicy=fa::convolve::SkipNoData;
            using NormalizePolicy=fa::convolve::DivideByWeights;
            using OutOfImagePolicy=
                fa::convolve::ReplaceOutOfImageByFocalAverage;
            using NoDataFocusElementPolicy=fa::convolve::KeepNoDataFocusElement;
            using InputNoDataPolicy=fa::InputNoDataPolicies<fa::SkipNoData>;
            using OutputNoDataPolicy=fa::DontMarkNoData;

            OutputNoDataPolicy output_no_data_policy;

            // Sequential, compile-time kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve<
                    AlternativeForNoDataPolicy,
                    NormalizePolicy,
                    OutOfImagePolicy,
                    NoDataFocusElementPolicy,
                    fa::unary::DiscardRangeErrors,
                    InputNoDataPolicy,
                    OutputNoDataPolicy>(
                        InputNoDataPolicy{{}}, output_no_data_policy,
                        sequential, argument, compile_time_kernel, result);
                compare_result2(result);
            }

            // Sequential, runtime kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve<
                    AlternativeForNoDataPolicy,
                    NormalizePolicy,
                    OutOfImagePolicy,
                    NoDataFocusElementPolicy,
                    fa::unary::DiscardRangeErrors,
                    InputNoDataPolicy,
                    OutputNoDataPolicy>(
                        InputNoDataPolicy{{}}, output_no_data_policy,
                        sequential, argument, runtime_kernel, result);
                compare_result2(result);
            }

            // Parallel, compile-time kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve<
                    AlternativeForNoDataPolicy,
                    NormalizePolicy,
                    OutOfImagePolicy,
                    NoDataFocusElementPolicy,
                    fa::unary::DiscardRangeErrors,
                    InputNoDataPolicy,
                    OutputNoDataPolicy>(
                        InputNoDataPolicy{{}}, output_no_data_policy,
                        parallel, argument, compile_time_kernel, result);
                compare_result2(result);
            }

            // Parallel, runtime kernel
            {
                fern::Array<double, 2> result(extents);
                fa::convolution::convolve<
                    AlternativeForNoDataPolicy,
                    NormalizePolicy,
                    OutOfImagePolicy,
                    NoDataFocusElementPolicy,
                    fa::unary::DiscardRangeErrors,
                    InputNoDataPolicy,
                    OutputNoDataPolicy>(
                        InputNoDataPolicy{{}}, output_no_data_policy,
                        parallel, argument, runtime_kernel, result);
                compare_result2(result);
            }
        }
    }

    // Kernel with radius 1.
    // This used to crash.
    {
        // Create input array:
        // +----+----+----+----+----+----+
        // |  0 |  1 |  2 |  3 |  4 |  5 |
        // +----+----+----+----+----+----+
        // |  6 |  7 |  8 |  9 | 10 | 11 |
        // +----+----+----+----+----+----+
        // | 12 | 13 | 14 | 15 | 16 | 17 |
        // +----+----+----+----+----+----+
        // | 18 | 19 | 20 | 21 | 22 | 23 |
        // +----+----+----+----+----+----+
        // | 24 | 25 | 26 | 27 | 28 | 29 |
        // +----+----+----+----+----+----+
        // | 30 | 31 | 32 | 33 | 34 | 35 |
        // +----+----+----+----+----+----+
        // | 36 | 37 | 38 | 39 | 40 | 41 |
        // +----+----+----+----+----+----+
        size_t const nr_rows = 7;
        size_t const nr_cols = 6;
        auto extents = fern::extents[nr_rows][nr_cols];
        fern::Array<double, 2> argument(extents);
        std::iota(argument.data(), argument.data() + argument.num_elements(),
            0);

        // Calculate local average.
        // Define kernel shape and weights.
        Weights weights{
            {1, 1, 1},
            {1, 1, 1},
            {1, 1, 1}
        };

        fern::Square<int, 1> compile_time_kernel(weights);
        fern::Kernel<int> runtime_kernel(1, weights);

        // Sequential, compile-time kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                sequential, argument, compile_time_kernel, result);
        }

        // Sequential, runtime kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                sequential, argument, runtime_kernel, result);
        }

        // Parallel, compile-time kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                parallel, argument, compile_time_kernel, result);
        }

        // Parallel, runtime kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                parallel, argument, runtime_kernel, result);
        }
    }
}

// TODO Test Small image with out of image policy with larger kernel.
// TODO Test Focal Sum, with kernel that doesn't weigh.

template<
    class Value,
    class Result>
using OutOfRangePolicy = fa::convolve::OutOfRangePolicy<Value, Result>;

BOOST_AUTO_TEST_CASE(out_of_range_policy)
{
    // Make sure that out of range can be detected and that no-data can be
    // written when it happens.

    {
        auto min_float32 = fern::min<fern::f32>();
        auto max_float32 = fern::max<fern::f32>();

        OutOfRangePolicy<fern::f32, fern::f32> policy;
        BOOST_CHECK(policy.within_range(5.0));
        BOOST_CHECK(policy.within_range(-5.0));
        BOOST_CHECK(policy.within_range(0.0));
        BOOST_CHECK(policy.within_range(min_float32));
        BOOST_CHECK(policy.within_range(max_float32));
        BOOST_CHECK(!policy.within_range(2 * max_float32));
    }
}

BOOST_AUTO_TEST_CASE(no_data_policies)
{
    using Weights = std::initializer_list<std::initializer_list<int>>;

    // Make sure that input no-data is detected and handled correctly.
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<fern::Mask<2>>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<fern::Mask<2>>;

    fa::SequentialExecutionPolicy sequential;

    size_t const nr_rows = 3;
    size_t const nr_cols = 3;
    auto extents = fern::extents[nr_rows][nr_cols];

    // Local average kernel.
    Weights weights_1{
        {1, 1, 1},
        {1, 1, 1},
        {1, 1, 1}
    };
    Weights weights_2{
        {2, 2, 2},
        {2, 2, 2},
        {2, 2, 2}
    };

    fern::Kernel<int> kernel_1(1, weights_1);
    fern::Kernel<int> kernel_2(1, weights_2);

    // Source image with a few masked values.
    {
        // +---+---+---+
        // | 0 | 1 | 2 |
        // +---+---+---+
        // | 3 | X | 5 |
        // +---+---+---+
        // | X | 7 | 8 |
        // +---+---+---+
        fern::MaskedArray<double, 2> source(extents);
        std::iota(source.data(), source.data() + source.num_elements(), 0);
        source.mask()[1][1] = true;
        source.mask()[2][0] = true;
        fern::MaskedArray<double, 2> destination(extents);

        // Skip no-data.
        {
            fern::MaskedArray<double, 2> destination(extents);
            destination.fill(999.9);
            OutputNoDataPolicy output_no_data_policy(destination.mask(), true);
            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::KeepNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{source.mask(), true}},
                    output_no_data_policy,
                    sequential,
                    source, kernel_1, destination);

            // Verify mask.
            uint64_t nr_masked_cells{0};
            fa::statistic::count(sequential, destination.mask(),
                true, nr_masked_cells);
            BOOST_CHECK_EQUAL(nr_masked_cells, 2u);
            BOOST_CHECK(destination.mask()[1][1]);
            BOOST_CHECK(destination.mask()[2][0]);

            // Verify values.
            fern::Array<double, 2> result_we_want({
                { 4.0/3.0, 11.0/5.0,  8.0/3.0},
                {11.0/4.0,    999.9, 23.0/5.0},
                {   999.9, 23.0/4.0, 20.0/3.0}
            });
            fern::Array<bool, 2> equal_cells(extents);
            fa::algebra::equal(sequential, destination, result_we_want,
                equal_cells);

            uint64_t nr_equal_cells{0};
            fa::statistic::count(sequential, equal_cells, true,
                nr_equal_cells);
            BOOST_CHECK_EQUAL(nr_equal_cells, 9u);
        }

        // TODO Remove this, replacing no-data is pre-processing.
        /// // Replace no-data by focal average.
        /// {
        ///     // Focal average of the no-data cells is
        ///     // - 26/7
        ///     // - 10/2
        ///     // +-----+--------+---+
        ///     // |  0  |     1  | 2 |
        ///     // +-----+--------+---+
        ///     // |  3  | (26/7) | 5 |
        ///     // +-----+--------+---+
        ///     // | (5) |     7  | 8 |
        ///     // +-----+--------+---+
        ///     fern::MaskedArray<double, 2> destination(extents);
        ///     destination.fill(999.9);
        ///     fa::convolution::convolve<
        ///         fa::convolve::DivideByWeights,
        ///         fa::convolve::SkipOutOfImage,
        ///         fern::unary::DiscardRangeErrors>(
        ///         fa::convolve::ReplaceNoDataByFocalAverage,
        ///             sequential,
        ///             InputNoDataPolicy{{source.mask(), true}},
        ///             OutputNoDataPolicy(destination.mask(), true),
        ///             source, kernel_1, destination);

        ///     // Verify mask.
        ///     size_t nr_masked_cells{0};
        ///     fa::statistic::count(destination.mask(), true, nr_masked_cells);
        ///     BOOST_CHECK_EQUAL(nr_masked_cells, 2);
        ///     BOOST_CHECK(destination.mask()[1][1]);
        ///     BOOST_CHECK(destination.mask()[2][0]);

        ///     // Verify values.
        ///     double const v1 = 26.0 / 7.0;
        ///     double const v2 = 10.0 / 2.0;
        ///     fern::Array<double, 2> result_we_want({
        ///         {       (4.0 + v1)/4.0,      (11.0 + v1)/6.0,   (8.0 + v1)/4.0},
        ///         { (11.0 + v1 + v2)/6.0,                999.9,  (23.0 + v1)/6.0},
        ///         {                999.9, (23.0 + v1 + v2)/6.0,  (20.0 + v1)/4.0}
        ///     });

        ///     fern::Array<bool, 2> equal_cells(extents);
        ///     fa::algebra::equal(destination, result_we_want, equal_cells);

        ///     size_t nr_equal_cells{0};
        ///     fa::statistic::count(equal_cells, true, nr_equal_cells);
        ///     BOOST_CHECK_EQUAL(nr_equal_cells, 9);
        /// }
    }

    // Source image with lots of masked values.
    {
        fern::MaskedArray<double, 2> source(extents);
        std::iota(source.data(), source.data() + source.num_elements(), 0);
        source.mask_all();
        fern::MaskedArray<double, 2> destination(extents);
        OutputNoDataPolicy output_no_data_policy(destination.mask(), true);

        fa::convolution::convolve<
            fa::convolve::SkipNoData,
            fa::convolve::DivideByWeights,
            fa::convolve::SkipOutOfImage,
            fa::convolve::KeepNoDataFocusElement,
            fa::unary::DiscardRangeErrors>(
                InputNoDataPolicy{{source.mask(), true}},
                output_no_data_policy,
                sequential, source, kernel_1, destination);

        uint64_t nr_masked_cells;
        fa::statistic::count(sequential, destination.mask(), true,
            nr_masked_cells);
        BOOST_CHECK_EQUAL(nr_masked_cells, nr_rows * nr_cols);
    }

    // Source image with very large values. Convolving these should result
    // in out-оf-range values. It must be possible to detect these and mark
    // them as no-data in the result.
    {
        fern::MaskedArray<double, 2> source(extents);
        std::fill(source.data(), source.data() + source.num_elements(),
            fern::max<double>());
        fern::MaskedArray<double, 2> destination(extents);
        OutputNoDataPolicy output_no_data_policy(destination.mask(), true);

        fa::convolution::convolve<
            fa::convolve::SkipNoData,
            fa::convolve::DivideByWeights,
            fa::convolve::SkipOutOfImage,
            fa::convolve::KeepNoDataFocusElement,
            fa::convolve::OutOfRangePolicy>(
                InputNoDataPolicy{{source.mask(), true}},
                output_no_data_policy,
                sequential, source, kernel_2, destination);

        uint64_t nr_masked_cells;
        fa::statistic::count(sequential, destination.mask(), true,
            nr_masked_cells);
        BOOST_CHECK_EQUAL(nr_masked_cells, nr_rows * nr_cols);
    }
}

BOOST_AUTO_TEST_CASE(boolean_kernel_weights)
{
    using Weights = std::initializer_list<std::initializer_list<bool>>;

    fa::ParallelExecutionPolicy parallel;
    fa::SequentialExecutionPolicy sequential;

    // Kernel with radius 1 and boolean weights.
    {
        // Create input array:
        // +----+----+----+----+----+----+
        // |  0 |  1 |  2 |  3 |  4 |  5 |
        // +----+----+----+----+----+----+
        // |  6 |  7 |  8 |  9 | 10 | 11 |
        // +----+----+----+----+----+----+
        // | 12 | 13 | 14 | 15 | 16 | 17 |
        // +----+----+----+----+----+----+
        // | 18 | 19 | 20 | 21 | 22 | 23 |
        // +----+----+----+----+----+----+
        // | 24 | 25 | 26 | 27 | 28 | 29 |
        // +----+----+----+----+----+----+
        // | 30 | 31 | 32 | 33 | 34 | 35 |
        // +----+----+----+----+----+----+
        // | 36 | 37 | 38 | 39 | 40 | 41 |
        // +----+----+----+----+----+----+
        size_t const nr_rows = 7;
        size_t const nr_cols = 6;
        auto extents = fern::extents[nr_rows][nr_cols];
        fern::Array<double, 2> argument(extents);
        std::iota(argument.data(), argument.data() + argument.num_elements(),
            0);

        // Define kernel shape and weights.
        Weights weights{
            {false, true, false},
            {true , true, true },
            {false, true, false}
        };
        fern::Square<bool, 1> compile_time_kernel{weights};
        fern::Kernel<bool> runtime_kernel{1, weights};

        // Sequential, compile-time kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                sequential, argument, compile_time_kernel, result);
            compare_result3(result);
        }

        // Sequential, runtime kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                sequential, argument, runtime_kernel, result);
            compare_result3(result);
        }

        // Parallel, compile-time kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                parallel, argument, compile_time_kernel, result);
            compare_result3(result);
        }

        // Parallel, runtime kernel
        {
            fern::Array<double, 2> result(extents);
            fa::convolution::convolve(
                parallel, argument, runtime_kernel, result);
            compare_result3(result);
        }
    }
}

BOOST_AUTO_TEST_CASE(no_data_focus_element_policy)
{
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<fern::Mask<2>>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<fern::Mask<2>>;

    fa::ParallelExecutionPolicy parallel;
    fa::SequentialExecutionPolicy sequential;

    // PCRaster window4total example
    {
        // Create input array:
        // +----+----+----+
        // |  8 |  6 | -2 |
        // +----+----+----+
        // |  4 |  1 |  0 |
        // +----+----+----+
        // | -7 |  8 |  X |
        // +----+----+----+
        size_t const nr_rows = 3;
        size_t const nr_cols = 3;
        auto extents = fern::extents[nr_rows][nr_cols];
        fern::MaskedArray<double, 2> argument(extents);

        argument[0][0] = 8;
        argument[0][1] = 6;
        argument[0][2] = -2;
        argument[1][0] = 4;
        argument[1][1] = 1;
        argument[1][2] = 0;
        argument[2][0] = -7;
        argument[2][1] = 8;
        argument.mask()[2][2] = true;

        // Define kernel shape and weights.
        // Similar to PCRaster's window4total algorithm.
        fern::Square<bool, 1> kernel({
            {false, true, false},
            {true , false, true },
            {false, true, false}
        });

        // Sequential.
        {
            fern::MaskedArray<double, 2> result(extents);
            OutputNoDataPolicy output_no_data_policy(result.mask(), true);

            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DontDivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::ReplaceNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{argument.mask(), true}},
                    output_no_data_policy,
                    sequential,
                    argument, kernel, result);

            compare_result4(result);
        }

        // Parallel.
        {
            fern::MaskedArray<double, 2> result(extents);
            OutputNoDataPolicy output_no_data_policy(result.mask(), true);

            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DontDivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::ReplaceNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{argument.mask(), true}},
                    output_no_data_policy,
                    parallel,
                    argument, kernel, result);

            compare_result4(result);
        }
    }

    {
        // Create input array:
        // +----+----+---+---+----+----+
        // |  0 |  1 | X | X |  4 |  5 |
        // +----+----+---+---+----+----+
        // |  6 |  7 | X | X | 10 | 11 |
        // +----+----+---+---+----+----+
        // | 12 | 13 | X | X |  X |  X |
        // +----+----+---+---+----+----+
        // | 18 | 19 | X | X | 22 | 23 |
        // +----+----+---+---+----+----+
        // | 24 | 25 | X | X | 28 | 29 |
        // +----+----+---+---+----+----+
        // | 30 | 31 | X | X | 34 | 35 |
        // +----+----+---+---+----+----+
        // | 36 | 37 | X | X | 40 | 41 |
        // +----+----+---+---+----+----+

        size_t const nr_rows = 7;
        size_t const nr_cols = 6;
        auto extents = fern::extents[nr_rows][nr_cols];
        fern::MaskedArray<double, 2> argument(extents);

        std::iota(argument.data(), argument.data() + argument.num_elements(),
            0);

        argument.mask()[0][2] = true;
        argument.mask()[0][3] = true;
        argument.mask()[1][2] = true;
        argument.mask()[1][3] = true;
        argument.mask()[2][2] = true;
        argument.mask()[2][3] = true;
        argument.mask()[2][4] = true;
        argument.mask()[2][5] = true;
        argument.mask()[3][2] = true;
        argument.mask()[3][3] = true;
        argument.mask()[4][2] = true;
        argument.mask()[4][3] = true;
        argument.mask()[5][2] = true;
        argument.mask()[5][3] = true;
        argument.mask()[6][2] = true;
        argument.mask()[6][3] = true;

        // Define kernel shape and weights.
        // Similar to PCRaster's window4total algorithm.
        fern::Square<bool, 1> kernel({
            {false, true, false},
            {true , false, true },
            {false, true, false}
        });

        // Sequential.
        {
            fern::MaskedArray<double, 2> result(extents);
            fa::convolution::convolve(sequential, argument, kernel,
                result);
            OutputNoDataPolicy output_no_data_policy(result.mask(), true);

            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DontDivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::ReplaceNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{argument.mask(), true}},
                    output_no_data_policy,
                    sequential,
                    argument, kernel, result);

            compare_result5(result);
        }

        // Parallel.
        {
            fern::MaskedArray<double, 2> result(extents);
            fa::convolution::convolve(sequential, argument, kernel,
                result);
            OutputNoDataPolicy output_no_data_policy(result.mask(), true);

            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DontDivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::ReplaceNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{argument.mask(), true}},
                    output_no_data_policy,
                    parallel,
                    argument, kernel, result);

            compare_result5(result);
        }
    }

    {
        // Create input array:
        // +----+----+----+----+----+----+
        // |  X |  X |  X |  X |  X |  X |
        // +----+----+----+----+----+----+
        // |  X |  X |  8 |  9 |  X |  X |
        // +----+----+----+----+----+----+
        // |  X | 13 | 14 | 15 | 16 |  X |
        // +----+----+----+----+----+----+
        // | 18 | 19 | 20 | 21 | 22 | 23 |
        // +----+----+----+----+----+----+
        // |  X | 25 | 26 | 27 | 28 |  X |
        // +----+----+----+----+----+----+
        // |  X |  X | 32 | 33 |  X |  X |
        // +----+----+----+----+----+----+
        // |  X |  X | 38 |  X |  X |  X |
        // +----+----+----+----+----+----+

        size_t const nr_rows = 7;
        size_t const nr_cols = 6;
        auto extents = fern::extents[nr_rows][nr_cols];
        fern::MaskedArray<double, 2> argument(extents);

        std::iota(argument.data(), argument.data() + argument.num_elements(),
            0);
        argument.mask() = {
            {  true,  true,  true,  true,  true,  true },
            {  true,  true, false, false,  true,  true },
            {  true, false, false, false, false,  true },
            { false, false, false, false, false, false },
            {  true, false, false, false, false,  true },
            {  true,  true, false, false,  true,  true },
            {  true,  true, false,  true,  true,  true }
        };

        // Define kernel shape and weights.
        // Similar to PCRaster's window4total algorithm.
        fern::Square<bool, 1> kernel({
            { false,  true, false },
            {  true, false,  true },
            { false,  true, false }
        });

        // Sequential.
        {
            fern::MaskedArray<double, 2> result(extents);
            // fa::convolution::convolve(sequential, argument, kernel,
            //     result);
            OutputNoDataPolicy output_no_data_policy(result.mask(), true);

            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DontDivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::ReplaceNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{argument.mask(), true}},
                    output_no_data_policy,
                    sequential,
                    argument, kernel, result);

            compare_result6(result);
        }

        // Parallel.
        {
            fern::MaskedArray<double, 2> result(extents);
            OutputNoDataPolicy output_no_data_policy(result.mask(), true);

            fa::convolution::convolve<
                fa::convolve::SkipNoData,
                fa::convolve::DontDivideByWeights,
                fa::convolve::SkipOutOfImage,
                fa::convolve::ReplaceNoDataFocusElement,
                fa::unary::DiscardRangeErrors>(
                    InputNoDataPolicy{{argument.mask(), true}},
                    output_no_data_policy,
                    parallel,
                    argument, kernel, result);

            compare_result6(result);
        }
    }
}

template<
    typename T>
void compare_result_use_case1(
    fern::MaskedArray<T, 2> const& result)
{
    // +-----+-----+-----+
    // | 0.0 | 0.0 | 0.0 |
    // +-----+-----+-----+
    // | 0.0 | 0.0 | 0.0 |
    // +-----+-----+-----+
    // | 0.0 | 0.0 | 0.0 |
    // +-----+-----+-----+

    std::vector<bool> no_data = {
        false, false, false,
        false, false, false,
        false, false, false
    };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(no_data.begin(), no_data.end(),
        result.mask().data(), result.mask().data() +
            result.mask().num_elements());

    std::vector<T> values = {
        0.0, 0.0, 0.0,
        0.0, 0.0, 0.0,
        0.0, 0.0, 0.0
    };

    BOOST_REQUIRE_EQUAL_COLLECTIONS(values.begin(), values.end(),
        result.data(), result.data() + result.num_elements());
}

template<
    typename T>
void test_use_case1(
    T const& value)
{
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<fern::Mask<2>>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<fern::Mask<2>>;

    fa::ParallelExecutionPolicy parallel;
    fa::SequentialExecutionPolicy sequential;

    // +-------+-------+-------+
    // | value | value | value |
    // +-------+-------+-------+
    // | value | value | value |
    // +-------+-------+-------+
    // | value | value | value |
    // +-------+-------+-------+

    size_t const nr_rows = 3;
    size_t const nr_cols = 3;
    auto extents = fern::extents[nr_rows][nr_cols];
    fern::MaskedArray<T, 2> argument(extents);

    std::fill(argument.data(), argument.data() + argument.num_elements(),
        value);

    fern::Square<T, 1> kernel({
        {1, 0, -1},
        {2, 0, -2},
        {1, 0, -1}
    });

    // Sequential.
    {
        fern::MaskedArray<T, 2> result(extents);
        OutputNoDataPolicy output_no_data_policy(result.mask(), true);

        fa::convolution::convolve<
            fa::convolve::ReplaceNoDataByFocalAverage,
            fa::convolve::DontDivideByWeights,
            fa::convolve::ReplaceOutOfImageByFocalAverage,
            fa::convolve::KeepNoDataFocusElement,
            fa::unary::DiscardRangeErrors>(
                InputNoDataPolicy{{argument.mask(), true}},
                output_no_data_policy,
                sequential,
                argument, kernel, result);

        compare_result_use_case1(result);
    }

    // Parallel.
    {
        fern::MaskedArray<T, 2> result(extents);
        OutputNoDataPolicy output_no_data_policy(result.mask(), true);

        fa::convolution::convolve<
            fa::convolve::ReplaceNoDataByFocalAverage,
            fa::convolve::DontDivideByWeights,
            fa::convolve::ReplaceOutOfImageByFocalAverage,
            fa::convolve::KeepNoDataFocusElement,
            fa::unary::DiscardRangeErrors>(
                InputNoDataPolicy{{argument.mask(), true}},
                output_no_data_policy,
                parallel,
                argument, kernel, result);

        compare_result_use_case1(result);
    }
}

BOOST_AUTO_TEST_CASE(use_case1)
{
    for(auto const& value: {0.0, 0.055, 0.0925, 0.1, 0.1225, 0.17, 1.0}) {
        test_use_case1<float>(value);
    }
}


#include <SDL.h> // Needed because it messes with main()

#include <iostream>
#include <string>
#include <math.h>

#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include "asynch_serial/buffered_async_serial.h"

#include "sdl_joystick.h"

using namespace boost;
using boost::posix_time::ptime;
using boost::posix_time::time_duration;
//using boost::posix_time::microsec_clock::local_time;

//Analog joystick dead zone
const double JOYSTICK_DEAD_ZONE = 0.15;
#define PI 3.14159265

string mecanum_control() {
  char command[100];

  // Input 0: X -left +right
  // Input 1: Y -up +down
  // Input 2: R -left + right

  double x = (double)controller_state[0][0] / 32768;
  double y = (double)controller_state[0][1] / 32768;

  double r = (double)controller_state[0][2] / 32768;

  double theta;
  double magnitude;

  // Clamp input within the dead zone
  if(x > -JOYSTICK_DEAD_ZONE && x < JOYSTICK_DEAD_ZONE && y > -JOYSTICK_DEAD_ZONE && y < JOYSTICK_DEAD_ZONE) {
    x = 0.0;
    y = 0.0;
    theta = 0.0;
    magnitude = 0.0;
  } else {
    // Turn x and y into polar coordinate (angle, magnitude clamped to 1.0)
    theta = atan2(x, -y);
    if(theta < 0.0) theta += 2*PI;
    magnitude = sqrt(x*x + y*y);
    if(magnitude > 1.0) magnitude = 1.0;
  }

  if(r > -JOYSTICK_DEAD_ZONE && r < JOYSTICK_DEAD_ZONE) r = 0.0;

  // Original wheel positions
  /*
  // wheels: 1: RF 2: RR 3: LR 4: LF
  // forward  =  1  1  1  1  (theta = 0)
  // right    =  1 -1  1 -1  (theta = pi/2)
  // backward = -1 -1 -1 -1  (theta = pi)
  // left     = -1  1 -1  1  (theta = 3 pi/2)

  // Desired motor movement for translation commands at full speed. (Forward/backward, left/right)
  double m1, m2, m3, m4;

  if(theta >= 0.0 && theta < PI/2) {
    m1 = m3 =  1.0;
    m2 = m4 =  1.0 - 4*theta/PI;
  } else if(theta >= PI/2 && theta < PI) {
    m1 = m3 = 1.0 - 4*(theta - PI/2)/PI;
    m2 = m4 = -1.0;
  } else if(theta >= PI && theta < 3*PI/2) {
    m1 = m3 = -1.0;
    m2 = m4 = -1.0 + 4*(theta - PI)/PI;
  } else {
    m1 = m3 = -1.0 + 4*(theta - 3*PI/2)/PI;
    m2 = m4 = 1.0;
  }
  */

  // Swapped wheel positions
  // wheels: 1: RF 2: RR 3: LR 4: LF
  // forward  =  1  1  1  1  (theta = 0)
  // right    = -1  1 -1  1  (theta = pi/2)
  // backward = -1 -1 -1 -1  (theta = pi)
  // left     =  1 -1  1 -1  (theta = 3 pi/2)

  // Desired motor movement for translation commands at full speed. (Forward/backward, left/right)
  double m1, m2, m3, m4;

  if(theta >= 0.0 && theta < PI/2) {
    m1 = m3 =  1.0 - 4*theta/PI;
    m2 = m4 =  1.0;
  } else if(theta >= PI/2 && theta < PI) {
    m1 = m3 = -1.0;
    m2 = m4 = 1.0 - 4*(theta - PI/2)/PI;
  } else if(theta >= PI && theta < 3*PI/2) {
    m1 = m3 = -1.0 + 4*(theta - PI)/PI;
    m2 = m4 = -1.0;
  } else {
    m1 = m3 = 1.0;
    m2 = m4 = -1.0 + 4*(theta - 3*PI/2)/PI;
  }

  // Scale by the magnitude of the control vector
  m1 *= magnitude;
  m2 *= magnitude;
  m3 *= magnitude;
  m4 *= magnitude;

  // Desired motor movement for rotation commands. (Left/right)
  double r1, r2, r3, r4;
  double r_mag = abs(r);

  r1 = -r;
  r2 = -r;
  r3 = r;
  r4 = r;

  // Weighted average the translation and rotation desired movements
  // Avoid the possibility of inflated commands from small inputs
  if(magnitude + r_mag > JOYSTICK_DEAD_ZONE) {
    m1 = (m1 * magnitude + r1 * r_mag) / (magnitude + r_mag);
    m2 = (m2 * magnitude + r2 * r_mag) / (magnitude + r_mag);
    m3 = (m3 * magnitude + r3 * r_mag) / (magnitude + r_mag);
    m4 = (m4 * magnitude + r4 * r_mag) / (magnitude + r_mag);
  }

  cout << "                         " << x << " " << y << " " << theta << " "  << magnitude << " " << r << " " << r1 << " " << r2 << " " << r3 << " " << r4 << endl;
  sprintf(command, "M %d %d %d %d \r\n", (int)(m1 * 255), (int)(m2 * 255), (int)(m3 * 255), (int)(m4 * 255));
  //cout << command << endl;

  //return "M 0 0 0 0\r\n";
  return command;

}

int main(int argc, char* argv[]) {
  string port = "COM8";
  if(argc > 1) {
    port = argv[1];
  }
  cout << "Using port: " << port.c_str() << endl;
  try {

    BufferedAsyncSerial serial(port.c_str(),115200);
    this_thread::sleep(posix_time::seconds(2)); // Let the serial port finish initializing

    if(!init_stl()) {
      cout << "Failed to initialize STL." << endl;
    } else {
      ptime now(boost::posix_time::microsec_clock::local_time());
      ptime last_msg(boost::posix_time::microsec_clock::local_time());
      time_duration dt;

      string response;
      string last_command = "M 0 0 0 0\r\n"; // Record last command to avoid spamming the serial line with duplicates.

      serial.writeString(last_command.c_str());
      this_thread::sleep(posix_time::seconds(2));
      cout << "Initial command response: " << serial.readStringUntil("\r\n") << endl;

      bool done = false;
      bool event_happened = false;
      while(!done) {
        // Check for a message from the rover.
        response = serial.readStringUntil("\r\n");
        if(response.length() > 0) {
          cout << "Response: " << response << endl;
        }
        // Check for controller input.
        stl_event_wait(done, event_happened);
        if(!done && event_happened) {
          //cout << "event!" << endl;
          now = boost::posix_time::microsec_clock::local_time();
          dt = now - last_msg;
          if(dt.total_milliseconds() > 20) {
            string command = mecanum_control();
            if(command != last_command) {
              last_msg = now;
              serial.writeString(command.c_str());
              cout << command;
              last_command = command;
            }
          }
        }
      }
    }

    close_stl(); // Free resources and close SDL
    serial.close();
  } catch(boost::system::system_error& e) {
    cout << "Error: " << e.what() << endl;
    return -1;
  }

  return 0;
}

Mecanum Wheel Rover/PC Controller Software/mecanum_drive.cpp