#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/quantizer.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

#ifdef USE_GEMMLOWP
#include "public/gemmlowp.h"
#endif

namespace caffe {

template<typename Dtype>
typename std::enable_if<unsigned_integer_is_same<Dtype>::value, bool>::type
gemmlowp_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
              const int_tp M, const int_tp N, const int_tp K,
              const Dtype alpha, const Dtype* A,
              const Dtype* B, const Dtype beta, Dtype* C,
              const QuantizerValues* const alpha_quant,
              const QuantizerValues* const a_quant,
              const QuantizerValues* const b_quant,
              const QuantizerValues* const beta_quant,
              const QuantizerValues* const c_quant) {
  // No gemmlowp available
  return false;
}

#ifdef USE_GEMMLOWP
template<>
typename std::enable_if<unsigned_integer_is_same<uint8_t>::value, bool>::type
gemmlowp_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
              const int_tp M, const int_tp N, const int_tp K,
              const uint8_t alpha, const uint8_t* A,
              const uint8_t* B, const uint8_t beta, uint8_t* C,
              const QuantizerValues* const alpha_quant,
              const QuantizerValues* const a_quant,
              const QuantizerValues* const b_quant,
              const QuantizerValues* const beta_quant,
              const QuantizerValues* const c_quant) {
  CHECK(a_quant && b_quant && c_quant)
       << "Integer type requires quantization values.";
  if (alpha == 1 && beta == 0 && alpha_quant == nullptr
                              && beta_quant == nullptr) {
    const int32_t lhs_offset = -a_quant->get_zero<int32_t>();
    const int32_t rhs_offset = -b_quant->get_zero<int32_t>();

    gemmlowp::OutputStageQuantizeDownInt32ByFixedPoint quantize_down_stage;
    quantize_down_stage.result_offset_after_shift =
        c_quant->get_zero<int32_t>();

    int32_t mult;
    int8_t shift;

    QuantizerBase::template MultiplicativeQuantVals<int32_t>(
        a_quant, b_quant, c_quant, &mult, &shift);

    quantize_down_stage.result_fixedpoint_multiplier = mult;
    quantize_down_stage.result_shift = static_cast<int32_t>(shift);

    if (quantize_down_stage.result_shift < 0) {
      // Gemmlowp doesn't handle negative exponents
      return false;
    }

    gemmlowp::OutputStageSaturatingCastToUint8 saturating_cast_stage;
    const auto& output_pipeline =
        std::make_tuple(quantize_down_stage, saturating_cast_stage);

    gemmlowp::GemmContext gemm_context;

    gemmlowp::MatrixMap<uint8_t, gemmlowp::MapOrder::RowMajor> rs(C, M, N);

    if (trans_A == CblasNoTrans && trans_B == CblasNoTrans) {
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::RowMajor>
                                                                   lhs(A, M, K);
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::RowMajor>
                                                                   rhs(B, K, N);
      gemmlowp::GemmWithOutputPipeline<uint8_t, uint8_t,
                                       gemmlowp::DefaultL8R8BitDepthParams>(
         &gemm_context, lhs, rhs, &rs, lhs_offset, rhs_offset, output_pipeline);
    } else if (trans_A == CblasNoTrans && trans_B == CblasTrans) {
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::RowMajor>
                                                                   lhs(A, M, K);
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::ColMajor>
                                                                   rhs(B, K, N);
      gemmlowp::GemmWithOutputPipeline<uint8_t, uint8_t,
                                       gemmlowp::DefaultL8R8BitDepthParams>(
         &gemm_context, lhs, rhs, &rs, lhs_offset, rhs_offset, output_pipeline);
    } else if (trans_A == CblasTrans && trans_B == CblasNoTrans) {
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::ColMajor>
                                                                   lhs(A, M, K);
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::RowMajor>
                                                                   rhs(B, K, N);
      gemmlowp::GemmWithOutputPipeline<uint8_t, uint8_t,
                                       gemmlowp::DefaultL8R8BitDepthParams>(
         &gemm_context, lhs, rhs, &rs, lhs_offset, rhs_offset, output_pipeline);
    } else {
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::ColMajor>
                                                                   lhs(A, M, K);
      gemmlowp::MatrixMap<const uint8_t, gemmlowp::MapOrder::ColMajor>
                                                                   rhs(B, K, N);
      gemmlowp::GemmWithOutputPipeline<uint8_t, uint8_t,
                                       gemmlowp::DefaultL8R8BitDepthParams>(
         &gemm_context, lhs, rhs, &rs, lhs_offset, rhs_offset, output_pipeline);
    }
    return true;
  }
  // Gemmlowp can't handle the case
  return false;
}
#endif  // USE_GEMMLOWP

template<typename Dtype>
typename std::enable_if<unsigned_integer_is_same<Dtype>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
           const int_tp M, const int_tp N, const int_tp K,
           const Dtype alpha, const Dtype* A,
           const Dtype* B, const Dtype beta, Dtype* C,
           const QuantizerValues* const alpha_quant,
           const QuantizerValues* const a_quant,
           const QuantizerValues* const b_quant,
           const QuantizerValues* const beta_quant,
           const QuantizerValues* const c_quant) {
  CHECK(a_quant && b_quant && c_quant)
       << "Integer type requires quantization values.";

  if (gemmlowp_gemm<Dtype>(trans_A, trans_B, M, N, K, alpha, A, B, beta, C,
                          alpha_quant, a_quant, b_quant, beta_quant, c_quant)) {
    // Handled by gemmlowp implementation
    return;
  }

  typedef typename std::conditional<sizeof(Dtype) == 1, int16_t,
          typename std::conditional<sizeof(Dtype) == 2, int32_t,
                                    int64_t>::type>::type Difftype;
  typedef typename std::conditional<sizeof(Dtype) == 1,
                                    int32_t, int64_t>::type Acctype;

  // std::cout << "Difftype: " << sizeof(Difftype) << std::endl;
  // std::cout << "Acctype: " << sizeof(Acctype) << std::endl;

  int8_t shift_bits = (32 / sizeof(Dtype)) - 1;

  int32_t mult;
  int8_t shift;
  int32_t alpha_mult;
  int8_t alpha_shift;
  int32_t beta_mult;
  int8_t beta_shift;
  Acctype c_max = c_quant->get_max<Acctype>();
  Acctype c_min = c_quant->get_min<Acctype>();
  Dtype lhs_off = a_quant->get_zero<Dtype>();
  Dtype rhs_off = b_quant->get_zero<Dtype>();
  Dtype alpha_off = alpha_quant ? alpha_quant->get_zero<Dtype>() : Dtype(0);
  Dtype beta_off = beta_quant ? beta_quant->get_zero<Dtype>() : Dtype(0);
  const Acctype result_off = c_quant->get_zero<Acctype>();

  QuantizerBase::template MultiplicativeQuantVals<int32_t>(
      a_quant, b_quant, c_quant, &mult, &shift, shift_bits);
  QuantizerBase::template MultiplicativeQuantVals<int32_t>(
      c_quant, alpha_quant, c_quant, &alpha_mult, &alpha_shift, shift_bits);
  QuantizerBase::template MultiplicativeQuantVals<int32_t>(
      c_quant, beta_quant, c_quant, &beta_mult, &beta_shift, shift_bits);

  int_tp inc_a = (trans_A == CblasNoTrans) ? 1 : M;
  int_tp inc_b = (trans_B == CblasNoTrans) ? N : 1;
  for (int_tp m = 0; m < M; ++m) {
#pragma omp parallel for
    for (int_tp n = 0; n < N; ++n) {
      Acctype acc = Acctype(0);
      int_tp b_index = trans_B == CblasNoTrans ? n : K * n;
      int_tp a_index = trans_A == CblasNoTrans ? K * m : m;
      for (int_tp k = 0; k < K; ++k) {
        Difftype a_diff = A[a_index] - lhs_off;
        Difftype b_diff = B[b_index] - rhs_off;
        // std::cout << "a_diff: " << a_diff << std::endl;
        // std::cout << "b_diff: " << b_diff << std::endl;
        acc += static_cast<Acctype>(a_diff) * static_cast<Acctype>(b_diff);
        a_index += inc_a;
        b_index += inc_b;
      }
      Acctype reg = acc * (alpha_quant ? Acctype(1) : alpha);
      // std::cout << "1: " << m << ", "<< n << ": " << reg << std::endl;
      // std::cout << "M: " << mult << std::endl;
      // std::cout << "S: " << static_cast<int32_t>(shift_bits) << std::endl;
      reg = static_cast<Acctype>((static_cast<int64_t>(reg) *
                             static_cast<int64_t>(mult)) / (1ll << shift_bits));
      // std::cout << "2: " << m << ", "<< n << ": " << reg << std::endl;
      if (shift >= 0) {
        reg = reg >> shift;
      } else {
        reg = reg << -shift;
      }
      if (alpha_quant) {
        Difftype alpha_diff = alpha - alpha_off;
        reg = static_cast<Acctype>(alpha_diff) * static_cast<Acctype>(reg);
        reg = static_cast<Acctype>((static_cast<int64_t>(reg) *
                       static_cast<int64_t>(alpha_mult)) / (1ll << shift_bits));
        if (alpha_shift >= 0) {
          reg = reg >> alpha_shift;
        } else {
          reg = reg << -alpha_shift;
        }
      }
      // std::cout << "3: " << m << ", "<< n << ": " << reg << std::endl;
      if (beta_quant) {
        Difftype beta_diff = beta - beta_off;
        Difftype c_diff = C[m * N + n] - static_cast<Difftype>(result_off);
        Acctype creg = static_cast<Acctype>(beta_diff)
                     * static_cast<Acctype>(c_diff);
        creg = static_cast<Acctype>((static_cast<int64_t>(creg) *
                        static_cast<int64_t>(beta_mult)) / (1ll << shift_bits));
        if (beta_shift >= 0) {
          creg = creg >> beta_shift;
        } else {
          creg = creg << -beta_shift;
        }
        reg = reg + creg;
      } else if (beta == Dtype(1)) {
        reg = reg + (C[m * N + n] - result_off);
      }
      reg = reg + result_off;
      // std::cout << "4: " << m << ", "<< n << ": " << reg << std::endl;
      C[m * N + n] = static_cast<Dtype>(std::min(std::max(reg, c_min), c_max));
    }
  }
}

