#include "CommonTools/Utils/interface/DynArray.h"
#include "DataFormats/ParticleFlowReco/interface/PFRecHit.h"
#include "DataFormats/ParticleFlowReco/interface/PFRecHitFwd.h"
#include "FWCore/Framework/interface/ConsumesCollector.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "FWCore/Utilities/interface/isFinite.h"
#include "RecoParticleFlow/PFClusterProducer/interface/CaloRecHitResolutionProvider.h"
#include "RecoParticleFlow/PFClusterProducer/interface/PFCPositionCalculatorBase.h"

#include "vdt/vdtMath.h"

#include <boost/iterator/function_output_iterator.hpp>

#include <cmath>
#include <iterator>
#include <memory>
#include <tuple>

class Basic2DGenericPFlowPositionCalc : public PFCPositionCalculatorBase {
public:
  Basic2DGenericPFlowPositionCalc(const edm::ParameterSet& conf, edm::ConsumesCollector& cc)
      : PFCPositionCalculatorBase(conf, cc),
        _posCalcNCrystals(conf.getParameter<int>("posCalcNCrystals")),
        _minAllowedNorm(conf.getParameter<double>("minAllowedNormalization")) {
    std::vector<int> detectorEnum;
    std::vector<int> depths;
    std::vector<double> logWeightDenom;
    std::vector<float> logWeightDenomInv;

    if (conf.exists("logWeightDenominatorByDetector")) {
      const std::vector<edm::ParameterSet>& logWeightDenominatorByDetectorPSet =
          conf.getParameterSetVector("logWeightDenominatorByDetector");

      for (const auto& pset : logWeightDenominatorByDetectorPSet) {
        if (!pset.exists("detector")) {
          throw cms::Exception("logWeightDenominatorByDetectorPSet") << "logWeightDenominator : detector not specified";
        }

        const std::string& det = pset.getParameter<std::string>("detector");

        if (det == std::string("HCAL_BARREL1") || det == std::string("HCAL_ENDCAP")) {
          std::vector<int> depthsT = pset.getParameter<std::vector<int> >("depths");
          std::vector<double> logWeightDenomT = pset.getParameter<std::vector<double> >("logWeightDenominator");
          if (logWeightDenomT.size() != depthsT.size()) {
            throw cms::Exception("logWeightDenominator") << "logWeightDenominator mismatch with the numbers of depths";
          }
          for (unsigned int i = 0; i < depthsT.size(); ++i) {
            if (det == std::string("HCAL_BARREL1"))
              detectorEnum.push_back(1);
            if (det == std::string("HCAL_ENDCAP"))
              detectorEnum.push_back(2);
            depths.push_back(depthsT[i]);
            logWeightDenom.push_back(logWeightDenomT[i]);
          }
        }
      }
    } else {
      detectorEnum.push_back(0);
      depths.push_back(0);
      logWeightDenom.push_back(conf.getParameter<double>("logWeightDenominator"));
    }

    for (unsigned int i = 0; i < depths.size(); ++i) {
      logWeightDenomInv.push_back(1. / logWeightDenom[i]);
    }

    //    _logWeightDenom = std::make_pair(depths,logWeightDenomInv);
    _logWeightDenom = std::make_tuple(detectorEnum, depths, logWeightDenomInv);

    _timeResolutionCalcBarrel.reset(nullptr);
    if (conf.exists("timeResolutionCalcBarrel")) {
      const edm::ParameterSet& timeResConf = conf.getParameterSet("timeResolutionCalcBarrel");
      _timeResolutionCalcBarrel = std::make_unique<CaloRecHitResolutionProvider>(timeResConf);
    }
    _timeResolutionCalcEndcap.reset(nullptr);
    if (conf.exists("timeResolutionCalcEndcap")) {
      const edm::ParameterSet& timeResConf = conf.getParameterSet("timeResolutionCalcEndcap");
      _timeResolutionCalcEndcap = std::make_unique<CaloRecHitResolutionProvider>(timeResConf);
    }

    switch (_posCalcNCrystals) {
      case 5:
      case 9:
      case -1:
        break;
      default:
        edm::LogError("Basic2DGenericPFlowPositionCalc") << "posCalcNCrystals not valid";
        assert(0);  // bug
    }
  }

  Basic2DGenericPFlowPositionCalc(const Basic2DGenericPFlowPositionCalc&) = delete;
  Basic2DGenericPFlowPositionCalc& operator=(const Basic2DGenericPFlowPositionCalc&) = delete;

  void calculateAndSetPosition(reco::PFCluster&) override;
  void calculateAndSetPositions(reco::PFClusterCollection&) override;

private:
  const int _posCalcNCrystals;
  std::tuple<std::vector<int>, std::vector<int>, std::vector<float> > _logWeightDenom;
  const float _minAllowedNorm;

  std::unique_ptr<CaloRecHitResolutionProvider> _timeResolutionCalcBarrel;
  std::unique_ptr<CaloRecHitResolutionProvider> _timeResolutionCalcEndcap;

  void calculateAndSetPositionActual(reco::PFCluster&) const;
};

DEFINE_EDM_PLUGIN(PFCPositionCalculatorFactory, Basic2DGenericPFlowPositionCalc, "Basic2DGenericPFlowPositionCalc");

namespace {
  inline bool isBarrel(int cell_layer) {
    return (cell_layer == PFLayer::HCAL_BARREL1 || cell_layer == PFLayer::HCAL_BARREL2 ||
            cell_layer == PFLayer::ECAL_BARREL);
  }
}  // namespace

void Basic2DGenericPFlowPositionCalc::calculateAndSetPosition(reco::PFCluster& cluster) {
  calculateAndSetPositionActual(cluster);
}

void Basic2DGenericPFlowPositionCalc::calculateAndSetPositions(reco::PFClusterCollection& clusters) {
  for (reco::PFCluster& cluster : clusters) {
    calculateAndSetPositionActual(cluster);
  }
}

void Basic2DGenericPFlowPositionCalc::calculateAndSetPositionActual(reco::PFCluster& cluster) const {
  if (!cluster.seed()) {
    throw cms::Exception("ClusterWithNoSeed") << " Found a cluster with no seed: " << cluster;
  }
  double cl_energy = 0;
  double cl_time = 0;
  double cl_timeweight = 0.0;
  double max_e = 0.0;
  PFLayer::Layer max_e_layer = PFLayer::NONE;
  // find the seed and max layer and also calculate time
  //Michalis : Even if we dont use timing in clustering here we should fill
  //the time information for the cluster. This should use the timing resolution(1/E)
  //so the weight should be fraction*E^2
  //calculate a simplistic depth now. The log weighted will be done
  //in different stage

  auto const recHitCollection =
      &(*cluster.recHitFractions()[0].recHitRef()) - cluster.recHitFractions()[0].recHitRef().key();
  auto nhits = cluster.recHitFractions().size();
  struct LHit {
    reco::PFRecHit const* hit;
    float energy;
    float fraction;
  };
  declareDynArray(LHit, nhits, hits);
  for (auto i = 0U; i < nhits; ++i) {
    auto const& hf = cluster.recHitFractions()[i];
    auto k = hf.recHitRef().key();
    auto p = recHitCollection + k;
    hits[i] = {p, (*p).energy(), float(hf.fraction())};
  }

  bool resGiven = bool(_timeResolutionCalcBarrel) & bool(_timeResolutionCalcEndcap);
  LHit mySeed = {};
  for (auto const& rhf : hits) {
    const reco::PFRecHit& refhit = *rhf.hit;
    if (refhit.detId() == cluster.seed())
      mySeed = rhf;
    const auto rh_fraction = rhf.fraction;
    const auto rh_rawenergy = rhf.energy;
    const auto rh_energy = rh_rawenergy * rh_fraction;
#ifdef PF_DEBUG
    if UNLIKELY (edm::isNotFinite(rh_energy)) {
      throw cms::Exception("PFClusterAlgo") << "rechit " << refhit.detId() << " has a NaN energy... "
                                            << "The input of the particle flow clustering seems to be corrupted.";
    }
#endif
    cl_energy += rh_energy;
    // If time resolution is given, calculated weighted average
    if (resGiven) {
      double res2 = 1.e-4;
      int cell_layer = (int)refhit.layer();
      res2 = isBarrel(cell_layer) ? 1. / _timeResolutionCalcBarrel->timeResolution2(rh_rawenergy)
                                  : 1. / _timeResolutionCalcEndcap->timeResolution2(rh_rawenergy);
      cl_time += rh_fraction * refhit.time() * res2;
      cl_timeweight += rh_fraction * res2;
    } else {  // assume resolution = 1/E**2
      const double rh_rawenergy2 = rh_rawenergy * rh_rawenergy;
      cl_timeweight += rh_rawenergy2 * rh_fraction;
      cl_time += rh_rawenergy2 * rh_fraction * refhit.time();
    }

    if (rh_energy > max_e) {
      max_e = rh_energy;
      max_e_layer = refhit.layer();
    }
  }

  cluster.setEnergy(cl_energy);
  cluster.setTime(cl_time / cl_timeweight);
  if (resGiven) {
    cluster.setTimeError(std::sqrt(1.0f / float(cl_timeweight)));
  }
  cluster.setLayer(max_e_layer);

  // calculate the position
  double depth = 0.0;
  double position_norm = 0.0;
  double x(0.0), y(0.0), z(0.0);
  if (nullptr != mySeed.hit) {
    auto seedNeighbours = mySeed.hit->neighbours();
    switch (_posCalcNCrystals) {
      case 5:
        seedNeighbours = mySeed.hit->neighbours4();
        break;
      case 9:
        seedNeighbours = mySeed.hit->neighbours8();
        break;
      default:
        break;
    }

    auto compute = [&](LHit const& rhf) {
      const reco::PFRecHit& refhit = *rhf.hit;

      int cell_layer = (int)refhit.layer();
      float threshold = 0;

      for (unsigned int j = 0; j < (std::get<2>(_logWeightDenom)).size(); ++j) {
        // barrel is detecor type1
        int detectorEnum = std::get<0>(_logWeightDenom)[j];
        int depth = std::get<1>(_logWeightDenom)[j];

        if ((cell_layer == PFLayer::HCAL_BARREL1 && detectorEnum == 1 && refhit.depth() == depth) ||
            (cell_layer == PFLayer::HCAL_ENDCAP && detectorEnum == 2 && refhit.depth() == depth) || detectorEnum == 0)
          threshold = std::get<2>(_logWeightDenom)[j];
      }

      const auto rh_energy = rhf.energy * rhf.fraction;
      const auto norm =
          (rhf.fraction < _minFractionInCalc ? 0.0f : std::max(0.0f, vdt::fast_logf(rh_energy * threshold)));
      const auto rhpos_xyz = refhit.position() * norm;
      x += rhpos_xyz.x();
      y += rhpos_xyz.y();
      z += rhpos_xyz.z();
      depth += refhit.depth() * norm;
      position_norm += norm;
    };

    if (_posCalcNCrystals != -1)  // sorted to make neighbour search faster (maybe)
      std::sort(hits.begin(), hits.end(), [](LHit const& a, LHit const& b) { return a.hit < b.hit; });

    if (_posCalcNCrystals == -1)
      for (auto const& rhf : hits)
        compute(rhf);
    else {  // only seed and its neighbours
      compute(mySeed);
      // search seedNeighbours to find energy fraction in cluster (sic)
      unInitDynArray(reco::PFRecHit const*, seedNeighbours.size(), nei);
      for (auto k : seedNeighbours) {
        nei.push_back(recHitCollection + k);
      }
      std::sort(nei.begin(), nei.end());
      struct LHitLess {
        auto operator()(LHit const& a, reco::PFRecHit const* b) const { return a.hit < b; }
        auto operator()(reco::PFRecHit const* b, LHit const& a) const { return b < a.hit; }
      };
      std::set_intersection(
          hits.begin(), hits.end(), nei.begin(), nei.end(), boost::make_function_output_iterator(compute), LHitLess());
    }
  } else {
    throw cms::Exception("Basic2DGenerticPFlowPositionCalc")
        << "Cluster seed hit is null, something is wrong with PFlow RecHit!";
  }

  if (position_norm < _minAllowedNorm) {
    edm::LogError("WeirdClusterNormalization") << "PFCluster too far from seeding cell: set position to (0,0,0).";
    cluster.setPosition(math::XYZPoint(0, 0, 0));
    cluster.calculatePositionREP();
  } else {
    const double norm_inverse = 1.0 / position_norm;
    x *= norm_inverse;
    y *= norm_inverse;
    z *= norm_inverse;
    depth *= norm_inverse;
    cluster.setPosition(math::XYZPoint(x, y, z));
    cluster.setDepth(depth);
    cluster.calculatePositionREP();
  }
}


