/**
 * @brief Functions for dense matrix and vector operations.
 *
 * @author Junyi Wang (jywang)
 *
 * @copyright Copyright (c) 2024, TBPLaS develop team.
 */

#ifndef TBPLAS_TBPM_DENSE_H
#define TBPLAS_TBPM_DENSE_H
#include <algorithm>
#include <cassert>
#include <cmath> // std::pow
#include <iostream>
#include <vector>

#include <eigen3/Eigen/Dense>

#include "base/datatypes.h"
#include "mkl_wrapper.h"

using DENSE_INT = MKL_INT;
using namespace tbplas::math::mkl;
namespace tbplas::tbpm {
using tbplas::base::complex_t;

// 优化：可使用std::vector代替指针，可以用变量定义大小，在栈上,可能做了一些优化
// 指针：malloc动态在堆上，手动free
// int size = 5;
// std::vector<double>v(size);
// double* x = v.data();

/**
 * @brief Calculate the inner product between double/complex vectors
 *
 * @param[in] x left operand
 * @param[in] x right operand
 * @return the inner product
 *
 * @note The conjugate of left operand is taken automatically.
 */
// complex_t 向量X的共轭，Y点积 dot=conj(X).trans()*Y
template <typename Derived>
auto vec_dot(const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y)
{
    using Scalar = typename Derived::Scalar;
    using math_scalar = type<Scalar>;
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    DENSE_INT inc = 1;
    math_scalar result;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    dotc<Scalar>::call(
        dim,
        x_data,
        inc,
        y_data,
        inc,
        &result);
    Scalar prod;
    assignment(prod, result);
    return prod;
}

/**
 * @brief Calculate the inner product between complex vectors without conjugation
 * of left operand.
 *
 * @param[in] x left operand
 * @param[in] x right operand
 * @param[in] check_dim whether to enable the dimension check assertion in debug mode
 * @return the inner product
 *
 * @note Currently, this function is used by fast TBPM algorithms, where we need
 * to calculate the product of two vectors with different dimensions. So we add
 * the check_dim parameter to disable the dimension check assertion in debug mode.
 * The default value of check_dim should be true, in order to be consistent with vec_dot.
 */
inline complex_t vec_dot_no_conj(const Eigen::VectorXcd& x, const Eigen::VectorXcd& y, bool check_dim = true)
{
    DENSE_INT dim = x.size();
    if (check_dim) {
        assert(dim == y.size());
    }
    using math_scalar = type<std::complex<double>>;
    math_scalar result;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    dotu<complex_t>::call(
        dim,
        x_data,
        1,
        y_data,
        1,
        &result);
    std::complex<double> prod = 0;
    assignment(prod, result);
    return prod;
}

/**
 * @brief Calculate the norm of vector
 *
 * @tparam vx_t datatype of vector
 * @param[in] x the vector
 * @return norm of vector
 */
// 向量X的二范数，返回double
// double sqrt(X.trans()*X)
// complex_t sqrt(conj(X).trans()*X).real())
// vx_t Eigen::VectorXcd
template <typename vx_t>
double vec_norm(const Eigen::MatrixBase<vx_t>& x)
{
    using Scalar = typename vx_t::Scalar;
    using math_scalar = type<Scalar>;
    DENSE_INT dim = x.size();
    double prod = 0.0;
    DENSE_INT inc = 1;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    prod = nrm2<Scalar>::call(
        dim,
        x_data,
        inc);
    return prod;
}

/**
 * @brief Calculate the norm^2 of vector
 *
 * @tparam vx_t datatype of vector
 * @param[in] x the vector
 * @return norm of vector
 */
template <typename vx_t>
double vec_norm2(const Eigen::MatrixBase<vx_t>& x)
{
    double tmp = vec_norm(x);
    // return tmp * tmp;
    return std::pow(tmp, 2);
}

/**
 * @brief Initialize vector X = a.
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @param[inout] x vector x
 * @param[in] a scalar a
 */
// 初始化，所有元素值为a，MKL无可用
template <typename vx_t, typename sa_t>
void vec_init(Eigen::MatrixBase<vx_t>& x, const sa_t& a)
{
    size_t dim = x.size();

    typedef typename vx_t::Scalar Scalar;
    Scalar* ptr = x.derived().data();
    // #pragma omp parallel for simd
    //         for (size_t i = 0; i < dim; ++i) {
    //             ptr[i] = a;
    //         }
    std::fill(ptr, ptr + dim, a);
}

/**
 * @brief Deep copy X to Y.
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @param[in] x vector x
 * @param[out] y vector y
 */
// 深拷贝  cblas_?copy
template <typename vx_t, typename vy_t>
void vec_copy(const Eigen::MatrixBase<vx_t>& x, Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    copy<Scalar>::call(
        dim,
        x_data,
        1,
        y_data,
        1);
}

/**
 * @brief Truncate a decreasing vector according to threshold.
 *
 * @tparam vx_t datatype of vector x
 * @param[inout] x vector to truncate
 * @param[in] eps threshold below which elements will be dropped
 */
// eigen类型为vx_t，mkl没有对应函数
template <typename vx_t>
void vec_trunc(vx_t& x, const double& eps)
{
    size_t dim = x.size();
    assert(dim > 0);
    size_t i_max = 0;
    bool truncate = false;
    typedef typename vx_t::Scalar Scalar;
    Scalar* ptr = x.data();
    for (size_t i = 0; i < dim - 1; ++i) {
        if ((std::abs(ptr[i]) <= eps) && (std::abs(ptr[i + 1]) <= eps)) {
            i_max = i;
            truncate = true;
            break;
        }
    }
    if (truncate) {
        x.conservativeResize(i_max + 1); // +1 to avoid yielding empty vector
    }
}

/**
 * @brief Allocate matrix X as std::vector<Eigen::VectorX*>.
 *
 * @tparam vx_t datatype of matrix X
 * @param[out] x matrix X
 * @param[in] num_row number of rows
 * @param[in] num_col number of columns
 */
template <typename vx_t>
void mat_alloc(std::vector<vx_t>& x, size_t num_row, size_t num_col)
{
    x = std::vector<vx_t>(num_col);
    for (vx_t& v : x) {
        v.resize(num_row);
    }
}

/**
 * @brief Initialize matrix X = a.
 *
 * @tparam vx_t datatype of matrix X
 * @tparam sa_t datatype of scalar a
 * @param[out] x matrix X
 * @param[in] a scalar a
 */
template <typename vx_t, typename sa_t>
void mat_init(std::vector<vx_t>& x, const sa_t& a)
{
    for (vx_t& v : x) {
        vec_init(v, a);
    }
}

/**
 * @brief Deep copy vector X to each column of matrix Y.
 *
 * @tparam vx_t datatype of x
 * @tparam vy_t datatype of y
 * @param[in] x vector x
 * @param[out] y matrix y
 */
template <typename vx_t, typename vy_t>
void mat_col_copy(const Eigen::MatrixBase<vx_t>& x, std::vector<vy_t>& y)
{
    size_t dim = x.size();
    assert(dim == y[0].size());
    for (vy_t& v : y) {
        vec_copy(x, v);
    }
}

/**
 * @brief Calculate Y = a * X + b * Y
 *
 * @tparam sa_t datatype of scalar a
 * @tparam vx_t datatype of vector x
 * @tparam sb_t datatype of scalar b
 * @tparam vy_t datatype of vector y
 * @param[in] a scalar a
 * @param[in] x vector x
 * @param[in] b scalar b
 * @param[inout] y vector y
 */
// cblas_?axpby
template <typename sa_t, typename vx_t, typename sb_t, typename vy_t>
void axpby(const sa_t& a, const Eigen::MatrixBase<vx_t>& x,
    const sb_t& b, Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar alpha, beta;
    assignment(alpha, a);
    assignment(beta, b);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    DENSE_INT inc = 1;
    axpby_<Scalar>::call(
        &dim,
        &alpha,
        x_data,
        &inc,
        &beta,
        y_data,
        &inc);
}

/**
 * @brief Calculate Y = a * X + Y
 *
 * @tparam sa_t datatype of scalar a
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @param[in] a scalar a
 * @param[in] x vector x
 * @param[inout] y vector y containing the result
 */
// cblas_?axpy
template <typename sa_t, typename vx_t, typename vy_t>
void axpy(const sa_t& a, const Eigen::MatrixBase<vx_t>& x,
    Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar alpha;
    assignment(alpha, a);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    DENSE_INT inc = 1;
    axpy_<Scalar>::call(
        &dim,
        &alpha,
        x_data,
        &inc,
        y_data,
        &inc);
}

/**
 * @brief Calculate Z = a * X + b * Y + Z
 *
 * @tparam sa_t datatype of scalar a
 * @tparam vx_t datatype of vector x
 * @tparam sb_t datatype of scalar b
 * @tparam vy_y datatype of vector y
 * @tparam vz_t datatype of vector z
 * @param[in] a scalar a
 * @param[in] x vector x
 * @param[in] b scalar b
 * @param[in] y vector y
 * @param[inout] z vector z
 */
template <typename sa_t, typename vx_t, typename sb_t, typename vy_y, typename vz_t>
void axpbypz(const sa_t& a, const Eigen::MatrixBase<vx_t>& x,
    const sb_t& b, const Eigen::MatrixBase<vy_y>& y, Eigen::MatrixBase<vz_t>& z)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    assert(dim == z.size());
    using Scalar = typename vy_y::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar alpha, beta;
    assignment(alpha, a);
    assignment(beta, b);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    math_scalar* z_data = (math_scalar*)z.derived().data();
    DENSE_INT inc = 1;
    // z=a*x+z
    axpy_<Scalar>::call(
        &dim,
        &alpha,
        x_data,
        &inc,
        z_data,
        &inc);
    // z=by+z
    axpy_<Scalar>::call(
        &dim,
        &beta,
        y_data,
        &inc,
        z_data,
        &inc);
}