template<typename Dtype>
typename std::enable_if<float_is_same<Dtype>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
           const int_tp M, const int_tp N, const int_tp K,
           const Dtype alpha, const Dtype* A,
           const Dtype* B, const Dtype beta, Dtype* C,
           const QuantizerValues* const alpha_quant,
           const QuantizerValues* const a_quant,
           const QuantizerValues* const b_quant,
           const QuantizerValues* const beta_quant,
           const QuantizerValues* const c_quant) {
  int_tp inc_a = (trans_A == CblasNoTrans) ? 1 : M;
  int_tp inc_b = (trans_B == CblasNoTrans) ? N : 1;
  for (int_tp m = 0; m < M; ++m) {
#pragma omp parallel for
    for (int_tp n = 0; n < N; ++n) {
      Dtype acc = 0;
      int_tp b_index = trans_B == CblasNoTrans ? n : K * n;
      int_tp a_index = trans_A == CblasNoTrans ? K * m : m;
      for (int_tp k = 0; k < K; ++k) {
        acc += A[a_index] * B[b_index];
        a_index += inc_a;
        b_index += inc_b;
      }
      if (beta != 0) {
        C[m * N + n] = acc * alpha + beta * C[m * N + n];
      }
      else {
        C[m * N + n] = acc * alpha;
      }
    }
  }
}

template
typename std::enable_if<float_is_same<half_fp>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
                const int_tp M, const int_tp N, const int_tp K,
                const half_fp alpha, const half_fp* A,
                const half_fp* B, const half_fp beta, half_fp* C,
                const QuantizerValues* const alpha_quant,
                const QuantizerValues* const a_quant,
                const QuantizerValues* const b_quant,
                const QuantizerValues* const beta_quant,
                const QuantizerValues* const c_quant);
template
typename std::enable_if<unsigned_integer_is_same<uint8_t>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
                const int_tp M, const int_tp N, const int_tp K,
                const uint8_t alpha, const uint8_t* A,
                const uint8_t* B, const uint8_t beta, uint8_t* C,
                const QuantizerValues* const alpha_quant,
                const QuantizerValues* const a_quant,
                const QuantizerValues* const b_quant,
                const QuantizerValues* const beta_quant,
                const QuantizerValues* const c_quant);
template
typename std::enable_if<unsigned_integer_is_same<uint16_t>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
           const int_tp M, const int_tp N, const int_tp K,
           const uint16_t alpha, const uint16_t* A,
           const uint16_t* B, const uint16_t beta, uint16_t* C,
           const QuantizerValues* const alpha_quant,
           const QuantizerValues* const a_quant,
           const QuantizerValues* const b_quant,
           const QuantizerValues* const beta_quant,
           const QuantizerValues* const c_quant);
template
typename std::enable_if<unsigned_integer_is_same<uint32_t>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
           const int_tp M, const int_tp N, const int_tp K,
           const uint32_t alpha, const uint32_t* A,
           const uint32_t* B, const uint32_t beta, uint32_t* C,
           const QuantizerValues* const alpha_quant,
           const QuantizerValues* const a_quant,
           const QuantizerValues* const b_quant,
           const QuantizerValues* const beta_quant,
           const QuantizerValues* const c_quant);
template
typename std::enable_if<unsigned_integer_is_same<uint64_t>::value, void>::type
caffe_gemm(const CBLAS_TRANSPOSE trans_A, const CBLAS_TRANSPOSE trans_B,
           const int_tp M, const int_tp N, const int_tp K,
           const uint64_t alpha, const uint64_t* A,
           const uint64_t* B, const uint64_t beta, uint64_t* C,
           const QuantizerValues* const alpha_quant,
           const QuantizerValues* const a_quant,
           const QuantizerValues* const b_quant,
           const QuantizerValues* const beta_quant,
           const QuantizerValues* const c_quant);

template<>
typename std::enable_if<float_is_same<float>::value, void>::type
caffe_gemm<float>(const CBLAS_TRANSPOSE trans_A,
                  const CBLAS_TRANSPOSE trans_B, const int_tp M,
                  const int_tp N, const int_tp K, const float alpha,
                  const float* A, const float* B, const float beta,
                  float* C,
                  const QuantizerValues* const alpha_quant,
                  const QuantizerValues* const a_quant,
                  const QuantizerValues* const b_quant,
                  const QuantizerValues* const beta_quant,
                  const QuantizerValues* const c_quant) {
  int_tp lda = (trans_A == CblasNoTrans) ? K : M;
  int_tp ldb = (trans_B == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, trans_A, trans_B, M, N, K, alpha, A, lda, B, ldb,
              beta, C, N);
}

template<>
typename std::enable_if<float_is_same<float>::value, void>::type
caffe_gemm<double>(const CBLAS_TRANSPOSE trans_A,
                   const CBLAS_TRANSPOSE trans_B, const int_tp M,
                   const int_tp N, const int_tp K, const double alpha,
                   const double* A, const double* B, const double beta,
                   double* C,
                   const QuantizerValues* const alpha_quant,
                   const QuantizerValues* const a_quant,
                   const QuantizerValues* const b_quant,
                   const QuantizerValues* const beta_quant,
                   const QuantizerValues* const c_quant) {
  int_tp lda = (trans_A == CblasNoTrans) ? K : M;
  int_tp ldb = (trans_B == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, trans_A, trans_B, M, N, K, alpha, A, lda, B, ldb,
              beta, C, N);
}

}  // namespace caffe


/*======================================================================
Copyright 2019 Google LLC

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

    https://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.
======================================================================*/

#include <Eigen/Dense>

#include "Filter.h"
#include "RAHjoy.h"

RAHjoy::RAHjoy()
{ init( 0 );
}

RAHjoy::RAHjoy(const Float hz)
{ init( hz );
}

void
RAHjoy::init(const Float hz)
{ x_max_=JOY_X_MAX;
  x_min_=JOY_X_MAX;
  y_max_=JOY_Y_MAX;
  y_min_=JOY_Y_MAX;
  x_dead_=JOY_X_DED;
  y_dead_=JOY_Y_DED;
  hz_=hz;
}

RAHjoy::RAHjoy(const Float hz, int xmax, int xmin, int ymax, int ymin)
{ x_max_=xmax;
  x_min_=xmin;
  y_max_=ymax;
  y_min_=ymin;
  x_dead_=JOY_X_DED;
  y_dead_=JOY_Y_DED;
  hz_=hz;
}

void
RAHjoy::norm(float &x, float &y, int &b)
{
  int xi,yi;
  int x_sign=1;
  int y_sign=1;

  joystick_hwread(xi,yi,b);
  x =  xi;
  y = -yi; // invert y coord.

  if(x<0)
  { x=-x;
    x_sign=-1;
  }
  if(y<0)
  { y=-y;
    y_sign=-1;
  }

  x -= x_dead_;
  x /= x_max_-x_dead_;
  if( x>1.0 ) x=1.0;
  if( x<0.0 ) x=0.0;
  x *= x_sign;

  y -= y_dead_;
  y /= y_max_-y_dead_;
  if( y>1.0 ) y=1.0;
  if( y<0.0 ) y=0.0;
  y *= y_sign;

} // func joyNorm

void
RAHjoy::smooth(float &x, float &y, int &b)
{
 if( hz_ ) // ONLY USEABLE IF CONSTRUCTED WITH SERVO RATE
 {
  static int k=0;
  static Filter Fcx;
  static Eigen::Vector2d  cx = Eigen::Vector2d::Zero();
  static Eigen::Vector2d fcx = Eigen::Vector2d::Zero();

  if(k==0)
  { joystick_calibrate();
    Fcx.LowPass(cx, hz_,  5);
  }

  if((++k) > hz_/25 ) // READS AT ~25 Hz
  { k = 1;
    float jx,jy;
    norm(jx,jy,b);
    cx[0] = jx;
    cx[1] = jy;
  }

  fcx = Fcx.Filt( cx );  // DO NOT FILTER BUTTON
  x = fcx[0];
  y = fcx[1];

 } // if(hz_)
 else        // NO KNOWN SERVO RATE SO RET. ZEROs & ERR
 {
   x =  0;
   y =  0;
   b = -1;
 }

}

Vector3d
RAHjoy::joyFill_cx()
{
 if( hz_ ) // ONLY USEABLE IF CONSTRUCTED WITH SERVO RATE
 {
  static int k=-1;
  static Filter Fcx;
  static Eigen::Vector3d cx = Eigen::Vector3d::Zero(3);

  float joy_x,joy_y;
  int joy_b;

  if(k==-1)
  { joystick_calibrate();
    Fcx.LowPass(cx, hz_,  5);
  }

  if((++k)%((int)hz_/50)==0)
  {
    k=0;
    norm(joy_x,joy_y,joy_b);
    if(joy_b == 1)
    { cx[0] = MAX_JOY_V*joy_x;
      cx[1] = MAX_JOY_V*joy_y;
      cx[2] = MAX_JOY_W*0.0  ;
    }
    else if(joy_b == 2)
    { cx[0] = MAX_JOY_V*0.0  ;
      cx[1] = MAX_JOY_V*joy_y;
      cx[2] = MAX_JOY_W*(-joy_x);
    }
    else if(joy_b == 3)
    { cx[0] = MAX_JOY_V*joy_x;
      cx[1] = MAX_JOY_V*joy_y;
      cx[2] = MAX_JOY_W*(-joy_x);
    }
    else
    { cx[0] = 0.0;
      cx[1] = 0.0;
      cx[2] = 0.0;
    }
  }

  cx_f = Fcx.Filt(cx);
 } // if(hz_)
 else cx_f.setZero(3);  // NO KNOWN SERVO RATE SO RET. ZEROs

 return cx_f;
}


#ifndef __abstract_MSSMNoFV_onshell_susy_parameters_gm2calc_1_2_0_hpp__
#define __abstract_MSSMNoFV_onshell_susy_parameters_gm2calc_1_2_0_hpp__

#include <cstddef>
#include <iostream>
#include <Eigen/Core>
#include <ostream>
#include "gambit/Backends/abstractbase.hpp"
#include "forward_decls_abstract_classes.hpp"
#include "forward_decls_wrapper_classes.hpp"

#include "identification.hpp"

namespace CAT_3(BACKENDNAME,_,SAFE_VERSION)
{

   namespace gm2calc
   {
      class Abstract_MSSMNoFV_onshell_susy_parameters : public virtual AbstractBase
      {
         public:

            virtual void print(::std::basic_ostream<char, std::char_traits<char> >&) const =0;

            virtual void clear() =0;

