/**
 * Copyright 2021 Huawei Technologies Co., Ltd
 *
 * 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.
 */

#include "minddata/dataset/audio/kernels/audio_utils.h"

#include <Eigen/Dense>
#include <fstream>

#include "mindspore/core/base/float16.h"
#include "minddata/dataset/core/type_id.h"
#include "minddata/dataset/kernels/data/data_utils.h"
#include "minddata/dataset/util/random.h"
#include "utils/file_utils.h"

namespace mindspore {
namespace dataset {
/// \brief Generate linearly spaced vector.
/// \param[in] start - Value of the startpoint.
/// \param[in] end - Value of the endpoint.
/// \param[in] n - N points in the output tensor.
/// \param[out] output - Tensor has n points with linearly space. The spacing between the points is (end-start)/(n-1).
/// \return Status return code.
template <typename T>
Status Linspace(std::shared_ptr<Tensor> *output, T start, T end, int n) {
  if (start > end) {
    std::string err = "Linspace: input param end must be greater than start.";
    RETURN_STATUS_UNEXPECTED(err);
  }
  n = std::isnan(n) ? 100 : n;
  TensorShape out_shape({n});
  std::vector<T> linear_vect(n);
  T interval = (n == 1) ? 0 : ((end - start) / (n - 1));
  for (auto i = 0; i < linear_vect.size(); ++i) {
    linear_vect[i] = start + i * interval;
  }
  std::shared_ptr<Tensor> out_t;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(linear_vect, out_shape, &out_t));
  linear_vect.clear();
  linear_vect.shrink_to_fit();
  *output = out_t;
  return Status::OK();
}

/// \brief Calculate complex tensor angle.
/// \param[in] input - Input tensor, must be complex, <channel, freq, time, complex=2>.
/// \param[out] output - Complex tensor angle.
/// \return Status return code.
template <typename T>
Status ComplexAngle(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output) {
  // check complex
  if (!input->IsComplex()) {
    std::string err_msg = "ComplexAngle: input tensor is not in shape of <..., 2>.";
    LOG_AND_RETURN_STATUS_SYNTAX_ERROR(err_msg);
  }
  TensorShape input_shape = input->shape();
  TensorShape out_shape({input_shape[0], input_shape[1], input_shape[2]});
  std::vector<T> phase(input_shape[0] * input_shape[1] * input_shape[2]);
  int ind = 0;

  for (auto itr = input->begin<T>(); itr != input->end<T>(); itr++, ind++) {
    auto x = (*itr);
    itr++;
    auto y = (*itr);
    phase[ind] = atan2(y, x);
  }

  std::shared_ptr<Tensor> out_t;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(phase, out_shape, &out_t));
  phase.clear();
  phase.shrink_to_fit();
  *output = out_t;
  return Status::OK();
}

/// \brief Calculate complex tensor abs.
/// \param[in] input - Input tensor, must be complex, <channel, freq, time, complex=2>.
/// \param[out] output - Complex tensor abs.
/// \return Status return code.
template <typename T>
Status ComplexAbs(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output) {
  // check complex
  if (!input->IsComplex()) {
    std::string err_msg = "ComplexAngle: input tensor is not in shape of <..., 2>.";
    LOG_AND_RETURN_STATUS_SYNTAX_ERROR(err_msg);
  }
  TensorShape input_shape = input->shape();
  TensorShape out_shape({input_shape[0], input_shape[1], input_shape[2]});
  std::vector<T> abs(input_shape[0] * input_shape[1] * input_shape[2]);
  int ind = 0;
  for (auto itr = input->begin<T>(); itr != input->end<T>(); itr++, ind++) {
    T x = (*itr);
    itr++;
    T y = (*itr);
    abs[ind] = sqrt(pow(y, 2) + pow(x, 2));
  }

  std::shared_ptr<Tensor> out_t;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(abs, out_shape, &out_t));
  *output = out_t;
  return Status::OK();
}

/// \brief Reconstruct complex tensor from norm and angle.
/// \param[in] abs - The absolute value of the complex tensor.
/// \param[in] angle - The angle of the complex tensor.
/// \param[out] output - Complex tensor, <channel, freq, time, complex=2>.
/// \return Status return code.
template <typename T>
Status Polar(const std::shared_ptr<Tensor> &abs, const std::shared_ptr<Tensor> &angle,
             std::shared_ptr<Tensor> *output) {
  // check shape
  if (abs->shape() != angle->shape()) {
    std::string err_msg = "Polar: input tensor shape of abs and angle must be the same.";
    LOG_AND_RETURN_STATUS_SYNTAX_ERROR(err_msg);
  }

  TensorShape input_shape = abs->shape();
  TensorShape out_shape({input_shape[0], input_shape[1], input_shape[2], 2});
  std::vector<T> complex_vec(input_shape[0] * input_shape[1] * input_shape[2] * 2);
  int ind = 0;
  auto itr_abs = abs->begin<T>();
  auto itr_angle = angle->begin<T>();

  for (; itr_abs != abs->end<T>(); itr_abs++, itr_angle++) {
    complex_vec[ind++] = cos(*itr_angle) * (*itr_abs);
    complex_vec[ind++] = sin(*itr_angle) * (*itr_abs);
  }

  std::shared_ptr<Tensor> out_t;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(complex_vec, out_shape, &out_t));
  *output = out_t;
  return Status::OK();
}

/// \brief Pad complex tensor.
/// \param[in] input - The complex tensor.
/// \param[in] length - The length of padding.
/// \param[in] dim - The dim index for padding.
/// \param[out] output - Complex tensor, <channel, freq, time, complex=2>.
/// \return Status return code.
template <typename T>
Status PadComplexTensor(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int length, int dim) {
  TensorShape input_shape = input->shape();
  std::vector<int64_t> pad_shape_vec = {input_shape[0], input_shape[1], input_shape[2], input_shape[3]};
  pad_shape_vec[dim] += static_cast<int64_t>(length);
  TensorShape input_shape_with_pad(pad_shape_vec);
  std::vector<T> in_vect(input_shape_with_pad[0] * input_shape_with_pad[1] * input_shape_with_pad[2] *
                         input_shape_with_pad[3]);
  auto itr_input = input->begin<T>();
  int64_t input_cnt = 0;
  /*lint -e{446} ind is modified in the body of the for loop */
  for (int ind = 0; ind < static_cast<int>(in_vect.size()); ind++) {
    in_vect[ind] = (*itr_input);
    input_cnt = (input_cnt + 1) % (input_shape[2] * input_shape[3]);
    itr_input++;
    // complex tensor last dim equals 2, fill zero count equals 2*width
    if (input_cnt == 0 && ind != 0) {
      for (int c = 0; c < length * 2; c++) {
        in_vect[++ind] = 0.0f;
      }
    }
  }
  std::shared_ptr<Tensor> out_t;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(in_vect, input_shape_with_pad, &out_t));
  *output = out_t;
  return Status::OK();
}

/// \brief Calculate phase.
/// \param[in] angle_0 - The angle.
/// \param[in] angle_1 - The angle.
/// \param[in] phase_advance - The phase advance.
/// \param[in] phase_time0 - The phase at time 0.
/// \param[out] output - Phase tensor.
/// \return Status return code.
template <typename T>
Status Phase(const std::shared_ptr<Tensor> &angle_0, const std::shared_ptr<Tensor> &angle_1,
             const std::shared_ptr<Tensor> &phase_advance, const std::shared_ptr<Tensor> &phase_time0,
             std::shared_ptr<Tensor> *output) {
  TensorShape phase_shape = angle_0->shape();
  std::vector<T> phase(phase_shape[0] * phase_shape[1] * phase_shape[2]);
  auto itr_angle_0 = angle_0->begin<T>();
  auto itr_angle_1 = angle_1->begin<T>();
  auto itr_pa = phase_advance->begin<T>();
  for (int ind = 0, input_cnt = 0; itr_angle_0 != angle_0->end<T>(); itr_angle_0++, itr_angle_1++, ind++) {
    if (ind != 0 && ind % phase_shape[2] == 0) {
      itr_pa++;
      if (itr_pa == phase_advance->end<T>()) {
        itr_pa = phase_advance->begin<T>();
      }
      input_cnt++;
    }
    phase[ind] = (*itr_angle_1) - (*itr_angle_0) - (*itr_pa);
    phase[ind] = phase[ind] - 2 * PI * round(phase[ind] / (2 * PI)) + (*itr_pa);
  }

  // concat phase time 0
  int64_t ind = 0;
  auto itr_p0 = phase_time0->begin<T>();
  (void)phase.insert(phase.begin(), (*itr_p0));
  itr_p0++;
  while (itr_p0 != phase_time0->end<T>()) {
    ind += phase_shape[2];
    phase[ind] = (*itr_p0);
    itr_p0++;
  }
  (void)phase.erase(phase.begin() + static_cast<int>(angle_0->Size()), phase.end());

  // cal phase accum
  for (ind = 0; ind < static_cast<int64_t>(phase.size()); ind++) {
    if (ind % phase_shape[2] != 0) {
      phase[ind] = phase[ind] + phase[ind - 1];
    }
  }
  std::shared_ptr<Tensor> phase_tensor;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(phase, phase_shape, &phase_tensor));
  *output = phase_tensor;
  return Status::OK();
}

/// \brief Calculate magnitude.
/// \param[in] alphas - The alphas.
/// \param[in] abs_0 - The norm.
/// \param[in] abs_1 - The norm.
/// \param[out] output - Magnitude tensor.
/// \return Status return code.
template <typename T>
Status Mag(const std::shared_ptr<Tensor> &abs_0, const std::shared_ptr<Tensor> &abs_1, std::shared_ptr<Tensor> *output,
           const std::vector<T> &alphas) {
  TensorShape mag_shape = abs_0->shape();
  std::vector<T> mag(mag_shape[0] * mag_shape[1] * mag_shape[2]);
  auto itr_abs_0 = abs_0->begin<T>();
  auto itr_abs_1 = abs_1->begin<T>();
  for (int ind = 0; itr_abs_0 != abs_0->end<T>(); itr_abs_0++, itr_abs_1++, ind++) {
    mag[ind] = alphas[ind % mag_shape[2]] * (*itr_abs_1) + (1 - alphas[ind % mag_shape[2]]) * (*itr_abs_0);
  }
  std::shared_ptr<Tensor> mag_tensor;
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(mag, mag_shape, &mag_tensor));
  *output = mag_tensor;
  return Status::OK();
}

template <typename T>
Status TimeStretch(std::shared_ptr<Tensor> input, std::shared_ptr<Tensor> *output, float rate,
                   std::shared_ptr<Tensor> phase_advance) {
  // pack <..., freq, time, complex>
  TensorShape input_shape = input->shape();
  TensorShape toShape({input->Size() / (input_shape[-1] * input_shape[-2] * input_shape[-3]), input_shape[-3],
                       input_shape[-2], input_shape[-1]});
  RETURN_IF_NOT_OK(input->Reshape(toShape));
  if (rate == 1.0) {
    *output = input;
    return Status::OK();
  }
  // calculate time step and alphas
  std::vector<dsize_t> time_steps_0, time_steps_1;
  std::vector<T> alphas;
  for (int ind = 0;; ind++) {
    auto val = ind * rate;
    if (val >= input_shape[-2]) {
      break;
    }
    int val_int = static_cast<int>(val);
    time_steps_0.push_back(val_int);
    time_steps_1.push_back(val_int + 1);
    alphas.push_back(fmod(val, 1));
  }

  // calculate phase on time 0
  std::shared_ptr<Tensor> spec_time0, phase_time0;
  RETURN_IF_NOT_OK(
    input->Slice(&spec_time0, std::vector<SliceOption>({SliceOption(true), SliceOption(true),
                                                        SliceOption(std::vector<dsize_t>{0}), SliceOption(true)})));
  RETURN_IF_NOT_OK(ComplexAngle<T>(spec_time0, &phase_time0));

  // time pad: add zero to time dim
  RETURN_IF_NOT_OK(PadComplexTensor<T>(input, &input, 2, 2));

  // slice
  std::shared_ptr<Tensor> spec_0;
  RETURN_IF_NOT_OK(input->Slice(&spec_0, std::vector<SliceOption>({SliceOption(true), SliceOption(true),
                                                                   SliceOption(time_steps_0), SliceOption(true)})));
  std::shared_ptr<Tensor> spec_1;
  RETURN_IF_NOT_OK(input->Slice(&spec_1, std::vector<SliceOption>({SliceOption(true), SliceOption(true),
                                                                   SliceOption(time_steps_1), SliceOption(true)})));

  // new slices angle and abs <channel, freq, time>
  std::shared_ptr<Tensor> angle_0, angle_1, abs_0, abs_1;
  RETURN_IF_NOT_OK(ComplexAngle<T>(spec_0, &angle_0));
  RETURN_IF_NOT_OK(ComplexAbs<T>(spec_0, &abs_0));
  RETURN_IF_NOT_OK(ComplexAngle<T>(spec_1, &angle_1));
  RETURN_IF_NOT_OK(ComplexAbs<T>(spec_1, &abs_1));

  // cal phase, there exists precision loss between mindspore and pytorch
  std::shared_ptr<Tensor> phase_tensor;
  RETURN_IF_NOT_OK(Phase<T>(angle_0, angle_1, phase_advance, phase_time0, &phase_tensor));

  // calculate magnitude
  std::shared_ptr<Tensor> mag_tensor;
  RETURN_IF_NOT_OK(Mag<T>(abs_0, abs_1, &mag_tensor, alphas));

  // reconstruct complex from norm and angle
  std::shared_ptr<Tensor> complex_spec_stretch;
  RETURN_IF_NOT_OK(Polar<T>(mag_tensor, phase_tensor, &complex_spec_stretch));

  // unpack
  auto output_shape_vec = input_shape.AsVector();
  output_shape_vec.pop_back();
  output_shape_vec.pop_back();
  output_shape_vec.push_back(complex_spec_stretch->shape()[-2]);
  output_shape_vec.push_back(input_shape[-1]);
  RETURN_IF_NOT_OK(complex_spec_stretch->Reshape(TensorShape(output_shape_vec)));
  *output = complex_spec_stretch;
  return Status::OK();
}