/* ============================================================================================== *
 *                                                                                *
 *                                     Galaxia Blockchain                                         *
 *                                                                                *
 * ---------------------------------------------------------------------------------------------- *
 * This file is part of the Xi framework.                                                         *
 * ---------------------------------------------------------------------------------------------- *
 *                                                                                *
 * Copyright 2018-2019 Xi Project Developers <support.xiproject.io>                               *
 *                                                                                *
 * This program is free software: you can redistribute it and/or modify it under the terms of the *
 * GNU General Public License as published by the Free Software Foundation, either version 3 of   *
 * the License, or (at your option) any later version.                                            *
 *                                                                                *
 * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;      *
 * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
 * See the GNU General Public License for more details.                                           *
 *                                                                                *
 * You should have received a copy of the GNU General Public License along with this program.     *
 * If not, see <https://www.gnu.org/licenses/>.                                                   *
 *                                                                                *
 * ============================================================================================== */

#include "Xi/Crypto/Hash/Crc.hpp"

#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable : 4701)
#endif

#include <boost/crc.hpp>

#if defined(_MSC_VER)
#pragma warning(pop)
#endif

#include <boost/endian/conversion.hpp>

#include <Xi/Exceptions.hpp>

XI_CRYPTO_HASH_DECLARE_HASH_IMPLEMENTATION(Xi::Crypto::Hash::Crc::Hash16, 16)
XI_CRYPTO_HASH_DECLARE_HASH_IMPLEMENTATION(Xi::Crypto::Hash::Crc::Hash32, 32)

void Xi::Crypto::Hash::Crc::compute(Xi::ConstByteSpan data, Hash16 &out) {
  static_assert(sizeof(boost::crc_16_type::value_type) == Hash16::bytes(), "invalid hash size for boost crc");
  boost::crc_16_type result{};
  result.process_bytes(data.data(), data.size());
  *reinterpret_cast<boost::crc_16_type::value_type *>(out.data()) = boost::endian::native_to_big(result.checksum());
}

void Xi::Crypto::Hash::Crc::compute(Xi::ConstByteSpan data, Hash32 &out) {
  static_assert(sizeof(boost::crc_32_type::value_type) == Hash32::bytes(), "invalid hash size for boost crc");
  boost::crc_32_type result{};
  result.process_bytes(data.data(), data.size());
  *reinterpret_cast<boost::crc_32_type::value_type *>(out.data()) = boost::endian::native_to_big(result.checksum());
}

Xi::Crypto::Hash::Crc::Hash16 Xi::Crypto::Hash::crc16(Xi::ConstByteSpan data) {
  Crc::Hash16 reval;
  compute(data, reval);
  return reval;
}

Xi::Crypto::Hash::Crc::Hash32 Xi::Crypto::Hash::crc32(Xi::ConstByteSpan data) {
  Crc::Hash32 reval;
  compute(data, reval);
  return reval;
}


#include "../agg.hpp"
#include <boost/accumulators/statistics/sum.hpp>

ARRAY_AGGREGATE_FNC(asum, tag::sum);
SQL_AGGREGATE_FNC(v_sum, tag::sum);


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// 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/.

#include <main.h>
#include <iostream>
#include <GL/glew.h>
#include <Eigen/OpenGLSupport>
#include <GL/glut.h>
using namespace Eigen;

#define VERIFY_MATRIX(CODE,REF) { \
    glLoadIdentity(); \
    CODE; \
    Matrix<float,4,4,ColMajor> m; m.setZero(); \
    glGet(GL_MODELVIEW_MATRIX, m); \
    if(!(REF).cast<float>().isApprox(m)) { \
      std::cerr << "Expected:\n" << ((REF).cast<float>()) << "\n" << "got\n" << m << "\n\n"; \
    } \
    VERIFY_IS_APPROX((REF).cast<float>(), m); \
  }

#define VERIFY_UNIFORM(SUFFIX,NAME,TYPE) { \
    TYPE value; value.setRandom(); \
    TYPE data; \
    int loc = glGetUniformLocation(prg_id, #NAME); \
    VERIFY((loc!=-1) && "uniform not found"); \
    glUniform(loc,value); \
    EIGEN_CAT(glGetUniform,SUFFIX)(prg_id,loc,data.data()); \
    if(!value.isApprox(data)) { \
      std::cerr << "Expected:\n" << value << "\n" << "got\n" << data << "\n\n"; \
    } \
    VERIFY_IS_APPROX(value, data); \
  }

#define VERIFY_UNIFORMi(NAME,TYPE) { \
    TYPE value = TYPE::Random().eval().cast<float>().cast<TYPE::Scalar>(); \
    TYPE data; \
    int loc = glGetUniformLocation(prg_id, #NAME); \
    VERIFY((loc!=-1) && "uniform not found"); \
    glUniform(loc,value); \
    glGetUniformiv(prg_id,loc,(GLint*)data.data()); \
    if(!value.isApprox(data)) { \
      std::cerr << "Expected:\n" << value << "\n" << "got\n" << data << "\n\n"; \
    } \
    VERIFY_IS_APPROX(value, data); \
  }

void printInfoLog(GLuint objectID)
{
    int infologLength, charsWritten;
    GLchar *infoLog;
    glGetProgramiv(objectID,GL_INFO_LOG_LENGTH, &infologLength);
    if(infologLength > 0)
    {
        infoLog = new GLchar[infologLength];
        glGetProgramInfoLog(objectID, infologLength, &charsWritten, infoLog);
        if (charsWritten>0)
          std::cerr << "Shader info : \n" << infoLog << std::endl;
        delete[] infoLog;
    }
}

GLint createShader(const char* vtx, const char* frg)
{
  GLint prg_id = glCreateProgram();
  GLint vtx_id = glCreateShader(GL_VERTEX_SHADER);
  GLint frg_id = glCreateShader(GL_FRAGMENT_SHADER);
  GLint ok;

  glShaderSource(vtx_id, 1, &vtx, 0);
  glCompileShader(vtx_id);
  glGetShaderiv(vtx_id,GL_COMPILE_STATUS,&ok);
  if(!ok)
  {
    std::cerr << "vtx compilation failed\n";
  }

  glShaderSource(frg_id, 1, &frg, 0);
  glCompileShader(frg_id);
  glGetShaderiv(frg_id,GL_COMPILE_STATUS,&ok);
  if(!ok)
  {
    std::cerr << "frg compilation failed\n";
  }

  glAttachShader(prg_id, vtx_id);
  glAttachShader(prg_id, frg_id);
  glLinkProgram(prg_id);
  glGetProgramiv(prg_id,GL_LINK_STATUS,&ok);
  if(!ok)
  {
    std::cerr << "linking failed\n";
  }
  printInfoLog(prg_id);

  glUseProgram(prg_id);
  return prg_id;
}