/**
 * @brief Calculate OUT = a * X + b * Y + c * Z
 *
 * @tparam sa_t datatype of scalar a
 * @tparam vx_t datatype of vector x
 * @tparam sb_t datatype of scalar b
 * @tparam vy_y datatype of vector y
 * @tparam sc_t datatype of scalar c
 * @tparam vz_t datatype of vector z
 * @tparam vout_t datatype of vector out
 * @param[in] a scalar a
 * @param[in] x vector x
 * @param[in] b scalar b
 * @param[in] y vector y
 * @param[in] c scalar c
 * @param[in] z vector z
 * @param[out] out vector out
 */
template <typename sa_t, typename vx_t,
    typename sb_t, typename vy_y,
    typename sc_t, typename vz_t,
    typename vout_t>
void axpbypcz_out(const sa_t& a, const Eigen::MatrixBase<vx_t>& x, const sb_t& b, const Eigen::MatrixBase<vy_y>& y,
    const sc_t& c, const Eigen::MatrixBase<vz_t>& z, Eigen::MatrixBase<vout_t>& out)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    assert(dim == z.size());
    using Scalar = typename vy_y::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar alpha, beta, gamma;
    assignment(alpha, a);
    assignment(beta, b);
    assignment(gamma, c);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    math_scalar* z_data = (math_scalar*)z.derived().data();
    math_scalar* out_data = (math_scalar*)out.derived().data();
    DENSE_INT inc = 1;
    // out=z
    copy<Scalar>::call(
        dim,
        z_data,
        inc,
        out_data,
        inc);
    // out=a*x+c*out
    axpby_<Scalar>::call(
        &dim,
        &alpha,
        x_data,
        &inc,
        &gamma,
        out_data,
        &inc);
    // out=b*y+out
    axpy_<Scalar>::call(
        &dim,
        &beta,
        y_data,
        &inc,
        out_data,
        &inc);
}