Status TimeStretch(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, float rate, float hop_length,
                   float n_freq) {
  std::shared_ptr<Tensor> phase_advance;
  switch (input->type().value()) {
    case DataType::DE_FLOAT32:
      RETURN_IF_NOT_OK(Linspace<float>(&phase_advance, 0, PI * hop_length, n_freq));
      RETURN_IF_NOT_OK(TimeStretch<float>(input, output, rate, phase_advance));
      break;
    case DataType::DE_FLOAT64:
      RETURN_IF_NOT_OK(Linspace<double>(&phase_advance, 0, PI * hop_length, n_freq));
      RETURN_IF_NOT_OK(TimeStretch<double>(input, output, rate, phase_advance));
      break;
    default:
      RETURN_STATUS_UNEXPECTED("TimeStretch: input tensor type should be float or double, but got: " +
                               input->type().ToString());
  }
  return Status::OK();
}

Status Dct(std::shared_ptr<Tensor> *output, int n_mfcc, int n_mels, NormMode norm) {
  TensorShape dct_shape({n_mels, n_mfcc});
  Tensor::CreateEmpty(dct_shape, DataType(DataType::DE_FLOAT32), output);
  auto iter = (*output)->begin<float>();
  float sqrt_2 = 1 / sqrt(2);
  float sqrt_2_n_mels = sqrt(2.0 / n_mels);
  for (int i = 0; i < n_mels; i++) {
    for (int j = 0; j < n_mfcc; j++) {
      // calculate temp:
      // 1. while norm = None, use 2*cos(PI*(i+0.5)*j/n_mels)
      // 2. while norm = Ortho, divide the first row by sqrt(2),
      //    then using sqrt(2.0 / n_mels)*cos(PI*(i+0.5)*j/n_mels)
      float temp = PI / n_mels * (i + 0.5) * j;
      temp = cos(temp);
      if (norm == NormMode::kOrtho) {
        if (j == 0) {
          temp *= sqrt_2;
        }
        temp *= sqrt_2_n_mels;
      } else {
        temp *= 2;
      }
      (*iter++) = temp;
    }
  }
  return Status::OK();
}

Status RandomMaskAlongAxis(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t mask_param,
                           float mask_value, int axis, std::mt19937 rnd) {
  std::uniform_int_distribution<int32_t> mask_width_value(0, mask_param);
  TensorShape input_shape = input->shape();
  int32_t mask_dim_size = axis == 1 ? input_shape[-2] : input_shape[-1];
  int32_t mask_width = mask_width_value(rnd);
  std::uniform_int_distribution<int32_t> min_freq_value(0, mask_dim_size - mask_width);
  int32_t mask_start = min_freq_value(rnd);

  return MaskAlongAxis(input, output, mask_width, mask_start, mask_value, axis);
}

Status MaskAlongAxis(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t mask_width,
                     int32_t mask_start, float mask_value, int32_t axis) {
  if (axis != 2 && axis != 1) {
    RETURN_STATUS_UNEXPECTED("MaskAlongAxis: only support Time and Frequency masking, axis should be 1 or 2.");
  }
  TensorShape input_shape = input->shape();
  // squeeze input
  TensorShape squeeze_shape = TensorShape({-1, input_shape[-2], input_shape[-1]});
  (void)input->Reshape(squeeze_shape);

  int check_dim_ind = (axis == 1) ? -2 : -1;
  CHECK_FAIL_RETURN_UNEXPECTED(0 <= mask_start && mask_start <= input_shape[check_dim_ind],
                               "MaskAlongAxis: mask_start should be less than the length of chosen dimension.");
  CHECK_FAIL_RETURN_UNEXPECTED(mask_start + mask_width <= input_shape[check_dim_ind],
                               "MaskAlongAxis: the sum of mask_start and mask_width is out of bounds.");

  int32_t cell_size = input->type().SizeInBytes();

  if (axis == 1) {
    // freq
    for (int ind = 0; ind < input->Size() / input_shape[-2] * mask_width; ind++) {
      int block_num = ind / (mask_width * input_shape[-1]);
      auto start_pos = ind % (mask_width * input_shape[-1]) + mask_start * input_shape[-1] +
                       input_shape[-1] * input_shape[-2] * block_num;
      auto start_mem_pos = const_cast<uchar *>(input->GetBuffer() + start_pos * cell_size);
      if (input->type() != DataType::DE_FLOAT64) {
        // tensor float 32
        auto mask_val = static_cast<float>(mask_value);
        CHECK_FAIL_RETURN_UNEXPECTED(memcpy_s(start_mem_pos, cell_size, &mask_val, cell_size) == 0,
                                     "MaskAlongAxis: mask failed, memory copy error.");
      } else {
        // tensor float 64
        CHECK_FAIL_RETURN_UNEXPECTED(memcpy_s(start_mem_pos, cell_size, &mask_value, cell_size) == 0,
                                     "MaskAlongAxis: mask failed, memory copy error.");
      }
    }
  } else {
    // time
    for (int ind = 0; ind < input->Size() / input_shape[-1] * mask_width; ind++) {
      int row_num = ind / mask_width;
      auto start_pos = ind % mask_width + mask_start + input_shape[-1] * row_num;
      auto start_mem_pos = const_cast<uchar *>(input->GetBuffer() + start_pos * cell_size);
      if (input->type() != DataType::DE_FLOAT64) {
        // tensor float 32
        auto mask_val = static_cast<float>(mask_value);
        CHECK_FAIL_RETURN_UNEXPECTED(memcpy_s(start_mem_pos, cell_size, &mask_val, cell_size) == 0,
                                     "MaskAlongAxis: mask failed, memory copy error.");
      } else {
        // tensor float 64
        CHECK_FAIL_RETURN_UNEXPECTED(memcpy_s(start_mem_pos, cell_size, &mask_value, cell_size) == 0,
                                     "MaskAlongAxis: mask failed, memory copy error.");
      }
    }
  }
  // unsqueeze input
  (void)input->Reshape(input_shape);
  *output = input;
  return Status::OK();
}

template <typename T>
Status Norm(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, float power) {
  // calculate the output dimension
  auto input_size = input->shape().AsVector();
  int32_t dim_back = static_cast<int32_t>(input_size.back());
  CHECK_FAIL_RETURN_UNEXPECTED(
    dim_back == 2, "ComplexNorm: expect complex input of shape <..., 2>, but got: " + std::to_string(dim_back));
  input_size.pop_back();
  TensorShape out_shape = TensorShape(input_size);
  RETURN_IF_NOT_OK(Tensor::CreateEmpty(out_shape, input->type(), output));

  // calculate norm, using: .pow(2.).sum(-1).pow(0.5 * power)
  auto itr_out = (*output)->begin<T>();
  auto itr_in = input->begin<T>();

  for (; itr_out != (*output)->end<T>(); ++itr_out) {
    auto a = static_cast<T>(*itr_in);
    ++itr_in;
    auto b = static_cast<T>(*itr_in);
    ++itr_in;
    auto res = pow(a, 2) + pow(b, 2);
    *itr_out = static_cast<T>(pow(res, (0.5 * power)));
  }

  return Status::OK();
}

Status ComplexNorm(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, float power) {
  try {
    if (input->type().value() >= DataType::DE_INT8 && input->type().value() <= DataType::DE_FLOAT16) {
      // convert the data type to float
      std::shared_ptr<Tensor> input_tensor;
      RETURN_IF_NOT_OK(TypeCast(input, &input_tensor, DataType(DataType::DE_FLOAT32)));

      RETURN_IF_NOT_OK(Norm<float>(input_tensor, output, power));
    } else if (input->type().value() == DataType::DE_FLOAT32) {
      RETURN_IF_NOT_OK(Norm<float>(input, output, power));
    } else if (input->type().value() == DataType::DE_FLOAT64) {
      RETURN_IF_NOT_OK(Norm<double>(input, output, power));
    } else {
      RETURN_STATUS_UNEXPECTED("ComplexNorm: input tensor type should be int, float or double, but got: " +
                               input->type().ToString());
    }
    return Status::OK();
  } catch (std::runtime_error &e) {
    RETURN_STATUS_UNEXPECTED("ComplexNorm: " + std::string(e.what()));
  }
}

template <typename T>
float sgn(T val) {
  return static_cast<float>(static_cast<T>(0) < val) - static_cast<float>(val < static_cast<T>(0));
}

template <typename T>
Status Decoding(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, T mu) {
  RETURN_IF_NOT_OK(Tensor::CreateEmpty(input->shape(), input->type(), output));
  auto itr_out = (*output)->begin<T>();
  auto itr = input->begin<T>();
  auto end = input->end<T>();

  while (itr != end) {
    auto x_mu = *itr;
    CHECK_FAIL_RETURN_SYNTAX_ERROR(mu != 0, "mu can not be zero.");
    x_mu = ((x_mu) / mu) * 2 - 1.0;
    x_mu = sgn(x_mu) * expm1(fabs(x_mu) * log1p(mu)) / mu;
    *itr_out = x_mu;
    ++itr_out;
    ++itr;
  }
  return Status::OK();
}

Status MuLawDecoding(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output,
                     int32_t quantization_channels) {
  if (input->type().IsInt() || input->type() == DataType(DataType::DE_FLOAT16) ||
      input->type() == DataType(DataType::DE_FLOAT32)) {
    float f_mu = static_cast<float>(quantization_channels) - 1;

    // convert the data type to float
    std::shared_ptr<Tensor> input_tensor;
    RETURN_IF_NOT_OK(TypeCast(input, &input_tensor, DataType(DataType::DE_FLOAT32)));

    RETURN_IF_NOT_OK(Decoding<float>(input_tensor, output, f_mu));
  } else if (input->type() == DataType(DataType::DE_FLOAT64)) {
    double f_mu = static_cast<double>(quantization_channels) - 1;

    RETURN_IF_NOT_OK(Decoding<double>(input, output, f_mu));
  } else {
    RETURN_STATUS_UNEXPECTED("MuLawDecoding: input tensor type should be int, float or double, but got: " +
                             input->type().ToString());
  }
  return Status::OK();
}

template <typename T>
Status Encoding(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, T mu) {
  RETURN_IF_NOT_OK(Tensor::CreateEmpty(input->shape(), DataType(DataType::DE_INT32), output));
  auto itr_out = (*output)->begin<int32_t>();
  auto itr = input->begin<T>();
  auto end = input->end<T>();

  while (itr != end) {
    auto x = *itr;
    x = sgn(x) * log1p(mu * fabs(x)) / log1p(mu);
    x = (x + 1) / 2 * mu + 0.5;
    *itr_out = static_cast<int32_t>(x);
    ++itr_out;
    ++itr;
  }
  return Status::OK();
}

Status MuLawEncoding(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output,
                     int32_t quantization_channels) {
  if (input->type().IsInt() || input->type() == DataType(DataType::DE_FLOAT16)) {
    float f_mu = static_cast<float>(quantization_channels) - 1;

    // convert the data type to float
    std::shared_ptr<Tensor> input_tensor;
    RETURN_IF_NOT_OK(TypeCast(input, &input_tensor, DataType(DataType::DE_FLOAT32)));

    RETURN_IF_NOT_OK(Encoding<float>(input_tensor, output, f_mu));
  } else if (input->type() == DataType(DataType::DE_FLOAT32)) {
    float f_mu = static_cast<float>(quantization_channels) - 1;

    RETURN_IF_NOT_OK(Encoding<float>(input, output, f_mu));
  } else if (input->type() == DataType(DataType::DE_FLOAT64)) {
    double f_mu = static_cast<double>(quantization_channels) - 1;

    RETURN_IF_NOT_OK(Encoding<double>(input, output, f_mu));
  } else {
    RETURN_STATUS_UNEXPECTED("MuLawEncoding: input tensor type should be int, float or double, but got: " +
                             input->type().ToString());
  }
  return Status::OK();
}

template <typename T>
Status FadeIn(std::shared_ptr<Tensor> *output, int32_t fade_in_len, FadeShape fade_shape) {
  T start = 0;
  T end = 1;
  RETURN_IF_NOT_OK(Linspace<T>(output, start, end, fade_in_len));
  for (auto iter = (*output)->begin<T>(); iter != (*output)->end<T>(); iter++) {
    switch (fade_shape) {
      case FadeShape::kLinear:
        break;
      case FadeShape::kExponential:
        // Compute the scale factor of the exponential function, pow(2.0, *in_ter - 1.0) * (*in_ter)
        *iter = static_cast<T>(std::pow(2.0, *iter - 1.0) * (*iter));
        break;
      case FadeShape::kLogarithmic:
        // Compute the scale factor of the logarithmic function, log(*in_iter + 0.1) + 1.0
        *iter = static_cast<T>(std::log10(*iter + 0.1) + 1.0);
        break;
      case FadeShape::kQuarterSine:
        // Compute the scale factor of the quarter_sine function, sin((*in_iter - 1.0) * PI / 2.0)
        *iter = static_cast<T>(std::sin((*iter) * PI / 2.0));
        break;
      case FadeShape::kHalfSine:
        // Compute the scale factor of the half_sine function, sin((*in_iter) * PI - PI / 2.0) / 2.0 + 0.5
        *iter = static_cast<T>(std::sin((*iter) * PI - PI / 2.0) / 2.0 + 0.5);
        break;
    }
  }
  return Status::OK();
}