void test_openglsupport()
{
  int argc = 0;
  glutInit(&argc, 0);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowPosition (0,0);
  glutInitWindowSize(10, 10);

  if(glutCreateWindow("Eigen") <= 0)
  {
    std::cerr << "Error: Unable to create GLUT Window.\n";
    exit(1);
  }

  glewExperimental = GL_TRUE;
  if(glewInit() != GLEW_OK)
  {
    std::cerr << "Warning: Failed to initialize GLEW\n";
  }

  Vector3f v3f;
  Matrix3f rot;
  glBegin(GL_POINTS);

  glVertex(v3f);
  glVertex(2*v3f+v3f);
  glVertex(rot*v3f);

  glEnd();

  // 4x4 matrices
  Matrix4f mf44; mf44.setRandom();
  VERIFY_MATRIX(glLoadMatrix(mf44), mf44);
  VERIFY_MATRIX(glMultMatrix(mf44), mf44);
  Matrix4d md44; md44.setRandom();
  VERIFY_MATRIX(glLoadMatrix(md44), md44);
  VERIFY_MATRIX(glMultMatrix(md44), md44);

  // Quaternion
  Quaterniond qd(AngleAxisd(internal::random<double>(), Vector3d::Random()));
  VERIFY_MATRIX(glRotate(qd), Projective3d(qd).matrix());

  Quaternionf qf(AngleAxisf(internal::random<double>(), Vector3f::Random()));
  VERIFY_MATRIX(glRotate(qf), Projective3f(qf).matrix());

  // 3D Transform
  Transform<float,3,AffineCompact> acf3; acf3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(acf3), Projective3f(acf3).matrix());
  VERIFY_MATRIX(glMultMatrix(acf3), Projective3f(acf3).matrix());

  Transform<float,3,Affine> af3(acf3);
  VERIFY_MATRIX(glLoadMatrix(af3), Projective3f(af3).matrix());
  VERIFY_MATRIX(glMultMatrix(af3), Projective3f(af3).matrix());

  Transform<float,3,Projective> pf3; pf3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(pf3), Projective3f(pf3).matrix());
  VERIFY_MATRIX(glMultMatrix(pf3), Projective3f(pf3).matrix());

  Transform<double,3,AffineCompact> acd3; acd3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(acd3), Projective3d(acd3).matrix());
  VERIFY_MATRIX(glMultMatrix(acd3), Projective3d(acd3).matrix());

  Transform<double,3,Affine> ad3(acd3);
  VERIFY_MATRIX(glLoadMatrix(ad3), Projective3d(ad3).matrix());
  VERIFY_MATRIX(glMultMatrix(ad3), Projective3d(ad3).matrix());

  Transform<double,3,Projective> pd3; pd3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(pd3), Projective3d(pd3).matrix());
  VERIFY_MATRIX(glMultMatrix(pd3), Projective3d(pd3).matrix());

  // translations (2D and 3D)
  {
    Vector2f vf2; vf2.setRandom(); Vector3f vf23; vf23 << vf2, 0;
    VERIFY_MATRIX(glTranslate(vf2), Projective3f(Translation3f(vf23)).matrix());
    Vector2d vd2; vd2.setRandom(); Vector3d vd23; vd23 << vd2, 0;
    VERIFY_MATRIX(glTranslate(vd2), Projective3d(Translation3d(vd23)).matrix());

    Vector3f vf3; vf3.setRandom();
    VERIFY_MATRIX(glTranslate(vf3), Projective3f(Translation3f(vf3)).matrix());
    Vector3d vd3; vd3.setRandom();
    VERIFY_MATRIX(glTranslate(vd3), Projective3d(Translation3d(vd3)).matrix());

    Translation<float,3> tf3; tf3.vector().setRandom();
    VERIFY_MATRIX(glTranslate(tf3), Projective3f(tf3).matrix());

    Translation<double,3> td3;  td3.vector().setRandom();
    VERIFY_MATRIX(glTranslate(td3), Projective3d(td3).matrix());
  }

  // scaling (2D and 3D)
  {
    Vector2f vf2; vf2.setRandom(); Vector3f vf23; vf23 << vf2, 1;
    VERIFY_MATRIX(glScale(vf2), Projective3f(Scaling(vf23)).matrix());
    Vector2d vd2; vd2.setRandom(); Vector3d vd23; vd23 << vd2, 1;
    VERIFY_MATRIX(glScale(vd2), Projective3d(Scaling(vd23)).matrix());

    Vector3f vf3; vf3.setRandom();
    VERIFY_MATRIX(glScale(vf3), Projective3f(Scaling(vf3)).matrix());
    Vector3d vd3; vd3.setRandom();
    VERIFY_MATRIX(glScale(vd3), Projective3d(Scaling(vd3)).matrix());

    UniformScaling<float> usf(internal::random<float>());
    VERIFY_MATRIX(glScale(usf), Projective3f(usf).matrix());

    UniformScaling<double> usd(internal::random<double>());
    VERIFY_MATRIX(glScale(usd), Projective3d(usd).matrix());
  }

  // uniform
  {
    const char* vtx = "void main(void) { gl_Position = gl_Vertex; }\n";

    if(GLEW_VERSION_2_0)
    {
      #ifdef GL_VERSION_2_0
      const char* frg = ""
        "uniform vec2 v2f;\n"
        "uniform vec3 v3f;\n"
        "uniform vec4 v4f;\n"
        "uniform ivec2 v2i;\n"
        "uniform ivec3 v3i;\n"
        "uniform ivec4 v4i;\n"
        "uniform mat2 m2f;\n"
        "uniform mat3 m3f;\n"
        "uniform mat4 m4f;\n"
        "void main(void) { gl_FragColor = vec4(v2f[0]+v3f[0]+v4f[0])+vec4(v2i[0]+v3i[0]+v4i[0])+vec4(m2f[0][0]+m3f[0][0]+m4f[0][0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      VERIFY_UNIFORM(fv,v2f, Vector2f);
      VERIFY_UNIFORM(fv,v3f, Vector3f);
      VERIFY_UNIFORM(fv,v4f, Vector4f);
      VERIFY_UNIFORMi(v2i, Vector2i);
      VERIFY_UNIFORMi(v3i, Vector3i);
      VERIFY_UNIFORMi(v4i, Vector4i);
      VERIFY_UNIFORM(fv,m2f, Matrix2f);
      VERIFY_UNIFORM(fv,m3f, Matrix3f);
      VERIFY_UNIFORM(fv,m4f, Matrix4f);
      #endif
    }
    else
      std::cerr << "Warning: opengl 2.0 was not tested\n";

    if(GLEW_VERSION_2_1)
    {
      #ifdef GL_VERSION_2_1
      const char* frg = "#version 120\n"
        "uniform mat2x3 m23f;\n"
        "uniform mat3x2 m32f;\n"
        "uniform mat2x4 m24f;\n"
        "uniform mat4x2 m42f;\n"
        "uniform mat3x4 m34f;\n"
        "uniform mat4x3 m43f;\n"
        "void main(void) { gl_FragColor = vec4(m23f[0][0]+m32f[0][0]+m24f[0][0]+m42f[0][0]+m34f[0][0]+m43f[0][0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      typedef Matrix<float,2,3> Matrix23f;
      typedef Matrix<float,3,2> Matrix32f;
      typedef Matrix<float,2,4> Matrix24f;
      typedef Matrix<float,4,2> Matrix42f;
      typedef Matrix<float,3,4> Matrix34f;
      typedef Matrix<float,4,3> Matrix43f;

      VERIFY_UNIFORM(fv,m23f, Matrix23f);
      VERIFY_UNIFORM(fv,m32f, Matrix32f);
      VERIFY_UNIFORM(fv,m24f, Matrix24f);
      VERIFY_UNIFORM(fv,m42f, Matrix42f);
      VERIFY_UNIFORM(fv,m34f, Matrix34f);
      VERIFY_UNIFORM(fv,m43f, Matrix43f);
      #endif
    }
    else
      std::cerr << "Warning: opengl 2.1 was not tested\n";

    if(GLEW_VERSION_3_0)
    {
      #ifdef GL_VERSION_3_0
      const char* frg = "#version 150\n"
        "uniform uvec2 v2ui;\n"
        "uniform uvec3 v3ui;\n"
        "uniform uvec4 v4ui;\n"
        "out vec4 data;\n"
        "void main(void) { data = vec4(v2ui[0]+v3ui[0]+v4ui[0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      typedef Matrix<unsigned int,2,1> Vector2ui;
      typedef Matrix<unsigned int,3,1> Vector3ui;
      typedef Matrix<unsigned int,4,1> Vector4ui;

      VERIFY_UNIFORMi(v2ui, Vector2ui);
      VERIFY_UNIFORMi(v3ui, Vector3ui);
      VERIFY_UNIFORMi(v4ui, Vector4ui);
      #endif
    }
    else
      std::cerr << "Warning: opengl 3.0 was not tested\n";

    #ifdef GLEW_ARB_gpu_shader_fp64
    if(GLEW_ARB_gpu_shader_fp64)
    {
      #ifdef GL_ARB_gpu_shader_fp64
      const char* frg = "#version 150\n"
        "uniform dvec2 v2d;\n"
        "uniform dvec3 v3d;\n"
        "uniform dvec4 v4d;\n"
        "out vec4 data;\n"
        "void main(void) { data = vec4(v2d[0]+v3d[0]+v4d[0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      typedef Vector2d Vector2d;
      typedef Vector3d Vector3d;
      typedef Vector4d Vector4d;

      VERIFY_UNIFORM(dv,v2d, Vector2d);
      VERIFY_UNIFORM(dv,v3d, Vector3d);
      VERIFY_UNIFORM(dv,v4d, Vector4d);
      #endif
    }
    else
      std::cerr << "Warning: GLEW_ARB_gpu_shader_fp64 was not tested\n";
    #else
      std::cerr << "Warning: GLEW_ARB_gpu_shader_fp64 was not tested\n";
    #endif
  }

}


#include "packet_protocol.h"
#include <boost/crc.hpp> // for boost::crc_basic, boost::crc_optimal

static uint32_t crc32(const void *data, size_t size) {
    boost::crc_32_type crc32_obj;
    crc32_obj.process_bytes(data, size);
    return crc32_obj();
}

int serialize_packet(OutputStream &out, FIELD *fields, int len) {
    char *body_base = 0;
    int n = 0;

    for (FIELD *p = fields; p->type >= 0; ++p, ++n) {
        /*Caculate CRC for header crc field*/
        if (n == FIELD_HCRC) {
            unsigned int length = out.write_ptr() - out.base_ptr();
            if (length > 0) {
                p->ivalue = crc32(out.base_ptr(), length);
            }
        }
            /*Caculate CRC for body crc field*/
        else if (len - 1 == n) {
            if (0 != body_base) {
                int length = out.write_ptr() - body_base;
                if (length > 0)
                    p->ivalue = crc32(body_base, length);
            }
        }

        /**************caculate binary stream************************/
        if (0 == p->data) {
            bool ret = false;
            switch (p->type) {
                case TYPE_CHAR:
                case TYPE_BYTE:
                    ret = out.write_char((Char) p->ivalue);
                    break;
                case TYPE_SHORT:
                    ret = out.write_short((Short) p->ivalue);
                    break;
                case TYPE_LONG:
                    ret = out.write_long((Long) p->ivalue);
                    break;
                case TYPE_LONGLONG:
                    ret = out.write_llong((LongLong) p->ivalue);
                    break;
                case TYPE_FLOAT:
                    ret = out.write_float((Float) p->fvalue);
                    break;
                case TYPE_DOUBLE:
                    ret = out.write_double((Double) p->fvalue);
                    break;
                default:
                    break;
            }
        } else /*p->data !=0*/
        {
            if (p->count > 0)/*p->data !=0 && p->count >0*/
            {
                int count = p->count * fields[FIELD_COUNT].ivalue;
                switch (p->type) {
                    case TYPE_CHAR:
                    case TYPE_BYTE:
                        out.write_char_array((Char *) p->data, count);
                        break;
                    case TYPE_SHORT:
                        out.write_short_array((Short *) p->data, count);
                        break;
                    case TYPE_LONG:
                        out.write_long_array((Long *) p->data, count);
                        break;
                    case TYPE_LONGLONG:
                        out.write_llong_array((LongLong *) p->data, count);
                        break;
                    case TYPE_FLOAT:
                        out.write_float_array((Float *) p->data, count);
                        break;
                    case TYPE_DOUBLE:
                        out.write_double_array((Double *) p->data, count);
                        break;
                    default:
                        break;
                }
            } else /*p->data !=0 && p->count==0*/
            {
                FIELD *prev = p - 1;
                if (prev->type != TYPE_SHORT && prev->type != TYPE_LONG)
                    return ERR_ENCODE_ERROE;

                int count = 0;
                switch (p->count) {
                    case -1: {
                        count = prev->ivalue;
                        switch (p->type) {
                            case TYPE_CHAR:
                            case TYPE_BYTE:
                                out.write_char_array((Char *) p->data, count);
                                break;
                            case TYPE_SHORT:
                                out.write_short_array((Short *) p->data, count);
                                break;
                            case TYPE_LONG:
                                out.write_long_array((Long *) p->data, count);
                                break;
                            case TYPE_LONGLONG:
                                out.write_llong_array((LongLong *) p->data, count);
                                break;
                            case TYPE_FLOAT:
                                out.write_float_array((Float *) p->data, count);
                                break;
                            case TYPE_DOUBLE:
                                out.write_double_array((Double *) p->data, count);
                                break;
                            default:
                                break;
                        }
                        break;
                    }
                    case -2://reserved!
                        break;
                    case -3://reserved!
                        break;
                    case -4://reserved!
                        break;
                    default:
                        break;
                }
            }
        }

        if (n == FIELD_BMARK - 1)
            body_base = out.write_ptr();

        if (!out.good())
            return ERR_ENCODE_ERROE;
    }

    return ERR_OK;
}