/**
 * @brief Calculate prod = x.conj() * m * y for diagonal matrix m
 *
 * @tparam vx_t datatype of vector x
 * @tparam vm_t datatype of diagonal matrix m
 * @tparam vy_t datatype of vector y
 * @param[in] x vector x
 * @param[in] m diagonal matrix m
 * @param[in] y vector y
 * @return the product
 */
// m可能是对角阵保留对角线元素的向量
// v?Mul c[i]=a[i]*b[i]  vml
// cblas_?dot 点积
// dotc_sub  求和conj(a[i])*b[i]
template <typename vx_t, typename vm_t, typename vy_t>
complex_t xmy_diag(const Eigen::MatrixBase<vx_t>& x,
    const Eigen::MatrixBase<vm_t>& m,
    const Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == m.size());
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar* m_data = (math_scalar*)m.derived().data();
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    // temp[i]=m[i]*y[i]
    vmul<Scalar>::call(
        dim,
        m_data,
        y_data,
        temp);
    math_scalar result;
    // conj(a[i])*temp[i]求和
    dotc<Scalar>::call(
        dim,
        x_data,
        1,
        temp,
        1,
        &result);
    complex_t prod = 0.0;
    assignment(prod, result);
    free(temp);
    return prod;
}

/* -------- Generated with gen_cpp.py -------- */