template <typename T>
Status FadeOut(std::shared_ptr<Tensor> *output, int32_t fade_out_len, FadeShape fade_shape) {
  T start = 0;
  T end = 1;
  RETURN_IF_NOT_OK(Linspace<T>(output, start, end, fade_out_len));
  for (auto iter = (*output)->begin<T>(); iter != (*output)->end<T>(); iter++) {
    switch (fade_shape) {
      case FadeShape::kLinear:
        // In fade out, invert *out_iter
        *iter = static_cast<T>(1.0 - *iter);
        break;
      case FadeShape::kExponential:
        // Compute the scale factor of the exponential function
        *iter = static_cast<T>(std::pow(2.0, -*iter) * (1.0 - *iter));
        break;
      case FadeShape::kLogarithmic:
        // Compute the scale factor of the logarithmic function
        *iter = static_cast<T>(std::log10(1.1 - *iter) + 1.0);
        break;
      case FadeShape::kQuarterSine:
        // Compute the scale factor of the quarter_sine function
        *iter = static_cast<T>(std::sin((*iter) * PI / 2.0 + PI / 2.0));
        break;
      case FadeShape::kHalfSine:
        // Compute the scale factor of the half_sine function
        *iter = static_cast<T>(std::sin((*iter) * PI + PI / 2.0) / 2.0 + 0.5);
        break;
    }
  }
  return Status::OK();
}

template <typename T>
Status Fade(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t fade_in_len,
            int32_t fade_out_len, FadeShape fade_shape) {
  RETURN_IF_NOT_OK(Tensor::CreateFromTensor(input, output));
  const TensorShape input_shape = input->shape();
  int32_t waveform_length = static_cast<int32_t>(input_shape[-1]);
  CHECK_FAIL_RETURN_UNEXPECTED(fade_in_len <= waveform_length, "Fade: fade_in_len exceeds waveform length.");
  CHECK_FAIL_RETURN_UNEXPECTED(fade_out_len <= waveform_length, "Fade: fade_out_len exceeds waveform length.");
  int32_t num_waveform = static_cast<int32_t>(input->Size() / waveform_length);
  TensorShape toShape = TensorShape({num_waveform, waveform_length});
  RETURN_IF_NOT_OK((*output)->Reshape(toShape));
  TensorPtr fade_in;
  RETURN_IF_NOT_OK(FadeIn<T>(&fade_in, fade_in_len, fade_shape));
  TensorPtr fade_out;
  RETURN_IF_NOT_OK(FadeOut<T>(&fade_out, fade_out_len, fade_shape));

  // Add fade in to input tensor
  auto output_iter = (*output)->begin<T>();
  for (auto fade_in_iter = fade_in->begin<T>(); fade_in_iter != fade_in->end<T>(); fade_in_iter++) {
    *output_iter = (*output_iter) * (*fade_in_iter);
    for (int32_t j = 1; j < num_waveform; j++) {
      output_iter += waveform_length;
      *output_iter = (*output_iter) * (*fade_in_iter);
    }
    output_iter -= ((num_waveform - 1) * waveform_length);
    ++output_iter;
  }

  // Add fade out to input tensor
  output_iter = (*output)->begin<T>();
  output_iter += (waveform_length - fade_out_len);
  for (auto fade_out_iter = fade_out->begin<T>(); fade_out_iter != fade_out->end<T>(); fade_out_iter++) {
    *output_iter = (*output_iter) * (*fade_out_iter);
    for (int32_t j = 1; j < num_waveform; j++) {
      output_iter += waveform_length;
      *output_iter = (*output_iter) * (*fade_out_iter);
    }
    output_iter -= ((num_waveform - 1) * waveform_length);
    ++output_iter;
  }
  (*output)->Reshape(input_shape);
  return Status::OK();
}

Status Fade(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t fade_in_len,
            int32_t fade_out_len, FadeShape fade_shape) {
  if (DataType::DE_INT8 <= input->type().value() && input->type().value() <= DataType::DE_FLOAT32) {
    std::shared_ptr<Tensor> waveform;
    RETURN_IF_NOT_OK(TypeCast(input, &waveform, DataType(DataType::DE_FLOAT32)));
    RETURN_IF_NOT_OK(Fade<float>(waveform, output, fade_in_len, fade_out_len, fade_shape));
  } else if (input->type().value() == DataType::DE_FLOAT64) {
    RETURN_IF_NOT_OK(Fade<double>(input, output, fade_in_len, fade_out_len, fade_shape));
  } else {
    RETURN_STATUS_UNEXPECTED("Fade: input tensor type should be int, float or double, but got: " +
                             input->type().ToString());
  }
  return Status::OK();
}

Status Magphase(const TensorRow &input, TensorRow *output, float power) {
  std::shared_ptr<Tensor> mag;
  std::shared_ptr<Tensor> phase;

  RETURN_IF_NOT_OK(ComplexNorm(input[0], &mag, power));
  if (input[0]->type() == DataType(DataType::DE_FLOAT64)) {
    RETURN_IF_NOT_OK(Angle<double>(input[0], &phase));
  } else {
    std::shared_ptr<Tensor> tmp;
    RETURN_IF_NOT_OK(TypeCast(input[0], &tmp, DataType(DataType::DE_FLOAT32)));
    RETURN_IF_NOT_OK(Angle<float>(tmp, &phase));
  }
  (*output).push_back(mag);
  (*output).push_back(phase);

  return Status::OK();
}

Status MedianSmoothing(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t win_length) {
  auto channel = input->shape()[0];
  auto num_of_frames = input->shape()[1];
  // Centered windowed
  int32_t pad_length = (win_length - 1) / 2;
  int32_t out_length = num_of_frames + pad_length - win_length + 1;
  TensorShape out_shape({channel, out_length});
  std::vector<int> signal;
  std::vector<int> out;
  std::vector<int> indices(channel * (num_of_frames + pad_length), 0);
  // "replicate" padding in any dimension
  for (auto itr = input->begin<int>(); itr != input->end<int>(); ++itr) {
    signal.push_back(*itr);
  }
  for (int i = 0; i < channel; ++i) {
    for (int j = 0; j < pad_length; ++j) {
      indices[i * (num_of_frames + pad_length) + j] = signal[i * num_of_frames];
    }
  }
  for (int i = 0; i < channel; ++i) {
    for (int j = 0; j < num_of_frames; ++j) {
      indices[i * (num_of_frames + pad_length) + j + pad_length] = signal[i * num_of_frames + j];
    }
  }
  for (int i = 0; i < channel; ++i) {
    int32_t index = i * (num_of_frames + pad_length);
    for (int j = 0; j < out_length; ++j) {
      std::vector<int> tem(indices.begin() + index, indices.begin() + win_length + index);
      std::sort(tem.begin(), tem.end());
      out.push_back(tem[pad_length]);
      ++index;
    }
  }
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(out, out_shape, output));
  return Status::OK();
}

Status DetectPitchFrequency(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t sample_rate,
                            float frame_time, int32_t win_length, int32_t freq_low, int32_t freq_high) {
  std::shared_ptr<Tensor> nccf;
  std::shared_ptr<Tensor> indices;
  std::shared_ptr<Tensor> smooth_indices;
  // pack batch
  TensorShape input_shape = input->shape();
  TensorShape to_shape({input->Size() / input_shape[-1], input_shape[-1]});
  RETURN_IF_NOT_OK(input->Reshape(to_shape));
  if (input->type() == DataType(DataType::DE_FLOAT32)) {
    RETURN_IF_NOT_OK(ComputeNccf<float>(input, &nccf, sample_rate, frame_time, freq_low));
    RETURN_IF_NOT_OK(FindMaxPerFrame<float>(nccf, &indices, sample_rate, freq_high));
  } else if (input->type() == DataType(DataType::DE_FLOAT64)) {
    RETURN_IF_NOT_OK(ComputeNccf<double>(input, &nccf, sample_rate, frame_time, freq_low));
    RETURN_IF_NOT_OK(FindMaxPerFrame<double>(nccf, &indices, sample_rate, freq_high));
  } else {
    RETURN_IF_NOT_OK(ComputeNccf<float16>(input, &nccf, sample_rate, frame_time, freq_low));
    RETURN_IF_NOT_OK(FindMaxPerFrame<float16>(nccf, &indices, sample_rate, freq_high));
  }
  RETURN_IF_NOT_OK(MedianSmoothing(indices, &smooth_indices, win_length));

  // Convert indices to frequency
  constexpr double EPSILON = 1e-9;
  TensorShape freq_shape = smooth_indices->shape();
  std::vector<float> out;
  for (auto itr_fre = smooth_indices->begin<int>(); itr_fre != smooth_indices->end<int>(); ++itr_fre) {
    out.push_back(sample_rate / (EPSILON + *itr_fre));
  }

  // unpack batch
  auto shape_vec = input_shape.AsVector();
  shape_vec[shape_vec.size() - 1] = freq_shape[-1];
  TensorShape out_shape(shape_vec);
  RETURN_IF_NOT_OK(Tensor::CreateFromVector(out, out_shape, output));
  return Status::OK();
}

Status GenerateWaveTable(std::shared_ptr<Tensor> *output, const DataType &type, Modulation modulation,
                         int32_t table_size, float min, float max, float phase) {
  RETURN_UNEXPECTED_IF_NULL(output);
  int32_t phase_offset = static_cast<int32_t>(phase / PI / 2 * table_size + 0.5);
  // get the offset of the i-th
  std::vector<int32_t> point;
  for (auto i = 0; i < table_size; i++) {
    point.push_back((i + phase_offset) % table_size);
  }

  std::shared_ptr<Tensor> wave_table;
  RETURN_IF_NOT_OK(Tensor::CreateEmpty(TensorShape({table_size}), DataType(DataType::DE_FLOAT32), &wave_table));

  auto iter = wave_table->begin<float>();

  if (modulation == Modulation::kSinusoidal) {
    for (int i = 0; i < table_size; iter++, i++) {
      // change phase
      *iter = (sin(point[i] * PI / table_size * 2) + 1) / 2;
    }
  } else {
    for (int i = 0; i < table_size; iter++, i++) {
      // change phase
      *iter = point[i] * 2.0 / table_size;
      // get complete offset
      int32_t value = static_cast<int>(4 * point[i] / table_size);
      // change the value of the square wave according to the number of complete offsets
      if (value == 0) {
        *iter = *iter + 0.5;
      } else if (value == 1 || value == 2) {
        *iter = 1.5 - *iter;
      } else if (value == 3) {
        *iter = *iter - 1.5;
      }
    }
  }
  for (iter = wave_table->begin<float>(); iter != wave_table->end<float>(); iter++) {
    *iter = *iter * (max - min) + min;
  }
  if (type.IsInt()) {
    for (iter = wave_table->begin<float>(); iter != wave_table->end<float>(); iter++) {
      if (*iter < 0) {
        *iter = *iter - 0.5;
      } else {
        *iter = *iter + 0.5;
      }
    }
    RETURN_IF_NOT_OK(TypeCast(wave_table, output, DataType(DataType::DE_INT32)));
  } else if (type.IsFloat()) {
    RETURN_IF_NOT_OK(TypeCast(wave_table, output, DataType(DataType::DE_FLOAT32)));
  }

  return Status::OK();
}

Status ReadWaveFile(const std::string &wav_file_dir, std::vector<float> *waveform_vec, int32_t *sample_rate) {
  RETURN_UNEXPECTED_IF_NULL(waveform_vec);
  RETURN_UNEXPECTED_IF_NULL(sample_rate);
  auto wav_realpath = FileUtils::GetRealPath(wav_file_dir.data());
  if (!wav_realpath.has_value()) {
    MS_LOG(ERROR) << "Invalid file, get real path failed, path=" << wav_file_dir;
    RETURN_STATUS_UNEXPECTED("Invalid file, get real path failed, path=" + wav_file_dir);
  }

  const float kMaxVal = 32767.0;
  const int kDataMove = 2;
  Path file_path(wav_realpath.value());
  CHECK_FAIL_RETURN_UNEXPECTED(file_path.Exists() && !file_path.IsDirectory(),
                               "Invalid file, failed to find metadata file:" + file_path.ToString());
  std::ifstream in(file_path.ToString(), std::ios::in | std::ios::binary);
  CHECK_FAIL_RETURN_UNEXPECTED(in.is_open(), "Invalid file, failed to open metadata file:" + file_path.ToString() +
                                               ", make sure the file not damaged or permission denied.");
  WavHeader *header = new WavHeader();
  in.read(reinterpret_cast<char *>(header), sizeof(WavHeader));
  *sample_rate = header->sampleRate;
  std::unique_ptr<char[]> data = std::make_unique<char[]>(header->subChunk2Size);
  in.read(data.get(), header->subChunk2Size);
  float bytesPerSample = header->bitsPerSample / 8;
  if (bytesPerSample == 0) {
    in.close();
    delete header;
    return Status(StatusCode::kMDUnexpectedError, __LINE__, __FILE__, "ReadWaveFile: divide zero error.");
  }
  int numSamples = header->subChunk2Size / bytesPerSample;
  waveform_vec->resize(numSamples);
  for (int i = 0; i < numSamples; i++) {
    (*waveform_vec)[i] = static_cast<int16_t>(data[kDataMove * i] / kMaxVal);
  }
  in.close();
  delete header;
  return Status::OK();
}