int unserialize_header_packet(InputStream &in, FIELD *fields, bool compare_crc) {
    Char c_val = 0;
    Short s_val = 0;
    Long l_val = 0;
    LongLong ll_val = 0;
    Float f_val = 0.;
    Double d_val = 0;
    int n = 0;
    unsigned int crc = 0;
    char *base = in.base_ptr();

    for (FIELD *p = fields; p->type >= 0; ++p, ++n) {
        if (n == FIELD_HCRC) {
            int length = in.read_ptr() - base;
            if (length > 0 && compare_crc)
                crc = crc32(base, length);
        }

        if (p->data == 0) {
            switch (p->type) {
                case TYPE_CHAR:
                case TYPE_BYTE:
                    in.read_char(c_val);
                    p->ivalue = c_val;
                    break;
                case TYPE_SHORT:
                    in.read_short(s_val);
                    p->ivalue = s_val;
                    break;
                case TYPE_LONG:
                    in.read_long(l_val);
                    p->ivalue = l_val;
                    break;
                case TYPE_LONGLONG:
                    in.read_llong(ll_val);
                    p->ivalue = ll_val;
                    break;
                case TYPE_FLOAT:
                    in.read_float(f_val);
                    p->fvalue = f_val;
                    break;
                case TYPE_DOUBLE:
                    in.read_double(d_val);
                    p->fvalue = d_val;
                    break;
                default:
                    break;
            }
        }

        if (n == FIELD_HMARK) {
            if ((unsigned int) p->ivalue != HEAD_MARK)
                return ERR_DECODE_ERROE;
        } else if (n == FIELD_HCRC) {
            if (compare_crc && crc != (unsigned int) p->ivalue)
                return ERR_CHECK_CRC_ERROR;

            break;
        }

        if (!in.good())
            return ERR_DECODE_ERROE;
    }

    return ERR_OK;
}

int unserialize_body_packet(InputStream &in, FIELD *fields, int len, bool compare_crc) {
    Char c_val = 0;
    Short s_val = 0;
    Long l_val = 0;
    LongLong ll_val = 0;
    Float f_val = 0.;
    Double d_val = 0;

    int n = FIELD_BMARK;
    unsigned int crc = 0;
    int cnt = fields[FIELD_COUNT].ivalue;
    char *base = in.read_ptr();

    for (FIELD *p = &fields[n]; p->type >= 0; ++p, ++n) {
        if (len - 1 == n && compare_crc) {
            int length = in.read_ptr() - base;
            if (length > 0)
                crc = crc32(base, length);
        }

        if (p->data == 0) {
            bool ret = false;
            switch (p->type) {
                case TYPE_CHAR:
                case TYPE_BYTE:
                    ret = in.read_char(c_val);
                    p->ivalue = c_val;
                    break;
                case TYPE_SHORT:
                    ret = in.read_short(s_val);
                    p->ivalue = s_val;
                    break;
                case TYPE_LONG:
                    ret = in.read_long(l_val);
                    p->ivalue = l_val;
                    break;
                case TYPE_LONGLONG:
                    ret = in.read_llong(ll_val);
                    p->ivalue = ll_val;
                    break;
                case TYPE_FLOAT:
                    ret = in.read_float(f_val);
                    p->fvalue = f_val;
                    break;
                case TYPE_DOUBLE:
                    ret = in.read_double(d_val);
                    p->fvalue = d_val;
                    break;
                default:
                    break;
            }

        } else // p->data != 0
        {
            int count = 0;
            if (p->count >= 0) // p->data != 0  && p->count >= 0
            {
                count = cnt * p->count;
                switch (p->type) {
                    case TYPE_CHAR:
                    case TYPE_BYTE:
                        in.read_char_array((Char *) p->data, count);
                        break;
                    case TYPE_SHORT:
                        in.read_short_array((Short *) p->data, count);
                        break;
                    case TYPE_LONG:
                        in.read_long_array((Long *) p->data, count);
                        break;
                    case TYPE_LONGLONG:
                        in.read_llong_array((LongLong *) p->data, count);
                        break;
                    case TYPE_FLOAT:
                        in.read_float_array((Float *) p->data, count);
                        break;
                    case TYPE_DOUBLE:
                        in.read_double_array((Double *) p->data, count);
                        break;
                    default:
                        break;
                }
            } else// p->data != 0  && p->count < 0
            {
                FIELD *prev = p - 1;
                if (prev->type != TYPE_SHORT && prev->type != TYPE_LONG)
                    return ERR_DECODE_ERROE;

                switch (p->count) {
                    case -1: {
                        if (prev->data != 0)
                            return ERR_DECODE_ERROE;

                        count = prev->ivalue;
                        if (count > 0) {
                            switch (p->type) {
                                case TYPE_CHAR:
                                case TYPE_BYTE:
                                    in.read_char_array((Char *) p->data, count);
                                    break;
                                case TYPE_SHORT:
                                    in.read_short_array((Short *) p->data, count);
                                    break;
                                case TYPE_LONG:
                                    in.read_long_array((Long *) p->data, count);
                                    break;
                                case TYPE_LONGLONG:
                                    in.read_llong_array((LongLong *) p->data, count);
                                    break;
                                case TYPE_FLOAT:
                                    in.read_float_array((Float *) p->data, count);
                                    break;
                                case TYPE_DOUBLE:
                                    in.read_double_array((Double *) p->data, count);
                                    break;
                                default:
                                    break;
                            }
                        }
                    }
                    case -2://reserved!
                        break;
                    case -3://reserved!
                        break;
                    case -4://reserved!
                        break;
                    default:
                        break;
                }
            }
        }

        if (n == len - 1) {
            if (compare_crc && (unsigned int) p->ivalue != crc)
                return ERR_CHECK_CRC_ERROR;
        }
    }

    return ERR_OK;

}

int copy_fields(FIELD *dest, FIELD *src, bool only_header/*=false*/) {
    int n = 0;
    for (FIELD *p = src; p->type > 0; ++p, ++n) {
        dest[n] = *p;
        if (only_header && n == FIELD_BMARK - 1)
            break;
    }

    if (!only_header)
        dest[n].type = -1;

    return n;
}

void fields_size(FIELD *fields, int *total, int *header, int *body, int *body_count) {
    if (total) *total = 0;
    if (header) *header = 0;
    if (body) *body = 0;

    int n = 0;
    int cnt = fields[FIELD_COUNT].ivalue;

    for (FIELD *p = fields; p->type >= 0; ++p, ++n) {
        int size = 0;
        switch (p->type) {
            case TYPE_CHAR:
            case TYPE_BYTE:
                size = 1;
                break;
            case TYPE_SHORT:
                size = 2;
                break;
            case TYPE_LONG:
                size = 4;
                break;
            case TYPE_LONGLONG:
                size = 8;
                break;
            case TYPE_FLOAT:
                size = 4;
                break;
            case TYPE_DOUBLE:
                size = 8;
                break;
            default:
                break;
        }

        if (p->data != 0) {
            if (p->count >= 0) {
                size *= p->count * cnt;
            } else {
                //reserved!
                switch (p->count) {
                    case -1: {
                        FIELD *prev = p - 1;
                        if (prev->data == 0)
                            size *= prev->ivalue;
                        break;
                    }
                    case -2:
                        break;
                    case -3:
                        break;
                    case -4:
                        break;
                    default:
                        break;
                }
            }
        }

        if (n == FIELD_NEXT) {
            if (body_count)
                *body_count = n;
        }

        if (total) {
            *total += size;
        }

        if (n <= FIELD_HCRC) {
            if (header)
                *header += size;
        } else {
            if (body)
                *body += size;
        }
    }

    if (body_count)
        *body_count = n - *body_count + 1;
}


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef OFFSPRING_GENERATOR_HPP_
#define OFFSPRING_GENERATOR_HPP_

#include <boost/property_tree/ptree.hpp>

#include "clotho/cuda/curand_state_pool.hpp"

#include "clotho/cuda/crossover/select_and_crossover_unordered_impl.hpp"

#include "clotho/cuda/distributions/poisson_distribution.hpp"
#include "clotho/recombination/recombination_rate_parameter.hpp"

template < class PopulationType >
class OffspringGenerator {
public:

    typedef PopulationType                                  population_type;
    typedef typename population_type::real_type             real_type;
    typedef typename population_type::int_type              int_type;
    typedef typename population_type::order_tag_type        order_tag_type;

    typedef typename population_type::allele_space_type::device_space_type     allele_space_type;

    typedef basic_data_space< int_type >                    selection_type;

    typedef clotho::cuda::curand_state_pool                 state_pool_type;

    typedef recombination_rate_parameter< real_type > recombination_rate_type;
    typedef poisson_cdf< real_type, 32 >                    poisson_type;

    OffspringGenerator( boost::property_tree::ptree & config ) :
        m_recomb_rate( config )
    {
        parse_configuration( config );
        initialize();
    }

    void operator()( population_type * parent, selection_type * sel, population_type * offspring ) {

        unsigned int bcount = state_pool_type::getInstance()->get_max_blocks();
        unsigned int tcount = state_pool_type::getInstance()->get_max_threads();

        select_and_crossover_kernel<<< bcount, tcount >>>( state_pool_type::getInstance()->get_device_states()
                                                           , parent->sequences.get_device_space()
                                                           , sel
                                                           , parent->alleles.get_device_space()
                                                           , offspring->free_space
                                                           , dPoisCDF
                                                           , offspring->sequences.get_device_space());
    }

    virtual ~OffspringGenerator() {
        cudaFree( dPoisCDF );
    }

protected:

    void parse_configuration( boost::property_tree::ptree & config ) {
        state_pool_type::getInstance()->initialize( config );
    }

    void initialize() {
        assert( cudaMalloc( (void **) &dPoisCDF, sizeof( poisson_type) ) == cudaSuccess );

        initialize_poisson( m_recomb_rate.m_rho, (order_tag_type *) NULL );
    }

    void initialize_poisson( real_type mean, unordered_tag * p ) {
        real_type lambda = (mean / (real_type) allele_space_type::ALIGNMENT_SIZE);

        make_poisson_cdf_maxk32<<< 1, 32 >>>( dPoisCDF, lambda );
    }

    void initialize_poisson( real_type mean, unit_ordered_tag< int_type > * p ) {
        real_type lambda = (mean / 32.0);

        make_poisson_cdf_maxk32<<< 1, 32 >>>( dPoisCDF, lambda );
    }

    poisson_type            * dPoisCDF;
    recombination_rate_type m_recomb_rate;
};

#endif  // OFFSPRING_GENERATOR_HPP_


//! ベクトルクラス基底ヘッダ
/*! 2-4次元のベクトルをアラインメント有りと無しでそれぞれ定義
	初期化や要素毎のアクセスなど基本的なメソッド */
#if !BOOST_PP_IS_ITERATING
	#if !defined(VECTOR_H_) || INCLUDE_VECTOR_LEVEL >= 1
		#define VECTOR_H_
		#include <boost/preprocessor.hpp>
		#include <boost/operators.hpp>
		#include <boost/serialization/access.hpp>
		#include <boost/serialization/level.hpp>
		#include <boost/serialization/nvp.hpp>
		#include <cmath>
		#include "spn_math.hpp"
		#include "tostring.hpp"
		#include "random/vector.hpp"

		#define DEF_ARGPAIR(index,data,elem)	(float BOOST_PP_CAT(f, elem))
		#define ENUM_ARGPAIR(subseq) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_FOR_EACH(DEF_ARGPAIR, NOTHING, subseq))
		#define DEF_ARGSET(index,data,elem)		elem = BOOST_PP_CAT(f, elem);
		#define SEQ_VECELEM (x)(y)(z)(w)

		// 要求された定義レベルを実体化
		#ifndef INCLUDE_VECTOR_LEVEL
			#define INCLUDE_VECTOR_LEVEL 0
		#endif
		#define BOOST_PP_ITERATION_PARAMS_1 (4, (2,4, "vector.hpp", INCLUDE_VECTOR_LEVEL))
		#include BOOST_PP_ITERATE()
		#undef INCLUDE_VECTOR_LEVEL
	#endif