/**
 * @brief Calculate Z = X + Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @tparam vz_t datatype of vector z
 * @param[in] x left operand
 * @param[in] y right operand
 * @param[out] z result
 */
// z[i]=x[i]+y[i] v?Add
template <typename vx_t, typename vy_t, typename vz_t>
void vec_add_vec(const Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y,
    Eigen::MatrixBase<vz_t>& z)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    assert(dim == z.size());
    using Scalar = typename vz_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    math_scalar* z_data = (math_scalar*)z.derived().data();
    vadd<Scalar>::call(
        dim,
        x_data,
        y_data,
        z_data);
}

/**
 * @brief Calculate Z = X - Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @tparam vz_t datatype of vector z
 * @param[in] x left operand
 * @param[in] y right operand
 * @param[out] z result
 */
// z[i] = x[i] - y[i]; v?Sub
template <typename vx_t, typename vy_t, typename vz_t>
void vec_sub_vec(const Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y,
    Eigen::MatrixBase<vz_t>& z)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    assert(dim == z.size());
    using Scalar = typename vz_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    math_scalar* z_data = (math_scalar*)z.derived().data();
    vsub<Scalar>::call(
        dim,
        x_data,
        y_data,
        z_data);
}

/**
 * @brief Calculate Z = X * Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @tparam vz_t datatype of vector z
 * @param[in] x left operand
 * @param[in] y right operand
 * @param[out] z result
 */
// 对应元素的乘积 z[i]=x[i]*y[i]
// v?Mul
template <typename vx_t, typename vy_t, typename vz_t>
void vec_mul_vec(const Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y,
    Eigen::MatrixBase<vz_t>& z)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    assert(dim == z.size());
    using Scalar = typename vz_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    math_scalar* z_data = (math_scalar*)z.derived().data();
    vmul<Scalar>::call(
        dim,
        x_data,
        y_data,
        z_data);
}

/**
 * @brief Calculate Z = X / Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @tparam vz_t datatype of vector z
 * @param[in] x left operand
 * @param[in] y right operand
 * @param[out] z result
 */
// r[i] = a[i] / b[i]   v?Div
template <typename vx_t, typename vy_t, typename vz_t>
void vec_div_vec(const Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y,
    Eigen::MatrixBase<vz_t>& z)
{

    DENSE_INT dim = x.size();
    assert(dim == y.size());
    assert(dim == z.size());
    using Scalar = typename vz_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    math_scalar* z_data = (math_scalar*)z.derived().data();
    vdiv<Scalar>::call(
        dim,
        x_data,
        y_data,
        z_data);
}

/**
 * @brief Calculate Y = X + a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @tparam vy_t datatype of vector y
 * @param[in] x left operand
 * @param[in] a right operand
 * @param[out] y result
 */