Status ComputeCmnStartAndEnd(int32_t cmn_window, int32_t min_cmn_window, bool center, int32_t idx, int32_t num_frames,
                             int32_t *cmn_window_start_p, int32_t *cmn_window_end_p) {
  RETURN_UNEXPECTED_IF_NULL(cmn_window_start_p);
  RETURN_UNEXPECTED_IF_NULL(cmn_window_end_p);
  CHECK_FAIL_RETURN_UNEXPECTED(
    cmn_window >= 0, "SlidingWindowCmn: cmn_window must be non negative, but got: " + std::to_string(cmn_window));
  CHECK_FAIL_RETURN_UNEXPECTED(min_cmn_window >= 0, "SlidingWindowCmn: min_cmn_window must be non negative, but got: " +
                                                      std::to_string(min_cmn_window));
  int32_t cmn_window_start = 0, cmn_window_end = 0;
  constexpr int window_center = 2;
  if (center) {
    cmn_window_start = idx - cmn_window / window_center;
    cmn_window_end = cmn_window_start + cmn_window;
  } else {
    cmn_window_start = idx - cmn_window;
    cmn_window_end = idx + 1;
  }
  if (cmn_window_start < 0) {
    cmn_window_end -= cmn_window_start;
    cmn_window_start = 0;
  }
  if (!center) {
    if (cmn_window_end > idx) {
      cmn_window_end = std::max(idx + 1, min_cmn_window);
    }
  }
  if (cmn_window_end > num_frames) {
    cmn_window_start -= (cmn_window_end - num_frames);
    cmn_window_end = num_frames;
    if (cmn_window_start < 0) {
      cmn_window_start = 0;
    }
  }

  *cmn_window_start_p = cmn_window_start;
  *cmn_window_end_p = cmn_window_end;
  return Status::OK();
}

template <typename T>
Status ComputeCmnWaveform(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *cmn_waveform_p,
                          int32_t num_channels, int32_t num_frames, int32_t num_feats, int32_t cmn_window,
                          int32_t min_cmn_window, bool center, bool norm_vars) {
  using ArrayXT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  constexpr int square_num = 2;
  int32_t last_window_start = -1, last_window_end = -1;
  ArrayXT cur_sum = ArrayXT(num_channels, num_feats);
  ArrayXT cur_sum_sq;
  if (norm_vars) {
    cur_sum_sq = ArrayXT(num_channels, num_feats);
  }
  for (int i = 0; i < num_frames; ++i) {
    int32_t cmn_window_start = 0, cmn_window_end = 0;
    RETURN_IF_NOT_OK(
      ComputeCmnStartAndEnd(cmn_window, min_cmn_window, center, i, num_frames, &cmn_window_start, &cmn_window_end));
    int32_t row = cmn_window_end - cmn_window_start * 2;
    int32_t cmn_window_frames = cmn_window_end - cmn_window_start;
    for (int32_t m = 0; m < num_channels; ++m) {
      if (last_window_start == -1) {
        auto it = reinterpret_cast<T *>(const_cast<uchar *>(input->GetBuffer()));
        it += (m * num_frames * num_feats + cmn_window_start * num_feats);
        auto tmp_map = Eigen::Map<ArrayXT>(it, row, num_feats);
        if (i > 0) {
          cur_sum.row(m) += tmp_map.colwise().sum();
          if (norm_vars) {
            cur_sum_sq.row(m) += tmp_map.pow(square_num).colwise().sum();
          }
        } else {
          cur_sum.row(m) = tmp_map.colwise().sum();
          if (norm_vars) {
            cur_sum_sq.row(m) = tmp_map.pow(square_num).colwise().sum();
          }
        }
      } else {
        if (cmn_window_start > last_window_start) {
          auto it = reinterpret_cast<T *>(const_cast<uchar *>(input->GetBuffer()));
          it += (m * num_frames * num_feats + last_window_start * num_feats);
          auto tmp_map = Eigen::Map<ArrayXT>(it, 1, num_feats);
          cur_sum.row(m) -= tmp_map;
          if (norm_vars) {
            cur_sum_sq.row(m) -= tmp_map.pow(square_num);
          }
        }
        if (cmn_window_end > last_window_end) {
          auto it = reinterpret_cast<T *>(const_cast<uchar *>(input->GetBuffer()));
          it += (m * num_frames * num_feats + last_window_end * num_feats);
          auto tmp_map = Eigen::Map<ArrayXT>(it, 1, num_feats);
          cur_sum.row(m) += tmp_map;
          if (norm_vars) {
            cur_sum_sq.row(m) += tmp_map.pow(square_num);
          }
        }
      }

      auto it = reinterpret_cast<T *>(const_cast<uchar *>(input->GetBuffer()));
      auto cmn_it = reinterpret_cast<T *>(const_cast<uchar *>((*cmn_waveform_p)->GetBuffer()));
      it += (m * num_frames * num_feats + i * num_feats);
      cmn_it += (m * num_frames * num_feats + i * num_feats);
      Eigen::Map<ArrayXT>(cmn_it, 1, num_feats) =
        Eigen::Map<ArrayXT>(it, 1, num_feats) - cur_sum.row(m) / cmn_window_frames;
      if (norm_vars) {
        if (cmn_window_frames == 1) {
          auto cmn_it_1 = reinterpret_cast<T *>(const_cast<uchar *>((*cmn_waveform_p)->GetBuffer()));
          cmn_it_1 += (m * num_frames * num_feats + i * num_feats);
          Eigen::Map<ArrayXT>(cmn_it_1, 1, num_feats).setZero();
        } else {
          auto variance = (Eigen::Map<ArrayXT>(cur_sum_sq.data(), num_channels, num_feats) / cmn_window_frames) -
                          (cur_sum.pow(2) / std::pow(cmn_window_frames, 2));
          auto cmn_it_2 = reinterpret_cast<T *>(const_cast<uchar *>((*cmn_waveform_p)->GetBuffer()));
          cmn_it_2 += (m * num_frames * num_feats + i * num_feats);
          Eigen::Map<ArrayXT>(cmn_it_2, 1, num_feats) =
            Eigen::Map<ArrayXT>(cmn_it_2, 1, num_feats) * (1 / variance.sqrt()).row(m);
        }
      }
    }
    last_window_start = cmn_window_start;
    last_window_end = cmn_window_end;
  }
  return Status::OK();
}

template <typename T>
Status SlidingWindowCmnHelper(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t cmn_window,
                              int32_t min_cmn_window, bool center, bool norm_vars) {
  int32_t num_frames = input->shape()[Tensor::HandleNeg(-2, input->shape().Size())];
  int32_t num_feats = input->shape()[Tensor::HandleNeg(-1, input->shape().Size())];

  int32_t first_index = 1;
  std::vector<dsize_t> input_shape = input->shape().AsVector();
  std::for_each(input_shape.begin(), input_shape.end(), [&first_index](const dsize_t &item) { first_index *= item; });
  RETURN_IF_NOT_OK(
    input->Reshape(TensorShape({static_cast<int>(first_index / (num_frames * num_feats)), num_frames, num_feats})));

  int32_t num_channels = static_cast<int32_t>(input->shape()[0]);
  TensorPtr cmn_waveform;
  RETURN_IF_NOT_OK(
    Tensor::CreateEmpty(TensorShape({num_channels, num_frames, num_feats}), input->type(), &cmn_waveform));
  RETURN_IF_NOT_OK(ComputeCmnWaveform<T>(input, &cmn_waveform, num_channels, num_frames, num_feats, cmn_window,
                                         min_cmn_window, center, norm_vars));

  std::vector<dsize_t> re_shape = input_shape;
  auto r_it = re_shape.rbegin();
  *r_it++ = num_feats;
  *r_it = num_frames;
  RETURN_IF_NOT_OK(cmn_waveform->Reshape(TensorShape(re_shape)));

  constexpr int specify_input_shape = 2;
  constexpr int specify_first_shape = 1;
  if (input_shape.size() == specify_input_shape && cmn_waveform->shape()[0] == specify_first_shape) {
    cmn_waveform->Squeeze();
  }
  *output = cmn_waveform;
  return Status::OK();
}

Status SlidingWindowCmn(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t cmn_window,
                        int32_t min_cmn_window, bool center, bool norm_vars) {
  TensorShape input_shape = input->shape();
  CHECK_FAIL_RETURN_UNEXPECTED(input_shape.Size() >= kMinAudioRank,
                               "SlidingWindowCmn: input tensor is not in shape of <..., freq, time>.");

  if (input->type().IsNumeric() && input->type().value() != DataType::DE_FLOAT64) {
    std::shared_ptr<Tensor> temp;
    RETURN_IF_NOT_OK(TypeCast(input, &temp, DataType(DataType::DE_FLOAT32)));
    RETURN_IF_NOT_OK(SlidingWindowCmnHelper<float>(temp, output, cmn_window, min_cmn_window, center, norm_vars));
  } else if (input->type().value() == DataType::DE_FLOAT64) {
    RETURN_IF_NOT_OK(SlidingWindowCmnHelper<double>(input, output, cmn_window, min_cmn_window, center, norm_vars));
  } else {
    RETURN_STATUS_UNEXPECTED("SlidingWindowCmn: input tensor type should be int, float or double, but got: " +
                             input->type().ToString());
  }
  return Status::OK();
}

template <typename T>
Status Pad(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t pad_left, int32_t pad_right,
           BorderType padding_mode, T value = 0) {
  CHECK_FAIL_RETURN_UNEXPECTED(input->shape().Size() >= 2, "Pad: input tensor is not in shape of <..., time>.");
  CHECK_FAIL_RETURN_UNEXPECTED(
    input->type().IsNumeric(),
    "Pad: input tensor type should be int, float or double, but got: " + input->type().ToString());
  CHECK_FAIL_RETURN_UNEXPECTED(pad_left >= 0 && pad_right >= 0,
                               "Pad: left and right padding values must be non negative, but got pad_left: " +
                                 std::to_string(pad_left) + " and pad_right: " + std::to_string(pad_right));
  TensorShape input_shape = input->shape();
  int32_t wave_length = input_shape[-1];
  int32_t num_wavs = static_cast<int32_t>(input->Size() / wave_length);
  TensorShape to_shape = TensorShape({num_wavs, wave_length});
  RETURN_IF_NOT_OK(input->Reshape(to_shape));
  int32_t pad_length = wave_length + pad_left + pad_right;
  TensorShape new_shape = TensorShape({num_wavs, pad_length});
  RETURN_IF_NOT_OK(Tensor::CreateEmpty(new_shape, input->type(), output));
  using MatrixXT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using Eigen::Map;
  constexpr int pad_mul = 2;
  T *input_data = reinterpret_cast<T *>(const_cast<uchar *>(input->GetBuffer()));
  T *output_data = reinterpret_cast<T *>(const_cast<uchar *>((*output)->GetBuffer()));
  auto input_map = Map<MatrixXT>(input_data, num_wavs, wave_length);
  auto output_map = Map<MatrixXT>(output_data, num_wavs, pad_length);
  output_map.block(0, pad_left, num_wavs, wave_length) = input_map;
  if (padding_mode == BorderType::kConstant) {
    output_map.block(0, 0, num_wavs, pad_left).setConstant(value);
    output_map.block(0, pad_left + wave_length, num_wavs, pad_right).setConstant(value);
  } else if (padding_mode == BorderType::kEdge) {
    output_map.block(0, 0, num_wavs, pad_left).colwise() = input_map.col(0);
    output_map.block(0, pad_left + wave_length, num_wavs, pad_right).colwise() = input_map.col(wave_length - 1);
  } else if (padding_mode == BorderType::kReflect) {
    // First, deal with the pad operation on the right.
    int32_t current_pad = wave_length - 1;
    while (pad_right >= current_pad) {
      // current_pad: the length of pad required for current loop.
      // pad_right: the length of the remaining pad on the right.
      output_map.block(0, pad_left + current_pad + 1, num_wavs, current_pad) =
        output_map.block(0, pad_left, num_wavs, current_pad).rowwise().reverse();
      pad_right -= current_pad;
      current_pad += current_pad;
    }
    output_map.block(0, pad_length - pad_right, num_wavs, pad_right) =
      output_map.block(0, pad_length - pad_right * pad_mul - 1, num_wavs, pad_right).rowwise().reverse();
    // Next, deal with the pad operation on the left.
    current_pad = wave_length - 1;
    while (pad_left >= current_pad) {
      // current_pad: the length of pad required for current loop.
      // pad_left: the length of the remaining pad on the left.
      output_map.block(0, pad_left - current_pad, num_wavs, current_pad) =
        output_map.block(0, pad_left + 1, num_wavs, current_pad).rowwise().reverse();
      pad_left -= current_pad;
      current_pad += current_pad;
    }
    output_map.block(0, 0, num_wavs, pad_left) =
      output_map.block(0, pad_left + 1, num_wavs, pad_left).rowwise().reverse();
  } else if (padding_mode == BorderType::kSymmetric) {
    // First, deal with the pad operation on the right.
    int32_t current_pad = wave_length;
    while (pad_right >= current_pad) {
      // current_pad: the length of pad required for current loop.
      // pad_right: the length of the remaining pad on the right.
      output_map.block(0, pad_left + current_pad, num_wavs, current_pad) =
        output_map.block(0, pad_left, num_wavs, current_pad).rowwise().reverse();
      pad_right -= current_pad;
      current_pad += current_pad;
    }
    output_map.block(0, pad_length - pad_right, num_wavs, pad_right) =
      output_map.block(0, pad_length - pad_right * pad_mul, num_wavs, pad_right).rowwise().reverse();
    // Next, deal with the pad operation on the left.
    current_pad = wave_length;
    while (pad_left >= current_pad) {
      // current_pad: the length of pad required for current loop.
      // pad_left: the length of the remaining pad on the left.
      output_map.block(0, pad_left - current_pad, num_wavs, current_pad) =
        output_map.block(0, pad_left, num_wavs, current_pad).rowwise().reverse();
      pad_left -= current_pad;
      current_pad += current_pad;
    }
    output_map.block(0, 0, num_wavs, pad_left) = output_map.block(0, pad_left, num_wavs, pad_left).rowwise().reverse();
  } else {
    RETURN_STATUS_UNEXPECTED("Pad: unsupported border type.");
  }
  std::vector<dsize_t> shape_vec = input_shape.AsVector();
  shape_vec[shape_vec.size() - 1] = static_cast<dsize_t>(pad_length);
  TensorShape output_shape(shape_vec);
  RETURN_IF_NOT_OK((*output)->Reshape(output_shape));
  return Status::OK();
}