#elif BOOST_PP_ITERATION_DEPTH() == 1
	// アラインメントイテレーション
	#define DIM		BOOST_PP_FRAME_ITERATION(1)
	#define BOOST_PP_ITERATION_PARAMS_2 (4, (0,1, "vector.hpp", BOOST_PP_FRAME_FLAGS(1)))
	#include BOOST_PP_ITERATE()
	#undef DIM
#else
	#define ALIGN	BOOST_PP_ITERATION()
	#define ALIGNB	BOOLNIZE(ALIGN)
	#define Vec		BOOST_PP_IF(ALIGN, alignas(16), NOTHING) VecT<DIM, ALIGNB>
	#define VT		VecT<DIM, ALIGNB>
	#include "local_macro.hpp"
	#if BOOST_PP_ITERATION_FLAGS() == 0
		// クラスのコンストラクタとメソッドのプロトタイプだけ定義
		namespace spn {
			class MTRandom;
			template <>
			struct Vec {
				constexpr static int width = DIM;
				constexpr static bool align = ALIGNB;
				using AVec = VecT<DIM,true>;
				using UVec = VecT<DIM,false>;
				union {
					struct {
						float BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM));
					};
					float m[DIM];
				};

				friend class boost::serialization::access;
				template <class Archive>
				void serialize(Archive& ar, const unsigned int /*ver*/) {
					ar & BOOST_SERIALIZATION_NVP(m);
				}

				// -------------------- ctor --------------------
				VecT() = default;
				explicit VecT(reg128 r);
				explicit VecT(float a);
				explicit VecT(ENUM_ARGPAIR(BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM)));
				template <class V, class=std::enable_if_t<std::is_floating_point<V>::value>>
				explicit VecT(const V* src);
				VecT(std::initializer_list<float> il);
				reg128 loadPS() const;
				reg128 loadPSZ() const;

				//! アラインメント済ベクトルで初期化
				VecT(const AVec& v);
				//! アラインメント無しベクトルで初期化
				VecT(const UVec& v);
				VecT& operator = (const AVec& v);
				VecT& operator = (const UVec& v);
				VecT& operator = (reg128 r);
				VecT& operator = (std::initializer_list<float> il);
				// -------------------- operators --------------------
				// ベクトルとの積算や除算は同じ要素同士の演算とする
				#define DEF_PRE(op, func)	VecT& operator BOOST_PP_CAT(op,=) (float s); \
					VecT operator op (float s) const; \
					VecT& operator BOOST_PP_CAT(op,=) (const AVec& v); \
					VecT& operator BOOST_PP_CAT(op,=) (const UVec& v); \
					AVec operator op (const AVec& v) const; \
					UVec operator op (const UVec& v) const; \
					AVec&& operator op (AVec&& v) const; \
					UVec&& operator op (UVec&& v) const; \
					friend VecT operator op (float fv, const VecT& v);
				DEF_PRE(+, reg_add_ps)
				DEF_PRE(-, reg_sub_ps)
				DEF_PRE(*, reg_mul_ps)
				DEF_PRE(/, _mmDivPs)

				// -------- Luaへのエクスポート用 --------
				VecT addV(const UVec& v) const;
				VecT subV(const UVec& v) const;
				VecT mulF(float s) const;
				VecT mulM(const MatT<DIM,DIM,align>& m) const;
				VecT divF(float s) const;
				VecT invert() const;
				bool equal(const UVec& v) const;
				std::string toString() const;

				friend std::ostream& operator << (std::ostream& os, const VT& v);
				// -------------------- others --------------------
				static float _sumup(reg128 xm);
				// ロード関数呼び出しのコストが許容出来るケースではloadPS()を呼び、そうでないケースはオーバーロードで対処
				template <bool A>
				float dot(const VecT<DIM,A>& v) const;
				//! 各要素を足しあわせた合計
				float sum() const;
				//! 各要素の算術平均
				float average() const;
				template <bool A>
				float distance(const VecT<DIM,A>& v) const;
				template <bool A>
				float dist_sq(const VecT<DIM,A>& v) const;

				//! 要素ごとに最小値選択
				template <bool A>
				VecT getMin(const VecT<DIM,A>& v) const;
				template <bool A>
				void selectMin(const VecT<DIM,A>& v);

				//! 要素ごとに最大値選択
				template <bool A>
				VecT getMax(const VecT<DIM,A>& v) const;
				template <bool A>
				void selectMax(const VecT<DIM,A>& v);

				VecT operator - () const;

				/*! \return 要素が全て等しい時にtrue, それ以外はfalse */
				template <bool A>
				bool operator == (const VecT<DIM,A>& v) const;
				template <bool A>
				bool operator != (const VecT<DIM,A>& v) const;

				float normalize();
				VecT normalization() const;
				/*! \return ベクトルの長さ */
				float length() const;
				/*! \return ベクトル長の2乗 */
				float len_sq() const;
				//! 要素の何れかがNaNになっているか
				bool isNaN() const;
				//! 要素の何れかがNaN又はinfになっているか
				bool isOutstanding() const;

				void saturate(float fMin, float fMax);
				VecT saturation(float fMin, float fMax) const;
				void lerp(const UVec& v, float r);
				template <bool A>
				VecT l_intp(const VecT<DIM,A>& v, float r) const;

				//! 要素の値がランダムなベクトル
				template <class RDF>
				static VecT Random(const RDF& rdf, const RangeF& r=random::DefaultRVecRange) {
					return random::GenRVec<VecT>(rdf, r);
				}
				template <class RDF>
				static VecT RandomF(const RDF& rdf) {
					return random::GenRVecF<VecT>(rdf);
				}
				//! ランダムなベクトル（但し長さが指定値より大きい）
				template <class RDF>
				static VecT RandomWithLength(const RDF& rdf, float th, const RangeF& r=random::DefaultRVecRange) {
					return random::GenRVecLen<VecT>(rdf, th, r);
				}
				//! ランダムなベクトル (但し全ての成分の絶対値がそれぞれ基準範囲内)
				template <class RDF>
				static VecT RandomWithAbs(const RDF& rdf, const RangeF& rTh, const RangeF& r=random::DefaultRVecRange) {
					return random::GenRVecAbs<VecT>(rdf, rTh, r);
				}
				//! ランダムな方向ベクトル
				template <class RDF>
				static VecT RandomDir(const RDF& rdf) {
					return random::GenRDir<VecT>(rdf);
				}
				// -------- Luaへのエクスポート用 --------
				static VecT luaRandom(MTRandom& mt, const RangeF& r);
				static VecT luaRandomWithLength(MTRandom& mt, float th, const RangeF& r);
				static VecT luaRandomWithAbs(MTRandom& mt, const RangeF& rTh, const RangeF& r);
				#if ALIGN==1
					//! AVec -> Vec へ暗黙変換
					operator VecT<DIM,false>& ();
					operator const VecT<DIM,false>& () const;
				#endif
				#if DIM==4
					using Vec3 = VecT<3,ALIGNB>;
					/*! \return x,y,z成分 */
					const Vec3& asVec3() const;
					/*! \return x,y,zそれぞれをwで割った値 */
					Vec3 asVec3Coord() const;
				#elif DIM==3
					//! ベクトルに垂直なベクトルを適当に定める
					VecT verticalVector() const;
					/*! \return this X v の外積ベクトル */
					template <bool A>
					VecT cross(const VecT<DIM,A>& v) const;
					//! 外積計算cross()と同義
					template <bool A>
					void operator %= (const VecT<DIM,A>& v);
					//! 外積計算cross()と同義
					template <bool A>
					VecT operator % (const VecT<DIM,A>& v) const;
					using Vec4 = VecT<4,ALIGNB>;
					Vec4 asVec4(float w) const;
					//! 平面との交差点を算出
					template <bool A>
					std::tuple<VecT,bool> planeDivide(const VecT<DIM,A>& v, const PlaneT<false>& p) const;
					//! 平面にてベクトルを反転
					void flip(const PlaneT<false>& plane);
					VecT& operator *= (const QuatT<ALIGNB>& q);
					VecT operator * (const QuatT<ALIGNB>& q) const;
					VecT&& operator * (QuatT<ALIGNB>&& q) const;
					/*! \return x,y 成分 */
					const VecT<2,ALIGNB>& asVec2() const;
					// -------- Luaへのエクスポート用 --------
					VecT modV(const UVec& v) const;
					VecT mulQ(const QuatT<ALIGNB>& q) const;
					std::tuple<VecT,bool> luaPlaneDivide(const VecT<DIM,false>& v, const PlaneT<false>& p) const;
				#elif DIM==2
					float ccw(const VecT<DIM,false>& v) const;
					float cw(const VecT<DIM,false>& v) const;
					#if ALIGN==0
						static float Ccw(const UVec& v0, const UVec& v1, const UVec& v2);
						static float Cw(const UVec& v0, const UVec& v1, const UVec& v2);
					#endif
					using Vec3 = VecT<3,ALIGNB>;
					Vec3 asVec3(float z) const;
				#endif
				//! 行列との積算 (左から掛ける)
				/*! 行ベクトルとして扱う */
				template <int N, bool A>
				VecT<N,ALIGNB> operator * (const MatT<DIM,N,A>& m) const;
				template <int N, bool A>
				VecT& operator *= (const MatT<DIM,N,A>& m);

				#if DIM>=3
					//! パックされた32bit数値から色を取り出す
					static VecT FromPacked(uint32_t val);
					//! 32bit数値に値をパック (1.0が255)
					uint32_t toPacked() const;
				#endif
			};
			#undef Vec
			// 使いやすいようにクラスの別名を定義
			using BOOST_PP_CAT(BOOST_PP_IF(ALIGN,A,NOTHING), BOOST_PP_CAT(Vec,DIM)) = VT;
		}
		BOOST_CLASS_IMPLEMENTATION(spn::VT, object_serializable)
		#elif BOOST_PP_ITERATION_FLAGS() == 1
		// 同次元ベクトルとの演算を定義
		namespace spn {
			VT::VecT(reg128 r){
				STORETHIS(r);
			}
			VT::VecT(float a) {
				STORETHIS(reg_load1_ps(&a));
			}
			VT::VecT(ENUM_ARGPAIR(BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM))) {
				BOOST_PP_SEQ_FOR_EACH(
					DEF_ARGSET,
					NOTHING,
					BOOST_PP_SEQ_SUBSEQ(SEQ_VECELEM, 0, DIM)
				)
			}
			template <class V, class>
			VT::VecT(const V* src) {
				for(int i=0 ; i<DIM ; i++)
					m[i] = src[i];
			}
			template VT::VecT(const float*);
			template VT::VecT(const double*);
			VT::VecT(std::initializer_list<float> il) {
				// 要素数が満たなかったら残りは0にする
				alignas(16) float tmp[4] = {};
				auto* pTmp = tmp;
				for(auto itr=il.begin() ; itr!=il.end() ; ++itr)
					*pTmp++ = *itr;
				STORETHIS(reg_load_ps(tmp));
			}
			reg128 VT::loadPS() const {
				return LOADTHIS();
			}
			reg128 VT::loadPSZ() const {
				return LOADTHISZ();
			}
			VT& VT::operator = (reg128 r) {
				STORETHIS(r);
				return *this;
			}
			VT& VT::operator = (std::initializer_list<float> il) {
				return *this = VecT(il);
			}
			float VT::_sumup(reg128 xm) {
				SUMVEC(xm)
				float ret;
				reg_store_ss(&ret, xm);
				return ret;
			}
			VT::VecT(const AVec& v) {
				STORETHIS(LOADPS(v.m)); }
			VT::VecT(const UVec& v) {
				STORETHIS(LOADPSU(v.m)); }
			bool VT::isNaN() const {
				reg128 r0 = LOADTHISZ(),
					r_zero = reg_setzero_ps();
				reg128 res = reg_or_ps(reg_cmple_ps(r0, r_zero),
										reg_cmpgt_ps(r0, r_zero));
				res = reg_andnot_ps(res, xmm_const::fullbit());
				SUMVEC(res)
				float f;
				reg_store_ss(&f, res);
				return f != 0;
			}
			bool VT::isOutstanding() const {
				const float f = std::numeric_limits<float>::infinity();
				reg128 r_inf = reg_load1_ps(&f),
						r_zero = reg_setzero_ps(),
						r0 = reg_and_ps(LOADTHIS(), xmm_const::absmask());
				reg128 r1 = reg_or_ps(reg_cmple_ps(r0, r_zero),
									reg_cmpgt_ps(r0, r_zero));
				r0 = reg_cmpeq_ps(r0, r_inf);

				r1 = reg_andnot_ps(r1, xmm_const::fullbit());
				r0 = reg_or_ps(r0, r1);
				SUMVEC(r0)

				float fv;
				reg_store_ss(&fv, r0);
				return fv != 0;
			}

			VT& VT::operator = (const AVec& v) {
				STORETHIS(LOADPS(v.m));
				return *this;
			}
			VT& VT::operator = (const UVec& v) {
				STORETHIS(LOADPSU(v.m));
				return *this;
			}
			#define DEF_OP(op, func) \
					VT& VT::operator BOOST_PP_CAT(op,=) (float s) { \
						STORETHIS(func(LOADTHIS(), reg_load_ps1(&s))); \
						return *this; } \
					VT VT::operator op (float s) const { \
						return VecT(func(LOADTHIS(), reg_load1_ps(&s))); } \
					VT& VT::operator BOOST_PP_CAT(op,=) (const AVec& v) { \
						STORETHIS(func(LOADTHIS(), LOADPS(v.m))); \
						return *this; } \
					VT& VT::operator BOOST_PP_CAT(op,=) (const UVec& v) { \
						STORETHIS(func(LOADTHIS(), LOADPSU(v.m))); \
						return *this; } \
					VT::AVec VT::operator op (const AVec& v) const { \
						return AVec(func(LOADTHIS(), LOADPS(v.m))); } \
					VT::UVec VT::operator op (const UVec& v) const { \
						return UVec(func(LOADTHIS(), LOADPSU(v.m))); } \
					VT::AVec&& VT::operator op (AVec&& v) const { \
						STOREPS(v.m, func(LOADTHIS(), LOADPS(v.m))); \
						return std::forward<AVec>(v); } \
					VT::UVec&& VT::operator op (UVec&& v) const { \
						STOREPSU(v.m, func(LOADTHIS(), LOADPSU(v.m))); \
						return std::forward<UVec>(v); } \
					VT operator op (float fv, const VT& v) { \
						return VT(func(LOADTHISPS(v.m), reg_load1_ps(&fv))); }
			DEF_OP(+, reg_add_ps)
			DEF_OP(-, reg_sub_ps)
			DEF_OP(*, reg_mul_ps)
			DEF_OP(/, _mmDivPs)
			#undef DEF_OP
			VT VT::addV(const UVec& v) const {
				return *this + v; }
			VT VT::subV(const UVec& v) const {
				return *this - v; }
			VT VT::mulF(float s) const {
				return *this * s; }
			VT VT::mulM(const MatT<DIM,DIM,align>& m) const {
				return *this * m; }
			VT VT::divF(float s) const {
				return *this / s; }
			VT VT::invert() const {
				return -(*this); }
			bool VT::equal(const UVec& v) const {
				return *this == v; }
			std::string VT::toString() const {
				return ToString(*this);
			}

			std::ostream& operator << (std::ostream& os, const VT& v) {
				os << BOOST_PP_IF(ALIGN, 'A', ' ') << "Vec" << DIM << '[';
				for(int i=0 ; i<DIM-1 ; i++)
					os << v.m[i] << ',';
				return os << v.m[DIM-1] << ']';
			}

			template <bool A>
			float VT::dot(const VecT<DIM,A>& v) const {
				return _sumup(reg_mul_ps(LOADTHISZ(), v.loadPSZ()));
			}
			template float VT::dot(const VecT<DIM,false>& v) const;
			template float VT::dot(const VecT<DIM,true>& v) const;

			float VT::sum() const {
				return _sumup(LOADTHISZ());
			}
			float VT::average() const {
				return sum() * spn::Rcp22Bit(DIM);
			}
			template <bool A>
			float VT::distance(const VecT<DIM,A>& v) const {
				return std::sqrt(dist_sq(v));
			}
			template float VT::distance(const VecT<DIM,false>&) const;
			template float VT::distance(const VecT<DIM,true>&) const;

			template <bool A>
			float VT::dist_sq(const VecT<DIM,A>& v) const {
				auto tv = v - *this;
				return tv.len_sq();
			}
			template float VT::dist_sq(const VecT<DIM,false>&) const;
			template float VT::dist_sq(const VecT<DIM,true>&) const;

			template <bool A>
			VT VT::getMin(const VecT<DIM,A>& v) const {
				return VT(reg_min_ps(LOADTHIS(), v.loadPS())); }
			template VT VT::getMin(const VecT<DIM,false>&) const;
			template VT VT::getMin(const VecT<DIM,true>&) const;

			template <bool A>
			void VT::selectMin(const VecT<DIM,A>& v) {
				STORETHIS(reg_min_ps(LOADTHIS(), v.loadPS())); }
			template void VT::selectMin(const VecT<DIM,false>&);
			template void VT::selectMin(const VecT<DIM,true>&);

			template <bool A>
			VT VT::getMax(const VecT<DIM,A>& v) const {
				return VT(reg_max_ps(LOADTHIS(), v.loadPS())); }
			template VT VT::getMax(const VecT<DIM,false>&) const;
			template VT VT::getMax(const VecT<DIM,true>&) const;

			template <bool A>
			void VT::selectMax(const VecT<DIM,A>& v) {
				STORETHIS(reg_max_ps(LOADTHIS(), v.loadPS())); }
			template void VT::selectMax(const VecT<DIM,false>&);
			template void VT::selectMax(const VecT<DIM,true>&);

			VT VT::operator - () const {
				return *this * -1.0f;
			}

			template <bool A>
			bool VT::operator == (const VecT<DIM,A>& v) const {
				reg128 r0 = reg_cmpeq_ps(LOADTHISZ(), v.loadPSZ());
				r0 = reg_and_ps(r0, reg_shuffle_ps(r0, r0, _REG_SHUFFLE(1,0,3,2)));
				r0 = reg_and_ps(r0, reg_shuffle_ps(r0, r0, _REG_SHUFFLE(0,1,2,3)));
				return reg_cvttss_si32(r0) != 0;
			}
			template <bool A>
			bool VT::operator != (const VecT<DIM,A>& v) const {
				return !(this->operator == (v));
			}
			template bool VT::operator == (const VecT<DIM,false>&) const;
			template bool VT::operator == (const VecT<DIM,true>&) const;
			template bool VT::operator != (const VecT<DIM,false>&) const;
			template bool VT::operator != (const VecT<DIM,true>&) const;

			float VT::normalize() {
				float len = length();
				*this *= spn::Rcp22Bit(len);
				return len;
			}
			VT VT::normalization() const {
				float tmp = length();
				reg128 r0 = reg_load_ps1(&tmp);
				r0 = reg_div_ps(LOADTHIS(), r0);
				return VT(r0);
			}
			/*! \return ベクトルの長さ */
			float VT::length() const {
				return std::sqrt(len_sq());
			}
			/*! \return ベクトル長の2乗 */
			float VT::len_sq() const {
				reg128 r0 = LOADTHISZ();
				r0 = reg_mul_ps(r0, r0);
				SUMVEC(r0)

				float ret;
				reg_store_ss(&ret, r0);
				return ret;
			}
			void VT::saturate(float fMin, float fMax) {
				*this = saturation(fMin, fMax);
			}
			VT VT::saturation(float fMin, float fMax) const {
				reg128 xm = reg_max_ps(LOADTHIS(), reg_load1_ps(&fMin));
				return VT(reg_min_ps(xm, reg_load1_ps(&fMax)));
			}
			void VT::lerp(const UVec& v, float r) {
				*this = l_intp(v,r);
			}
			template <bool A>
			VT VT::l_intp(const VecT<DIM,A>& v, float r) const {
				reg128 ths = LOADTHIS();
				ths = reg_add_ps(ths, reg_mul_ps(reg_sub_ps(v.loadPS(), ths), reg_load1_ps(&r)));
				return VT(ths);
			}
			template VT VT::l_intp(const VecT<DIM,false>&, float) const;
			template VT VT::l_intp(const VecT<DIM,true>&, float) const;

			#if DIM>=3
				VT VT::FromPacked(uint32_t val) {
					alignas(16) const uint32_t tmp[4] = {val, val>>8, val>>16, val>>24};
					alignas(16) const uint32_t mask[4] = {0xff,0xff,0xff,0xff};

					reg128 x = reg_and_ps(reg_load_ps((const float*)mask), reg_load_ps((const float*)tmp));
					x = reg_cvtepi32_ps((reg128i)x);

					const float f255 = 1.f/255;
					x = reg_mul_ps(x, reg_load1_ps(&f255));
					return VecT(x);
				}
				uint32_t VT::toPacked() const {
					reg128 x = reg_mul_ps(reg_set_ps(255,255,255,255), LOADTHISZ());
					reg128i x2 = reg_cvtps_epi32(x);
					x2 = reg_packs_epi32(x2,x2);
					x2 = reg_packus_epi16(x2,x2);
					float ret;
					reg_store_ss(&ret, (reg128)x2);
					return *reinterpret_cast<uint32_t*>(&ret);
				}
			#endif
		}
	#elif BOOST_PP_ITERATION_FLAGS() == 2
		// 他次元ベクトルとの演算を定義
		namespace spn {
			#if ALIGN==1
				//! AVec -> Vec へ暗黙変換
				VT::operator VecT<DIM,false>& () { return *reinterpret_cast<VecT<DIM,false>*>(this); }
				VT::operator const VecT<DIM,false>& () const { return *reinterpret_cast<const VecT<DIM,false>*>(this); }
			#endif
			#if DIM==4
				#define _Vec3 VecT<3,ALIGNB>
				/*! \return x,y,z成分 */
				const _Vec3& VT::asVec3() const {
					return reinterpret_cast<const _Vec3&>(*this);
				}
				/*! \return x,y,zそれぞれをwで割った値 */
				_Vec3 VT::asVec3Coord() const {
					reg128 r0 = reg_rcp_ps(reg_load_ps1(&w));
					return _Vec3(reg_mul_ps(LOADTHIS(), r0));
				}
			#elif DIM==3
				const VecT<2,ALIGNB>& VT::asVec2() const {
					return reinterpret_cast<const VecT<2,ALIGNB>&>(*this);
				}
				//! ベクトルに垂直なベクトルを適当に定める
				VT VT::verticalVector() const {
					VT ret = VT(1,0,0) % *this;
					float len_s = ret.len_sq();
					if(len_s < 1e-6f) {
						ret = VT(0,0,1) % *this;
						return ret.normalization();
					}
					return ret * RSqrt(len_s);
				}
				/*! \return this X v の外積ベクトル */
				template <bool A>
				VT VT::cross(const VecT<DIM,A>& v) const {
					reg128 r0 = LOADTHIS(),
							r1 = v.loadPS();
					// r0[y,z,x]
					reg128 m0 = reg_shuffle_ps(r0, r0, _REG_SHUFFLE(0,0,2,1)),
					// r0[z,x,y]
							m1 = reg_shuffle_ps(r0, r0, _REG_SHUFFLE(0,1,0,2)),
					// r1[z,x,y]
							m2 = reg_shuffle_ps(r1, r1, _REG_SHUFFLE(0,1,0,2)),
					// r1[y,z,x]
							m3 = reg_shuffle_ps(r1, r1, _REG_SHUFFLE(0,0,2,1));
					r0 = reg_mul_ps(m0,m2);
					r1 = reg_mul_ps(m1,m3);
					r0 = reg_sub_ps(r0, r1);
					return VT(r0);
				}
				template VT VT::cross(const VecT<DIM,false>&) const;
				template VT VT::cross(const VecT<DIM,true>&) const;

				//! 外積計算cross()と同義
				template <bool A>
				void VT::operator %= (const VecT<DIM,A>& v) {
					*this = cross(v);
				}
				template void VT::operator %= (const VecT<DIM,false>&);
				template void VT::operator %= (const VecT<DIM,true>&);

				//! 外積計算cross()と同義
				template <bool A>
				VT VT::operator % (const VecT<DIM,A>& v) const {
					return cross(v);
				}
				template VT VT::operator % (const VecT<DIM,false>&) const;
				template VT VT::operator % (const VecT<DIM,true>&) const;

				VT VT::modV(const UVec& v) const {
					return *this % v; }
				VT VT::mulQ(const QuatT<ALIGNB>& q) const {
					return *this * q; }
				std::tuple<VT,bool> VT::luaPlaneDivide(const VecT<DIM,false>& v, const PlaneT<false>& p) const {
					return planeDivide(v, p);
				}
				#define _Vec4 VecT<4,ALIGNB>
				_Vec4 VT::asVec4(float w) const {
					return _Vec4(x,y,z,w);
				}
			#elif DIM==2
				//! counter clockwise
				/*! 半時計回りが正数を返す */
				float VT::ccw(const UVec& v) const {
					return x*v.y - y*v.x;
				}
				//! clockwise
				/*! 時計回りが正数を返す */
				float VT::cw(const UVec& v) const {
					return -x*v.y + y*v.x;
				}
				#define _Vec3 VecT<3,ALIGNB>
				_Vec3 VT::asVec3(float z) const {
					return _Vec3(x,y,z);
				}
				#if ALIGN==0
					float VT::Ccw(const UVec& v0, const UVec& v1, const UVec& v2) {
						return (v0-v1).ccw(v2-v1);
					}
					float VT::Cw(const UVec& v0, const UVec& v1, const UVec& v2) {
						return (v0-v1).cw(v2-v1);
					}
				#endif
			#endif
		}
	#else
		#include "random.hpp"
		namespace spn {
			/*	Pseudo-code:
				template <int N, bool A>
				VecT<N,ALIGNB> VT::operator * (const MatT<DIM,N,A>& m) const {
					reg128 ths = LOADTHIS(),
							accum = reg_setzero_ps();
					for(int i=0 ; i<DIM ; i++) {
						reg128 tmp = reg_shuffle_ps(ths, ths, _REG_SHUFFLE(i,i,i,i));
						accum = reg_add_ps(accum, reg_mul_ps(tmp, LOADPS_(N)(m.ma[i])));
					}
					return VecT<N,ALIGNB>(accum);
				}
			*/
			#define LOOP_MULOP(z,n,loadf)		{reg128 tmp=reg_shuffle_ps(ths,ths, _REG_SHUFFLE(n,n,n,n)); \
						accum = reg_add_ps(accum, reg_mul_ps(tmp, loadf(mat.ma[n])));}
			#define DEF_MULOPA(z,n,align)	template <> VecT<n,ALIGNB> VT::operator * (const MatT<DIM,n,BOOLNIZE(align)>& mat) const { \
						reg128 ths = LOADTHIS(), \
						accum = reg_setzero_ps(); \
						BOOST_PP_REPEAT_##z(DIM, LOOP_MULOP, BOOST_PP_CAT(LOADPS_, BOOST_PP_CAT(AFLAG(align),n))) \
						return VecT<n,ALIGNB>(accum); }
			#define DEF_MULOPA_2(z,n,align)	template <> VT& VT::operator *= (const MatT<DIM,DIM,BOOLNIZE(align)>& mat) { \
					return *this = *this * mat; }

			#define DEF_MULOP(z,align,dummy)	BOOST_PP_REPEAT_FROM_TO_##z(2,5, DEF_MULOPA, align) DEF_MULOPA_2(dummy,dummy,align)
			BOOST_PP_REPEAT(2, DEF_MULOP, NOTHING)
			#undef DEF_MULOP

			#if DIM==3
				VT& VT::operator *= (const QuatT<ALIGNB>& q) {
					*this = (*this * q);
					return *this;
				}
				VT VT::operator * (const QuatT<ALIGNB>& q) const {
					auto tq = q * QuatT<ALIGNB>(*this,0) * q.conjugation();
					return tq.getVector();
				}
				VT&& VT::operator * (QuatT<ALIGNB>&& q) const {
					VT& rVT = *reinterpret_cast<VT*>(&q);
					rVT = *this * q;
					return std::move(rVT);
				}
				void VT::flip(const Plane& plane) {
					const auto& nml = plane.getNormal();
					float d = plane.dot(*this);
					*this += nml*-d*2;
				}
				template <bool A>
				std::tuple<VT,bool> VT::planeDivide(const VecT<DIM,A>& v, const PlaneT<false>& p) const {
					// 線分が平面をまたぐか
					float distf = p.dot(*this);
					float distb = p.dot(v);
					if(distf * distb >= 0.0f)
						return std::make_tuple(VecT(),false);

					float ratio = fabs(distf) / (fabs(distf) + fabs(distb));
					// 平面と線分の交点 -> tv
					Vec3 tv = v - (*this);
					tv *= ratio;
					VecT cp = tv + (*this);
					return std::make_tuple(cp,true);
				}
				template std::tuple<VT,bool> VT::planeDivide(const VecT<DIM,true>&,const PlaneT<false>&) const;
				template std::tuple<VT,bool> VT::planeDivide(const VecT<DIM,false>&,const PlaneT<false>&) const;
			#endif

			VT VT::luaRandom(MTRandom& mt, const RangeF& r) {
				return Random(mt.getUniformF<float>(), r);
			}
			VT VT::luaRandomWithLength(MTRandom& mt, float th, const RangeF& r) {
				return RandomWithLength(mt.getUniformF<float>(), th, r);
			}
			VT VT::luaRandomWithAbs(MTRandom& mt, const RangeF& rTh, const RangeF& r) {
				return RandomWithAbs(mt.getUniformF<float>(), rTh, r);
			}
		}
	#endif
	#include "local_unmacro.hpp"
	#undef ALIGN
	#undef ALIGNB
	#undef Vec
	#undef VT