// y[i] = x[i] + a[i]    v?Add
template <typename vx_t, typename sa_t, typename vy_t>
void vec_add_sca(const Eigen::MatrixBase<vx_t>& x, const sa_t& a,
    Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar alpha;
    assignment(alpha, a);
    std::fill(temp, temp + dim, alpha);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    vadd<Scalar>::call(
        dim,
        temp,
        x_data,
        y_data);
    free(temp);

    // y={a,a,a...}
    // Y=X+Y
}

/**
 * @brief Calculate Y = X - a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @tparam vy_t datatype of vector y
 * @param[in] x left operand
 * @param[in] a right operand
 * @param[out] y result
 */
// y[i] = x[i] - a
// y[i] = x[i] - a[i] v?Sub
template <typename vx_t, typename sa_t, typename vy_t>
void vec_sub_sca(const Eigen::MatrixBase<vx_t>& x, const sa_t& a,
    Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar alpha;
    assignment(alpha, a);
    std::fill(temp, temp + dim, alpha);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    vsub<Scalar>::call(
        dim,
        x_data,
        temp,
        y_data);
    free(temp);
}

/**
 * @brief Calculate Y = X * a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @tparam vy_t datatype of vector y
 * @param[in] x left operand
 * @param[in] a right operand
 * @param[out] y result
 */
// y[i] = x[i] * a;

template <typename vx_t, typename sa_t, typename vy_t>
void vec_mul_sca(const Eigen::MatrixBase<vx_t>& x, const sa_t& a,
    Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar alpha;
    assignment(alpha, a);
    std::fill(temp, temp + dim, alpha);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    vmul<Scalar>::call(
        dim,
        temp,
        x_data,
        y_data);
    free(temp);

    // double b = 0.0;
    // axpby(a, x, b, y);

    // y.setZero();
    // axpy(a, x, y);
}

/**
 * @brief Calculate Y = X / a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @tparam vy_t datatype of vector y
 * @param[in] x left operand
 * @param[in] a right operand
 * @param[out] y result
 */
// y[i] = x[i] / a;
// y[i] = x[i] / a[i];  v?Div
template <typename vx_t, typename sa_t, typename vy_t>
void vec_div_sca(const Eigen::MatrixBase<vx_t>& x, const sa_t& a,
    Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar alpha;
    assignment(alpha, a);
    std::fill(temp, temp + dim, alpha);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    vdiv<Scalar>::call(
        dim,
        x_data,
        temp,
        y_data);
    free(temp);

    // double b = 0.0;
    // axpby(1.0/a, x, b, y);

    // y.setZero();
    // axpy(1.0/a, x, y);
}

/*
 * @brief Calculate X += Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @param[inout] x left operand
 * @param[in] y right operand
 */
// x[i] += y[i];  X=1*Y+X  cblas_?axpy
template <typename vx_t, typename vy_t>
void vec_self_add_vec(Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y)
{
    axpy(1.0, y, x);
}

/**
 * @brief Calculate X -= Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @param[inout] x left operand
 * @param[in] y right operand
 */
template <typename vx_t, typename vy_t>
void vec_self_sub_vec(Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y)
{
    axpy(-1.0, y, x);
}

/**
 * @brief Calculate X *= Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @param[inout] x left operand
 * @param[in] y right operand
 */
// x[i] *= y[i];
// vdMul(n, x, y, x);结果不稳定，时对时不对
// vdMul(n, x, y, z);
template <typename vx_t, typename vy_t>
void vec_self_mul_vec(Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    copy<Scalar>::call(
        dim,
        x_data,
        1,
        temp,
        1);
    vmul<Scalar>::call(
        dim,
        temp,
        y_data,
        x_data);
    free(temp);
}

/**
 * @brief Calculate X /= Y
 *
 * @tparam vx_t datatype of vector x
 * @tparam vy_t datatype of vector y
 * @param[inout] x left operand
 * @param[in] y right operand
 */