template <typename T>
Status ComputeDeltasImpl(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int all_freqs,
                         int n_frame, int n) {
  using VectorXT = Eigen::Matrix<T, Eigen::Dynamic, 1>;
  using MatrixXT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using Eigen::Map;
  int32_t denom = n * (n + 1) * (n * 2 + 1) / 3;
  // twice sum of integer squared
  VectorXT kernel = VectorXT::LinSpaced(2 * n + 1, -n, n);                         // 2n+1
  T *input_data = reinterpret_cast<T *>(const_cast<uchar *>(input->GetBuffer()));  // [all_freq,n_fram+2n]
  RETURN_IF_NOT_OK(Tensor::CreateEmpty(TensorShape{all_freqs, n_frame}, input->type(), output));
  T *output_data = reinterpret_cast<T *>(const_cast<uchar *>((*output)->GetBuffer()));
  for (int freq = 0; freq < all_freqs; ++freq) {  // conv with im2col
    auto input_map = Map<MatrixXT, 0, Eigen::OuterStride<1>>(input_data + freq * (n_frame + 2 * n), n_frame,
                                                             2 * n + 1);  // n_frmae,2n+1
    Map<VectorXT>(output_data + freq * n_frame, n_frame) = (input_map * kernel).array() / T(denom);
  }
  return Status::OK();
}

Status ComputeDeltas(const std::shared_ptr<Tensor> &input, std::shared_ptr<Tensor> *output, int32_t win_length,
                     const BorderType &mode) {
  constexpr int min_shape_dim = 2;
  auto raw_shape = input->shape();
  CHECK_FAIL_RETURN_UNEXPECTED(raw_shape.Size() >= min_shape_dim,
                               "ComputeDeltas: input tensor is not in shape of <..., freq, time>.");
  CHECK_FAIL_RETURN_UNEXPECTED(
    input->type().IsNumeric(),
    "ComputeDeltas: input tensor type should be int, float or double, but got: " + input->type().ToString());

  // reshape Tensor from <..., freq, time> to <-1, time>
  int32_t n_frames = raw_shape[-1];
  int32_t all_freqs = raw_shape.NumOfElements() / n_frames;
  RETURN_IF_NOT_OK(input->Reshape(TensorShape{all_freqs, n_frames}));

  int32_t n = (win_length - 1) / 2;

  std::shared_ptr<Tensor> specgram_local_pad;
  if (input->type() == DataType(DataType::DE_FLOAT64)) {
    RETURN_IF_NOT_OK(Pad<double>(input, &specgram_local_pad, n, n, mode));
    RETURN_IF_NOT_OK(ComputeDeltasImpl<double>(specgram_local_pad, output, all_freqs, n_frames, n));
  } else {
    std::shared_ptr<Tensor> float_tensor;
    RETURN_IF_NOT_OK(TypeCast(input, &float_tensor, DataType(DataType::DE_FLOAT32)));
    RETURN_IF_NOT_OK(Pad<float>(float_tensor, &specgram_local_pad, n, n, mode));
    RETURN_IF_NOT_OK(ComputeDeltasImpl<float>(specgram_local_pad, output, all_freqs, n_frames, n));
  }
  RETURN_IF_NOT_OK((*output)->Reshape(raw_shape));
  return Status::OK();
}
}  // namespace dataset
}  // namespace mindspore


#include <Eigen/Core>
#include <iostream>
using namespace std;
using namespace Eigen;

template<typename Derived>
const Reshaped<const Derived>
reshape_helper(const MatrixBase<Derived>& m, int rows, int cols)
{
  return Reshaped<const Derived>(m.derived(), rows, cols);
}

int main(int, char**)
{
  MatrixXd m(3, 4);
  m << 1, 4, 7, 10,
       2, 5, 8, 11,
       3, 6, 9, 12;
  cout << m << endl;
  Ref<const MatrixXd> n = reshape_helper(m, 2, 6);
  cout << "Matrix m is:" << endl << m << endl;
  cout << "Matrix n is:" << endl << n << endl;
}


// BernoulliDistribution.hpp
//
// (C) Datasim Education BV  2009

#pragma once

#include <boost/math/distributions.hpp>

using namespace System;

namespace Wrapper
{
	// Wrapper for the boost::math::bernoulli_distribution class
	// We use the .NET naming conventions instead of the original C++ name
	public ref class BernoulliDistribution
	{
	private:
		// The wrapped native class (only pointers to native classes can be a C++/CLI class datamember)
		boost::math::bernoulli_distribution<>* m_distribution;

	public:
		// Default constructor
		BernoulliDistribution();

		// Constructor with value
		BernoulliDistribution(double value);

		// Finaliser (called by garbage collector or destructor)
		!BernoulliDistribution();

		// Destructor (Dispose)
		~BernoulliDistribution();

		// Get the success fraction
		double SuccessFraction();

		// Get the native object
		boost::math::bernoulli_distribution<>* GetNative();
	};
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/Wrapper/BernoulliDistribution.hpp


#ifndef POLYNOMIAL_H
#define POLYNOMIAL_H

#include <Eigen/Dense>
#include <iosfwd>

template <unsigned D> class Polynomial
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
	Eigen::Matrix<float, D + 1, 1> c;

	static Eigen::Matrix<float, D + 1, 1> powers(const float &x);
	inline float value(const float &x) const
	{
		return c.cwiseProduct(powers(x)).sum();
	}
	Polynomial(const Eigen::Matrix2Xf &xy);
	Polynomial() = default;
	template <unsigned Deg>
	friend std::ostream &operator<<(std::ostream &os,
					const Polynomial<Deg> &p);
};

template <unsigned D> Polynomial<D>::Polynomial(const Eigen::Matrix2Xf &xy)
{
	const int N = xy.cols();
	assert(N > 0);
	Eigen::Matrix<float, Eigen::Dynamic, D + 1> X(N, D + 1);
	for (int i = 0; i < N; ++i) {
		X.row(i) = powers(xy(0, i));
	}
	c = X.householderQr().solve(xy.row(1).transpose());
	// c = X.fullPivHouseholderQr().solve(xy.row(1).transpose());
}

template <unsigned D>
std::ostream &operator<<(std::ostream &os, const Polynomial<D> &p)
{
	os << p.c[0];
	if (D > 0) {
		os << " + " << p.c[1] << " * x";
	}
	for (int i = 2; i < D + 1; ++i) {
		os << " + " << p.c[i] << " * x^" << i;
	}
	return os;
}

template <> Eigen::Matrix<float, 2, 1> Polynomial<1>::powers(const float &x)
{
	return Eigen::Matrix<float, 2, 1>(1.0f, x);
}

template <> Eigen::Matrix<float, 3, 1> Polynomial<2>::powers(const float &x)
{
	return Eigen::Matrix<float, 3, 1>(1.0f, x, x * x);
}

template <> Eigen::Matrix<float, 4, 1> Polynomial<3>::powers(const float &x)
{
	return Eigen::Matrix<float, 4, 1>(1.0f, x, x * x, x * x * x);
}

template <unsigned int D>
Eigen::Matrix<float, D + 1, 1> Polynomial<D>::powers(const float &x)
{
	Eigen::Matrix<float, D + 1, 1> vec;
	vec[0] = 1.0f;
	for (auto i = 1; i < D + 1; ++i) {
		vec[i] = vec[i - 1] * x;
	}
	return vec;
}
#endif // POLYNOMIAL_H


/**
 * @example types/test/position_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/types/types.h>
#include <iostream>

BOOST_AUTO_TEST_SUITE(position_test)

using namespace boost::unit_test;
using namespace usml::types;

/**
 * @ingroup types_test
 * @{
 */

/**
 * Compare the earth's radius at specific latitudes to known values.
 * Generate errors if values differ by more that 1E-6 percent.
 *
 * @xref WGS 84 IMPLEMENTATION MANUAL, Version 2.4, 1998.
 *       See http://www.dqts.net/wgs84.htm for more information.
 */
BOOST_AUTO_TEST_CASE( earth_radius_test ) {
    cout << "=== position_test: earth_radius_test ===" << endl;

    double radius90 = 6399593.62578493 ;
    double radius45 = 6378101.030201019 ;
    double radius00 = 6356752.314245179 ;

    wposition::compute_earth_radius(90.0);
    cout << "earth_radius at 90.0 latitude: "
         << wposition::earth_radius << endl;
    BOOST_CHECK_CLOSE( wposition::earth_radius, radius90, 1e-6 );

    wposition::compute_earth_radius(45.0);
    cout << "earth_radius at 45.0 latitude: "
         << wposition::earth_radius << endl;
    BOOST_CHECK_CLOSE( wposition::earth_radius, radius45, 1e-6 );

    wposition::compute_earth_radius(0.0);
    cout << "earth_radius at  0.0 latitude: "
         << wposition::earth_radius << endl;
    BOOST_CHECK_CLOSE( wposition::earth_radius, radius00, 1e-6 );
}

/**
 * Compute the dot product between set of vectors at different latitudes
 * and a vector pointing due north.  If correct, the dots products
 * should correspond to the angles defined by the latitudes of
 * the original points.
 *
 * Generate errors if values differ by more that 1E-6 percent.
 */
BOOST_AUTO_TEST_CASE( dot_test ) {
    cout << "=== position_test: dot_test ===" << endl;

    // define a series of lat/long points on earth

    wposition points(10);
    for ( int n=0; n < 10; ++n ) {
        points.latitude( n, 0, 10.0*n );
        points.longitude( n, 0, 90.0+10.0*n );
    }

    // dot this with vector through the north pole

    wposition1 north( 90.0, 0.0 );
    matrix<double> angle( 10, 1 );
    points.dotnorm( north, angle );
    angle = to_degrees( acos(angle) );

    // check the answer

    for ( int n=0; n < 10; ++n ) {
        double analytic = 90.0 - 10.0 * n ;
        cout << "angle=" << angle(n,0) << " analytic=" << analytic << endl;
        BOOST_CHECK_CLOSE( angle(n,0), analytic, 1e-6 );
    }
}

/**
 * Compute the dot product between a vector at different latitudes
 * and a vector pointing due north.  If correct, the dots products
 * should correspond to the angles defined by the latitudes of
 * the original points.
 *
 * Generate errors if values differ by more that 1E-6 percent.
 */
BOOST_AUTO_TEST_CASE( dot1_test ) {
    cout << "=== position_test: dot1_test ===" << endl;

    // define a series of lat/long points on earth

    wposition1 point ;
    for ( int n=0; n < 10; ++n ) {
        point.latitude( 10.0*n );
        point.longitude( 90.0+10.0*n );

        // dot this with vector through the north pole

        wposition1 north( 90.0, 0.0 );
        double angle = point.dotnorm( north );
        angle = to_degrees( acos(angle) );

        // check the answer

        double analytic = 90.0 - 10.0 * n ;
        cout << "angle=" << angle << " analytic=" << analytic << endl;
        BOOST_CHECK_CLOSE( angle, analytic, 1e-6 );
    }
}

/**
 * Compute the straight line distance between a wposition vector
 * at different latitudes and the point 40N 45E. The results are
 * compared to the analytic solution
 *

 *          distance^2 = 2 R^2 ( 1-cos(40-latitude) )
 *

 * Generate errors if values differ by more that 1E-6 meters.  The one
 * expection to this accuracy is the case where distance is almost zero.
 * In that case, the calculation accuracy is limited to 0.02 meters.
 */
BOOST_AUTO_TEST_CASE( distance_test ) {
    cout << "=== position_test: distance_test ===" << endl;

    // define a series of lat/long points on earth

    wposition points(10);
    for ( int n=0; n < 10; ++n ) {
        points.latitude( n, 0, 10.0*n );
        points.longitude( n, 0, 45.0 );
    }

    // compute the distance to some point on earth

    wposition1 origin( 40.0, 45.0 );
    matrix<double> distance( 10, 1 );
    points.distance( origin, distance );

    // check the answer
    // distance^2 = 2 R^2 ( 1-cos(angle) )

    for ( int n=0; n < 10; ++n ) {
        double analytic = wposition::earth_radius
            * sqrt( 2.0 * ( 1.0 - cos( to_radians(40.0-10.0*n) ) ) );
        cout << "distance=" << distance(n,0) << " analytic=" << analytic << endl;
        if ( analytic > 1e-6 ) {
            BOOST_CHECK_CLOSE( distance(n,0), analytic, 1e-10 );
        } else {
            BOOST_CHECK_SMALL( distance(n,0)-analytic, 0.1 );
        }
    }
}

/**
 * Compute the straight line distance between a series of wposition1 vectors
 * at different latitudes and the point 40N 45E. The results are compared
 * to the analytic solution
 *

 *          distance^2 = 2 R^2 ( 1-cos(40-latitude) )
 *

 * Generate errors if values differ by more that 1E-6 meters.  The one
 * expection to this accuracy is the case where distance is almost zero.
 * In that case, the calculation accuracy is limited to 0.02 meters.
 */