#endif


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2012-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <algorithm>  // all_of, copy
#include <deque>
#include <initializer_list>
#include <iosfwd>
#include <stdexcept>  //invalid_argument
#include <unordered_map>

#include <boost/container/flat_set.hpp>

#include "sdd/dd/definition.hh"
#include "sdd/hom/context_fwd.hh"
#include "sdd/hom/definition_fwd.hh"
#include "sdd/hom/identity.hh"
#include "sdd/hom/local.hh"
#include "sdd/order/order.hh"
#include "sdd/util/packed.hh"

namespace sdd { namespace hom {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief intersection homomorphism.
template <typename C>
struct _intersection
{
  /// @brief The type of the homomorphism operands' set.
  using operands_type = boost::container::flat_set<homomorphism<C>>;

  /// @brief The type of a const iterator on this intersection's operands.
  using const_iterator = typename operands_type::const_iterator;

  /// @brief The homomorphism operands' set.
  const operands_type operands;

  /// @brief Evaluation.
  SDD<C>
  operator()(context<C>& cxt, const order<C>& o, const SDD<C>& x)
  const
  {
    dd::intersection_builder<C, SDD<C>> intersection_operands(cxt.sdd_context());
    intersection_operands.reserve(operands.size());
    for (const auto& op : operands)
    {
      auto res = op(cxt, o, x);
      if (res.empty())
      {
        return zero<C>();
      }
      intersection_operands.add(std::move(res));
    }
    return dd::intersection(cxt.sdd_context(), std::move(intersection_operands));
  }