            virtual void set_scale(double) =0;

            virtual double get_scale() const =0;

            virtual void set_Yd(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_Yd(int, int, double) =0;

            virtual void set_Ye(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_Ye(int, int, double) =0;

            virtual void set_Yu(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_Yu(int, int, double) =0;

            virtual void set_Mu(double) =0;

            virtual void set_g1(double) =0;

            virtual void set_g2(double) =0;

            virtual void set_g3(double) =0;

            virtual void set_vd(double) =0;

            virtual void set_vu(double) =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_Yd() const =0;

            virtual double get_Yd(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_Ye() const =0;

            virtual double get_Ye(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_Yu() const =0;

            virtual double get_Yu(int, int) const =0;

            virtual double get_Mu() const =0;

            virtual double get_g1() const =0;

            virtual double get_g2() const =0;

            virtual double get_g3() const =0;

            virtual double get_vd() const =0;

            virtual double get_vu() const =0;

         public:
            virtual void pointer_assign__BOSS(Abstract_MSSMNoFV_onshell_susy_parameters*) =0;
            virtual Abstract_MSSMNoFV_onshell_susy_parameters* pointer_copy__BOSS() =0;

         private:
            MSSMNoFV_onshell_susy_parameters* wptr;
            bool delete_wrapper;
         public:
            MSSMNoFV_onshell_susy_parameters* get_wptr() { return wptr; }
            void set_wptr(MSSMNoFV_onshell_susy_parameters* wptr_in) { wptr = wptr_in; }
            bool get_delete_wrapper() { return delete_wrapper; }
            void set_delete_wrapper(bool del_wrp_in) { delete_wrapper = del_wrp_in; }

         public:
            Abstract_MSSMNoFV_onshell_susy_parameters()
            {
               wptr = 0;
               delete_wrapper = false;
            }

            Abstract_MSSMNoFV_onshell_susy_parameters(const Abstract_MSSMNoFV_onshell_susy_parameters&)
            {
               wptr = 0;
               delete_wrapper = false;
            }

            Abstract_MSSMNoFV_onshell_susy_parameters& operator=(const Abstract_MSSMNoFV_onshell_susy_parameters&) { return *this; }

            virtual void init_wrapper() =0;

            MSSMNoFV_onshell_susy_parameters* get_init_wptr()
            {
               init_wrapper();
               return wptr;
            }

            MSSMNoFV_onshell_susy_parameters& get_init_wref()
            {
               init_wrapper();
               return *wptr;
            }

            virtual ~Abstract_MSSMNoFV_onshell_susy_parameters() =0;
      };
   }

}

#include "gambit/Backends/backend_undefs.hpp"

#endif /* __abstract_MSSMNoFV_onshell_susy_parameters_gm2calc_1_2_0_hpp__ */


/* -*- c++ -*- */
/*
 * Copyright 2013-2014 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include "ber_sink_b_impl.h"
#include <boost/math/special_functions/erf.hpp>
#include <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include <gnuradio/fft/fft.h>
#include <volk/volk.h>

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

namespace gr {
  namespace qtgui {

    ber_sink_b::sptr
    ber_sink_b::make(std::vector<float> esnos, int curves,
                     int ber_min_errors, float ber_limit,
                     std::vector<std::string> curvenames,
                     QWidget *parent)
    {
      return gnuradio::get_initial_sptr
        (new ber_sink_b_impl(esnos, curves,
                             ber_min_errors, ber_limit,
                             curvenames, parent));
    }

    ber_sink_b_impl::ber_sink_b_impl(std::vector<float> esnos, int curves,
                                     int ber_min_errors, float ber_limit,
                                     std::vector<std::string> curvenames,
                                     QWidget *parent)
      : block("ber_sink_b",
              io_signature::make(curves*esnos.size()*2, curves*esnos.size()*2, sizeof(unsigned char)),
              io_signature::make(0, 0, 0)),
        d_ber_min_errors(ber_min_errors),
        d_ber_limit(ber_limit),
        d_parent(parent),
        d_nconnections(esnos.size()),
        d_last_time(0)
    {
      d_main_gui = NULL;

      // Enough curves for the input streams plus the BPSK AWGN curve.
      d_curves = curves;
      d_esno_buffers.reserve(curves + 1);
      d_ber_buffers.reserve(curves + 1);
      d_total.reserve(curves * esnos.size());
      d_total_errors.reserve(curves * esnos.size());

      for(int j = 0; j < curves; j++) {
	d_esno_buffers.push_back((double*)volk_malloc(esnos.size()*sizeof(double),
                                                        volk_get_alignment()));
	d_ber_buffers.push_back((double*)volk_malloc(esnos.size()*sizeof(double),
                                                        volk_get_alignment()));

        for(int i = 0; i < d_nconnections; i++) {
          d_esno_buffers[j][i] = esnos[i];
          d_ber_buffers[j][i] = 0.0;
          d_total.push_back(0);
          d_total_errors.push_back(1);
        }
      }

      // Now add the known curves
      d_esno_buffers.push_back((double*)volk_malloc(esnos.size()*sizeof(double),
                                                      volk_get_alignment()));
      d_ber_buffers.push_back((double*)volk_malloc(esnos.size()*sizeof(double),
                                                      volk_get_alignment()));
      for(size_t i = 0; i < esnos.size(); i++) {
        double e = pow(10.0, esnos[i]/10.0);
        d_esno_buffers[curves][i] = esnos[i];
        d_ber_buffers[curves][i] = log10(0.5*boost::math::erfc(sqrt(e)));
      }

      // Initialize and set up some of the curve visual properties
      initialize();
      for(int j= 0; j < curves; j++) {
        set_line_width(j, 1);
        //35 unique styles supported
        set_line_style(j, (j%5) + 1);
        set_line_marker(j, (j%7));
      }

      if(curvenames.size() == (unsigned int)curves) {
        for(int j = 0; j < curves; j++) {
          if(curvenames[j] != "") {
            set_line_label(j, curvenames[j]);
          }
        }
      }

      set_line_label(d_curves, "BPSK AWGN");
      set_line_style(d_curves, 5);    // non-solid line
      set_line_marker(d_curves, -1);  // no marker
      set_line_alpha(d_curves, 0.25); // high transparency
    }

    ber_sink_b_impl::~ber_sink_b_impl()
    {
      if(!d_main_gui->isClosed()) {
        d_main_gui->close();
      }

      for(unsigned int i = 0; i < d_esno_buffers.size(); i++) {
	volk_free(d_esno_buffers[i]);
	volk_free(d_ber_buffers[i]);
      }
    }

    bool
    ber_sink_b_impl::check_topology(int ninputs, int noutputs)
    {
      return ninputs == (int)(d_curves * d_nconnections * 2);
    }

    void
    ber_sink_b_impl::initialize()
    {
      if(qApp != NULL) {
        d_qApplication = qApp;
      }
      else {
        int argc=0;
        char **argv = NULL;
        d_qApplication = new QApplication(argc, argv);
      }

      d_main_gui = new ConstellationDisplayForm(d_esno_buffers.size(), d_parent);

      d_main_gui->setNPoints(d_nconnections);
      d_main_gui->getPlot()->setAxisTitle(QwtPlot::yLeft, "LogScale BER");
      d_main_gui->getPlot()->setAxisTitle(QwtPlot::xBottom, "ESNO");
      // initialize update time to 10 times a second
      set_update_time(0.1);
    }

    void
    ber_sink_b_impl::exec_()
    {
      d_qApplication->exec();
    }

    QWidget*
    ber_sink_b_impl::qwidget()
    {
      return d_main_gui;
    }

#ifdef ENABLE_PYTHON
    PyObject*
    ber_sink_b_impl::pyqwidget()
    {
      PyObject *w = PyLong_FromVoidPtr((void*)d_main_gui);
      PyObject *retarg = Py_BuildValue("N", w);
      return retarg;
    }
#else
    void *
    ber_sink_b_impl::pyqwidget()
    {
      return NULL;
    }
#endif

    void
    ber_sink_b_impl::set_y_axis(double min, double max)
    {
      d_main_gui->setYaxis(min, max);
    }

    void
    ber_sink_b_impl::set_x_axis(double min, double max)
    {
      d_main_gui->setXaxis(min, max);
    }

    void
    ber_sink_b_impl::set_update_time(double t)
    {
      //convert update time to ticks
      gr::high_res_timer_type tps = gr::high_res_timer_tps();
      d_update_time = t * tps;
      d_main_gui->setUpdateTime(t);
      d_last_time = 0;
    }

    void
    ber_sink_b_impl::set_title(const std::string &title)
    {
      d_main_gui->setTitle(title.c_str());
    }

    void
    ber_sink_b_impl::set_line_label(int which, const std::string &label)
    {
      d_main_gui->setLineLabel(which, label.c_str());
    }

    void
    ber_sink_b_impl::set_line_color(int which, const std::string &color)
    {
      d_main_gui->setLineColor(which, color.c_str());
    }

    void
    ber_sink_b_impl::set_line_width(int which, int width)
    {
      d_main_gui->setLineWidth(which, width);
    }

    void
    ber_sink_b_impl::set_line_style(int which, int style)
    {
      d_main_gui->setLineStyle(which, (Qt::PenStyle)style);
    }

    void
    ber_sink_b_impl::set_line_marker(int which, int marker)
    {
      d_main_gui->setLineMarker(which, (QwtSymbol::Style)marker);
    }

    void
    ber_sink_b_impl::set_line_alpha(int which, double alpha)
    {
      d_main_gui->setMarkerAlpha(which, (int)(255.0*alpha));
    }

    void
    ber_sink_b_impl::set_size(int width, int height)
    {
      d_main_gui->resize(QSize(width, height));
    }

    std::string
    ber_sink_b_impl::title()
    {
      return d_main_gui->title().toStdString();
    }

    std::string
    ber_sink_b_impl::line_label(int which)
    {
      return d_main_gui->lineLabel(which).toStdString();
    }

    std::string
    ber_sink_b_impl::line_color(int which)
    {
      return d_main_gui->lineColor(which).toStdString();
    }

    int
    ber_sink_b_impl::line_width(int which)
    {
      return d_main_gui->lineWidth(which);
    }

    int
    ber_sink_b_impl::line_style(int which)
    {
      return d_main_gui->lineStyle(which);
    }

    int
    ber_sink_b_impl::line_marker(int which)
    {
      return d_main_gui->lineMarker(which);
    }

    double
    ber_sink_b_impl::line_alpha(int which)
    {
      return (double)(d_main_gui->markerAlpha(which))/255.0;
    }

    int
    ber_sink_b_impl::nsamps() const
    {
      return d_nconnections;
    }

    void
    ber_sink_b_impl::enable_menu(bool en)
    {
      d_main_gui->enableMenu(en);
    }

    void
    ber_sink_b_impl::enable_autoscale(bool en)
    {
      d_main_gui->autoScale(en);
    }

    int
    ber_sink_b_impl::general_work(int noutput_items,
                                  gr_vector_int& ninput_items,
                                  gr_vector_const_void_star &input_items,
                                  gr_vector_void_star &output_items)
    {
      if(gr::high_res_timer_now() - d_last_time > d_update_time) {
        d_last_time = gr::high_res_timer_now();
        d_qApplication->postEvent(d_main_gui,
                                  new ConstUpdateEvent(d_esno_buffers,
                                                       d_ber_buffers,
                                                       d_nconnections));
      }

      //check stopping condition
      int done=0, maxed=0;
      for(int j = 0; j < d_curves; ++j) {
        for(int i = 0; i < d_nconnections; ++i) {

          if(d_total_errors[j * d_nconnections + i] >= d_ber_min_errors) {
            done++;
          }
          else if(log10(((double)d_ber_min_errors)/(d_total[j * d_nconnections + i] * 8.0)) < d_ber_limit) {
            maxed++;
          }
        }
      }

      if(done+maxed == (int)(d_nconnections * d_curves)) {
        d_qApplication->postEvent(d_main_gui,
                                  new ConstUpdateEvent(d_esno_buffers,
                                                       d_ber_buffers,
                                                       d_nconnections));
        return -1;
      }

      float ber;
      for(unsigned int i = 0; i < ninput_items.size(); i += 2) {

        if((d_total_errors[i >> 1] < d_ber_min_errors) && \
           (log10(((double)d_ber_min_errors)/(d_total[i >> 1] * 8.0)) >= d_ber_limit)) {

          int items = ninput_items[i] <= ninput_items[i+1] ? ninput_items[i] : ninput_items[i+1];

          unsigned char *inbuffer0 = (unsigned char *)input_items[i];
          unsigned char *inbuffer1 = (unsigned char *)input_items[i+1];

          if(items > 0) {
            uint32_t ret;
            for(int j = 0; j < items; j++) {
              volk_32u_popcnt(&ret, static_cast<uint32_t>(inbuffer0[j]^inbuffer1[j]));
              d_total_errors[i >> 1] += ret;
            }

            d_total[i >> 1] += items;

            ber = log10(((double)d_total_errors[i >> 1])/(d_total[i >> 1] * 8.0));
            d_ber_buffers[i/(d_nconnections * 2)][(i%(d_nconnections * 2)) >> 1] = ber;

          }
          consume(i, items);
          consume(i + 1, items);

          if(d_total_errors[i >> 1] >= d_ber_min_errors) {
            GR_LOG_INFO(d_logger, boost::format("    %1% over %2%  -->  %3%") \
                        % d_total_errors[i >> 1] % (d_total[i >> 1] * 8) % ber);
          }
          else if(log10(((double)d_ber_min_errors)/(d_total[i >> 1] * 8.0)) < d_ber_limit) {
            GR_LOG_INFO(d_logger, "BER Limit Reached");
            d_ber_buffers[i/(d_nconnections * 2)][(i%(d_nconnections * 2)) >> 1] = d_ber_limit;
            d_total_errors[i >> 1] = d_ber_min_errors + 1;
          }
        }
        else {
          consume(i, ninput_items[i]);
          consume(i+1, ninput_items[i+1]);
        }
      }

      return 0;
    }

  } /* namespace qtgui */
} /* namespace gr */


//============================================================================
// Name        : duohmm.cpp
// Author      : Jared O'Connell
// Version     :
// Copyright   :
// Description :
//============================================================================

#include <iostream>
#include <algorithm>
#include <vector>
#include <iterator>
#include "hapmodule.h"
#include "pedigree.h"
#include "hmm.h"
#include "pedhap.h"
#include <boost/progress.hpp>

using namespace std;

int main(int argc,char **argv) {
  int start,stop;
  if(argc!=4) {
    cout << "Usage: duohmm hapfile famfile genetic_map --corrected output --recombination output --genotypingError output" << endl;
    return(0);
  }

  cout << "Reading haplotypes from " << argv[1] <<".haps" << endl;
  Haplotypes haps(argv[1]);

  cout << "Reading pedigree information from " << argv[2] << endl;

  pedigree p(argv[2],haps.ids);

  vector<string> idorder;
  p.orderSamples(p.pedigrees[0],idorder);
  for(int i=0;i<idorder.size();i++)
    cout << i <<": "<< idorder[i]<<endl;

  geneticMap gm(argv[3]);

  vector<double> cM;
  gm.interpolate(haps.positions,cM);
  for(int i=0;i<30;i++)
    cout << haps.positions[i] << "\t" << cM[i] << endl;

  DuoHMM duo(haps.positions,gm);
  cout << "RHO:"<<endl;
  for(int i=0;i<20;i++) {
    cout << duo.male_rho[i]<<"\t" << duo.male_norho[i]<<"\t" << duo.female_rho[i]<<"\t" << duo.female_norho[i]<<endl;
  }

  DuoHMM duo2(haps.positions,gm);

  int i1=43;
  int i2=46;
  cout << haps.ids[i1] << "\t" << haps.ids[i2] << endl;
  //checks haplotypes are initialsed
  for(int i=0;i<30;i++)
    cout << (int)haps.H[i1*2][i]  << " " << (int)haps.H[i1*2+1][i] << " " << (int)haps.H[i2*2][i] << " " << (int)haps.H[i2*2+1][i] << endl;
  cout << endl;

  start = 4850;
  for(int i=start;i<start+20;i++)
    cout << (int)haps.H[i1*2][i] << " " << (int)haps.H[i1*2+1][i] << " " << (int)haps.H[i2*2][i] << " " << (int)haps.H[i2*2+1][i] << endl;
  cout << haps.nsnp << " " << haps.positions.size()<<endl;

  cout << endl;

  cout << "Setting haps..." <<endl;
  duo2.setHaps(haps.getHap("ID305"),haps.getHap("ID089"),"2");
  duo.setHaps(haps.getHap("ID203"),haps.getHap("ID089"),"2");
  cout << "EM"<<endl;
  duo.EM(5);
  duo2.EM(5);
  cout << "viterbi" <<endl;
  duo.viterbi();
  duo2.viterbi();

  cout << "Trio HMM"<<endl;
  TrioHMM trio(haps.positions,gm);
  trio.setHaps(haps.getHap("ID305"),haps.getHap("ID203"),haps.getHap("ID089"));
  cout << "EM"<<endl;
  trio.EM(5);
  cout << "viterbi"<<endl;
  trio.viterbi();

  cout << "printing"<<endl;

  unsigned char **d = haps.getHap("ID305");
  unsigned char **m = haps.getHap("ID203");
  unsigned char **c = haps.getHap("ID089");

  start = 0;
  stop = 7960;

  for(int i=start;i<stop;i++) {

    cout << i << "\t" <<haps.positions[i] << " ";

    // for(int j=0;j<4;j++)  {
    //   cout.precision(2);
    //   cout <<  duo.posterior[j][i]<<" ";
    // }
    cout << "\t" << (int)duo2.stateseq[i]<<" "<< (int) duo.stateseq[i] << "\t";

    // for(int j=0;j<8;j++)  {
    //   cout.precision(2);
    //   cout <<  trio.posterior[j][i]<<" ";
    // }

    // for(int j=0;j<8;j++)
    //   cout <<  trio.alpha[j][i]<<" ";
    int s = trio.stateseq[i];
    cout << "\t" << s << "\t" << 2*(s/4) + s%2 << " "<< 2*((s%4)/2) + ((s%2)+1)%2 << "\t";

    cout   << (unsigned int)c[0][i] << (unsigned int)c[1][i] << " " << (unsigned int)d[0][i]  << (unsigned int)d[1][i] << " " << (unsigned int)m[0][i] << (unsigned int)m[1][i] << " " <<  endl;
  }

  return(0);

  /*
  for(int i=0;i<20;i++) {
    cout << duo.scale[i]<<endl;
    for(int j=0;j<4;j++)
      cout << duo.alpha[j][i]<<"\t";
    cout << endl;
    for(int j=0;j<4;j++)
      cout << duo.beta[j][i]<<"\t";
    cout << endl;
    for(int j=0;j<4;j++)
      cout << duo.posterior[j][i]<<"\t";
    cout << endl;
  }
  cout << endl;

  start=830;
  int stop =850;

  for(int i=start;i<stop;i++) {
    cout << duo.scale[i]<<endl;
    for(int j=0;j<4;j++)
      cout << duo.alpha[j][i]<<"\t";
    cout << endl;
    for(int j=0;j<4;j++)
      cout << duo.beta[j][i]<<"\t";
    cout << endl;
    for(int j=0;j<4;j++)
      cout << duo.posterior[j][i]<<"\t";
    cout << endl;
  }
  */

  cout << duo.rho[100] << " " << duo.rho[1000] << endl;
  cout << endl;
  start = 4850;
  for(int i=start;i<start+20;i++) {
    cout << i << "\t";
    for(int j=0;j<4;j++)
      cout << duo.posterior[j][i]<<"\t";
    cout << "\t" << duo.stateseq[i] << endl;
  }

  cout << "Making pedhap"<<endl;
  pedhap ph(argv[1],argv[2],argv[3]);
  ph.correct();

  duo.setHaps(ph.haps->getHap("ID178"),ph.haps->getHap("ID276"),"2");
  cout << "EM"<<endl;
  duo.EM(10);
  cout << "viterbi" <<endl;
  duo.viterbi();
  start = 4850;
  for(int i=start;i<start+20;i++) {
    cout << i << "\t";
    for(int j=0;j<4;j++)
      cout << duo.posterior[j][i]<<"\t";
    cout << "\t" << duo.stateseq[i] << endl;
  }
  ph.haps->writeHaps(argv[1]);
  cout <<"\nAll tests completed.\nExiting..."<<endl;  return(0);

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SIGN_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SIGN_HPP_INCLUDED

#include <boost/simd/toolbox/ieee/functions/sign.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/negate.hpp>
#include <boost/simd/include/functions/simd/shrai.hpp>
#include <boost/simd/include/functions/simd/if_one_else_zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/sdk/config.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sign_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sA0;
      return shrai(a0, sizeof(sA0)*8-1)-shrai(-a0, sizeof(sA0)*8-1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sign_, tag::cpu_,
                                    (A0)(X),
                                    ((simd_<unsigned_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sA0;
      return if_one_else_zero(a0); //shri(-a0, sizeof(sA0)*8-1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sign_, tag::cpu_,
                       (A0)(X),
                       ((simd_<floating_<A0>,X>))
                      )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      #ifdef BOOST_SIMD_NO_NANS
      return boost::simd::negate(One<A0>(),a0);
      #else
      return seladd(is_nan(a0),boost::simd::negate(One<A0>(),a0),a0);
      #endif
    }
  };
} } }
#endif


#include "bsp.h"
#include "gameobject.h"
#include "material.h"
#include "mesh.h"
#include "texture.h"

using glm::clamp;
using glm::distance;
using glm::dot;
using glm::pow;
using glm::vec2;
using glm::vec3;

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/trim.hpp>
using boost::is_any_of;
using boost::split;
using boost::trim_if;

#include <limits>
#include <regex>
#include <stdexcept>
using std::ifstream;
using std::numeric_limits;
using std::regex;
using std::runtime_error;
using std::sregex_iterator;
using std::stoi;
using std::stof;
using std::string;
using std::vector;
using std::unordered_map;

// Inches to Meters
constexpr float Worldscale = 0.0254f;

void BSP::LoadBSPFile(string filename, bool bHDR)
{
    pFile.open(filename, ifstream::in | ifstream::binary);

    if (!pFile.is_open())
    {
        throw runtime_error("Could not open file");
    }

    pFile.read(reinterpret_cast<char *>(&g_pBSPHeader), sizeof(g_pBSPHeader));

    if (g_pBSPHeader.ident != IDBSPHEADER)
    {
        throw runtime_error("Bad signature");
    }

    CopyLump(LUMP_MODELS, dmodels);
    CopyLump(bHDR ? LUMP_LIGHTING_HDR
             : LUMP_LIGHTING,
             dlightdata);
    CopyLump(LUMP_ENTITIES, dentdata);
    CopyLump(LUMP_VERTEXES, dvertexes);
    CopyLump(LUMP_TEXINFO, texinfo);
    CopyLump(LUMP_TEXDATA, dtexdata);
    CopyLump(LUMP_DISPINFO, g_dispinfo);
    CopyLump(LUMP_DISP_VERTS, g_DispVerts);
    CopyLump(bHDR ? LUMP_FACES_HDR
             : LUMP_FACES,
             dfaces);
    CopyLump(LUMP_EDGES, dedges);
    CopyLump(LUMP_SURFEDGES, dsurfedges);
    CopyLump(LUMP_TEXDATA_STRING_DATA, g_TexDataStringData);
    CopyLump(LUMP_TEXDATA_STRING_TABLE, g_TexDataStringTable);

    pFile.close();

    root = new GameObject;
    root->SetScale(vec3(Worldscale));

    ParseEntities();

    dmodels.clear();
    dlightdata.clear();
    dentdata.clear();
    dvertexes.clear();
    texinfo.clear();
    dtexdata.clear();
    g_dispinfo.clear();
    g_DispVerts.clear();
    dfaces.clear();
    dedges.clear();
    g_TexDataStringData.clear();
    g_TexDataStringTable.clear();
}

void BSP::ParseEntities()
{
    string s(dentdata.begin(), dentdata.end());

    regex r1(R"(\{[^}]*\})");
    regex r2(R"(\"[^\"]*\")");

    for (auto it = sregex_iterator(s.begin(), s.end(), r1); it != sregex_iterator(); it++)
    {
        unordered_map<string, string> data;

        auto ent = (*it).str();

        for (auto it = sregex_iterator(ent.begin(), ent.end(), r2); it != sregex_iterator(); it++)
        {
            auto key = (*it++).str();
            auto value = (*it).str();
            trim_if(key, is_any_of("\""));
            trim_if(value, is_any_of("\""));
            data[key] = value;
        }

        GameObject *object;

        if (data["classname"] == "worldspawn")
        {
            object = BuildModel(0);
        }
        else
        {
            if (data.find("model") != data.end())
            {
                if (data["model"][0] == '*')
                {
                    object = BuildModel(stoi(data["model"].substr(1)));
                }
                else
                {
                    // Studiomodel
                    object = new GameObject;
                }
            }
            else
            {
                object = new GameObject;
            }
        }

        if (data.find("origin") != data.end())
        {
            vector<string> origin;
            split(origin, data["origin"], is_any_of(" "));
            object->SetPosition(FlipVector(vec3(stof(origin[0]), stof(origin[1]), stof(origin[2]))) * Worldscale);
        }

        object->SetParent(root);
    }
}

Surface BSP::BuildFace(int index)
{
    if (dfaces[index].dispinfo != -1)
    {
        return BuildDisplacement(index);
    }

    Surface surface;
    surface.index = index;

    for (int i = 1; i < dfaces[index].numedges - 1; i++)
    {
        surface.indices.push_back(0);
        surface.indices.push_back(i);
        surface.indices.push_back(i + 1);
    }

    for (int i = dfaces[index].firstedge; i < dfaces[index].firstedge + dfaces[index].numedges; i++)
    {
        auto vertex = FlipVector(dsurfedges[i] > 0
                                 ? dvertexes[dedges[abs(dsurfedges[i])].v[0]]
                                 : dvertexes[dedges[abs(dsurfedges[i])].v[1]]);

        auto tc1 = dot(vertex, FlipVector(texinfo[dfaces[index].texinfo].textureU))
                   + texinfo[dfaces[index].texinfo].textureUOffset;
        auto tc2 = dot(vertex, FlipVector(texinfo[dfaces[index].texinfo].textureV))
                   + texinfo[dfaces[index].texinfo].textureVOffset;
        auto tc3 = dot(vertex, FlipVector(texinfo[dfaces[index].texinfo].lightmapU))
                   + texinfo[dfaces[index].texinfo].lightmapUOffset
                   + .5f
                   - dfaces[index].m_LightmapTextureMinsInLuxels[0];
        auto tc4 = dot(vertex, FlipVector(texinfo[dfaces[index].texinfo].lightmapV))
                   + texinfo[dfaces[index].texinfo].lightmapVOffset
                   + .5f
                   - dfaces[index].m_LightmapTextureMinsInLuxels[1];

        tc1 /= dtexdata[texinfo[dfaces[index].texinfo].texdata].width;
        tc2 /= dtexdata[texinfo[dfaces[index].texinfo].texdata].height;
        tc3 /= dfaces[index].m_LightmapTextureSizeInLuxels[0] + 1;
        tc4 /= dfaces[index].m_LightmapTextureSizeInLuxels[1] + 1;

        surface.vertexes.push_back(
        {
            vertex,
            vec2(tc1, tc2),
            vec2(tc3, tc4)
        });
    }

    return surface;
}

Surface BSP::BuildDisplacement(int index)
{
    Surface surface;
    surface.index = index;

    vector<vec3> vertexes;

    for (int i = dfaces[index].firstedge; i < dfaces[index].firstedge + dfaces[index].numedges; i++)
    {
        vertexes.push_back(dsurfedges[i] > 0
                           ? dvertexes[dedges[abs(dsurfedges[i])].v[0]]
                           : dvertexes[dedges[abs(dsurfedges[i])].v[1]]);
    }

    auto minDist = numeric_limits<float>::max();
    int minIndex = 0;

    for (int i = 0; i < 4; i++)
    {
        auto dist = distance(vertexes[i], g_dispinfo[dfaces[index].dispinfo].startPosition);

        if (dist < minDist)
        {
            minDist = dist;
            minIndex = i;
        }
    }

    for (int i = 0; i < minIndex; i++)
    {
        auto temp = vertexes[0];
        vertexes[0] = vertexes[1];
        vertexes[1] = vertexes[2];
        vertexes[2] = vertexes[3];
        vertexes[3] = temp;
    }

    auto leftEdge = vertexes[1] - vertexes[0];
    auto rightEdge = vertexes[2] - vertexes[3];

    auto numEdgeVertices = (1 << g_dispinfo[dfaces[index].dispinfo].power) + 1;

    auto subdivideScale = 1.f / (numEdgeVertices - 1);

    auto lightdelta = 1.f / (numEdgeVertices - 1);

    auto leftEdgeStep = leftEdge * subdivideScale;
    auto rightEdgeStep = rightEdge * subdivideScale;

    for (int i = 0; i < numEdgeVertices; i++)
    {
        auto leftEnd = leftEdgeStep * (float) i;
        leftEnd += vertexes[0];
        auto rightEnd = rightEdgeStep * (float) i;
        rightEnd += vertexes[3];

        auto leftRightSeg = rightEnd - leftEnd;
        auto leftRightStep = leftRightSeg * subdivideScale;

        for (int j = 0; j < numEdgeVertices; j++)
        {
            auto dispVertIndex = g_dispinfo[dfaces[index].dispinfo].m_iDispVertStart;
            dispVertIndex += i * numEdgeVertices + j;
            auto dispVertInfo = g_DispVerts[dispVertIndex];

            auto flatVertex = leftEnd + (leftRightStep * (float) j);

            auto dispVertex = dispVertInfo.m_vVector * dispVertInfo.m_flDist;
            dispVertex += flatVertex;

            auto tc1 = dot(flatVertex, texinfo[dfaces[index].texinfo].textureU)
                       + texinfo[dfaces[index].texinfo].textureUOffset;
            auto tc2 = dot(flatVertex, texinfo[dfaces[index].texinfo].textureV)
                       + texinfo[dfaces[index].texinfo].textureVOffset;
            auto tc3 = lightdelta * j * dfaces[index].m_LightmapTextureSizeInLuxels[0]
                       + .5f;
            auto tc4 = lightdelta * i * dfaces[index].m_LightmapTextureSizeInLuxels[1]
                       + .5f;

            tc1 /= dtexdata[texinfo[dfaces[index].texinfo].texdata].width;
            tc2 /= dtexdata[texinfo[dfaces[index].texinfo].texdata].height;
            tc3 /= dfaces[index].m_LightmapTextureSizeInLuxels[0] + 1;
            tc4 /= dfaces[index].m_LightmapTextureSizeInLuxels[1] + 1;

            surface.vertexes.push_back(
            {
                FlipVector(dispVertex),
                vec2(tc1, tc2),
                vec2(tc3, tc4)
            });
        }
    }

    for (int i = 0; i < numEdgeVertices - 1; i++)
    {
        for (int j = 0; j < numEdgeVertices - 1; j++)
        {
            auto index = i * numEdgeVertices + j;

            if (index % 2)
            {
                surface.indices.push_back(index + numEdgeVertices);
                surface.indices.push_back(index + numEdgeVertices + 1);
                surface.indices.push_back(index + 1);
                surface.indices.push_back(index + numEdgeVertices);
                surface.indices.push_back(index + 1);
                surface.indices.push_back(index);
            }
            else
            {
                surface.indices.push_back(index + numEdgeVertices + 1);
                surface.indices.push_back(index + 1);
                surface.indices.push_back(index);
                surface.indices.push_back(index + numEdgeVertices);
                surface.indices.push_back(index + numEdgeVertices + 1);
                surface.indices.push_back(index);
            }
        }
    }

    return surface;
}

GameObject *BSP::BuildModel(int index)
{
    auto model = new GameObject;

    unordered_map<int, vector<int>> dict;

    for (int i = dmodels[index].firstface; i < dmodels[index].firstface + dmodels[index].numfaces; i++)
    {
        dict[dtexdata[texinfo[dfaces[i].texinfo].texdata].nameStringTableID].push_back(i);
    }

    for (size_t i = 0; i < g_TexDataStringTable.size(); i++)
    {
        if (dict.find(i) == dict.end())
        {
            continue;
        }

        vector<uint32_t> indices;
        vector<Vertex> vertexes;
        vector<Surface> surfaces;

        for (size_t j = 0; j < dict[i].size(); j++)
        {
            if (texinfo[dfaces[dict[i][j]].texinfo].flags & (SURF_SKY | SURF_NODRAW | SURF_HINT | SURF_SKIP))
            {
                continue;
            }

            surfaces.push_back(BuildFace(dict[i][j]));
        }

        auto submesh = new GameObject;
        submesh->SetParent(model);

        auto lightmap = PackLightmaps(surfaces);

        for (const auto &surface : surfaces)
        {
            auto pointOffset = vertexes.size();

            for (size_t k = 0; k < surface.indices.size(); k++)
            {
                indices.push_back(surface.indices[k] + pointOffset);
            }

            vertexes.insert(vertexes.end(), surface.vertexes.begin(), surface.vertexes.end());
        }

        auto material = new Material;
        auto mesh = new Mesh(indices, vertexes);

        material->SetTexture("_LightmapTex", lightmap);

        submesh->AddComponent(material);
        submesh->AddComponent(mesh);
    }

    return model;
}

Texture *BSP::PackLightmaps(vector<Surface> &surfaces)
{
    vector<Texture *> lightmaps;

    for (const auto &surface : surfaces)
    {
        if (dfaces[surface.index].lightofs == -1)
        {
            continue;
        }

        auto lightmap = new Texture(dfaces[surface.index].m_LightmapTextureSizeInLuxels[0] + 1,
                                    dfaces[surface.index].m_LightmapTextureSizeInLuxels[1] + 1);
        auto color = (ColorRGBExp32 *)(dlightdata.data() + dfaces[surface.index].lightofs);

        for (uintmax_t i = 0; i < lightmap->GetArea(); i++)
        {
            lightmap->SetPixel(i, Color(clamp<int>(color[i].r * pow(2, color[i].exponent), 0, 255),
                                        clamp<int>(color[i].g * pow(2, color[i].exponent), 0, 255),
                                        clamp<int>(color[i].b * pow(2, color[i].exponent), 0, 255),
                                        255));
        }

        lightmaps.push_back(lightmap);
    }

    auto atlas = new Texture(1, 1);
    auto rc = atlas->PackTextures(lightmaps);

    for (size_t i = 0; i < lightmaps.size(); i++)
    {
        delete lightmaps[i];
    }

    for (size_t i = 0; i < surfaces.size(); i++)
    {
        if (dfaces[surfaces[i].index].lightofs == -1)
        {
            continue;
        }

        for (size_t j = 0; j < surfaces[i].vertexes.size(); j++)
        {
            auto x = surfaces[i].vertexes[j].uv2.x * rc[i].size.x + rc[i].position.x;
            auto y = surfaces[i].vertexes[j].uv2.y * rc[i].size.y + rc[i].position.y;

            x /= atlas->GetWidth();
            y /= atlas->GetHeight();

            surfaces[i].vertexes[j].uv2 = vec2(x, y);
        }
    }

    atlas->Apply(false);

    return atlas;
}

template<typename T>
void BSP::CopyLump(int lump, vector<T> &dest)
{
    dest.resize(g_pBSPHeader.lumps[lump].filelen / sizeof(T));
    pFile.seekg(g_pBSPHeader.lumps[lump].fileofs, pFile.beg);
    pFile.read(reinterpret_cast<char *>(dest.data()), g_pBSPHeader.lumps[lump].filelen);
}

vec3 BSP::FlipVector(const vec3 &v)
{
    return vec3(v.x, v.z, -v.y);
}


/*
 *          Copyright Andrey Semashev 2007 - 2015.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file   timer.cpp
 * \author Andrey Semashev
 * \date   02.12.2007
 *
 * \brief  This header is the Boost.Log library implementation, see the library documentation
 *         at http://www.boost.org/doc/libs/release/libs/log/doc/html/index.html.
 */

#include <boost/log/detail/config.hpp>
#include <boost/log/attributes/timer.hpp>
#include <boost/log/attributes/attribute_value_impl.hpp>

#if defined(BOOST_WINDOWS) && !defined(BOOST_LOG_NO_QUERY_PERFORMANCE_COUNTER)

#include <boost/assert.hpp>
#include <boost/cstdint.hpp>
#if !defined(BOOST_LOG_NO_THREADS)
#include <boost/log/detail/locks.hpp>
#include <boost/thread/mutex.hpp>
#endif
#include <windows.h>
#include <boost/log/detail/header.hpp>

namespace boost {

BOOST_LOG_OPEN_NAMESPACE

namespace attributes {

//! Factory implementation
class BOOST_SYMBOL_VISIBLE timer::impl :
    public attribute::impl
{
private:
#if !defined(BOOST_LOG_NO_THREADS)
    //! Synchronization mutex type
    typedef boost::mutex mutex_type;
    //! Synchronization mutex
    mutex_type m_Mutex;
#endif
    //! Frequency factor for calculating duration
    double m_FrequencyFactor;
    //! Last value of the performance counter
    uint64_t m_LastCounter;
    //! Elapsed time duration, in microseconds
    uint64_t m_Duration;

public:
    //! Constructor
    impl() : m_Duration(0)
    {
        LARGE_INTEGER li;
        QueryPerformanceFrequency(&li);
        BOOST_ASSERT(li.QuadPart != 0LL);
        m_FrequencyFactor = 1000000.0 / static_cast< double >(li.QuadPart);

        QueryPerformanceCounter(&li);
        m_LastCounter = static_cast< uint64_t >(li.QuadPart);
    }

    //! The method returns the actual attribute value. It must not return NULL.
    attribute_value get_value()
    {
        uint64_t duration;
        {
            BOOST_LOG_EXPR_IF_MT(log::aux::exclusive_lock_guard< mutex_type > lock(m_Mutex);)

            LARGE_INTEGER li;
            QueryPerformanceCounter(&li);
            const uint64_t counter = static_cast< uint64_t >(li.QuadPart);
            const uint64_t counts = counter - m_LastCounter;
            m_LastCounter = counter;
            duration = m_Duration + static_cast< uint64_t >(counts * m_FrequencyFactor);
            m_Duration = duration;
        }

        return attribute_value(new attribute_value_impl< value_type >(boost::posix_time::microseconds(duration)));
    }
};

//! Constructor
timer::timer() : attribute(new impl())
{
}

//! Constructor for casting support
timer::timer(cast_source const& source) : attribute(source.as< impl >())
{
}

} // namespace attributes

BOOST_LOG_CLOSE_NAMESPACE // namespace log

} // namespace boost

#include <boost/log/detail/footer.hpp>

#else // defined(BOOST_WINDOWS) && !defined(BOOST_LOG_NO_QUERY_PERFORMANCE_COUNTER)

#include <boost/log/detail/header.hpp>

namespace boost {

BOOST_LOG_OPEN_NAMESPACE

namespace attributes {

//! Factory implementation
class BOOST_SYMBOL_VISIBLE timer::impl :
    public attribute::impl
{
public:
    //! Time type
    typedef utc_time_traits::time_type time_type;

private:
    //! Base time point
    const time_type m_BaseTimePoint;

public:
    /*!
     * Constructor. Starts time counting.
     */
    impl() : m_BaseTimePoint(utc_time_traits::get_clock()) {}

    attribute_value get_value() BOOST_OVERRIDE
    {
        return attribute_value(new attribute_value_impl< value_type >(
            utc_time_traits::get_clock() - m_BaseTimePoint));
    }
};

//! Constructor
timer::timer() : attribute(new impl())
{
}

//! Constructor for casting support
timer::timer(cast_source const& source) : attribute(source.as< impl >())
{
}

} // namespace attributes

BOOST_LOG_CLOSE_NAMESPACE // namespace log

} // namespace boost

#include <boost/log/detail/footer.hpp>

#endif // defined(BOOST_WINDOWS) && !defined(BOOST_LOG_NO_QUERY_PERFORMANCE_COUNTER)


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
//#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};
class Physics;

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#include "vector_x86.hpp"
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