BOOST_AUTO_TEST_CASE( distance1_test ) {
    cout << "=== position_test: distance1_test ===" << endl;

    // define a series of lat/long points on earth

    wposition1 point ;
    for ( int n=0; n < 10; ++n ) {
        point.latitude( 10.0*n );
        point.longitude( 45.0 );

        // compute the distance to some point on earth

        wposition1 origin( 40.0, 45.0 );
        double distance = point.distance( origin );

        // check the answer
        // distance^2 = 2 R^2 ( 1-cos(angle) )

        double analytic = wposition::earth_radius
            * sqrt( 2.0 * ( 1.0 - cos( to_radians(40.0-10.0*n) ) ) );
        cout << "distance=" << distance << " analytic=" << analytic << endl;
        if ( analytic > 1e-6 ) {
            BOOST_CHECK_CLOSE( distance, analytic, 1e-10 );
        } else {
            BOOST_CHECK_SMALL( distance-analytic, 0.1 );
        }
    }
}

/**
 * Compute the great circle range and bearing between JFK and LAX airports.
 * Based on the "Worked Examples" in Aviation Formulary.
 * Requires an earth radius based on the definition where
 * 1 nmi = 1 min latitude = 1852.0 meters. Generate errors if values differ
 * from Williams' answers, in radians, by more that 1e-4%
 *
 * The process is then reversed to predict the location of JFK using this
 * range and bearing from LAX.  Those answers are required to be within
 * 1e-10% of the true location for JFK.
 *
 * @xref E. Williams, "Aviation Formulary V1.43",
 * http://williams.best.vwh.net/avform.htm , July 2010.
 */
BOOST_AUTO_TEST_CASE( gc_range_test ) {
    cout << "=== position_test: gc_range_test ===" << endl;
    double orig_radius = wposition::earth_radius ;
    wposition::earth_radius =  180.0/M_PI*60.0*1852.0 ; // radius used in Aviation Formulary.

    // compute range and bearing from LAX to JFK

    wposition1 jfk( 40.0 + 38.0/60.0, -(73.0 + 47.0/60.0) ) ;
    wposition1 lax( 33.0 + 57.0/60.0, -(118.0 + 24.0/60.0) ) ;

    double range, bearing ;
    range = lax.gc_range( jfk, &bearing ) ;

    cout << "LAX to JFK: range = " << (range/wposition::earth_radius) << " rad bearing = " << bearing << " rad" << endl ;
    cout << "LAX to JFK: range = " << (range/1852.0) << " nmi bearing = " << to_degrees(bearing) << " deg" << endl ;

    BOOST_CHECK_CLOSE( range/wposition::earth_radius, 0.623585, 1e-4 );
    BOOST_CHECK_CLOSE( bearing, 1.150035, 1e-4 );

    // reverse the process by computing location at this range/bearing from LAX

    wposition1 unk( lax, range, bearing ) ;
    cout << "JFK: " << jfk.latitude() << "N " << -jfk.longitude() << "W" << endl ;
    cout << "UNK: " << unk.latitude() << "N " << -unk.longitude() << "W" << endl ;

    BOOST_CHECK_CLOSE( unk.latitude(), jfk.latitude(), 1e-10 );
    BOOST_CHECK_CLOSE( unk.longitude(), jfk.longitude(), 1e-10 );

    wposition::earth_radius = orig_radius ;
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


#ifndef POLYNOMIAL_H_
#define POLYNOMIAL_H_

#include <Eigen/Core>
#include <Eigen/QR>

using Eigen::VectorXd;
using Eigen::MatrixXd;

// Evaluate a polynomial.
double polyeval(const VectorXd & coeffs, double x);
// Fit a polynomial.
VectorXd polyfit(const VectorXd & xvals, const VectorXd & yvals, int order);

#endif


/**
 * @file qfeinterpolator.cc
 * @brief NPDE homework DebuggingFEM code
 * @author Simon Meierhans
 * @date 27/03/2019
 * @copyright Developed at ETH Zurich
 */

#include "qfeinterpolator.h"

#include <lf/mesh/mesh.h>

#include <Eigen/Core>

namespace DebuggingFEM {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d globalCoordinate(int idx, const lf::mesh::Entity &cell) {
  // Consistency check for arguments
  LF_ASSERT_MSG(cell.RefEl() == lf::base::RefEl::kTria(),
                "Implemented for triangles only");
  // Fetch pointer to asscoiated geometry object
  lf::geometry::Geometry *geom = cell.Geometry();
  // For returning the global coordinates of the interpolation node
  Eigen::Vector2d result;
  // Reference coordinates of the vertices of the triangle
  Eigen::Matrix<double, 2, 3> corners(2, 3);
  corners << 0., 1., 0., 0., 0., 1.;
  switch (idx) {
    case (0):
      result = geom->Global(corners.col(0));
      break;
    case (1):
      result = geom->Global(corners.col(1));
      break;
    case (2):
      result = geom->Global(corners.col(2));
      break;
    case (3):
      result = geom->Global((corners.col(0) + corners.col(1)) / 2.);
      break;
    case (4):
      result = geom->Global((corners.col(1) + corners.col(2)) / 2.);
      break;
    case (5):
      result = geom->Global((corners.col(2) + corners.col(0)) / 2.);
      break;
    default:
      throw std::invalid_argument("idx needs to be in range [0,5]");
  }
  return result;
}
/* SAM_LISTING_END_1 */

}  // namespace DebuggingFEM


//------------------------------------------------------------------------------
// \file BitsStream_test.cpp
//------------------------------------------------------------------------------
#include "Utilities/BitsStream.h"

#include "Cpp/Utilities/SuperBitSet.h"

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <limits>
#include <string>
// https://stackoverflow.com/questions/20731/how-do-you-clear-a-stringstream-variable

using Cpp::Utilities::SuperBitSet;
using Utilities::BitsStream;

BOOST_AUTO_TEST_SUITE(Utilities)
BOOST_AUTO_TEST_SUITE(BitsStream_tests)

// cf. https://stackoverflow.com/questions/34588650/uint128-t-does-not-name-a-type
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(__Uint128_tExists)
{
  BOOST_TEST(sizeof(__uint128_t) == 16);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateMinus0In1sComplement)
{
  SuperBitSet<16> bits {BitsStream::minus_0_1s_complement};
  BOOST_TEST(bits.to_string() == "1111111111111111");
  BOOST_TEST(bits.to_ulong() == 65535);
}

BOOST_AUTO_TEST_SUITE_END() // BitsStream_tests
BOOST_AUTO_TEST_SUITE_END() // Utilities


// Compile command: g++ -Wall -Wextra -std=c++17 -O2 -pthread -I/usr/include/eigen3 -I/usr/include/python3.8 -o triangular_distorted triangular_distorted.cpp -lpython3.8
#include <iostream>
#include <fstream>
#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <vector>
#include <utility>
#include <functional>
#include <thread>
#include <map>
#include <string>
#include <algorithm>
#include <ctime>
#include <iomanip>
#include <filesystem>
#include <Python.h>
#include "matplotlibcpp.h"
#include "tinycolormap.hpp"

namespace plt = matplotlibcpp;
using Eigen::MatrixXd;
using Eigen::Matrix2cd;
using Eigen::Vector2cd;
using Eigen::Vector3f;
using std::vector;
using std::sqrt;
using std::cos;
using std::acos;
using std::sin;
using namespace std::complex_literals;

typedef std::complex<double> cd;
typedef vector<vector<vector<cd>>> grid_t;
typedef std::function<Matrix2cd(int, double, double)> gen_coin_t;

// Settings here: epsilon and size (the factor before /sqrt(eps))
// Other settings:
double eps = 0.001;
int num_steps = 10000;
int deformation = 0;
int initialState = 0;
vector<std::string> initialStateName = {"center", "vertical", "center-side0", "center-side1", "center-side2", "shift-ur", "shift-ur3", "sinx", "siny", "square"};
double dy = sqrt(3);
bool showVectField = false; // Set to true to show the vector field
bool forceSphere = false;
int xspan = (int)(3/sqrt(eps));
//int xspan = 200;
int yspan = (int)(xspan/dy);
int ntriangles_x = 2*xspan;
int ntriangles_y = 2*yspan;
int center[2] = {xspan, yspan};

std::time_t now = time(0);
std::tm *ltm = localtime(&now);

std::string prefix;

grid_t zerogrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, 0. + 0i)));
}

grid_t grid = zerogrid();

double sumAmplitudes() {
    double ret = 0.0;
    for(int x = -xspan; x < xspan; x++)
        for(int y = -yspan; y < yspan; y++)
            for(int side = 0; side < 3; side++) {
                cd val = grid[x+center[0]][y+center[1]][side];
                ret += std::real(val*std::conj(val));
            }
    return ret;
}

void normalizeGrid() {
    double target = sumAmplitudes();
    double mul = 1/sqrt(target);
    if(target == 0)
        return;
    for(int x = -xspan; x < xspan; x++)
        for(int y = -yspan; y < yspan; y++)
            for(int side = 0; side < 3; side++)
                grid[x+center[0]][y+center[1]][side] *= mul;
}

Matrix2cd H, Q;

inline int modulo(int a, int b) {
    return (a%b+b)%b;
}

void init_HQ() {
    H << 1, 1, 1, -1;
    H /= sqrt(2);
    Q << 1, -1i, 1, 1i;
    Q /= sqrt(2);
}

Matrix2cd gen_Ui(double thetai) {
    Matrix2cd ret;
    ret <<
        cos(thetai/2), sin(thetai/2),
        -sin(thetai/2), cos(thetai/2);
    return ret;
}

inline int sign(double d) {
    if(d>=0)
        return 1;
    else
        return -1;
}

inline double sq(double d) {
    return d*d;
}

vector<std::string> deform_name = {"ident", "conic", "3fold", "3fold-x", "3fold-y", "sphere-singul", "sphere-nosingul", "cone", "zeroy"};

Matrix2cd lamb(double rx, double ry) {
    Matrix2cd ret;
    // Identité
    if(deformation == 0) {
        ret <<
            1, 0,
            0, 1;
    }
    // Conique : pas de moyen simple d'éviter la singularité
    else if(deformation == 1) {
        if(rx == 0) {
            if(ry >= 0) {
                ret <<
                    -1, 0,
                    0, 1/sqrt(1+4*ry*ry);
            }
            else {
                ret <<
                    1, 0,
                    0, 1/sqrt(1+4*ry*ry);
            }
        }
        else {
            ret <<
                -sign(rx)*ry/rx/sqrt(1+sq(ry/rx)),
                sign(rx)/sqrt(1+sq(2*rx+2*ry*ry/rx)+sq(ry/rx)),
                sign(rx)/sqrt(1+sq(ry/rx)),
                sign(rx)*ry/rx/sqrt(1+sq(2*rx+2*ry*ry/rx)+sq(ry/rx));
        }
    }
    else if(deformation == 2) { // 10-fold expansion of space
        ret <<
            3, 0,
            0, 3;
    }
    else if(deformation == 3) { // 3-fold expansion of space in x direction
        ret <<
            3, 0,
            0, 1;
    }
    else if(deformation == 4) { // 3-fold expansion of space in y direction
        ret <<
            1, 0,
            0, 3;
    }
    // Sphérique
    else if(deformation == 5) { // Sphérique, coordonnées tournantes (singularité en (0,0))
        if(rx == 0 && ry == 0) {
            ret <<
                1, 0,
                0, 1;
        }
        else {
            Vector2cd er, retheta;
            double norm = sqrt(rx*rx+ry*ry);
            double phi = acos(1-4/(norm*norm/2+2));
            er << rx/norm, ry/norm;
            retheta << -ry, rx;
            Vector2cd vect1 = 1/sin(phi)*retheta;
            Vector2cd vect2 = sq(norm*norm/2+2)*sin(phi)/(4*norm)*er;
            ret <<
                vect1(0), vect1(1),
                vect2(0), vect2(1);
        }
    }
    else if(deformation == 6) { // Sphère, sans singularité
        double den = sq(rx*rx+ry*ry+4);
        Vector3f partialx(
            4*(-rx*rx+ry*ry+4)/den,
            -8*rx*ry/den,
            16*rx/den
        );
        Vector3f partialy(
            -8*rx*ry/den,
            4*(rx*rx-ry*ry+4)/den,
            16*ry/den
        );
        ret <<
            1/partialx.norm(), 0,
            0, 1/partialy.norm();
    }
    else if(deformation == 7) {
        double xi = sqrt(rx*rx+ry*ry);
        if(xi <= 1e-10)
            return Matrix2cd::Identity();
        double phi = atan2(ry, rx);
        const double a = 1.0, c = 1.0;
        Vector3f partial_xi(
            cos(phi),
            sin(phi),
            c*xi/a/a/sqrt(1+sq(xi)/sq(a)));
        Vector3f partial_phi(
            -sin(phi),
            cos(phi),
            0
        );
        Vector2cd exi(cos(phi), sin(phi));
        Vector2cd ephi(-sin(phi), cos(phi));
        Vector2cd vect1 = exi/partial_xi.norm(), vect2 = ephi/partial_phi.norm();
        ret <<
            vect1(0), vect2(0),
            vect1(1), vect2(1);
    }
    else if(deformation == 8) {
        ret <<
            1, 0,
            0, 0;
    }
    else
        throw std::runtime_error("Invalid deformation");
    return ret;
}

vector<cd> l(double rx, double ry) {
    Matrix2cd lam = lamb(rx, ry);
    double sqrt3 = sqrt(3.0);
    return {lam(0,0), lam(1,0)/sqrt3, -lam(1,0)/sqrt3, lam(0,1), lam(1,1)/sqrt3, -lam(1,1)/sqrt3};
}