  /// @brief Skip variable predicate.
  bool
  skip(const order<C>& o)
  const noexcept
  {
    return std::all_of( operands.begin(), operands.end()
                      , [&o](const homomorphism<C>& h){return h.skip(o);});
  }

  /// @brief Selector predicate
  bool
  selector()
  const noexcept
  {
    return std::all_of( operands.begin(), operands.end()
                      , [](const homomorphism<C>& h){return h.selector();});
  }

  /// @brief Get an iterator to the first operand.
  ///
  /// O(1).
  const_iterator
  begin()
  const noexcept
  {
    return operands.begin();
  }

  /// @brief Get an iterator to the end of operands.
  ///
  /// O(1).
  const_iterator
  end()
  const noexcept
  {
    return operands.end();
  }

  friend
  bool
  operator==(const _intersection& lhs, const _intersection& rhs)
  noexcept
  {
    return lhs.operands == rhs.operands;
  }

  friend
  std::ostream&
  operator<<(std::ostream& os, const _intersection& s)
  {
    os << "(";
    std::copy( s.operands.begin(), std::prev(s.operands.end())
              , std::ostream_iterator<homomorphism<C>>(os, " & "));
    return os << *std::prev(s.operands.end()) << ")";
  }

};

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Help optimize an intersection's operands.
template <typename C>
struct intersection_builder
{
  using operands_type = typename _intersection<C>::operands_type;
  using hom_list_type = std::deque<homomorphism<C>> ;
  using locals_type = std::unordered_map< typename C::variable_type, hom_list_type>;

  operands_type& operands_;
  locals_type& locals_;

  /// @brief Flatten nested intersections.
  void
  operator()(const _intersection<C>& s, const homomorphism<C>&)
  const
  {
    for (const auto& op : s.operands)
    {
      visit(*this, op, op);
    }
  }

  /// @brief Regroup locals.
  void
  operator()(const _local<C>& l, const homomorphism<C>&)
  const
  {
    auto insertion = locals_.emplace(l.target, hom_list_type{});
    insertion.first->second.emplace_back(l.h);
  }