    /*
    void write(FILE * fp = stdout) {
        fprintf(fp, "%8d %2d %+e",  this->id, this->istar, this->mass);
        fprintf(fp, " %+e %+e %+e", this->pos[0], this->pos[1], this->pos[2]);
        fprintf(fp, " %+e %+e %+e", this->vel[0], this->vel[1], this->vel[2]);
        fprintf(fp, " %+e %+e %+e", this->uene, this->alph, this->alphu);
        fprintf(fp, " %+e", this->ksr);
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            fprintf(fp, " %+.3e", this->cmps[k]);
        }
        fprintf(fp, "\n");
    }
    */

    inline PS::F64mat calcMomentOfInertia(PS::F64vec xc) {
        PS::F64mat qq = 0.;
        PS::F64vec dx = this->pos - xc;
        PS::F64 r2 = dx * dx;
        qq.xx = this->mass * (r2 - dx[0] * dx[0]);
        qq.xy = this->mass * (   - dx[0] * dx[1]);
        qq.xz = this->mass * (   - dx[0] * dx[2]);
        qq.yy = this->mass * (r2 - dx[1] * dx[1]);
        qq.yz = this->mass * (   - dx[1] * dx[2]);
        qq.zz = this->mass * (r2 - dx[2] * dx[2]);
        return qq;
    }