template <typename vx_t, typename vy_t>
void vec_self_div_vec(Eigen::MatrixBase<vx_t>& x, const Eigen::MatrixBase<vy_t>& y)
{
    DENSE_INT dim = x.size();
    assert(dim == y.size());
    using Scalar = typename vy_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar* y_data = (math_scalar*)y.derived().data();
    copy<Scalar>::call(
        dim,
        x_data,
        1,
        temp,
        1);
    vdiv<Scalar>::call(
        dim,
        temp,
        y_data,
        x_data);
    free(temp);
}

/**
 * @brief Calculate X += a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @param[inout] x left operand
 * @param[in] a right operand
 */
// x[i]+=a    X=a*ones+X
template <typename vx_t, typename sa_t>
void vec_self_add_sca(Eigen::MatrixBase<vx_t>& x, const sa_t& a)
{
    DENSE_INT dim = x.size();
    using Scalar = typename vx_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar* temp = (math_scalar*)malloc(dim * sizeof(math_scalar));
    if (temp == NULL) {
        std::cerr << "Malloc failed!\n";
        std::exit(-1);
    }
    math_scalar alpha;
    assignment(alpha, a);
    std::fill(temp, temp + dim, alpha);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    math_scalar unit;
    assignment(unit, 1.0);
    DENSE_INT inc = 1;
    axpy_<Scalar>::call(
        &dim,
        &unit,
        temp,
        &inc,
        x_data,
        &inc);
    free(temp);
}

/**
 * @brief Calculate X -= a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @param[inout] x left operand
 * @param[in] a right operand
 */
template <typename vx_t, typename sa_t>
void vec_self_sub_sca(Eigen::MatrixBase<vx_t>& x, const sa_t& a)
{
    vec_self_add_sca(x, -a);
}

/**
 * @brief Calculate X *= a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @param[inout] x left operand
 * @param[in] a right operand
 */
// x[i] *= a;  cblas_?scal
template <typename vx_t, typename sa_t>
void vec_self_mul_sca(Eigen::MatrixBase<vx_t>& x, const sa_t& a)
{
    DENSE_INT dim = x.size();
    using Scalar = typename vx_t::Scalar;
    using math_scalar = type<Scalar>;
    math_scalar alpha;
    assignment(alpha, a);
    math_scalar* x_data = (math_scalar*)x.derived().data();
    scal<Scalar>::call(
        dim,
        &alpha,
        x_data,
        1);
}

/**
 * @brief Calculate X /= a
 *
 * @tparam vx_t datatype of vector x
 * @tparam sa_t datatype of scalar a
 * @param[inout] x left operand
 * @param[in] a right operand
 */
template <typename vx_t, typename sa_t>
void vec_self_div_sca(Eigen::MatrixBase<vx_t>& x, const sa_t& a)
{
    vec_self_mul_sca(x, 1.0 / a);
}


/**
 * @brief Calculate Y += X.cwiseAbs2().
 * Specifically, for each element i: Y[i] += |X[i]|^2
 *
 * @tparam vx_t datatype of vector x (e.g., Eigen::VectorXcd)
 * @tparam vy_t datatype of vector y (e.g., Eigen::VectorXcd or Eigen::VectorXd)
 * @param[in] x The input vector whose elements' absolute squares are to be added.
 * @param[inout] y The output vector to which the absolute squares are added.
 */
template <typename vx_t, typename vy_t>
void vec_abs2_add(const Eigen::MatrixBase<vx_t>& x, Eigen::MatrixBase<vy_t>& y)
{
    size_t dim = x.size();
    assert(dim == y.size()); // Ensure dimensions match

#pragma omp parallel for simd
    for (size_t i = 0; i < dim; ++i) {
        // For complex types, std::norm(x) is just |x|^2.
        // For real types, std::norm(x) is just x*x.
        y[i] += std::norm(x[i]);
        // y[i] += (std::conj(x[i]) * x[i]).real();
    }
}

} // namespace tbplas::tbpm
#endif