  /// @brief Insert normally all other operands.
  template <typename T>
  void
  operator()(const T&, const homomorphism<C>& h)
  const
  {
    operands_.insert(h);
  }
};

} // namespace hom

/*------------------------------------------------------------------------------------------------*/

/// @brief Create the intersection homomorphism.
/// @related homomorphism
template <typename C, typename InputIterator>
homomorphism<C>
intersection(const order<C>& o, InputIterator begin, InputIterator end)
{
  const auto size = std::distance(begin, end);

  if (size == 0)
  {
    throw std::invalid_argument("Empty operands at intersection construction.");
  }

  typename hom::_intersection<C>::operands_type operands;
  operands.reserve(size);

  typename hom::intersection_builder<C>::locals_type locals;
  hom::intersection_builder<C> ib {operands, locals};
  for (; begin != end; ++begin)
  {
    visit(ib, *begin, *begin);
  }

  // insert remaining locals
  for (const auto& l : locals)
  {
    operands.insert(local<C>(l.first, intersection(o, l.second.begin(), l.second.end())));
  }

  if (operands.size() == 1)
  {
    return *operands.begin();
  }
  else
  {
    operands.shrink_to_fit();
    return hom::make<C, hom::_intersection<C>>(std::move(operands));
  }
}

/*------------------------------------------------------------------------------------------------*/

/// @brief Create the intersection homomorphism.
/// @related homomorphism
template <typename C>
homomorphism<C>
intersection(const order<C>& o, std::initializer_list<homomorphism<C>> operands)
{
  return intersection(o, operands.begin(), operands.end());
}

/*------------------------------------------------------------------------------------------------*/

} // namespace sdd

namespace std {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Hash specialization for sdd::hom::_intersection.
template <typename C>
struct hash<sdd::hom::_intersection<C>>
{
  std::size_t
  operator()(const sdd::hom::_intersection<C>& s)
  const
  {
    using namespace sdd::hash;
    return seed() (range(s.operands));
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace std


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

//#define BOOST_TEST_DYN_LINK
//#define BOOST_TEST_MAIN

#include <limits>
#include <string>

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

#include "tudat/simulation/estimation.h"

//namespace tudat
//{
//namespace unit_tests
//{

using namespace tudat;
using namespace tudat::observation_models;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::orbital_element_conversions;
using namespace tudat::ephemerides;
using namespace tudat::propagators;
using namespace tudat::basic_astrodynamics;
using namespace tudat::coordinate_conversions;
using namespace tudat::statistics;

//BOOST_AUTO_TEST_SUITE( test_observation_noise_models )

//// Function to conver
//double ignoreInputeVariable( std::function< double( ) > inputFreeFunction, const double dummyInput )
//{
//    return inputFreeFunction( );
//}

//! Test whether observation noise is correctly added when simulating noisy observations
int main( )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Define bodies in simulation
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Moon" );

    // Specify initial time
    double initialEphemerisTime = double( 1.0E7 );
    double finalEphemerisTime = double( 1.0E7 + 3.0 * physical_constants::JULIAN_DAY );

    // Create bodies needed in simulation
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - 3600.0, finalEphemerisTime + 3600.0 );
    bodySettings.at( "Earth" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
                "ECLIPJ2000", "IAU_Earth",
                spice_interface::computeRotationQuaternionBetweenFrames(
                    "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ),
                initialEphemerisTime, 2.0 * mathematical_constants::PI /
                ( physical_constants::JULIAN_DAY ) );

    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    // Creatre ground stations
    std::vector< std::string > groundStationNames;
    groundStationNames.push_back( "Station1" );
    groundStationNames.push_back( "Station2" );
    groundStationNames.push_back( "Station3" );

    createGroundStation( bodies.at( "Earth" ), "Station1", ( Eigen::Vector3d( ) << 0.0, 0.35, 0.0 ).finished( ), geodetic_position );
    createGroundStation( bodies.at( "Earth" ), "Station2", ( Eigen::Vector3d( ) << 0.0, -0.55, 2.0 ).finished( ), geodetic_position );
    createGroundStation( bodies.at( "Earth" ), "Station3", ( Eigen::Vector3d( ) << 0.0, 0.05, 4.0 ).finished( ), geodetic_position );

    // Define parameters.
    std::vector< LinkEnds > stationReceiverLinkEnds;
    std::vector< LinkEnds > stationTransmitterLinkEnds;
    std::vector< LinkEnds > twoWayLinkEnds;

    // Define link ends to/from ground stations to Moon
    for( unsigned int i = 0; i < groundStationNames.size( ); i++ )
    {
        LinkEnds linkEnds;
        linkEnds[ transmitter ] = std::make_pair( "Earth", groundStationNames.at( i ) );
        linkEnds[ receiver ] = std::make_pair( "Moon", "" );
        stationTransmitterLinkEnds.push_back( linkEnds );

        linkEnds.clear( );
        linkEnds[ receiver ] = std::make_pair( "Earth", groundStationNames.at( i ) );
        linkEnds[ transmitter ] = std::make_pair( "Moon", "" );
        stationReceiverLinkEnds.push_back( linkEnds );

        twoWayLinkEnds.clear( );
        linkEnds[ receiver ] = std::make_pair( "Earth", groundStationNames.at( i ) );
        linkEnds[ retransmitter ] = std::make_pair( "Moon", "" );
        linkEnds[ transmitter ] = std::make_pair( "Earth", groundStationNames.at( i ) );
        twoWayLinkEnds.push_back( linkEnds );
    }

    // Define (arbitrary) link ends for each observable
    std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable;
    linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 0 ] );
    linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 1 ] );

    linkEndsPerObservable[ one_way_doppler ].push_back( stationReceiverLinkEnds[ 0 ] );
    linkEndsPerObservable[ one_way_doppler ].push_back( stationReceiverLinkEnds[ 1 ] );

    // Define observation settings for each observable/link ends combination
    std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList;
    for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
         linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
    {
        ObservableType currentObservable = linkEndIterator->first;
        std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;

        for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
        {
            // Create observation settings
            observationSettingsList.push_back( std::make_shared< ObservationModelSettings >(
                                                   currentObservable, currentLinkEndsList.at( i ) ) );
        }
    }

    // Create observation simulators
    std::vector< std::shared_ptr< ObservationSimulatorBase< double, double > > >  observationSimulators =
            createObservationSimulators( observationSettingsList, bodies );

    // Define osbervation times.
    std::vector< double > baseTimeList;
    double observationTimeStart = initialEphemerisTime + 1000.0;
    double observationInterval = 10.0;
    for( unsigned int i = 0; i < 14; i++ )
    {
        for( unsigned int j = 0; j < 4320; j++ )
        {
            baseTimeList.push_back( observationTimeStart + static_cast< double >( i ) * 86400.0 +
                                    static_cast< double >( j ) * observationInterval );
        }
    }

    // Define observation simulation settings (observation type, link end, times and reference link end)
    std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput;
    for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
         linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
    {
        ObservableType currentObservable = linkEndIterator->first;
        std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
        for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
        {
            measurementSimulationInput.push_back(
                        std::make_shared< TabulatedObservationSimulationSettings< > >(
                            currentObservable, currentLinkEndsList.at( i ), baseTimeList, receiver ) );
        }
    }

    std::vector< std::shared_ptr< observation_models::ObservationViabilitySettings > > viabilitySettingsList;
    viabilitySettingsList.push_back( elevationAngleViabilitySettings(
                                         std::make_pair( "Earth", "Station1" ), 25.0 * mathematical_constants::PI / 180.0 ) );
    viabilitySettingsList.push_back( elevationAngleViabilitySettings(
                                         std::make_pair( "Earth", "Station2" ), 25.0 * mathematical_constants::PI / 180.0 ) );

    addViabilityToObservationSimulationSettings(
            measurementSimulationInput, viabilitySettingsList );

    // Define settings for dependent variables
    std::vector< std::shared_ptr< ObservationDependentVariableSettings > > dependentVariableList;

    std::shared_ptr< ObservationDependentVariableSettings > elevationAngleSettings1 =
            std::make_shared< StationAngleObservationDependentVariableSettings >(
                station_elevation_angle, std::make_pair( "Earth", "Station1" ) );
    std::shared_ptr< ObservationDependentVariableSettings > azimuthAngleSettings1 =
            std::make_shared< StationAngleObservationDependentVariableSettings >(
                station_azimuth_angle, std::make_pair( "Earth", "Station1" ) );

//    std::shared_ptr< ObservationDependentVariableSettings > elevationAngleSettings2 =
//            std::make_shared< StationAngleObservationDependentVariableSettings >(
//                station_elevation_angle, std::make_pair( "Earth", "Station2" ) );

    dependentVariableList.push_back( elevationAngleSettings1 );
    dependentVariableList.push_back( azimuthAngleSettings1 );

    addDependentVariablesToObservationSimulationSettings(
                measurementSimulationInput, dependentVariableList, bodies );

    // Simulate noise-free observations
    std::shared_ptr< ObservationCollection< > > idealObservationsAndTimes = simulateObservations< double, double >(
                measurementSimulationInput, observationSimulators, bodies );

    std::cout<<"Simulated observations "<<std::endl;
    std::map< double, Eigen::VectorXd > elevationAngles;
    std::map< double, Eigen::VectorXd > azimuthAngles;

    std::cout<<"Getting elevation angle"<<std::endl;
    elevationAngles = getDependentVariableResultList(
                idealObservationsAndTimes, elevationAngleSettings1,
                one_way_range );
    input_output::writeDataMapToTextFile( elevationAngles,
                                          "elevationAngles1_range.dat",
                                          "/home/dominic/Software/Tudat30Bundle/test-output/" );

    std::cout<<"Getting azimuth angle"<<std::endl;
    azimuthAngles = getDependentVariableResultList(
                idealObservationsAndTimes, azimuthAngleSettings1,
                one_way_range );
    input_output::writeDataMapToTextFile( azimuthAngles,
                                          "azimuthAngles1_range.dat",
                                          "/home/dominic/Software/Tudat30Bundle/test-output/" );

//    elevationAngles = getDependentVariableResultList(
//                idealObservationsAndTimes, elevationAngleSettings1,
//                one_way_range, stationReceiverLinkEnds[ 0 ] );
//    input_output::writeDataMapToTextFile( elevationAngles,
//                                          "elevationAngles2_range.dat",
//                                          "/home/dominic/Software/Tudat30Bundle/test-output/" );

//    elevationAngles = getDependentVariableResultList(
//                idealObservationsAndTimes, elevationAngleSettings2,
//                one_way_doppler );
//    input_output::writeDataMapToTextFile( elevationAngles,
//                                          "elevationAngles1_doppler.dat",
//                                          "/home/dominic/Software/Tudat30Bundle/test-output/" );

//    elevationAngles = getDependentVariableResultList(
//                idealObservationsAndTimes, elevationAngleSettings2,
//                one_way_doppler, stationReceiverLinkEnds[ 1 ] );
//    input_output::writeDataMapToTextFile( elevationAngles,
//                                          "elevationAngles2_doppler.dat",
//                                          "/home/dominic/Software/Tudat30Bundle/test-output/" );

}

//class ObservationDependentVariableWrapper
//{
//  ObservableType observableType_;

//  LinkEnds linkEnds;

//  std::map< double, Eigen::VectorXd > dependentVariables_;

//  std::vector< std::shared_ptr< ObservationDependentVariableSettings > > dependentVariableList;

//  std::vector< int > sizeIndices_;

//  std::map< double, Eigen::VectorXd > getSingleDependentVariables( std::shared_ptr< ObservationDependentVariableSettings > );

//};

//BOOST_AUTO_TEST_SUITE_END( )

//}

//}