    void copyFromForce(const Physics & physics);
};

class BlackHoleNeutronStarAnalysis : public BlackHoleNeutronStar {
public:
    BlackHoleNeutronStarAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->acc   = 0.;
        this->eps   = 0.;
        this->pot   = 0.;
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf",
               &this->id, &this->istar, &this->mass,
               &this->pos[0], &this->pos[1], &this->pos[2],
               &this->vel[0], &this->vel[1], &this->vel[2],
               &this->acc[0], &this->acc[1], &this->acc[2],
               &this->eps, &this->pot);
    }

    inline PS::F64mat calcMomentOfInertia(PS::F64vec xc) {
        PS::F64mat qq = 0.;
        PS::F64vec dx = this->pos - xc;
        PS::F64 r2 = dx * dx;
        qq.xx = this->mass * (r2 - dx[0] * dx[0]);
        qq.xy = this->mass * (   - dx[0] * dx[1]);
        qq.xz = this->mass * (   - dx[0] * dx[2]);
        qq.yy = this->mass * (r2 - dx[1] * dx[1]);
        qq.yz = this->mass * (   - dx[1] * dx[2]);
        qq.zz = this->mass * (r2 - dx[2] * dx[2]);
        return qq;
    }
};

class MeshData : public SPHAnalysis {
public:
    PS::F64 tempu;