double gen_theta(int i, double rx, double ry) {
    return std::real(M_PI/2 + sqrt(eps)*l(rx, ry)[i]);
}

Matrix2cd gen_U(int i, double rx, double ry) {
    return gen_Ui(gen_theta(i, rx, ry));
}

Matrix2cd gen_Ubis(int i, double rx, double ry) {
    return gen_U(i+3, rx, ry);
}

Matrix2cd gen_Ustar(int i, double rx, double ry) {
    return gen_U(i, rx, ry).adjoint();
}

Matrix2cd gen_Ubisstar(int i, double rx, double ry) {
    return gen_Ustar(i+3, rx, ry);
}

grid_t shift(grid_t grid) {
    grid_t ngrid = zerogrid();
    for(int x = -xspan; x < xspan; x++) {
        for(int y = -yspan; y < yspan; y++) {
            for(int i = 0; i < 3; i++) {
                int iprec = ((i-1)%3+3)%3;
                ngrid[x+center[0]][y+center[1]][i] = grid[x+center[0]][y+center[1]][iprec];
            }
        }
    }
    return ngrid;
}

std::pair<double, double> real_coords(int iside, int x, int y, bool show = false) {
    double dec;
    if(show)
        dec = .4;
    else
        dec = .5;
    double xcoord, ycoord;
    if((x+y)%2==0) {
        if(iside == 0) {
            xcoord = x-dec;
            ycoord = y*dy;
        }
        else if(iside == 1) {
            xcoord = x+dec;
            ycoord = y*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+dec)*dy;
        }
    }
    else {
        if(iside == 0) {
            xcoord = x+dec;
            ycoord = y*dy;
        }
        else if(iside == 1) {
            xcoord = x-dec;
            ycoord = y*dy;
        }
        else {
            xcoord = x;
            ycoord = (y-dec)*dy;
        }
    }
    return std::make_pair(sqrt(eps)*xcoord, sqrt(eps)*ycoord);
}

const int DELTAS[][2] = {{-1,0}, {1,0}, {0,1}};
const int NUM_THREADS = 8;

void applyCoinsPartial(grid_t &ngrid, grid_t &grid, gen_coin_t &gen_coin, int xmin, int xmax) {
    for(int x = xmin; x < xmax; x++) {
        for(int y = -yspan; y < yspan; y++) {
            if((x+y)%2)
                continue;
            for(int iside = 0; iside < 3; iside++) {
                cd thisval = grid[x+center[0]][y+center[1]][iside];
                int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                cd otherval = grid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside];
                Vector2cd vect;
                vect << thisval, otherval;
                double rx, ry;
                std::tie(rx, ry) = real_coords(iside, x, y);
                Matrix2cd coin = gen_coin(iside, rx, ry);
                Vector2cd newvect = coin*vect;
                ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                ngrid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside] = newvect(1);
            }
        }
    }
}

grid_t applyCoins(grid_t grid, gen_coin_t gen_coin, bool multithread = true) {
    grid_t ngrid = zerogrid();
    if(!multithread) {
        for(int x = -xspan; x < xspan; x++) {
            for(int y = -yspan; y < yspan; y++) {
                if((x+y)%2)
                    continue;
                for(int iside = 0; iside < 3; iside++) {
                    cd thisval = grid[x+center[0]][y+center[1]][iside];
                    int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                    cd otherval = grid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside];
                    Vector2cd vect;
                    vect << thisval, otherval;
                    double rx, ry;
                    std::tie(rx, ry) = real_coords(iside, x, y);
                    Matrix2cd coin = gen_coin(iside, rx, ry);
                    Vector2cd newvect = coin*vect;
                    ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                    ngrid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside] = newvect(1);
                }
            }
        }
    }
    else {
        std::thread threads[NUM_THREADS];
        int delta_x = ntriangles_x/NUM_THREADS;
        for(int iThread = 0; iThread < NUM_THREADS-1; iThread++) {
            threads[iThread] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(gen_coin), -xspan+iThread*delta_x, -xspan+(iThread+1)*delta_x);
        }
        threads[NUM_THREADS-1] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(gen_coin), -xspan+(NUM_THREADS-1)*delta_x, xspan);
        for(int iThread = 0; iThread < NUM_THREADS; iThread++)
            threads[iThread].join();
    }
    return ngrid;
}

void plotVectorField(double minx, double maxx, double miny, double maxy, int gridstep = 20) {
    vector<double> xloc, yloc;
    vector<double> vectx, vecty;
    double dx = (maxx-minx)/gridstep, dy = (maxy-miny)/gridstep;
    for(double x = minx; x <= maxx; x += dx)
        for(double y = miny; y <= maxy; y += dy) {
            for(int i = 0; i < 2; i++) {
                xloc.push_back(x);
                yloc.push_back(y);
                Matrix2cd deform = lamb(x,y);
                vectx.push_back(std::real(deform(0,i)));
                vecty.push_back(std::real(deform(1,i)));
            }
        }
    plt::quiver(xloc, yloc, vectx, vecty, {{"pivot","tail"}, {"color", "grey"}});
}

vector<double> sphereCoords(double x, double y) {
    double den = .5*(x*x+y*y)+2;
    return {2*x/den, 2*y/den, 1-4/den};
}

void plotSphere(int iGrid = -1) {
    vector<vector<double>> xgrid(2*xspan, vector<double>(2*yspan, 0));
    vector<vector<double>> ygrid(2*xspan, vector<double>(2*yspan, 0));
    vector<vector<double>> zgrid(2*xspan, vector<double>(2*yspan, 0));
    vector<vector<double>> colgrid(2*xspan, vector<double>(2*yspan, 0));
    vector<double> listcol;
    for(int x = -xspan; x < xspan; x++)
        for(int y = -yspan; y < yspan; y++) {
            double rx, ry;
            std::tie(rx, ry) = real_coords(0, x, y, true);
            vector<double> coords = sphereCoords(rx, ry);
            xgrid[x+center[0]][y+center[1]] = coords[0];
            ygrid[x+center[0]][y+center[1]] = coords[1];
            zgrid[x+center[0]][y+center[1]] = coords[2];
            vector<cd> &vals = grid[x+center[0]][y+center[1]];
            double sum = 0.0;
            for(int i = 0; i < 3; i++)
                sum += std::real(vals[i]*std::conj(vals[i]));
            //sum /= approxArea(rx, ry);
            colgrid[x+center[0]][y+center[1]] = sum;
            listcol.push_back(sum);
        }
    std::sort(listcol.rbegin(), listcol.rend());
    double maxcol = (listcol[0]+listcol[1])/2;
    if(maxcol == 0.0)
        maxcol = 1.0;
    maxcol *= .6;
    vector<vector<vector<double>>> facecolors(2*xspan, vector<vector<double>> (2*yspan, vector<double> (4,1.0)));
    for(int x = -xspan; x < xspan; x++)
        for(int y = -yspan; y < yspan; y++) {
            double val = std::max(0.0, std::min(1.0, colgrid[x+center[0]][y+center[1]]/maxcol));
            tinycolormap::Color col = tinycolormap::GetGistHeatColor(1-val);
            for(int i = 0; i < 3; i++)
                facecolors[x+center[0]][y+center[1]][i] = col.data[i];
        }
    PyObject* fig = plt::plot_surface(xgrid, ygrid, zgrid, {}, facecolors, 1000, 1000, {-45.0, 30.0});
    std::ostringstream filename;
    filename << prefix << "_sphere_" << iGrid << ".png";
    plt::save(prefix + "/" + filename.str());
    plt::cla();
    plt::clf();
    plt::close(fig);
    Py_DECREF(fig);
}

void plot(int iGrid = -1) {
    PyObject* fig = plt::figure_size(1000,1000);
    /*plt::xlim(-xspan*sqrt(eps), xspan*sqrt(eps));
    plt::ylim(-yspan*sqrt(eps)*dy, yspan*sqrt(eps)*dy);*/
    plt::set_aspect_equal();
    vector<vector<double>> imgrid(2*yspan, vector<double>(2*xspan, 0.0));
    vector<double> listvals;
    for(int y = -yspan; y < yspan; y++) {
        for(int x = -xspan; x < xspan; x++) {
            double sum = 0.0;
            for(int iside = 0; iside < 3; iside++) {
                cd val = grid[x+center[0]][y+center[1]][iside];
                double col = std::real(val*std::conj(val));
                sum += col;
            }
            listvals.push_back(sum);
            imgrid[y+center[1]][x+center[0]] = sum;
        }
    }
    std::sort(listvals.rbegin(), listvals.rend());
    double maxi = (listvals[0]+listvals[1])/2;
    if(maxi == 0.0)
        maxi = 1.0;
    maxi *= .6;
    double minx = sqrt(eps)*(-xspan-.5);
    double maxx = sqrt(eps)*(xspan-.5);
    double miny = sqrt(eps)*dy*(-yspan-.5);
    double maxy = sqrt(eps)*dy*(yspan-.5);
    plt::imshow(imgrid, {minx, maxx, miny, maxy}, {{"origin", "lower"}, {"cmap", "gist_heat_r"}, {"vmin", "0.0"}, {"vmax", std::to_string(maxi)}});
    if(showVectField)
        plotVectorField(-xspan*sqrt(eps), (xspan-1)*sqrt(eps), -yspan*sqrt(eps)*dy, (yspan-1)*sqrt(eps)*dy, 20);
    std::ostringstream filename;
    filename << prefix << "_" << iGrid << ".png";
    plt::save(prefix + "/" + filename.str());
    plt::cla();
    plt::clf();
    plt::close(fig);
    Py_DECREF(fig);
}

void step_walk(int step = -1) {
    std::cerr << "Begin step " << step << std::endl;
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = H;
        return ret;
    });
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ustar);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_U(modulo(i-1,3), rx, ry);
            return ret;
        });
    }
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_U);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ustar(modulo(i-1,3), rx, ry);
            return ret;
        });
    }
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = Q*H;
        return ret;
    });
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ubisstar);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ubis(modulo(i-1,3), rx, ry);
            return ret;
        });
    }
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ubis);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ubisstar(modulo(i-1,3), rx, ry);
            return ret;
        });
    }
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = Q.adjoint();
        return ret;
    });
    std::cerr << "Total amplitude: " << sumAmplitudes() << "\n";
    std::cerr << "End step " << step << std::endl;
}

void print_params() {
    std::ofstream ostream(prefix + "/settings.txt");
    ostream << "num_steps = " << num_steps << "\n";
    ostream << "eps = " << eps << "\n";
    ostream << "xspan = " << xspan << "\n";
    ostream << "yspan = " << yspan << "\n";
    ostream << "deformation: " << deform_name[deformation] << "\n";
    ostream << "initial state: " << initialStateName[initialState] << "\n";
    /*ostream << "Deformation matrix at (1,1):\n" << lamb(1,1) << "\n";
    for(int i = 0; i < 6; i++) {
        ostream << "U" << i << "(1,1):\n" << gen_U(i,1,1) << "\n";
    }
    ostream << "H=\n" << H << "\n";
    ostream << "Q=\n" << Q << std::endl;*/
    ostream.close();
}

void listDeformations() {
    for(size_t i = 0; i < deform_name.size(); i++) {
        std::cerr << "- " << i << ": " << deform_name[i] << "\n";
    }
}

void listInitialStates() {
    for(size_t i = 0; i < initialStateName.size(); i++)
        std::cerr << "- " << i << ": " << initialStateName[i] << "\n";
}