    MeshData() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void writeAscii(FILE *fp) {
        fprintf(fp, "%8d", this->id);
        fprintf(fp, " %+e %+e %+e", this->pos[0], this->pos[1], this->pos[2]);
        fprintf(fp, " %+e %+e %+e", this->dens, this->temp, this->tempu);
        fprintf(fp, " %+e", this->vsnd);
        fprintf(fp, "\n");
    }

    void copyFromForce(const Physics & physics);
};

template <class Tmesh>
void generateMeshData(Tmesh & mesh) {
    if(PS::Comm::getRank() != 0) {
        return;
    }
    PS::F64 xmin = -5e8;
    PS::F64 xmax = +5e8;
    PS::S64 nx   = 100;
    PS::S64 nx3  = nx * nx * nx;
    PS::F64 dx   = (xmax - xmin) / (PS::F64)nx;
    mesh.setNumberOfParticleLocal(nx3);
    for(PS::S64 i = 0; i < nx; i++) {
        for(PS::S64 j = 0; j < nx; j++) {
            for(PS::S64 k = 0; k < nx; k++) {
                PS::S64 id = i * nx * nx + j * nx + k;
                mesh[id].id     = id;
                mesh[id].mass   = 0.;
                mesh[id].pos[0] = xmin + dx * (PS::F64)i;
                mesh[id].pos[1] = xmin + dx * (PS::F64)j;
                mesh[id].pos[2] = xmin + dx * (PS::F64)k;
                mesh[id].ksr    = 1e7;
                mesh[id].dens   = 0.;
                mesh[id].temp   = 0.;
            }
        }
    }
}

class Physics {
public:
    PS::F64 dens;
    PS::F64 temp;
    PS::F64 uene;
    void clear() {
        dens = 0.;
        temp = 0.;
        uene = 0.;
    }
};

void SPHAnalysis::copyFromForce(const Physics & physics) {
    this->dens = physics.dens;
    this->temp = physics.temp;
    this->uene = physics.uene;
}

void MeshData::copyFromForce(const Physics & physics) {
    this->dens = physics.dens;
    this->temp = physics.temp;
    this->uene = physics.uene;
}

class PhysicsEPI {
public:
    PS::S32    id;
    PS::F64    mass;
    PS::F64vec pos;
    bool       meshornot;

    void copyFromFP(const SPHAnalysis & sph) {
        this->id   = sph.id;
        this->mass = sph.mass;
        this->pos  = sph.pos;
        this->meshornot = false;
    }

    void copyFromFP(const MeshData & mesh) {
        this->id   = mesh.id;
        this->mass = mesh.mass;
        this->pos  = mesh.pos;
        this->meshornot = true;
    }

    PS::F64vec getPos() const {
        return this->pos;
    }

};

class PhysicsEPJ {
public:
    PS::S32    id;
    PS::F64    mass;
    PS::F64vec pos;
    PS::F64    ksr;
    PS::F64    hinv;
    PS::F64    dinv;
    PS::F64    temp;
    PS::F64    uene;
    bool       meshornot;

    void copyFromFP(const SPHAnalysis & sph) {
        this->id   = sph.id;
        this->mass = sph.mass;
        this->pos  = sph.pos;
        this->ksr  = sph.ksr;
        this->hinv = 1. / sph.ksr;
        this->dinv = 1. / sph.dens;
        this->temp = sph.temp;
        this->uene = sph.uene;
        this->meshornot = false;
    }

    void copyFromFP(const MeshData & mesh) {
        this->id   = mesh.id;
        this->mass = mesh.mass;
        this->pos  = mesh.pos;
        this->ksr  = mesh.ksr;
        this->hinv = 1. / mesh.ksr;
        this->dinv = 1. / mesh.dens;
        this->temp = mesh.temp;
        this->uene = mesh.uene;
        this->meshornot = true;
    }

    PS::F64vec getPos() const {
        return this->pos;
    }

    PS::F64 getRSearch() const {
        return this->ksr;
    }

    void setPos(const PS::F64vec pos_new) {
        pos = pos_new;
    }
};

struct calcPhysics {

    const PS::F64 ceff0 = +3.342253804929802286e+00;
    inline PS::F64 kernel0th(const PS::F64 r) {
        PS::F64 rmin  = ((1. - r > 0.) ? (1. - r) : 0.);
        PS::F64 rmin2 = rmin * rmin;
        return ceff0 * rmin2 * rmin2 * (1. + 4. * r);
    }

    void operator () (const PhysicsEPI * epi,
                      const PS::S32 nip,
                      const PhysicsEPJ * epj,
                      const PS::S32 njp,
                      Physics * physics) {

        for(PS::S64 i = 0; i < nip; i++) {

            if(!epi[i].meshornot) {
                continue;
            }

            PS::F64 dens = 0.;
            PS::F64 temp = 0.;
            PS::F64 uene = 0.;

            for(PS::S64 j = 0; j < njp; j++) {
                PS::F64vec dx  = epi[i].pos - epj[j].pos;
                PS::F64    r2  = dx * dx;
                PS::F64    r1  = sqrt(r2);
                PS::F64    q   = r1 * epj[j].hinv;
                PS::F64    kw0 = kernel0th(q);
                PS::F64    hi3 = epj[j].hinv * epj[j].hinv * epj[j].hinv;

                PS::F64 densi = epj[j].mass * hi3 * kw0;
                PS::F64 tempi = epj[j].mass * hi3 * kw0 * epj[j].temp * epj[j].dinv;
                PS::F64 uenei = epj[j].mass * hi3 * kw0 * epj[j].uene * epj[j].dinv;
                dens += ((epj[j].mass != 0.) ? densi : 0.);
                temp += ((epj[j].mass != 0.) ? tempi : 0.);
                uene += ((epj[j].mass != 0.) ? uenei : 0.);
            }

            physics[i].dens = dens;
            physics[i].temp = temp;
            physics[i].uene = uene;
        }

    }

};

template <class Tdinfo,
          class Tsph,
          class Tmesh,
          class Tphysics>
void calcMeshData(Tdinfo & dinfo,
                  Tsph & sph,
                  Tmesh & mesh,
                  Tphysics & physics) {
    PS::MT::init_genrand(0);
    dinfo.decomposeDomainAll(sph);
    sph.exchangeParticle(dinfo);
    mesh.exchangeParticle(dinfo);
    physics.setParticleLocalTree(mesh);
    physics.setParticleLocalTree(sph, false);
    physics.calcForceMakingTree(calcPhysics(), dinfo);
    PS::S64 nmesh = mesh.getNumberOfParticleLocal();
    for(PS::S64 i = 0; i < nmesh; i++) {
        mesh[i].copyFromForce(physics.getForce(i));
    }
    for(PS::S64 i = 0; i < nmesh; i++) {
        NR::Nucleon cmps;
        cmps[1] = cmps[2] = 0.5;
        PS::F64 dens = mesh[i].dens;
        PS::F64 uene = mesh[i].uene;
        PS::F64 temp = mesh[i].temp;
        PS::F64 tout = 0.;
        PS::F64 pout, cout, sout;
        if(dens != 0.) {
            flash_helmholtz_(&dens, &uene, &temp, cmps.getPointer(),
                             &pout, &cout, &tout, &sout);
        }
        mesh[i].tempu = tout;
        mesh[i].vsnd  = sout;
    }
}

template <class Tsph,
          class Tbhns>
void shiftCenter(Tsph & sph,
                 Tbhns & bhns) {
    PS::S64    nbhns = bhns.getNumberOfParticleLocal();
    PS::F64vec ploc  = 0.;
    if(nbhns == 1) {
        ploc = bhns[0].pos;
    } else {
        ploc = 0.;
    }
    PS::F64vec pbhns = PS::Comm::getSum(ploc);

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos -= pbhns;
    }
}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    init_flash_helmholtz_(&CodeUnit::FractionOfCoulombCorrection);

    PS::DomainInfo dinfo;
    dinfo.initialize();
    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);
    PS::ParticleSystem<BlackHoleNeutronStarAnalysis> bhns;
    bhns.initialize();
    bhns.createParticle(0);
    bhns.setNumberOfParticleLocal(0);
    PS::ParticleSystem<MeshData> mesh;
    mesh.initialize();
    mesh.createParticle(0);
    mesh.setNumberOfParticleLocal(0);
    PS::TreeForForceShort<Physics, PhysicsEPI, PhysicsEPJ>::Scatter physics;
    physics.initialize(0);

    if(PS::Comm::getRank() == 0) {
        printf("AT comments: NPROCESS %8d\n", PS::Comm::getNumberOfProc());
        printf("AT comments: NTHREAD  %8d\n", PS::Comm::getNumberOfThread());
    }

    char idir[1024], otype[1024];
    PS::S64 ibgn, iend;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", otype);
    fscanf(fp, "%lld%lld", &ibgn, &iend);
    fclose(fp);

    generateMeshData(mesh);

    for(PS::S64 itime = ibgn; itime <= iend; itime++) {
        char sfile[1024], bfile[1024];
        sprintf(sfile, "%s/sph_t%04d.dat",  idir, itime);
        sprintf(bfile, "%s/bhns_t%04d.dat", idir, itime);
        fp = fopen(sfile, "r");
        if(fp == NULL) {
            continue;
        }
        sph.readParticleAscii(sfile);
        fclose(fp);
        fp = fopen(bfile, "r");
        if(fp == NULL) {
            continue;
        }
        bhns.readParticleAscii(bfile);
        fclose(fp);

        shiftCenter(sph, bhns);
        calcMeshData(dinfo, sph, mesh, physics);

        char ofile[1024];
        sprintf(ofile, "%s_t%04d.dat", otype, itime);
        mesh.writeParticleAscii(ofile);
    }

    PS::Finalize();

    return 0;
}