int main(int argc, char **argv)
{
    if(argc <= 2) {
        std::cerr << "Usage: " << std::string(argv[0]) << " <deformation> <initial state> [force sphere].\n";
        std::cerr << "Deformations:\n";
        listDeformations();
        std::cerr << "Initial states:\n";
        listInitialStates();
        std::cerr << "Put any non-empty third argument to force plotting on a sphere.\n";
        return 1;
    }
    deformation = std::atoi(argv[1]);
    if(deformation < 0 || deformation >= (int)deform_name.size()) {
        std::cerr << "Invalid deformation " << deformation << ". List of deformations:\n";
        listDeformations();
        return 2;
    }
    initialState = std::atoi(argv[2]);
    if(initialState < 0 || initialState >= (int)initialStateName.size()) {
        std::cerr << "Invalid initial state " << initialState << ". List of initial states:\n";
        listInitialStates();
        return 3;
    }
    forceSphere = argc >= 4;
    std::cout << "Deformation selected: " << deformation << " " << deform_name[deformation] << std::endl;
    std::ostringstream str;
    str <<
        "simul_distorted_" << deform_name[deformation] << "_" << initialStateName[initialState] << "_" <<
        std::setw(4) << std::setfill('0') << ltm->tm_year+1900 << "-" <<
        std::setw(2) << ltm->tm_mon+1 << "-" <<
        std::setw(2) << ltm->tm_mday << "_" <<
        std::setw(2) << ltm->tm_hour << "-" <<
        std::setw(2) << ltm->tm_min << "-" <<
        std::setw(2) << ltm->tm_sec;
    prefix = str.str();
    std::filesystem::create_directory(prefix);
    init_HQ();
    print_params();
    // Initial state
    // Vertical
    if(initialState == 1)
        for(int y = -yspan; y < yspan; y++)
            for(int k = 0; k < 3; k++)
                grid[center[0]][y+center[1]][k] = 1;
    // Centered
    else if(initialState == 0)
        for(int k = 0; k < 3; k++)
            grid[center[0]][center[1]][k]=1/sqrt(3);
    // Centered, side 0
    else if(initialState == 2)
        grid[center[0]][center[1]][0] = 1;
    // Centered, side 1
    else if(initialState == 3)
        grid[center[0]][center[1]][1] = 1;
    // Centered, side 2
    else if(initialState == 4)
        grid[center[0]][center[1]][2] = 1;
    // Shifted up-right at half
    else if(initialState == 5)
        for(int k = 0; k < 3; k++)
            grid[center[0]+xspan/2][center[1]+yspan/2][k] = 1/sqrt(3);
    // Shifted up-right at third
    else if(initialState == 6)
        for(int k = 0; k < 3; k++)
            grid[center[0]+xspan/3][center[1]+yspan/3][k] = 1/sqrt(3);
    // Sine, x dimension
    else if(initialState == 7) {
        const double rxmin = (-xspan-.5)*sqrt(eps);
        const double rxmax = (xspan-.5)*sqrt(eps);
        for(int x = -xspan; x < xspan; x++)
            for(int y = -yspan; y < yspan; y++)
                for(int k = 0; k < 3; k++) {
                    double rx, ry;
                    std::tie(rx, ry) = real_coords(k,x,y);
                    grid[center[0]+x][center[1]+y][k] = sin(2*M_PI*(float)(rx-rxmin)/(rxmax-rxmin));
                }
        normalizeGrid();
    }
    // Sine, y dimension
    else if(initialState == 8) {
        const double rymin = (-yspan-.5)*sqrt(eps);
        const double rymax = (yspan-.5)*sqrt(eps);
        for(int x = -xspan; x < xspan; x++)
            for(int y = -yspan; y < yspan; y++)
                for(int k = 0; k < 3; k++) {
                    double rx, ry;
                    std::tie(rx, ry) = real_coords(k,x,y);
                    grid[center[0]+x][center[1]+y][k] = sin(2*M_PI*(float)(ry-rymin)/(rymax-rymin));
                }
        normalizeGrid();
    }
    // A rectangle
    else if(initialState == 9) {
        for(int x = -xspan/5; x <= (xspan-1)/5; x++)
            for(int y = -yspan/5; y <= (yspan-1)/5; y++)
                for(int k = 0; k < 3; k++)
                    grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    plot(0);
    bool isSphere = deformation == 5 || deformation == 6;
    if(isSphere || forceSphere)
        plotSphere(0);
    for(int i = 0; i < num_steps; i++) {
        step_walk(i);
        if((i+1)%10 == 0) {
            plot(i+1);
            if(isSphere || forceSphere)
                plotSphere(i+1);
        }
    }
}


/* Bubble Dynamics with Chebyshev Spectral Collocation */

#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <algorithm>
#include <random>
#include <string>
#include <sstream>
#include <chrono>
#include <thread>
#include <boost/numeric/odeint.hpp>
#include <Eigen/Core>

using namespace std;
using namespace boost::numeric::odeint;

const double rho_L = 9.970639504998557e+02;
const double p_inf = 1.0e+5;
const double sigma = 0.071977583160056;
//const double R_E = 10e-6; //1...10u
const double gamma = 1.33;
const double c_L = 1.497251785455527e+03; // water 25 Celsius
const double mu_L = 8.902125058209557e-04; //25 Celsius
const double lambda = 0.6084; //water 25 Celsius
const double T_inf = 298.15; // 25 Celsius
const int N = 16; //24:25s  32:104s 48:731s
const double tolerance = 1e-13;
const double t_max = 0.5;
const int sample = 25000;

typedef double value_type;
typedef vector<value_type> state_type;
typedef Eigen::Matrix<value_type, N/2, N/2> matrix_type;

using namespace boost::numeric;

//ode function of bubble dynamic
class bubble {
public:
	std::vector<value_type> C; //constants of the right hand side
	Eigen::Matrix<value_type, N/2, 1> y; //collocation points (half)
	Eigen::Matrix<value_type, N/2, 1> y_sq;
	matrix_type D_E; //Derivative matrix for even functions
	matrix_type D_O; //Derivative matrix for odd functions
	Eigen::Matrix<value_type, 1, N/2> D_E0; //first row of even D matrix

	//p_A pressure amplitude, f pressure frequence, N number of collocation points
	bubble(value_type p_A, value_type f, value_type R_E){
		const double omega = 2*M_PI*f;
		value_type pi2wRE = 2*M_PI/(omega*R_E);

		C = std::vector<value_type>(13);
		C[0] = omega*R_E/(2*M_PI*c_L);
		C[1] = 4*mu_L/(c_L*rho_L*R_E);
		C[2] = 4*mu_L/(rho_L*R_E)*pi2wRE;
		C[3] = 2*sigma*pi2wRE*pi2wRE/(rho_L*R_E);
		C[4] = p_inf/rho_L*pi2wRE*pi2wRE;
		C[5] = p_A/rho_L*pi2wRE*pi2wRE;
		C[6] = pi2wRE*p_inf/(c_L*rho_L);
		C[7] = pi2wRE*p_A/(c_L*rho_L);
		C[8] = 2*M_PI*pi2wRE*p_A/(c_L*rho_L);
		C[9] = lambda*(gamma-1)/gamma*pi2wRE/R_E*T_inf/p_inf;
		C[10] = lambda*(gamma-1)*pi2wRE/R_E*T_inf/p_inf;
		C[11] = (gamma-1)/gamma;
		C[12] = 1.0/(3.0*gamma);

		Eigen::Matrix<value_type, N, 1> y_full(N);
		value_type rec_cpn =  1.0/(N-1);
		for(int i = 0; i < N; i++){
			y_full[i] = cos(M_PI*i*rec_cpn);
			//std::cout << y_full[i] << std::endl;
		}
		Eigen::Matrix<value_type, N, N> D(N, N);
		for(int i = 0; i < N; i++){
			for(int j = 0; j < N; j++){
				if(i == j){
					if(i == N-1){
						D(N-1,N-1) = -(1+2*(N-1)*(N-1))/6.0;
					}else if(i == 0){
						D(0,0) = (1+2*(N-1)*(N-1))/6.0;
					}else{
						D(i,i) = -y_full[i]/(2.0*(1.0-y_full[i]*y_full[i]));
					}
				}else{
					D(i,j) = std::pow(-1,i+j)*(i==0 || i==N-1?2.0:1.0)
							/((j==0 || j==N-1?2.0:1.0) * (y_full[i]-y_full[j]) );
				}
				//cout << D(i,j) <<" ";
			}
			//cout << endl;
		}
		D_E = matrix_type(N/2,N/2);
		D_O = matrix_type(N/2,N/2);
		for(int i = 0; i < N/2;i++){
		    for(int j = 0; j < N/2; j++){
		    	D_E(i,j) = D(i,j) + D(i,N-1-j);
		    	if(i==0) D_E0(j) = D_E(i,j);
		    	//cout << D_E(i,j) <<" ";
			}
		    //cout << endl;
		}
		for(int i = 0; i < N/2;i++){
			for(int j = 0; j < N/2; j++){
				D_O(i,j) = D(i,j) - D(i,N-1-j);
				//cout << D_O(i,j) <<" ";
		    }
			//cout << endl;
		}
		y = y_full.head<N/2>();
		y_sq = y.cwiseProduct(y);
    }

	void operator()(const state_type &x, state_type &dxdt, const value_type t){
	    value_type rec_xR = 1.0 / x[0];
	    value_type rec_xp = 1.0 / x[2];

	    Eigen::Map<const Eigen::Matrix<value_type, N/2,1>> z(x.data()+3);
	    Eigen::Map<Eigen::Matrix<value_type, N/2,1>> dzdt(dxdt.data()+3);

	    Eigen::Matrix<value_type, N/2, 1> De_x = D_E*z; //derivative of z (dimless temperature)
		/*for(int i = 0; i < N/2; i++){
	    	std::cout << "t" << y_sq[i] << std::endl;
	    }*/

	    //bubble pressure evolution
	    dxdt[2] = 3*rec_xR*(C[10]*rec_xR* De_x[0] - gamma*x[1]*x[2]);

	    //discretized PDE of bubble temperature
	    dzdt = De_x.cwiseProduct( x[1]*rec_xR*y - C[9]*rec_xR*rec_xR*rec_xp* De_x //this might show error, but it will not fail at compile time, valid syntax
	    		+ C[12]*rec_xp*dxdt[2]*y )
	    		+ C[11]*rec_xp*dxdt[2]*z + C[9]*rec_xp*rec_xR*rec_xR* z
				.cwiseProduct(y_sq.cwiseInverse()).cwiseProduct(D_O * (y_sq.cwiseProduct(De_x)));
	    dxdt[3] = 0.0; //Boundary condition

	    //Keller-Miksis equation
	    dxdt[0] = x[1];
	    value_type sin2pit = sin(2*M_PI*t);
	    value_type den = x[0] - C[0]*x[0]*x[1] + C[1];
	    value_type nom = 0.5*C[0] * x[1]*x[1]*x[1] - 1.5* x[1]*x[1] - C[2]*x[1]*rec_xR - C[3]*rec_xR
	    		+ C[4] * x[2] - C[4] - C[5] * sin2pit + C[6] * x[1]*x[2] - C[6] * x[1] - C[7] * x[1]*sin2pit
	            - C[8] * x[0]*cos(2*M_PI*t) + C[6] * x[0]*dxdt[2];
	    dxdt[1] = nom/den;
	}
};

//vector<double> fs = {20e3, 100e3, 500e3, 2e6};
//vector<double> pAs = {0.5e5, 1.0e5, 1.5e5, 2.0e5};
typedef runge_kutta_dopri5< state_type , value_type , state_type , value_type > stepper_type;

int main() {
	cout << "Bubble dynamics runs started\n" << setprecision(17) << endl;
	auto t1 = chrono::high_resolution_clock::now();

	double f = 100e3;
	double R_E = 10e-6;
	double p_A = 0.5e5;

    std::mt19937 gen;
	gen.seed(42); //reporducibility
    std::uniform_real_distribution<> dis(0.0, 0.5);

	vector<double> times(sample);
	for(int jj=0;jj<sample;jj++){
		times[jj] = dis(gen);
	}
	/*sort(times.begin(),times.end());
	auto last = unique(times.begin(),times.end()); //to get rid of duplicates, if any
	times.erase(last, times.end());*/
	int sampled = times.size();

	state_type x(3+N/2);
	state_type dxdt(3+N/2);

	bubble bubi(p_A, f, R_E);
	auto stepper = make_controlled( tolerance , tolerance, stepper_type() );

	stringstream ss("");
	ss << "../data/bubble_sim_p" << p_A/1e5 << "_f" << f/1e3 << "_Re" << R_E*1e6 << "_N" << N << "_t" << t_max << "_" << sampled << ".txt";
	string file_name = ss.str();
	ofstream ofs(file_name);

	if(!ofs.is_open()){
		cout << "File cannot be opened." << endl;
		exit(-1);
	}
	ofs.precision(17);
	ofs.flags(ios::scientific);

	x[0] = 1.0;
	x[1] = 0.0;
	x[2] = 1.0 + 2.0*sigma/(p_inf*R_E);
	for(int i=0; i < N/2;i++) x[3+i] = 1.0;

	double t_start = 0.0;

	try{
		for(int jj=0; jj < sampled; jj++){
			integrate_adaptive(boost::ref(stepper), boost::ref(bubi), x, t_start, times[jj], 0.01);
			t_start = times[jj];
			ofs << times[jj] << ", " << x[0];
			for(int i =1; i < 3+N/2;i++){
				ofs << ", " << x[i];
			}
			bubi(x, dxdt, times[jj]);
			for(int i =0; i < 3+N/2;i++){
				ofs << ", " << dxdt[i];
			}
			ofs << ", " << sin(2*M_PI*times[jj]);
			ofs << endl;
		}
		cout << "Written " << sampled << " lines." << endl;
	}catch(int i){
		cout << " - aborted: R_E=" << R_E << ", f=" << f << ", p_A=" << p_A << endl;
	}

	auto t2 = chrono::high_resolution_clock::now();
	cout << "Fertig" << endl;
	cout << "Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;
	return 0;
}


// unit test file asinh.hpp for the special functions test suite

//  (C) Copyright Hubert Holin 2003. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#include <functional>
#include <iomanip>
#include <iostream>

#include <boost/math/special_functions/asinh.hpp>

#include <boost/test/unit_test.hpp>

template<typename T>
T    asinh_error_evaluator(T x)
{
    using    ::std::abs;
    using    ::std::sinh;
    using    ::std::cosh;

    using    ::std::numeric_limits;

    using    ::boost::math::asinh;

    static T const    epsilon = numeric_limits<float>::epsilon();

    T                y = sinh(x);
    T                z = asinh(y);

    T                absolute_error = abs(z-x);
    T                relative_error = absolute_error*cosh(x);
    T                scaled_error = relative_error/epsilon;

    return(scaled_error);
}

template<typename T>
void    asinh_test(const char * more_blurb)
{
    BOOST_MESSAGE("Testing asinh in the real domain for "
        << more_blurb << ".");

    for    (int i = 0; i <= 100; i++)
    {
        T    x = static_cast<T>(i-50)/static_cast<T>(5);

        BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
            (
                asinh_error_evaluator(x),
                static_cast<T>(4)
            ));
    }
}

void    asinh_manual_check()
{
    BOOST_MESSAGE("asinh");

    for    (int i = 0; i <= 100; i++)
    {
        float        xf = static_cast<float>(i-50)/static_cast<float>(5);
        double       xd = static_cast<double>(i-50)/static_cast<double>(5);
        long double  xl =
                static_cast<long double>(i-50)/static_cast<long double>(5);

        BOOST_MESSAGE(  ::std::setw(15)
                     << asinh_error_evaluator(xf)
                     << ::std::setw(15)
                     << asinh_error_evaluator(xd)
                     << ::std::setw(15)
                     << asinh_error_evaluator(xl));
    }

    BOOST_MESSAGE(" ");
}