//==================================================================================================
/**
  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)
**/
//==================================================================================================

#ifndef BOOST_SIMD_FUNCTION_SIMD_SQRT1PM1_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_SQRT1PM1_HPP_INCLUDED

#include <boost/simd/function/scalar/sqrt1pm1.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/sqrt1pm1.hpp>

#endif


#ifndef SLIDE_DETAILS_ALIGN_BOARD_HPP_
#define SLIDE_DETAILS_ALIGN_BOARD_HPP_

#include <vector>
#include <boost/format.hpp>

#include "../AlignBoard.hpp"
#include "util/color.hpp"
#include "util/StopWatch.hpp"

namespace slide
{

template<typename Base>
inline bool AlignBoard<Base>::extractSrcPiece(const Point src, const Point dst)
{
    if(fixed.countRowZero(src.y) == 1 || fixed.countColZero(src.x) == 1){
        rep(i, 4){
            const Direction dir(i);
            const Point tmp = src + Point::delta(dir);
            if(tmp.isIn(height(), width()) && fixed(tmp) && !isAligned(tmp)){
                fixed.reset(tmp);
                if(trajectPiece(src, dst)){
                    unfixCandidate(tmp);
                    return true;
                }
                fixed.set(tmp);
            }
        }
        return false;
    }
    rep(i, 4){
        const Direction dir(i);
        const Point tmp = src + Point::delta(dir);
        if(tmp.isIn(height(), width()) && fixed(tmp)){
            fixed.reset(tmp);
            if(trajectPiece(src, dst)){
                unfixCandidate(tmp);
                return true;
            }
            fixed.set(tmp);
        }
    }
    return false;
}

template<typename Base>
inline bool AlignBoard<Base>::moveOneAnyDirection()
{
    rep(i, 4){
        const Direction dir(i);
        const Point to = selected + Point::delta(dir);
        if(to.isIn(height(), width()) && !fixed(to)){
            move(dir);
            return true;
        }
    }
    return false;
}

template<typename Base>
bool AlignBoard<Base>::align(const Point dst)
{
    // はじめにselectedが溝にいる場合は溝から出る
    if(countAdjacentFixedCell(selected) == 3){
        moveOneAnyDirection();
    }

    const int numRemain = std::min(width() - fixed.countRow(dst.y), height() - fixed.countCol(dst.x));
    if(numRemain > 2){
        const Point src = find(correctId(dst));
        if(src == dst){
            fixCandidate(dst);
            return true;
        }

        if(trajectPiece(src, dst)){
            fixCandidate(dst);
            return true;
        }
        else{
            // srcが溝に埋まっている場合はその隣接するマスのfixedを解除して,
            // trajectPieceできるか判断し, 成功したならtrueを返す.
            if(countAdjacentFixedCell(src) == 3){
                return extractSrcPiece(src, dst);
            }
        }
    }
    else if(numRemain == 2){
        bool flag;
        Point src;
        if(fixed.countRowZero(dst.y) < fixed.countColZero(dst.x)){
            if(!Point(dst.y, dst.x+1).isIn(height(), width()) ||
                (Point(dst.y, dst.x+1).isIn(height(), width()) && fixed(dst.y, dst.x+1))){
                src = find( (dst+Point(0, -1)).toInt() );
                if( src == selected ) return false;
                if(countAdjacentFixedCell(src) == 3){
                    extractSrcPiece(src, dst);
                    src = dst;
                }
                flag = trajectPiece(src, dst);
            }
            else {
                src = find((dst+Point(0, +1)).toInt());
                if( src == selected )return false;
                if(countAdjacentFixedCell(src) == 3){
                    extractSrcPiece(src, dst);
                    src = dst;
                }
                flag = trajectPiece(src, dst);
            }
        } else {
            if(!Point(dst.y+1, dst.x).isIn(height(), width()) ||
                (Point(dst.y+1, dst.x).isIn(height(), width()) && fixed(dst.y+1, dst.x))){
                src = find((dst+Point(-1, 0)).toInt());
                if( src == selected )return false;
                if(countAdjacentFixedCell(src) == 3){
                    extractSrcPiece(src, dst);
                    src = dst;
                }
                flag = trajectPiece(src, dst);
            } else {
                src = find( (dst+Point(+1, 0)).toInt() );
                if( src == selected )return false;
                if(countAdjacentFixedCell(src) == 3){
                    extractSrcPiece(src, dst);
                    src = dst;
                }
                flag = trajectPiece(src, dst);
            }
        }
        if(flag){
            fixCandidate(dst);
            return true;
        }
    }
    else if(numRemain == 1){
        if(alignLastOne(dst)){
            fixCandidate(dst);
            return true;
        }
    }

    BOOST_ASSERT(false);
    return false;
}

template<typename Base>
bool AlignBoard<Base>::alignLastOne(Point dst)
{
    // dstの横のnot alignedなマスを探す.
    Point p(0, 0);
    rep(i, 4){
        const Direction dir(i);
        p = dst + Point::delta(dir);
        if( p.isIn(height(), width()) && fixed(p) && !isAligned(p)){
            break;
        };
    }
    // 本来入るべき番号srcを探す.
    Point src = find(p.toInt());

    if(src == dst){
        return swapAdjacentPiece(src, p);
    }

    // srcが入るtoの位置を探す.
    Point to;
    rep(i, 4){
        const Direction dir(i);
        to = p + Point::delta(dir);
        if( to.isIn(height(), width()) && !fixed(to) && to != dst){
            break;
        }
    }

    // srcが埋まっている場合はまわりのfixedを開放してとってくる.
    if(countAdjacentFixedCell(src) == 3){
        extractSrcPiece(src, to);
        src = to;
    }

    // ピースが正しく動かせる位置にある場合
    if(trajectPiece(src, to)){
        // ピースが正しく動かせても, selectedがはまってしまっている場合もある.
        fixed.set(to, true);
        moveToOptimally(dst); // selectedだけ動かす

        // 4方向を見て今いれようとしている, ブロックをさがして, その方向に進む
        rep(i, 4){
            const Direction dir(i);
            if( (dst+Point::delta(dir)).isIn(height(), width()) &&
                dst.toInt() == at(dst+Point::delta(dir))){
                move(dir);
                fixed.set(dst, true);
                dst += Point::delta(dir);
                break;
            }
        }
        rep(i, 4){
            const Direction dir(i);
            if( (dst+Point::delta(dir)).isIn(height(), width()) &&
                dst.toInt() == at(dst+Point::delta(dir))){
                move(dir);
                fixed.set(dst+Point::delta(dir), false);
                break;
            }
        }
        return true;
    }
    // ピースが動かせない場合
    else {
        // std::cout << src.toInt() << " " << at(dst) << std::endl;
        // std::cout << src << " " << to << std::endl;
        std::cout << "failed" << std::endl;
        std::cout << *this << std::endl;
        //exit(-1);
        return false;
    }
}

template<typename Base>
bool AlignBoard<Base>::swapAdjacentPiece(Point a, Point b)
{
    Direction dir(0);
    if(a.x == b.x){
        Point tmp = a + Point(0,-1);
        if(!tmp.isIn(height(), width()) || fixed(tmp) ){
            dir = Direction::Left;
        } else {
            dir = Direction::Right;
        }
        if( (int)(b.y < a.y) == (0 < (static_cast<int>(dir) & 2)) ) {
            std::swap(a, b);
        }
    } else if( a.y == b.y ){
        Point tmp = a + Point(-1,0);
        if(!tmp.isIn(height(), width()) || fixed(tmp)){
            dir = Direction::Up;
        } else {
            dir = Direction::Down;
        }
        if( (int)(b.x < a.x) == (0 < static_cast<int>(dir) )){
            std::swap(a, b);
        }
    }

    fixed.set(a, false);
    fixed.set(b, true);
    moveToOptimally(a);
    fixed.set(b, false);
    moveToOptimally(b);

    move( static_cast<Direction>((static_cast<int>(Direction::Down) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Right) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Down) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Left) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Up) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Up) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Right) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Down) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Left) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Down) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Right) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Up) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Up) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Left) + static_cast<int>(dir) ) & 0x03) );
    move( static_cast<Direction>((static_cast<int>(Direction::Down) + static_cast<int>(dir) ) & 0x03) );

    BOOST_ASSERT( isAligned(a) && isAligned(b) );
    fixed.set(a, true);
    fixed.set(b, true);
    return true;
}

template<typename Base>
std::ostream& operator<<(std::ostream& out, const AlignBoard<Base>& board)
{
    std::vector<Point> candidates;
    board.getCandidates(candidates);

    std::vector<std::vector<bool>> bb(board.height(), std::vector<bool>(board.width(), false));
    for(Point p : candidates){
        bb[p.y][p.x] = true;
    }

    rep(i, board.height()){
        rep(j, board.width()){
            out << '|';

            bool changed;
            if(bb[i][j]){
                out << util::ForeYellow;
                changed = true;
            }
            else if(!board.fixed(i, j) || !board.isAligned(i, j)){
                out << util::ForeRed;
                changed = true;
            }
            else{
                changed = false;
            }

            if(Point(i, j) == board.selected){
                out << util::BackGreen;
                changed = true;
            }

            out << boost::format("%02X") % int(board(i, j));

            if(changed){
                out << util::Default;
            }
        }
        out << "|\n";
    }

    return out;
}

} // end of namespace slide

#endif