/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "dsp/MathUtilities.h"

#include <cmath>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TestMathUtilities)

BOOST_AUTO_TEST_CASE(round)
{
    BOOST_CHECK_EQUAL(MathUtilities::round(0.5), 1.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(0.49), 0.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(0.99), 1.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(0.01), 0.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(0.0), 0.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(100.0), 100.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(-0.2), 0.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(-0.5), -1.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(-0.99), -1.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(-1.0), -1.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(-1.1), -1.0);
    BOOST_CHECK_EQUAL(MathUtilities::round(-1.5), -2.0);
}

BOOST_AUTO_TEST_CASE(mean)
{
    BOOST_CHECK_EQUAL(MathUtilities::mean(0, 0), 0);
    double d0[] = { 0, 4, 3, -1 };
    BOOST_CHECK_EQUAL(MathUtilities::mean(d0, 4), 1.5);
    double d1[] = { -2.6 };
    BOOST_CHECK_EQUAL(MathUtilities::mean(d1, 1), -2.6);
    std::vector<double> v;
    v.push_back(0);
    v.push_back(4);
    v.push_back(3);
    v.push_back(-1);
    BOOST_CHECK_EQUAL(MathUtilities::mean(v, 0, 4), 1.5);
    BOOST_CHECK_EQUAL(MathUtilities::mean(v, 1, 2), 3.5);
    BOOST_CHECK_EQUAL(MathUtilities::mean(v, 3, 1), -1);
    BOOST_CHECK_EQUAL(MathUtilities::mean(v, 3, 0), 0);
}

BOOST_AUTO_TEST_CASE(sum)
{
    BOOST_CHECK_EQUAL(MathUtilities::sum(0, 0), 0);
    double d0[] = { 0, 4, 3, -1 };
    BOOST_CHECK_EQUAL(MathUtilities::sum(d0, 4), 6);
    double d1[] = { -2.6 };
    BOOST_CHECK_EQUAL(MathUtilities::sum(d1, 1), -2.6);
}

BOOST_AUTO_TEST_CASE(median)
{
    BOOST_CHECK_EQUAL(MathUtilities::median(0, 0), 0);
    double d0[] = { 0, 4, 3, -1 };
    BOOST_CHECK_EQUAL(MathUtilities::median(d0, 4), 1.5);
    double d1[] = { 0, 4, 3, -1, -1 };
    BOOST_CHECK_EQUAL(MathUtilities::median(d1, 5), 0);
    double d2[] = { 1.0, -2.0 };
    BOOST_CHECK_EQUAL(MathUtilities::median(d2, 2), -0.5);
    double d3[] = { -2.6 };
    BOOST_CHECK_EQUAL(MathUtilities::median(d3, 1), -2.6);
}

BOOST_AUTO_TEST_CASE(princarg)
{
    BOOST_CHECK_EQUAL(MathUtilities::princarg(M_PI), M_PI);
    BOOST_CHECK_EQUAL(MathUtilities::princarg(-M_PI), M_PI);
    BOOST_CHECK_EQUAL(MathUtilities::princarg(2 * M_PI), 0.0);
    BOOST_CHECK_EQUAL(MathUtilities::princarg(5 * M_PI), M_PI);
    BOOST_CHECK_EQUAL(MathUtilities::princarg(1.0), 1.0);
    BOOST_CHECK_EQUAL(MathUtilities::princarg(-1.0), -1.0);
    BOOST_CHECK_EQUAL(MathUtilities::princarg(-10.0), -10.0 + 4 * M_PI);
}

BOOST_AUTO_TEST_CASE(isPowerOfTwo)
{
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(0), false);
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(1), true);
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(-2), false);
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(2), true);
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(3), false);
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(12), false);
    BOOST_CHECK_EQUAL(MathUtilities::isPowerOfTwo(16), true);
}

BOOST_AUTO_TEST_CASE(nextPowerOfTwo)
{
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(0), 1);
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(1), 1);
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(-2), 1);
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(2), 2);
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(3), 4);
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(12), 16);
    BOOST_CHECK_EQUAL(MathUtilities::nextPowerOfTwo(16), 16);
}

BOOST_AUTO_TEST_CASE(previousPowerOfTwo)
{
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(0), 1);
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(1), 1);
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(-2), 1);
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(2), 2);
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(3), 2);
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(12), 8);
    BOOST_CHECK_EQUAL(MathUtilities::previousPowerOfTwo(16), 16);
}

BOOST_AUTO_TEST_CASE(nearestPowerOfTwo)
{
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(0), 1);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(1), 1);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(-2), 1);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(2), 2);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(3), 4);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(11), 8);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(12), 16);
    BOOST_CHECK_EQUAL(MathUtilities::nearestPowerOfTwo(16), 16);
}

BOOST_AUTO_TEST_CASE(factorial)
{
    BOOST_CHECK_EQUAL(MathUtilities::factorial(-10), 0.0);
    BOOST_CHECK_EQUAL(MathUtilities::factorial(0), 1.0);
    BOOST_CHECK_EQUAL(MathUtilities::factorial(1), 1.0);
    BOOST_CHECK_EQUAL(MathUtilities::factorial(2), 2.0);
    BOOST_CHECK_EQUAL(MathUtilities::factorial(3), 6.0);
    BOOST_CHECK_EQUAL(MathUtilities::factorial(4), 24.0);

    // Too big for an int, hence double return value from factorial
    BOOST_CHECK_EQUAL(MathUtilities::factorial(20), 2432902008176640000.0);
}

BOOST_AUTO_TEST_CASE(gcd)
{
    BOOST_CHECK_EQUAL(MathUtilities::gcd(1, 1), 1);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(2, 1), 1);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(2, 3), 1);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(4, 2), 2);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(18, 24), 6);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(27, 18), 9);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(18, 36), 18);
    BOOST_CHECK_EQUAL(MathUtilities::gcd(37, 18), 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/ut.hpp>
#include <random>
#include <scistats/bayesian/bootstrap.h>
#include <scistats/bayesian/theorem.h>
#include <scistats/descriptive/mean.h>
#include <vector>

int main() {
    using namespace boost::ut;
    using namespace scistats;

    test("Bayes Theorem") = [&] {
        // Bayes theorem
        double p_hypothesis = 0.2;
        double likelihood = 0.8;
        double p_evidence = 0.6;
        expect(bayes_theorem(likelihood, p_hypothesis, p_evidence) == 0.26_d);
    };

    test("Bootstrap") = [&] {
        int i = 0;
        auto generate_i_values = [&]() { return i++; };

        auto v = bootstrap(generate_i_values, 1000);
        expect(sum(v) == 499500_i);
    };

    return 0;
}


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include <iostream>
#include <vector>
#include <bitset>
#include <string>
using namespace std;
Ctxt operator + (Ctxt left, Ctxt right)
{
	Ctxt temp(left);
	temp+=right;
	return temp;
}
Ctxt operator * (Ctxt left, Ctxt right)
{
	Ctxt temp(left);
	temp*=right;
	return temp;
}
int biggerThan(int plainText1, int plainText2)
{
	FHEcontext * context;
	FHESecKey *secretKey;
	const FHEPubKey * publicKey;
	long p = 2;
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	long m = FindM(k, L, c, p, d, s, 0);
	unsigned bits =2;

	context = new FHEcontext(m, p, r);
	buildModChain(*context, L, c);

	ZZX G = context->alMod.getFactorsOverZZ()[0];

	secretKey = new FHESecKey(*context);
	publicKey = secretKey;
	secretKey->GenSecKey(w);

	//plain text
	vector<long> plainTextBinaryVector1(4, 0);
	vector<long> plainTextBinaryVector2(4, 0);

	//ciphertext
	vector<Ctxt> ctxtVector1(4, Ctxt(*publicKey));
	vector<Ctxt> ctxtVector2(4, Ctxt(*publicKey));

	//转换成二进制串
	bitset<4> b1(plainText1);
	bitset<4> b2(plainText2);

	for(int i=0; i<b1.size(); i++)
	{
		plainTextBinaryVector1[i]=b1[i];
		plainTextBinaryVector2[i]=b2[i];
	}

	for(int i=0; i<b1.size(); i++)
	{
		publicKey->Encrypt(ctxtVector1[i], to_ZZX(plainTextBinaryVector1[i]));
		publicKey->Encrypt(ctxtVector2[i], to_ZZX(plainTextBinaryVector2[i]));
	}

	//boolean circuit

	//1s
	Ctxt ctxtOne(*publicKey);
	publicKey->Encrypt(ctxtOne, to_ZZX(1));

	ZZX temp;
	Ctxt comparisionResult(*publicKey);
	publicKey->Encrypt(comparisionResult, to_ZZX(0));

	comparisionResult = ctxtVector1[3]*(ctxtVector2[3]+ctxtOne);
	// comparisionResult = ctxtVector1[3]*=(ctxtVector2[3]+=ctxtOne);
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"1:  "<<temp<<endl;

	comparisionResult = comparisionResult+ (ctxtVector1[3]+ctxtVector2[3]+ctxtOne)
	*ctxtVector1[2]*(ctxtVector2[2]+ctxtOne);
	//这里用XOR可以代替OR，因为运算表达式决定了两个加数不可能同时为1
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"2:  "<<temp<<endl;

	comparisionResult = comparisionResult +
	(ctxtVector1[3]+ctxtVector2[3]+ctxtOne) *
	(ctxtVector1[2]+ctxtVector2[2]+ctxtOne) *
	ctxtVector1[1]*(ctxtVector2[1]+ctxtOne);
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"3:  "<<temp<<endl;

	comparisionResult = comparisionResult +
	(ctxtVector1[3]+ctxtVector2[3]+ctxtOne) *
	(ctxtVector1[2]+ctxtVector2[2]+ctxtOne) *
	(ctxtVector1[1]+ctxtVector2[1]+ctxtOne) *
	ctxtVector1[0]*(ctxtVector2[0]+ctxtOne);
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"4:  "<<temp<<endl;

	ZZX result;
	secretKey->Decrypt(result, comparisionResult);
	//cout<<"result : "<<result<<endl;
	long hh;
	conv(hh, result[0]);

	delete context;
	delete secretKey;

	return int(hh);
}
int main()
{
	for(int x=0; x<16; x++)
	{
		for(int y=0; y<16; y++)
		{
			cout<<"x  "<<x<<"y  "<<y<<endl;
			if(biggerThan(x, y) != (x>y))
			{
				cout<<"error!"<<endl;
				cout<<"biggerThan : "<<biggerThan(x,y)<<endl;
				cout<<"x>y: "<<(x>y)<<endl;
				return 0;
			}
		}
	}
	while(true)
	{
		int a;
		int b;
		cin>>a;
		cin>>b;
		cout<<biggerThan(a, b)<<endl;
	}
}


// Copyright (c) 2018-2021 FRC Team 3512. All Rights Reserved.

#include "controllers/DrivetrainController.hpp"

#include <algorithm>
#include <cmath>

#include <Eigen/QR>
#include <frc/RobotController.h>
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/geometry/Pose2d.h>
#include <frc/system/NumericalJacobian.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/TrajectoryGenerator.h>
#include <frc/trajectory/constraint/DifferentialDriveVelocitySystemConstraint.h>
#include <wpi/MathExtras.h>

using namespace frc3512;
using namespace frc3512::Constants;
using namespace frc3512::Constants::Drivetrain;

frc::LinearSystem<2, 2, 2> DrivetrainController::m_plant =
    frc::LinearSystemId::IdentifyDrivetrainSystem(
        Constants::Drivetrain::kLinearV, Constants::Drivetrain::kLinearA,
        Constants::Drivetrain::kAngularV, Constants::Drivetrain::kAngularA);

DrivetrainController::DrivetrainController(const std::array<double, 5>& Qelems,
                                           const std::array<double, 2>& Relems,
                                           units::second_t dt) {
    m_localY.setZero();
    m_globalY.setZero();
    Reset();

    Eigen::Matrix<double, 10, 1> x0;
    x0.setZero();
    x0(State::kLeftVelocity, 0) = 1e-9;
    x0(State::kRightVelocity, 0) = 1e-9;
    Eigen::Matrix<double, 10, 1> x1;
    x1.setZero();
    x1(State::kLeftVelocity, 0) = 1;
    x1(State::kRightVelocity, 0) = 1;
    Eigen::Matrix<double, 2, 1> u0;
    u0.setZero();

    Eigen::Matrix<double, 5, 5> A0 =
        frc::NumericalJacobianX<10, 10, 2>(Dynamics, x0, u0).block<5, 5>(0, 0);
    Eigen::Matrix<double, 5, 5> A1 =
        frc::NumericalJacobianX<10, 10, 2>(Dynamics, x1, u0).block<5, 5>(0, 0);
    m_B =
        frc::NumericalJacobianU<10, 10, 2>(Dynamics, x0, u0).block<5, 2>(0, 0);

    m_K0 = frc::LinearQuadraticRegulator<5, 2>(A0, m_B, Qelems, Relems, dt).K();
    m_K1 = frc::LinearQuadraticRegulator<5, 2>(A1, m_B, Qelems, Relems, dt).K();
}

void DrivetrainController::Enable() { m_isEnabled = true; }

void DrivetrainController::Disable() { m_isEnabled = false; }

bool DrivetrainController::IsEnabled() const { return m_isEnabled; }

void DrivetrainController::SetWaypoints(
    const std::vector<frc::Pose2d>& waypoints) {
    frc::DifferentialDriveKinematics kinematics{kWidth};
    frc::DifferentialDriveVelocitySystemConstraint constraint{m_plant,
                                                              kinematics, 8_V};
    frc::TrajectoryConfig config{kMaxV, kMaxA};
    config.AddConstraint(constraint);

    std::lock_guard lock(m_trajectoryMutex);
    m_goal = waypoints.back();
    m_trajectory =
        frc::TrajectoryGenerator::GenerateTrajectory(waypoints, config);
}

bool DrivetrainController::AtGoal() const {
    frc::Pose2d ref{units::meter_t{m_r(State::kX, 0)},
                    units::meter_t{m_r(State::kY, 0)},
                    units::radian_t{m_r(State::kHeading, 0)}};
    return m_goal == ref && m_atReferences;
}

void DrivetrainController::SetMeasuredLocalOutputs(
    units::radian_t heading, units::meter_t leftPosition,
    units::meter_t rightPosition) {
    m_localY << heading.to<double>(), leftPosition.to<double>(),
        rightPosition.to<double>();
}

void DrivetrainController::SetMeasuredGlobalOutputs(
    units::meter_t x, units::meter_t y, units::radian_t heading,
    units::meter_t leftPosition, units::meter_t rightPosition,
    units::radians_per_second_t angularVelocity) {
    m_globalY << x.to<double>(), y.to<double>(), heading.to<double>(),
        leftPosition.to<double>(), rightPosition.to<double>(),
        angularVelocity.to<double>();
}

frc::LinearSystem<2, 2, 2> DrivetrainController::GetPlant() const {
    return m_plant;
}

const Eigen::Matrix<double, 5, 1>& DrivetrainController::GetReferences() const {
    return m_nextR;
}

const Eigen::Matrix<double, 10, 1>& DrivetrainController::GetStates() const {
    return m_observer.Xhat();
}

Eigen::Matrix<double, 2, 1> DrivetrainController::GetInputs() const {
    return m_cappedU;
}

const Eigen::Matrix<double, 3, 1>& DrivetrainController::GetOutputs() const {
    return m_localY;
}

Eigen::Matrix<double, 3, 1> DrivetrainController::EstimatedLocalOutputs()
    const {
    return LocalMeasurementModel(m_observer.Xhat(),
                                 Eigen::Matrix<double, 2, 1>::Zero());
}

Eigen::Matrix<double, 6, 1> DrivetrainController::EstimatedGlobalOutputs()
    const {
    return GlobalMeasurementModel(m_observer.Xhat(),
                                  Eigen::Matrix<double, 2, 1>::Zero());
}

void DrivetrainController::Update(units::second_t dt,
                                  units::second_t elapsedTime) {
    frc::Trajectory::State ref;
    {
        std::lock_guard lock(m_trajectoryMutex);
        ref = m_trajectory.Sample(elapsedTime);
    }

    auto [vlRef, vrRef] =
        ToWheelVelocities(ref.velocity, ref.curvature, kWidth);

    positionLogger.Log(elapsedTime, m_observer.Xhat(State::kX),
                       m_observer.Xhat(State::kY),
                       ref.pose.Translation().X().to<double>(),
                       ref.pose.Translation().Y().to<double>(),
                       m_localY(LocalOutput::kLeftPosition, 0),
                       m_localY(LocalOutput::kRightPosition, 0),
                       m_observer.Xhat(State::kLeftPosition),
                       m_observer.Xhat(State::kRightPosition),
                       m_odometer.GetPose().Translation().X().to<double>(),
                       m_odometer.GetPose().Translation().Y().to<double>());

    angleLogger.Log(elapsedTime, m_localY(LocalOutput::kHeading),
                    m_observer.Xhat(State::kHeading),
                    ref.pose.Rotation().Radians().to<double>(),
                    m_observer.Xhat(State::kAngularVelocityError));
    velocityLogger.Log(elapsedTime, m_observer.Xhat(State::kLeftVelocity),
                       m_observer.Xhat(State::kRightVelocity),
                       m_nextR(State::kLeftVelocity, 0),
                       m_nextR(State::kRightVelocity, 0), vlRef.to<double>(),
                       vrRef.to<double>());
    voltageLogger.Log(elapsedTime, m_cappedU(Input::kLeftVoltage, 0),
                      m_cappedU(Input::kRightVoltage, 0),
                      m_observer.Xhat(State::kLeftVoltageError),
                      m_observer.Xhat(State::kRightVoltageError),
                      frc::RobotController::GetInputVoltage());
    errorCovLogger.Log(
        elapsedTime, m_observer.P(State::kX, State::kX),
        m_observer.P(State::kY, State::kY),
        m_observer.P(State::kHeading, State::kHeading),
        m_observer.P(State::kLeftVelocity, State::kLeftVelocity),
        m_observer.P(State::kRightVelocity, State::kRightVelocity),
        m_observer.P(State::kLeftPosition, State::kLeftPosition),
        m_observer.P(State::kRightPosition, State::kRightPosition),
        m_observer.P(State::kLeftVoltageError, State::kLeftVoltageError),
        m_observer.P(State::kRightVoltageError, State::kRightVoltageError),
        m_observer.P(State::kAngularVelocityError,
                     State::kAngularVelocityError));

    m_odometer.Update(units::radian_t{m_localY(LocalOutput::kHeading)},
                      units::meter_t{m_localY(LocalOutput::kLeftPosition)},
                      units::meter_t{m_localY(LocalOutput::kRightPosition)});
    m_observer.Correct(m_cappedU, m_localY);

    m_nextR << ref.pose.Translation().X().to<double>(),
        ref.pose.Translation().Y().to<double>(),
        ref.pose.Rotation().Radians().to<double>(), vlRef.to<double>(),
        vrRef.to<double>();

    // Compute feedforward
    Eigen::Matrix<double, 5, 1> rdot = (m_nextR - m_r) / dt.to<double>();
    Eigen::Matrix<double, 10, 1> rAugmented;
    rAugmented.block<5, 1>(0, 0) = m_r;
    rAugmented.block<5, 1>(5, 0).setZero();
    Eigen::Matrix<double, 2, 1> uff = m_B.householderQr().solve(
        rdot - Dynamics(rAugmented, Eigen::Matrix<double, 2, 1>::Zero())
                   .block<5, 1>(0, 0));

    if (m_isEnabled) {
        m_cappedU = Controller(m_observer.Xhat(), m_nextR) + uff;
    } else {
        m_cappedU = Eigen::Matrix<double, 2, 1>::Zero();
    }
    ScaleCapU(&m_cappedU);

    Eigen::Matrix<double, 5, 1> error =
        m_r - m_observer.Xhat().block<5, 1>(0, 0);
    m_atReferences = std::abs(error(0, 0)) < kPositionTolerance &&
                     std::abs(error(1, 0)) < kPositionTolerance &&
                     std::abs(error(2, 0)) < kAngleTolerance &&
                     std::abs(error(3, 0)) < kVelocityTolerance &&
                     std::abs(error(4, 0)) < kVelocityTolerance;

    m_r = m_nextR;
    m_observer.Predict(m_cappedU, dt);

    if (ref.pose == m_goal) {
        Disable();
    } else {
        Enable();
    }
}

void DrivetrainController::Reset() {
    m_observer.Reset();
    m_r.setZero();
    m_nextR.setZero();
    m_cappedU.setZero();
}

void DrivetrainController::Reset(const frc::Pose2d& initialPose) {
    m_observer.Reset();

    Eigen::Matrix<double, 10, 1> xHat;
    xHat(0, 0) = initialPose.Translation().X().to<double>();
    xHat(1, 0) = initialPose.Translation().Y().to<double>();
    xHat(2, 0) = initialPose.Rotation().Radians().to<double>();
    xHat.block<7, 1>(3, 0).setZero();
    m_observer.SetXhat(xHat);

    m_r.setZero();
    m_nextR.setZero();
    m_cappedU.setZero();
}

Eigen::Matrix<double, 2, 1> DrivetrainController::Controller(
    const Eigen::Matrix<double, 10, 1>& x,
    const Eigen::Matrix<double, 5, 1>& r) {
    double kx = m_K0(0, 0);
    double ky0 = m_K0(0, 1);
    double kvpos0 = m_K0(0, 3);
    double kvneg0 = m_K0(1, 3);
    double ky1 = m_K1(0, 1);
    double ktheta1 = m_K1(0, 2);
    double kvpos1 = m_K1(0, 3);

    double v = (x(State::kLeftVelocity, 0) + x(State::kRightVelocity, 0)) / 2.0;
    double sqrtAbsV = std::sqrt(std::abs(v));

    Eigen::Matrix<double, 2, 5> K;
    K(0, 0) = kx;
    K(0, 1) = (ky0 + (ky1 - ky0) * sqrtAbsV) * wpi::sgn(v);
    K(0, 2) = ktheta1 * sqrtAbsV;
    K(0, 3) = kvpos0 + (kvpos1 - kvpos0) * sqrtAbsV;
    K(0, 4) = kvneg0 - (kvpos1 - kvpos0) * sqrtAbsV;
    K(1, 0) = kx;
    K(1, 1) = -K(0, 1);
    K(1, 2) = -K(0, 2);
    K(1, 3) = K(0, 4);
    K(1, 4) = K(0, 3);

    Eigen::Matrix<double, 2, 1> uError;
    uError << x(State::kLeftVoltageError, 0), x(State::kRightVoltageError, 0);

    Eigen::Matrix<double, 5, 5> inRobotFrame =
        Eigen::Matrix<double, 5, 5>::Identity();
    inRobotFrame(0, 0) = std::cos(x(2, 0));
    inRobotFrame(0, 1) = std::sin(x(2, 0));
    inRobotFrame(1, 0) = -std::sin(x(2, 0));
    inRobotFrame(1, 1) = std::cos(x(2, 0));

    Eigen::Matrix<double, 5, 1> error = r - x.block<5, 1>(0, 0);
    error(State::kHeading, 0) = NormalizeAngle(error(State::kHeading, 0));
    return K * inRobotFrame * error;
}

Eigen::Matrix<double, 10, 1> DrivetrainController::Dynamics(
    const Eigen::Matrix<double, 10, 1>& x,
    const Eigen::Matrix<double, 2, 1>& u) {
    // constexpr auto motors = frc::DCMotor::MiniCIM(3);

    // constexpr units::dimensionless_t Glow = 15.32;  // Low gear ratio
    // constexpr units::dimensionless_t Ghigh = 7.08;  // High gear ratio
    // constexpr auto r = 0.0746125_m;                 // Wheel radius
    // constexpr auto m = 63.503_kg;                   // Robot mass
    // constexpr auto J = 5.6_kg_sq_m;                 // Robot moment of
    // inertia

    // constexpr auto C1 =
    //     -1.0 * Ghigh * Ghigh * motors.Kt / (motors.Kv * motors.R * r * r);
    // constexpr auto C2 = Ghigh * motors.Kt / (motors.R * r);
    // constexpr auto k1 = (1 / m + rb * rb / J);
    // constexpr auto k2 = (1 / m - rb * rb / J);

    Eigen::Matrix<double, 4, 2> B;
    B.block<2, 2>(0, 0) = m_plant.B();
    B.block<2, 2>(2, 0).setZero();
    Eigen::Matrix<double, 4, 7> A;
    A.block<2, 2>(0, 0) = m_plant.A();

    A.block<2, 2>(2, 0).setIdentity();
    A.block<4, 2>(0, 2).setZero();
    A.block<4, 2>(0, 4) = B;
    A.block<4, 1>(0, 6) << 0, 0, 1, -1;

    double v = (x(State::kLeftVelocity, 0) + x(State::kRightVelocity, 0)) / 2.0;

    Eigen::Matrix<double, 10, 1> result;
    result(0, 0) = v * std::cos(x(State::kHeading, 0));
    result(1, 0) = v * std::sin(x(State::kHeading, 0));
    result(2, 0) = ((x(State::kRightVelocity, 0) - x(State::kLeftVelocity, 0)) /
                    (2.0 * rb))
                       .to<double>();
    result.block<4, 1>(3, 0) = A * x.block<7, 1>(3, 0) + B * u;
    result.block<3, 1>(7, 0).setZero();
    return result;
}

Eigen::Matrix<double, 3, 1> DrivetrainController::LocalMeasurementModel(
    const Eigen::Matrix<double, 10, 1>& x,
    const Eigen::Matrix<double, 2, 1>& u) {
    static_cast<void>(u);

    Eigen::Matrix<double, 3, 1> y;
    y << x(State::kHeading, 0), x(State::kLeftPosition, 0),
        x(State::kRightPosition, 0);
    return y;
}

Eigen::Matrix<double, 6, 1> DrivetrainController::GlobalMeasurementModel(
    const Eigen::Matrix<double, 10, 1>& x,
    const Eigen::Matrix<double, 2, 1>& u) {
    static_cast<void>(u);

    Eigen::Matrix<double, 6, 1> y;
    y.block<3, 1>(0, 0) = x.block<3, 1>(0, 0);
    y(3, 0) = x(State::kLeftPosition, 0);
    y(4, 0) = x(State::kRightPosition, 0);
    y(5, 0) = (x(State::kRightVelocity, 0) - x(State::kLeftVelocity, 0)) /
              (2.0 * rb.to<double>());
    return y;
}

void DrivetrainController::ScaleCapU(Eigen::Matrix<double, 2, 1>* u) {
    bool outputCapped =
        std::abs((*u)(0, 0)) > 12.0 || std::abs((*u)(1, 0)) > 12.0;

    if (outputCapped) {
        *u *= 12.0 / u->lpNorm<Eigen::Infinity>();
    }
}


#include <iostream>
#include <fstream>
#include <Eigen>

using namespace std;
using namespace Eigen;

// function definitions for reading matrices from .txt file
void read_sparse_matrix(const std::string& filename, SparseMatrix<float, RowMajor>& matrix);
void read_matrix(std::string file, MatrixXf& matrix);

int main()
{
    // read matrices from benchmark test files
    SparseMatrix<float, RowMajor> P;
    read_sparse_matrix("../benchmark/P.txt", P);
    SparseMatrix<float, RowMajor> Gamma;
    read_sparse_matrix("../benchmark/SH.txt", Gamma);
    SparseMatrix<float, RowMajor> invCeta;
    read_sparse_matrix("../benchmark/invCeta.txt", invCeta);
    SparseMatrix<float, RowMajor> F;
    read_sparse_matrix("../benchmark/F.txt", F);
    MatrixXf invCphi(P.cols(),P.cols());
    read_matrix("../benchmark/invCphi.txt", invCphi);

    // read sensor measurements from benchmark test files
    VectorXf s(invCeta.cols());
    fstream input("../benchmark/s.txt");
    string line;
    int indx = 0;
    if (input.is_open()) {
        while (getline(input,line)) {
            s(indx)=stof(line);
            indx++;
        }
        input.close();
    }
    cout << "read input done!" << endl;

    // calculate reconstruction matrix R
    MatrixXf R(P.cols(), P.cols());
    cout << "start computation..." << endl;
    R = (Gamma*P).transpose()*invCeta*(Gamma*P)+invCphi;

    // reconstruct turbulent layers from sensor measurements
    cout << "reconstruct layers from sensor measurements..." << endl;
    VectorXf b = (Gamma*P).transpose()*invCeta*s;
    VectorXf phi = R.colPivHouseholderQr().solve(b);

    // apply mirror fitting
    cout << "compute actuator commands..." << endl;
    VectorXf a(F.cols());
    a = F*phi;
    cout << "computation done!" << endl;

    // save actuator commands to file
    ofstream output("../benchmark/out.txt");
    if (output.is_open()) {
        for(int i = 0; i < a.size(); i++) {
            output << a(i) << '\n';
        }
    }
    cout << "stored output file out.txt!" << endl;

    return 0;
}

void read_sparse_matrix(const std::string& filename, SparseMatrix<float, RowMajor>& matrix) {
    ifstream fin(filename);
    if(fin.is_open()) {
        int M = 0, N = 0, L = 0;
        fin >> M >> N >> L;
        vector<Eigen::Triplet<float>> triple;
        triple.reserve(L);
        int m, n;
        float data;
        while (fin >> n >> m >> data) {
            triple.push_back(Triplet<float>(m-1, n-1, data));// m - 1 and n - 1 to set index start from 0
        }
        fin.close();

        matrix.resize(M, N);
        matrix.reserve(L);
        matrix.setFromTriplets(triple.begin(), triple.end());
    }
    else {
        std::cout << "Can not open sparse matrix file: " << filename << std::endl;
    }
}

void read_matrix(std::string file, MatrixXf& matrix) {
    std::ifstream in(file);
    std::string line;
    int row=0, col=0;

    if (in.is_open()) {
        while (std::getline(in, line)) {
            char* ptr = (char*) line.c_str();
            int len = line.length();

            col = 0;
            char* start = ptr;
            for (int i = 0; i<len; i++) {
                if (ptr[i]=='\t') {
                    matrix(row, col++) = stof(start);
                    start = ptr+i+1;
                }
            }
            row++;
        }
        in.close();
    }
}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/Splines>
#include "mader_types.hpp"

void CPs2TrajAndPwp(std::vector<Eigen::Vector3d> &q, std::vector<mt::state> &traj, mt::PieceWisePol &solution_, int N, int p,
                    int num_pol, Eigen::RowVectorXd &knots, double dc);

Eigen::Spline3d findInterpolatingBsplineNormalized(const std::vector<double> &times,
                                                   const std::vector<Eigen::Vector3d> &positions);


/*
 *
 * Copyright (c) Karl Meerbergen 2008
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_PTSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_PTSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A tridiagonal symmetric or Hermitian positive definite matrix
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * ptsv() computes the solution to a system of linear equations
     * A*X = B, where A is an N-by-N Hermitian positive definite tridiagonal
     * matrix, and X and B are N-by-NRHS matrices.
     *
     * A is factored as A = L*D*L**T, and the factored form of A is then
     * used to solve the system of equations.
     */

    namespace detail {

      inline
      void ptsv ( int const n, int const nrhs,
                 float* d, float* e, float* b, int const ldb, int* info)
      {
        LAPACK_SPTSV (&n, &nrhs, d, e, b, &ldb, info);
      }

      inline
      void ptsv ( int const n, int const nrhs,
                 double* d, double* e, double* b, int const ldb, int* info)
      {
        LAPACK_DPTSV (&n, &nrhs, d, e, b, &ldb, info);
      }

      inline
      void ptsv ( int const n, int const nrhs,
                 float* d, traits::complex_f* e, traits::complex_f* b, int const ldb,
                 int* info)
      {
        LAPACK_CPTSV (&n, &nrhs, d, traits::complex_ptr(e), traits::complex_ptr(b), &ldb, info);
      }

      inline
      void ptsv ( int const n, int const nrhs,
                 double* d, traits::complex_d* e, traits::complex_d* b, int const ldb,
                 int* info)
      {
        LAPACK_ZPTSV (&n, &nrhs, d, traits::complex_ptr(e), traits::complex_ptr(b), &ldb, info);
      }

    }

    template <typename D, typename E, typename B>
    inline int ptsv( D& d, E& e, B& b ) {
      int const n = traits::vector_size(d) ;
      assert( n==traits::vector_size(e)+1 ) ;
      assert( n==traits::matrix_num_rows(b) ) ;

      int info ;
      detail::ptsv( n, traits::matrix_num_columns (b)
                  , traits::vector_storage(d)
                  , traits::vector_storage(e)
                  , traits::matrix_storage(b)
                  , traits::leading_dimension(b)
                  , &info
                  ) ;
      return info ;
    } // ptsv()

    /*
     * pttrf() computes the L * D * L^H factorization of a Hermitian
     * positive definite tridiagonal matrix A.  The factorization may also
     * be regarded as having the form A = U^H * D *U.
     */

    namespace detail {

      inline
      void pttrf ( int const n, float* d, float* e, int* info) {
        LAPACK_SPTTRF ( &n, d, e, info) ;
      }

      inline
      void pttrf ( int const n, double* d, double* e, int* info) {
        LAPACK_DPTTRF ( &n, d, e, info);
      }

      inline
      void pttrf ( int const n, float* d, traits::complex_f* e, int* info)
      {
        LAPACK_CPTTRF ( &n, d, traits::complex_ptr(e), info);
      }

      inline
      void pttrf ( int const n, double* d, traits::complex_d* e, int* info)
      {
        LAPACK_ZPTTRF ( &n, d, traits::complex_ptr(e), info);
      }

    }

    template <typename D, typename E>
    inline
    int pttrf (D& d, E& e) {
      int const n = traits::vector_size (d);
      assert (n == traits::vector_size (e) + 1);
      int info;
      detail::pttrf ( n, traits::vector_storage(d), traits::vector_storage(e), &info);
      return info;
    }

    /*
     * pttrs() solves a tridiagonal system of the form
     *   A * X = B
     * using the factorization A = U^H * D * U or A = L * D * L^H computed by pttrf().
     * D is a diagonal matrix specified in the vector D, U (or L) is a unit
     * bidiagonal matrix whose superdiagonal (subdiagonal) is specified in
     * the vector E, and X and B are N by NRHS matrices.
     */

    namespace detail {

      inline
      void pttrs (char const uplo, int const n, int const nrhs,
                  float const* d, float const* e, float* b, int const ldb, int* info)
      {
        LAPACK_SPTTRS (&n, &nrhs, d, e, b, &ldb, info);
      }

      inline
      void pttrs (char const uplo, int const n, int const nrhs,
                  double const* d, double const* e, double* b, int const ldb, int* info)
      {
        LAPACK_DPTTRS (&n, &nrhs, d, e, b, &ldb, info);
      }

      inline
      void pttrs (char const uplo, int const n, int const nrhs,
                  float const* d,
                  traits::complex_f const* e,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CPTTRS (&uplo, &n, &nrhs, d,
                       traits::complex_ptr (e),
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void pttrs (char const uplo, int const n, int const nrhs,
                  double const* d,
                  traits::complex_d const* e,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZPTTRS (&uplo, &n, &nrhs, d,
                       traits::complex_ptr (e),
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename D, typename E, typename MatrB>
    inline
    int pttrs (char uplo, D const& d, E const& e, MatrB& b) {
      int const n = traits::vector_size (d);
      assert (n == traits::vector_size (e) + 1);
      assert (n == traits::matrix_num_rows (b));

      int info;
      detail::pttrs (uplo, n, traits::matrix_num_columns (b),
                     traits::vector_storage (d),
                     traits::vector_storage (e),
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    } // pttrs()

  }

}}}

#endif


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
Authors:  Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
Licensed under Apache2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <Eigen/Dense>
#include <ct/core/core.h>
#include "Quadrotor.hpp"

namespace ct {
namespace models {

class QuadrotorLinear : public ct::core::LinearSystem<quadrotor::nStates, quadrotor::nControls>
{
public:
    typedef ct::core::StateVector<quadrotor::nStates> state_vector_t;
    typedef ct::core::ControlVector<quadrotor::nControls> control_vector_t;

    typedef Eigen::Matrix<double, quadrotor::nStates, quadrotor::nStates> state_matrix_t;
    typedef Eigen::Matrix<double, quadrotor::nStates, quadrotor::nControls> state_control_matrix_t;

    virtual QuadrotorLinear* clone() const override { return new QuadrotorLinear(*this); }
    virtual const state_matrix_t& getDerivativeState(const state_vector_t& x,
        const control_vector_t& u,
        const ct::core::Time t = 0.0) override
    {
        A_ = A_quadrotor(x, u);
        return A_;
    }

    virtual const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
        const control_vector_t& u,
        const ct::core::Time t = 0.0) override
    {
        B_ = B_quadrotor(x, u);
        return B_;
    }

private:
    state_matrix_t A_;
    state_control_matrix_t B_;
};
}
}


#ifdef _DEBUG
#include "../../../library/src/debug_template.hpp"
#define DMP(...) dump(#__VA_ARGS__, __VA_ARGS__)
#else
#define DMP(...) ((void)0)
#endif

#include <cassert>
#include <cstdio>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>

#include <boost/multiprecision/cpp_dec_float.hpp>
using dec_float = boost::multiprecision::number<boost::multiprecision::cpp_dec_float<10>>;

using namespace std;
using lint = long long;
constexpr int INF = 1010101010;
constexpr lint LINF = 1LL << 60;
struct init {
    init() {
        cin.tie(nullptr);
        ios::sync_with_stdio(false);
        cout << fixed << setprecision(10);
    }
} init_;

int main() {

    dec_float X, Y, R;
    cin >> X >> Y >> R;

    int l = (int)ceil(X - R), r = (int)floor(X + R);

    lint ans = 0;
    for (int i = l; i <= r; i++) {
        dec_float p = sqrt(R * R - abs(X - i) * abs(X - i));
        ans += (int)floor(Y + p) - (int)ceil(Y - p) + 1;
    }

    cout << ans << '\n';

    return 0;
}


#include "TestScheme.h"

#include <NTL/BasicThreadPool.h>
#include <NTL/RR.h>
#include <NTL/ZZ.h>
#include <cstdlib>
#include <iostream>
#include <string>

#include "Cipher.h"
#include "CZZ.h"
#include "EvaluatorUtils.h"
#include "NumUtils.h"
#include "Params.h"
#include "PubKey.h"
#include "Scheme.h"
#include "SchemeAlgo.h"
#include "SchemeAux.h"
#include "SecKey.h"
#include "StringUtils.h"
#include "TimeUtils.h"

using namespace NTL;

//-----------------------------------------

void TestScheme::testEncodeBatch(long logN, long logq, long precisionBits, long logSlots) {
	cout << "!!! START TEST ENCODE BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = (1 << logSlots);
	CZZ* mvec = EvaluatorUtils::evaluateRandomVals(slots, precisionBits);
	//-----------------------------------------
	timeutils.start("Encrypt batch");
	Cipher cipher = scheme.encrypt(mvec, slots);
	timeutils.stop("Encrypt batch");
	//-----------------------------------------
	timeutils.start("Decrypt batch");
	CZZ* dvec = scheme.decrypt(secretKey, cipher);
	timeutils.stop("Decrypt batch");
	//-----------------------------------------
	StringUtils::showcompare(mvec, dvec, slots, "val");
	//-----------------------------------------
	cout << "!!! END TEST ENCODE BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testConjugateBatch(long logN, long logq, long precisionBits, long logSlots) {
	cout << "!!! START TEST CONJUGATE BATCH !!!" << endl;
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = (1 << logSlots);
	CZZ* mvec = EvaluatorUtils::evaluateRandomVals(slots, precisionBits);
	CZZ* mvecconj = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		mvecconj[i] = mvec[i].conjugate();
	}

	Cipher cipher = scheme.encrypt(mvec, slots);
	timeutils.start("Conjugate batch");
	Cipher cconj = scheme.conjugate(cipher);
	timeutils.stop("Conjugate batch");

	CZZ* dvecconj = scheme.decrypt(secretKey, cconj);

	StringUtils::showcompare(mvecconj, dvecconj, slots, "conj");

	cout << "!!! END TEST CONJUGATE BATCH !!!" << endl;
}

void TestScheme::testimultBatch(long logN, long logq, long precisionBits, long logSlots) {
	cout << "!!! START TEST i MULTIPLICATION BATCH !!!" << endl;
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = (1 << logSlots);
	CZZ* mvec = EvaluatorUtils::evaluateRandomVals(slots, precisionBits);
	CZZ* imvec = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		imvec[i].r = -mvec[i].i;
		imvec[i].i = mvec[i].r;
	}

	Cipher cipher = scheme.encrypt(mvec, slots);
	timeutils.start("Multiplication by i batch");
	Cipher icipher = scheme.imult(cipher, precisionBits);
	timeutils.stop("Multiplication by i batch");

	CZZ* idvec = scheme.decrypt(secretKey, icipher);

	StringUtils::showcompare(imvec, idvec, slots, "imult");

	cout << "!!! END TEST i MULTIPLICATION BATCH !!!" << endl;
}

void TestScheme::testRotateByPo2Batch(long logN, long logq, long precisionBits, long rotlogSlots, long logSlots, bool isLeft) {
	cout << "!!! START TEST ROTATE BY POWER OF 2 BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = (1 << logSlots);
	long rotSlots = (1 << rotlogSlots);
	CZZ* mvec = EvaluatorUtils::evaluateRandomVals(slots, precisionBits);
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	if(isLeft) {
		timeutils.start("Left Rotate by power of 2 batch");
		scheme.leftRotateByPo2AndEqual(cipher, rotlogSlots);
		timeutils.stop("Left Rotate by power of 2 batch");
	} else {
		timeutils.start("Right Rotate by power of 2 batch");
		scheme.rightRotateByPo2AndEqual(cipher, rotlogSlots);
		timeutils.stop("Right Rotate by power of 2 batch");
	}
	//-----------------------------------------
	CZZ* dvec = scheme.decrypt(secretKey, cipher);
	if(isLeft) {
		EvaluatorUtils::leftRotateAndEqual(mvec, slots, rotSlots);
	} else {
		EvaluatorUtils::rightRotateAndEqual(mvec, slots, rotSlots);
	}
	StringUtils::showcompare(mvec, dvec, slots, "val");
	//-----------------------------------------
	cout << "!!! END TEST ROTATE BY POWER OF 2 BATCH !!!" << endl;
}

void TestScheme::testRotateBatch(long logN, long logq, long precisionBits, long rotSlots, long logSlots, bool isLeft) {
	cout << "!!! START TEST ROTATE BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = (1 << logSlots);
	CZZ* mvec = EvaluatorUtils::evaluateRandomVals(slots, precisionBits);
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	if(isLeft) {
		timeutils.start("Left rotate batch");
		scheme.leftRotateAndEqual(cipher, rotSlots);
		timeutils.stop("Left rotate batch");
	} else {
		timeutils.start("Right rotate batch");
		scheme.rightRotateAndEqual(cipher, rotSlots);
		timeutils.stop("Right rotate batch");
	}
	//-----------------------------------------
	CZZ* dvec = scheme.decrypt(secretKey, cipher);
	if(isLeft) {
		EvaluatorUtils::leftRotateAndEqual(mvec, slots, rotSlots);
	} else {
		EvaluatorUtils::rightRotateAndEqual(mvec, slots, rotSlots);
	}
	StringUtils::showcompare(mvec, dvec, slots, "val");
	//-----------------------------------------
	cout << "!!! END TEST ROTATE BATCH !!!" << endl;
}

void TestScheme::testSlotsSum(long logN, long logq, long precisionBits, long logSlots) {
	cout << "!!! START TEST SLOTS SUM !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = (1 << logSlots);
	CZZ* mvec = EvaluatorUtils::evaluateRandomVals(slots, precisionBits);
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start("slots sum");
	algo.partialSlotsSumAndEqual(cipher, slots);
	timeutils.stop("slots sum");
	//-----------------------------------------
	CZZ* dvec = scheme.decrypt(secretKey, cipher);
	CZZ msum = CZZ();
	for (long i = 0; i < slots; ++i) {
		msum += mvec[i];
	}
	StringUtils::showcompare(msum, dvec, slots, "slotsum");
	//-----------------------------------------
	cout << "!!! END TEST SLOTS SUM !!!" << endl;
}

//-----------------------------------------

void TestScheme::testPowerOf2Batch(long logN, long logq, long precisionBits, long logDegree, long logSlots) {
	cout << "!!! START TEST POWER OF 2 BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* mpow = new CZZ[slots];

	CZZ** mpows = new CZZ*[logDegree + 1];
	for (long i = 0; i < logDegree + 1; ++i) {
		mpows[i] = new CZZ[slots];
	}

	for (long i = 0; i < slots; ++i) {
		RR angle = random_RR();
		RR mr = cos(angle * 2 * Pi);
		RR mi = sin(angle * 2 * Pi);
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		mpow[i] = EvaluatorUtils::evaluatePow2(mr, mi, logDegree, precisionBits);
		for (int j = 0; j < logDegree + 1; ++j) {
			mpows[j][i] = EvaluatorUtils::evaluatePow2(mr, mi, j, precisionBits);
		}
	}
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start("Power of 2 batch");
	Cipher cpow = algo.powerOf2(cipher, precisionBits, logDegree);
	timeutils.stop("Power of 2 batch");

	//-----------------------------------------
	CZZ* dpow = scheme.decrypt(secretKey, cpow);
	StringUtils::showcompare(mpow, dpow, slots, "pow");
	//-----------------------------------------
	cout << "!!! END TEST POWER OF 2 BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testPowerBatch(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST POWER BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* mpow = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		RR angle = random_RR();
		RR mr = cos(angle * 2 * Pi);
		RR mi = sin(angle * 2 * Pi);
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		mpow[i] = EvaluatorUtils::evaluatePow(mr, mi, degree, precisionBits);
	}
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start("Power batch");
	Cipher cpow = algo.power(cipher, precisionBits, degree);
	timeutils.stop("Power batch");
	//-----------------------------------------
	CZZ* dpow = scheme.decrypt(secretKey, cpow);
	StringUtils::showcompare(mpow, dpow, slots, "pow");
	//-----------------------------------------
	cout << "!!! END TEST POWER BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testProdOfPo2Batch(long logN, long logq, long precisionBits, long logDegree, long logSlots) {
	cout << "!!! START TEST PROD OF POWER OF 2 BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(4);
	long slots = 1 << logSlots;
	long degree = 1 << logDegree;
	Cipher* cvec = new Cipher[degree];
	CZZ** mvec = new CZZ*[degree];
	for (long i = 0; i < degree; ++i) {
		mvec[i] = new CZZ[slots];
	}
	CZZ* pvec = new CZZ[slots];
	for (long i = 0; i < degree; ++i) {
		for (long j = 0; j < slots; ++j) {
			mvec[i][j] = EvaluatorUtils::evaluateRandomCircleVal(precisionBits);
		}
	}
	for (long j = 0; j < slots; ++j) {
		pvec[j] = mvec[0][j];
		for (long i = 1; i < degree; ++i) {
			pvec[j] *= mvec[i][j];
			pvec[j] >>= precisionBits;
		}
	}
	for (long i = 0; i < degree; ++i) {
		cvec[i] = scheme.encrypt(mvec[i], slots);
	}
	//-----------------------------------------
	timeutils.start("Product of power of 2 batch");
	Cipher cprod = algo.prodOfPo2(cvec, precisionBits, logDegree);
	timeutils.stop("Product of power of 2 batch");
	//-----------------------------------------
	CZZ* dvec = scheme.decrypt(secretKey, cprod);
	StringUtils::showcompare(pvec, dvec, slots, "prod");
	//-----------------------------------------
	cout << "!!! END TEST PROD OF POWER OF 2 BATCH !!!" << endl;
}

void TestScheme::testProdBatch(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST PROD BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(4);
	long slots = 1 << logSlots;
	Cipher* cvec = new Cipher[degree];
	CZZ** mvec = new CZZ*[degree];
	for (long i = 0; i < degree; ++i) {
		mvec[i] = new CZZ[slots];
	}
	CZZ* pvec = new CZZ[slots];
	for (long i = 0; i < degree; ++i) {
		for (long j = 0; j < slots; ++j) {
			mvec[i][j] = EvaluatorUtils::evaluateRandomCircleVal(precisionBits);
		}
	}
	for (long j = 0; j < slots; ++j) {
		pvec[j] = mvec[0][j];
		for (long i = 1; i < degree; ++i) {
			pvec[j] *= mvec[i][j];
			pvec[j] >>= precisionBits;
		}
	}
	for (long i = 0; i < degree; ++i) {
		cvec[i] = scheme.encrypt(mvec[i], slots);
	}
	//-----------------------------------------
	timeutils.start("Product batch");
	Cipher cprod = algo.prod(cvec, precisionBits, degree);
	timeutils.stop("Product batch");
	//-----------------------------------------
	CZZ* dvec = scheme.decrypt(secretKey, cprod);
	StringUtils::showcompare(pvec, dvec, slots, "prod");
	//-----------------------------------------
	cout << "!!! END TEST PROD BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testInverseBatch(long logN, long logq, long precisionBits, long invSteps, long logSlots) {
	cout << "!!! START TEST INVERSE BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* minv = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		RR angle = random_RR() / 20;
		RR mr = cos(angle * 2 * Pi);
		RR mi = sin(angle * 2 * Pi);
		mvec[i] = EvaluatorUtils::evaluateVal(1 - mr, -mi, precisionBits);
		minv[i] = EvaluatorUtils::evaluateInverse(mr, mi, precisionBits);
	}
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start("Inverse batch");
	Cipher cinv = algo.inverse(cipher, precisionBits, invSteps);
	timeutils.stop("Inverse batch");
	//-----------------------------------------
	CZZ* dinv = scheme.decrypt(secretKey, cinv);
	StringUtils::showcompare(minv, dinv, slots, "inv");
	//-----------------------------------------
	cout << "!!! END TEST INVERSE BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testLogarithmBatch(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST LOGARITHM BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* mlog = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		double mr = (double)rand() / RAND_MAX / 20;
		double mi = (double)rand() / RAND_MAX / 20;
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		mlog[i] = EvaluatorUtils::evaluateLogarithm(1 + mr, mi, precisionBits);
	}
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start(LOGARITHM + " batch");
	Cipher clog = algo.function(cipher, LOGARITHM, precisionBits, degree);
	timeutils.stop(LOGARITHM + " batch");
	//-----------------------------------------
	CZZ* dlog = scheme.decrypt(secretKey, clog);
	StringUtils::showcompare(mlog, dlog, slots, LOGARITHM);
	//-----------------------------------------
	cout << "!!! END TEST LOGARITHM BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testExponentBatch(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST EXPONENT BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* mexp = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		RR mr = random_RR();
		RR mi = random_RR();
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		mexp[i] = EvaluatorUtils::evaluateExponent(mr, mi, precisionBits);
	}
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start(EXPONENT + " batch");
	Cipher cexp = algo.function(cipher, EXPONENT, precisionBits, degree);
	timeutils.stop(EXPONENT + " batch");
	//-----------------------------------------
	CZZ* dexp = scheme.decrypt(secretKey, cexp);
	StringUtils::showcompare(mexp, dexp, slots, EXPONENT);
	//-----------------------------------------
	cout << "!!! END TEST EXPONENT BATCH !!!" << endl;
}

void TestScheme::testExponentBatchLazy(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST EXPONENT LAZY !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* mexp = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		RR mr = random_RR();
		RR mi = random_RR();
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		mexp[i] = EvaluatorUtils::evaluateExponent(mr, mi, precisionBits);
	}
	EvaluatorUtils::leftShiftAndEqual(mexp, slots, precisionBits);
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start(EXPONENT + " lazy");
	Cipher cexp = algo.functionLazy(cipher, EXPONENT, precisionBits, degree);
	timeutils.stop(EXPONENT + " lazy");
	//-----------------------------------------
	CZZ* dexp = scheme.decrypt(secretKey, cexp);
	StringUtils::showcompare(mexp, dexp, slots, EXPONENT);
	//-----------------------------------------
	cout << "!!! END TEST EXPONENT LAZY !!!" << endl;
}

//-----------------------------------------

void TestScheme::testSigmoidBatch(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST SIGMOID BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* msig = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		RR mr = random_RR();
		RR mi = random_RR();
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		msig[i] = EvaluatorUtils::evaluateSigmoid(mr, mi, precisionBits);
	}
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start(SIGMOID + " batch");
	Cipher csig = algo.function(cipher, SIGMOID, precisionBits, degree);
	timeutils.stop(SIGMOID + " batch");
	//-----------------------------------------
	CZZ* dsig = scheme.decrypt(secretKey, csig);
	StringUtils::showcompare(msig, dsig, slots, SIGMOID);
	//-----------------------------------------
	cout << "!!! END TEST SIGMOID BATCH !!!" << endl;
}

void TestScheme::testSigmoidBatchLazy(long logN, long logq, long precisionBits, long degree, long logSlots) {
	cout << "!!! START TEST SIGMOID LAZY !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	long slots = 1 << logSlots;
	CZZ* mvec = new CZZ[slots];
	CZZ* msig = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		RR mr = random_RR();
		RR mi = random_RR();
		mvec[i] = EvaluatorUtils::evaluateVal(mr, mi, precisionBits);
		msig[i] = EvaluatorUtils::evaluateSigmoid(mr, mi, precisionBits);
	}
	EvaluatorUtils::leftShiftAndEqual(msig, slots, precisionBits);
	Cipher cipher = scheme.encrypt(mvec, slots);
	//-----------------------------------------
	timeutils.start(SIGMOID + " lazy");
	Cipher csig = algo.functionLazy(cipher, SIGMOID, precisionBits, degree);
	timeutils.stop(SIGMOID + " lazy");
	//-----------------------------------------
	CZZ* dsig = scheme.decrypt(secretKey, csig);
	StringUtils::showcompare(msig, dsig, slots, SIGMOID);
	//-----------------------------------------
	cout << "!!! END TEST SIGMOID LAZY !!!" << endl;
}

//-----------------------------------------

void TestScheme::testFFTBatch(long logN, long logq, long precisionBits, long logSlots, long logfftdim) {
	cout << "!!! START TEST FFT BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(8);
	//-----------------------------------------
	long fftdim = 1 << logfftdim;
	long slots = 1 << logSlots;
	CZZ** mvec1 = new CZZ*[slots];
	CZZ** mvec2 = new CZZ*[slots];

	for (long i = 0; i < slots; ++i) {
		mvec1[i] = EvaluatorUtils::evaluateRandomVals(fftdim, precisionBits);
		mvec2[i] = EvaluatorUtils::evaluateRandomVals(fftdim, precisionBits);
	}

	Cipher* cvec1 = new Cipher[fftdim];
	Cipher* cvec2 = new Cipher[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		CZZ* mvals1 = new CZZ[slots];
		CZZ* mvals2	= new CZZ[slots];
		for (long i = 0; i < slots; ++i) {
			mvals1[i] = mvec1[i][j];
			mvals2[i] = mvec2[i][j];
		}
		cvec1[j] = scheme.encrypt(mvals1, slots);
		cvec2[j] = scheme.encrypt(mvals2, slots);
		delete[] mvals1;
		delete[] mvals2;
	}

	for (long i = 0; i < slots; ++i) {
		NumUtils::fft(mvec1[i], fftdim, schemeaux);
		NumUtils::fft(mvec2[i], fftdim, schemeaux);
		for (long j = 0; j < fftdim; ++j) {
			mvec1[i][j] *= mvec2[i][j];
			mvec1[i][j] >>= precisionBits;
		}
		NumUtils::fftInv(mvec1[i], fftdim, schemeaux);
	}
	//-----------------------------------------
	timeutils.start("ciphers fft 1 batch");
	algo.fft(cvec1, fftdim);
	timeutils.stop("ciphers fft 1 batch");
	//-----------------------------------------
	timeutils.start("ciphers fft 2 batch");
	algo.fft(cvec2, fftdim);
	timeutils.stop("ciphers fft 2 batch");
	//-----------------------------------------
	timeutils.start("ciphers hadamard mult batch");
	algo.multModSwitchAndEqualVec(cvec1, cvec2, precisionBits, fftdim);
	timeutils.stop("ciphers hadamard mult batch");
	//-----------------------------------------
	delete[] cvec2;
	//-----------------------------------------
	timeutils.start("ciphers fft inverse batch");
	algo.fftInv(cvec1, fftdim);
	timeutils.stop("ciphers fft inverse batch");
	//-----------------------------------------
	CZZ** dvec1 = new CZZ*[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		dvec1[j] = scheme.decrypt(secretKey, cvec1[j]);
	}
	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			StringUtils::showcompare(mvec1[i][j], dvec1[j][i], "fft");
		}
	}
	//-----------------------------------------
	cout << "!!! END TEST FFT BATCH !!!" << endl;
}

void TestScheme::testFFTBatchLazy(long logN, long logq, long precisionBits, long logSlots, long logfftdim) {
	cout << "!!! START TEST FFT BATCH LAZY !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(8);
	//-----------------------------------------
	long fftdim = 1 << logfftdim;
	long slots = 1 << logSlots;
	CZZ** mvec1 = new CZZ*[slots];
	CZZ** mvec2 = new CZZ*[slots];

	for (long i = 0; i < slots; ++i) {
		mvec1[i] = EvaluatorUtils::evaluateRandomVals(fftdim, precisionBits);
		mvec2[i] = EvaluatorUtils::evaluateRandomVals(fftdim, precisionBits);
	}

	Cipher* cvec1 = new Cipher[fftdim];
	Cipher* cvec2 = new Cipher[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		CZZ* mvals1 = new CZZ[slots];
		CZZ* mvals2	= new CZZ[slots];
		for (long i = 0; i < slots; ++i) {
			mvals1[i] = mvec1[i][j];
			mvals2[i] = mvec2[i][j];
		}
		cvec1[j] = scheme.encrypt(mvals1, slots);
		cvec2[j] = scheme.encrypt(mvals2, slots);
		delete[] mvals1;
		delete[] mvals2;
	}

	for (long i = 0; i < slots; ++i) {
		NumUtils::fft(mvec1[i], fftdim, schemeaux);
		NumUtils::fft(mvec2[i], fftdim, schemeaux);
		for (long j = 0; j < fftdim; ++j) {
			mvec1[i][j] *= mvec2[i][j];
			mvec1[i][j] >>= precisionBits;
		}
		NumUtils::fftInvLazy(mvec1[i], fftdim, schemeaux);
	}
	//-----------------------------------------
	timeutils.start("ciphers fft 1");
	algo.fft(cvec1, fftdim);
	timeutils.stop("ciphers fft 1");
	//-----------------------------------------
	timeutils.start("ciphers fft 2");
	algo.fft(cvec2, fftdim);
	timeutils.stop("ciphers fft 2");
	//-----------------------------------------
	timeutils.start("ciphers hadamard mult");
	algo.multModSwitchAndEqualVec(cvec1, cvec2, precisionBits, fftdim);
	timeutils.stop("ciphers hadamard mult");
	//-----------------------------------------
	delete[] cvec2;
	//-----------------------------------------
	timeutils.start("ciphers fft inverse lazy");
	algo.fftInvLazy(cvec1, fftdim);
	timeutils.stop("ciphers fft inverse lazy");
	//-----------------------------------------
	CZZ** dvec1 = new CZZ*[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		dvec1[j] = scheme.decrypt(secretKey, cvec1[j]);
	}
	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			StringUtils::showcompare(mvec1[i][j], dvec1[j][i], "fft");
		}
	}
	//-----------------------------------------
	cout << "!!! END TEST FFT BATCH LAZY !!!" << endl;
}

void TestScheme::testFFTBatchLazyMultipleHadamard(long logN, long logq, long precisionBits, long logSlots, long logfftdim, long logHdim) {
	cout << "!!! START TEST FFT BATCH LAZY MULTIPLE HADAMARD !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Params params(logN, logq);
	SecKey secretKey(params);
	PubKey publicKey(params, secretKey);
	SchemeAux schemeaux(logN);
	Scheme scheme(params, publicKey, schemeaux);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(8);
	//-----------------------------------------
	long fftdim = 1 << logfftdim;
	long hdim = 1 << logHdim;
	long slots = 1 << logSlots;
	CZZ*** mvecs = new CZZ**[hdim];
	Cipher** cvecs = new Cipher*[hdim];
	for (long h = 0; h < hdim; ++h) {
		mvecs[h] = new CZZ*[slots];

		for (long i = 0; i < slots; ++i) {
			mvecs[h][i] = EvaluatorUtils::evaluateRandomVals(fftdim, precisionBits);
		}

		cvecs[h] = new Cipher[fftdim];

		for (long j = 0; j < fftdim; ++j) {
			CZZ* mvals = new CZZ[slots];
			for (long i = 0; i < slots; ++i) {
				mvals[i] = mvecs[h][i][j];
			}
			cvecs[h][j] = scheme.encrypt(mvals, slots);
			delete[] mvals;
		}
		for (long i = 0; i < slots; ++i) {
			NumUtils::fft(mvecs[h][i], fftdim, schemeaux);
		}
	}

	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			for (long s = logHdim - 1; s >= 0; --s) {
				long spow = 1 << s;
				for (long h = 0; h < spow; ++h) {
					mvecs[h][i][j] *= mvecs[h+spow][i][j];
					mvecs[h][i][j] >>= precisionBits;
				}
			}
		}
	}

	for (long i = 0; i < slots; ++i) {
		NumUtils::fftInvLazy(mvecs[0][i], fftdim, schemeaux);
	}

	for (long h = 1; h < hdim; ++h) {
		for (long i = 0; i < slots; ++i) {
			delete[] mvecs[h][i];
		}
		delete[] mvecs[h];
	}

	for (long h = 0; h < hdim; ++h) {
		timeutils.start("ciphers fft");
		algo.fft(cvecs[h], fftdim);
		timeutils.stop("ciphers fft");
	}
	for (long s = logHdim - 1; s >= 0; --s) {
		long spow = 1 << s;
		for (long h = 0; h < spow; ++h) {
			timeutils.start("ciphers hadamard mult");
			algo.multModSwitchAndEqualVec(cvecs[h], cvecs[h+spow], precisionBits, fftdim);
			timeutils.stop("ciphers hadamard mult");
			delete[] cvecs[h+spow];
		}
	}

	timeutils.start("ciphers fft inverse lazy");
	algo.fftInvLazy(cvecs[0], fftdim);
	timeutils.stop("ciphers fft inverse lazy");

	//-----------------------------------------
	CZZ** dvec = new CZZ*[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		dvec[j] = scheme.decrypt(secretKey, cvecs[0][j]);
	}
	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			StringUtils::showcompare(mvecs[0][i][j], dvec[j][i], "fft");
		}
	}
	//-----------------------------------------
	cout << "!!! END TEST FFT BATCH LAZY MULTIPLE HADAMARD !!!" << endl;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/MedianFilter.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class HPSS
{
public:
  using ArrayXXd = Eigen::ArrayXXd;
  using ArrayXXcd = Eigen::ArrayXXcd;
  using ArrayXcd = Eigen::ArrayXcd;

  enum HPSSMode { kClassic, kCoupled, kAdvanced };

  HPSS(index maxFFTSize, index maxHSize)
      : mMaxH(maxFFTSize / 2 + 1, maxHSize),
        mMaxV(maxFFTSize / 2 + 1, maxHSize),
        mMaxBuf(maxFFTSize / 2 + 1, maxHSize)
  {
    mMaxH.setZero();
    mMaxV.setZero();
    mMaxBuf.setZero();
  }

  void init(index nBins, index hSize)
  {
    using namespace Eigen;
    assert(hSize % 2);
    assert(nBins <= mMaxBuf.rows());
    assert(hSize <= mMaxBuf.cols());

    mH = mMaxH.block(0, 0, nBins, hSize);
    mV = mMaxV.block(0, 0, nBins, hSize);
    mBuf = mMaxBuf.block(0, 0, nBins, hSize);
    mH.setZero();
    mV.setZero();
    mBuf.setZero();

    mHFilters = std::vector<MedianFilter>(asUnsigned(nBins));
    for (index i = 0; i < nBins; i++) { mHFilters[asUnsigned(i)].init(hSize); }
    mInitialized = true;
  }

  void processFrame(const ComplexVectorView in, ComplexMatrixView out,
                    index vSize, index hSize, index mode, double hThresholdX1,
                    double hThresholdY1, double hThresholdX2,
                    double hThresholdY2, double pThresholdX1,
                    double pThresholdY1, double pThresholdX2,
                    double pThresholdY2)
  {
    using namespace Eigen;
    assert(mInitialized);

    index    h2 = (hSize - 1) / 2;
    index    v2 = (vSize - 1) / 2;
    index    nBins = in.size();
    ArrayXcd frame = _impl::asEigen<Array>(in);
    ArrayXd  mag = frame.abs().real();

    mV.block(0, 0, nBins, hSize - 1) = mV.block(0, 1, nBins, hSize - 1);
    mH.block(0, 0, nBins, hSize - 1) = mH.block(0, 1, nBins, hSize - 1);
    mBuf.block(0, 0, nBins, hSize - 1) = mBuf.block(0, 1, nBins, hSize - 1);

    ArrayXd padded = ArrayXd::Zero(2 * vSize + nBins);
    ArrayXd resultV = ArrayXd::Zero(padded.size());
    ArrayXd tmp = ArrayXd::Zero(padded.size());

    padded.segment(v2, nBins) = mag;
    mVFilter.init(vSize);
    for (index i = 0; i < padded.size(); i++)
    { tmp(i) = mVFilter.processSample(padded(i)); }
    mV.block(0, hSize - 1, nBins, 1) = tmp.segment(v2 * 3, nBins);
    mBuf.block(0, hSize - 1, nBins, 1) = frame;
    ArrayXd tmpRow = ArrayXd::Zero(2 * hSize);
    for (index i = 0; i < nBins; i++)
    { mH(i, h2 + 1) = mHFilters[asUnsigned(i)].processSample(mag(i)); }
    ArrayXXcd result(nBins, 3);
    ArrayXd   harmonicMask = ArrayXd::Ones(nBins);
    ArrayXd   percussiveMask = ArrayXd::Ones(nBins);
    ArrayXd   residualMask =
        mode == kAdvanced ? ArrayXd::Ones(nBins) : ArrayXd::Zero(nBins);
    switch (mode)
    {
    case kClassic: {
      ArrayXd HV = mH.col(0) + mV.col(0);
      ArrayXd mult = (1.0 / HV.max(epsilon));
      harmonicMask = (mH.col(0) * mult);
      percussiveMask = (mV.col(0) * mult);
      break;
    }
    case kCoupled: {
      harmonicMask = ((mH.col(0) / mV.col(0)) >
                      makeThreshold(nBins, hThresholdX1, hThresholdY1,
                                    hThresholdX2, hThresholdY2))
                         .cast<double>();
      percussiveMask = 1 - harmonicMask;
      break;
    }
    case kAdvanced: {
      harmonicMask = ((mH.col(0) / mV.col(0)) >
                      makeThreshold(nBins, hThresholdX1, hThresholdY1,
                                    hThresholdX2, hThresholdY2))
                         .cast<double>();
      percussiveMask = ((mV.col(0) / mH.col(0)) >
                        makeThreshold(nBins, pThresholdX1, pThresholdY1,
                                      pThresholdX2, pThresholdY2))
                           .cast<double>();
      residualMask = residualMask * (1 - harmonicMask);
      residualMask = residualMask * (1 - percussiveMask);
      ArrayXd maskNorm =
          (1. / (harmonicMask + percussiveMask + residualMask)).max(epsilon);
      harmonicMask = harmonicMask * maskNorm;
      percussiveMask = percussiveMask * maskNorm;
      residualMask = residualMask * maskNorm;
      break;
    }
    }
    result.col(0) = mBuf.col(0) * harmonicMask.min(1.0);
    result.col(1) = mBuf.col(0) * percussiveMask.min(1.0);
    result.col(2) = mBuf.col(0) * residualMask.min(1.0);
    out = _impl::asFluid(result);
  }
  bool initialized() { return mInitialized; }

private:
  Eigen::ArrayXd makeThreshold(index nBins, double x1, double y1, double x2,
                               double y2)
  {
    using namespace Eigen;
    ArrayXd threshold = ArrayXd::Ones(nBins);
    index   kneeStart = static_cast<index>(std::floor(x1 * nBins));
    index   kneeEnd = static_cast<index>(std::floor(x2 * nBins));
    index   kneeLength = kneeEnd - kneeStart;
    threshold.segment(0, kneeStart) =
        ArrayXd::Constant(kneeStart, 10).pow(y1 / 20.0);
    threshold.segment(kneeStart, kneeLength) =
        ArrayXd::Constant(kneeLength, 10)
            .pow(ArrayXd::LinSpaced(kneeLength, y1, y2) / 20.0);
    threshold.segment(kneeEnd, nBins - kneeEnd) =
        ArrayXd::Constant(nBins - kneeEnd, 10).pow(y2 / 20.0);
    return threshold;
  }

  std::vector<MedianFilter> mHFilters;
  MedianFilter              mVFilter;

  ArrayXXd  mMaxH;
  ArrayXXd  mMaxV;
  ArrayXXcd mMaxBuf;
  ArrayXXd  mV;
  ArrayXXd  mH;
  ArrayXXcd mBuf;
  bool      mInitialized{false};
};
} // namespace algorithm
} // namespace fluid


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/basics/testMacros.h"

#include "tudat/astro/basic_astro/geodeticCoordinateConversions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_geodetic_coordinate_conversions )

BOOST_AUTO_TEST_CASE( testGeodeticCoordinateConversions )
{
    using namespace coordinate_conversions;
    using namespace unit_conversions;

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testGeodeticPosition( -63.667,
                                                convertDegreesToRadians( -7.26654999 ),
                                                convertDegreesToRadians( 72.36312094 ) );

    // Central body characteristics (WGS84 Earth ellipsoid).
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    // Test conversion to geodetic coordinates.
    {
        // Calculate geodetic position.
        const Eigen::Vector3d calculatedGeodeticPosition =
                convertCartesianToGeodeticCoordinates(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );

        // Compare per coefficients (different tolerances).
        BOOST_CHECK_SMALL( calculatedGeodeticPosition.x( ) - testGeodeticPosition.x( ), 1.0E-4 );
        BOOST_CHECK_SMALL( calculatedGeodeticPosition.y( ) - testGeodeticPosition.y( ), 1.0E-10 );
        BOOST_CHECK_SMALL( calculatedGeodeticPosition.z( ) - testGeodeticPosition.z( ), 1.0E-10 );
    }

    // Test separate functions for altitude and geodetic latitude.
    {
        // Calculate altitude and geodetic latitude using dedicated functions.
        const double directAltitude = calculateAltitudeOverOblateSpheroid(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );
        const double directGeodeticLatitude = calculateGeodeticLatitude(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );

        // Compare values.
        BOOST_CHECK_SMALL( directAltitude - testGeodeticPosition.x( ), 1.0E-4 );
        BOOST_CHECK_SMALL( directGeodeticLatitude - testGeodeticPosition.y( ), 1.0E-10 );
    }

    // Test conversions from geodetic coordinates to cartesian position.
    {
        const Eigen::Vector3d calculateCartesianPosition =
                convertGeodeticToCartesianCoordinates(
                    testGeodeticPosition, equatorialRadius, flattening  );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    calculateCartesianPosition, testCartesianPosition, 1.0E-9 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef STAN_MATH_PRIM_SCAL_PROB_HYPERGEOMETRIC_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_HYPERGEOMETRIC_LPMF_HPP

#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <boost/math/distributions.hpp>

namespace stan {
namespace math {

// Hypergeometric(n|N, a, b)  [0 <= n <= a;  0 <= N-n <= b;  0 <= N <= a+b]
// n: #white balls drawn;  N: #balls drawn;
// a: #white balls;  b: #black balls
template <bool propto, typename T_n, typename T_N, typename T_a, typename T_b>
double hypergeometric_lpmf(const T_n& n, const T_N& N, const T_a& a,
                           const T_b& b) {
  static const char* function = "hypergeometric_lpmf";

  if (size_zero(n, N, a, b))
    return 0.0;

  scalar_seq_view<T_n> n_vec(n);
  scalar_seq_view<T_N> N_vec(N);
  scalar_seq_view<T_a> a_vec(a);
  scalar_seq_view<T_b> b_vec(b);
  size_t size = max_size(n, N, a, b);

  double logp(0.0);
  check_bounded(function, "Successes variable", n, 0, a);
  check_greater(function, "Draws parameter", N, n);
  for (size_t i = 0; i < size; i++) {
    check_bounded(function, "Draws parameter minus successes variable",
                  N_vec[i] - n_vec[i], 0, b_vec[i]);
    check_bounded(function, "Draws parameter", N_vec[i], 0,
                  a_vec[i] + b_vec[i]);
  }
  check_consistent_sizes(function, "Successes variable", n, "Draws parameter",
                         N, "Successes in population parameter", a,
                         "Failures in population parameter", b);

  if (!include_summand<propto>::value)
    return 0.0;

  for (size_t i = 0; i < size; i++)
    logp += math::binomial_coefficient_log(a_vec[i], n_vec[i])
            + math::binomial_coefficient_log(b_vec[i], N_vec[i] - n_vec[i])
            - math::binomial_coefficient_log(a_vec[i] + b_vec[i], N_vec[i]);
  return logp;
}

template <typename T_n, typename T_N, typename T_a, typename T_b>
inline double hypergeometric_lpmf(const T_n& n, const T_N& N, const T_a& a,
                                  const T_b& b) {
  return hypergeometric_lpmf<false>(n, N, a, b);
}

}  // namespace math
}  // namespace stan
#endif


#include <Eigen/Array>

int main(int argc, char *argv[])
{
  std::cout.precision(2);

  // demo static functions
  Eigen::Matrix3f m3 = Eigen::Matrix3f::Random();
  Eigen::Matrix4f m4 = Eigen::Matrix4f::Identity();

  std::cout << "*** Step 1 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // demo non-static set... functions
  m4.setZero();
  m3.diagonal().setOnes();

  std::cout << "*** Step 2 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // demo fixed-size block() expression as lvalue and as rvalue
  m4.block<3,3>(0,1) = m3;
  m3.row(2) = m4.block<1,3>(2,0);

  std::cout << "*** Step 3 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // demo dynamic-size block()
  {
    int rows = 3, cols = 3;
    m4.block(0,1,3,3).setIdentity();
    std::cout << "*** Step 4 ***\nm4:\n" << m4 << std::endl;
  }

  // demo vector blocks
  m4.diagonal().block(1,2).setOnes();
  std::cout << "*** Step 5 ***\nm4.diagonal():\n" << m4.diagonal() << std::endl;
  std::cout << "m4.diagonal().start(3)\n" << m4.diagonal().start(3) << std::endl;

  // demo coeff-wise operations
  m4 = m4.cwise()*m4;
  m3 = m3.cwise().cos();
  std::cout << "*** Step 6 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // sums of coefficients
  std::cout << "*** Step 7 ***\n m4.sum(): " << m4.sum() << std::endl;
  std::cout << "m4.col(2).sum(): " << m4.col(2).sum() << std::endl;
  std::cout << "m4.colwise().sum():\n" << m4.colwise().sum() << std::endl;
  std::cout << "m4.rowwise().sum():\n" << m4.rowwise().sum() << std::endl;

  // demo intelligent auto-evaluation
  m4 = m4 * m4; // auto-evaluates so no aliasing problem (performance penalty is low)
  Eigen::Matrix4f other = (m4 * m4).lazy(); // forces lazy evaluation
  m4 = m4 + m4; // here Eigen goes for lazy evaluation, as with most expressions
  m4 = -m4 + m4 + 5 * m4; // same here, Eigen chooses lazy evaluation for all that.
  m4 = m4 * (m4 + m4); // here Eigen chooses to first evaluate m4 + m4 into a temporary.
                       // indeed, here it is an optimization to cache this intermediate result.
  m3 = m3 * m4.block<3,3>(1,1); // here Eigen chooses NOT to evaluate block() into a temporary
    // because accessing coefficients of that block expression is not more costly than accessing
    // coefficients of a plain matrix.
  m4 = m4 * m4.transpose(); // same here, lazy evaluation of the transpose.
  m4 = m4 * m4.transpose().eval(); // forces immediate evaluation of the transpose

  std::cout << "*** Step 8 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;
}


/*
 * Ewald.hpp
 *
 *  Created on: Mar 2, 2017
 *      Author: wyan
 */

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 * !!!   For unit cubic box only !!!
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 * */

#ifndef STOKES3D3D_EWALD_HPP_
#define STOKES3D3D_EWALD_HPP_

#include <cmath>

#include <Eigen/Dense>

constexpr int DIRECTLAYER = 2;
constexpr double PI314 = 3.1415926535897932384626433;

inline double ERFC(double x) { return std::erfc(x); }

inline double ERF(double x) { return std::erf(x); }

inline double boxperiodic(double x, double xlow, double xhigh) {
    double temp = (x - xlow) / (xhigh - xlow);
    return (x - xlow) - floor(temp) * (xhigh - xlow);
}

inline void Gkernel(const Eigen::Vector3d &target,
                    const Eigen::Vector3d &source, Eigen::Matrix3d &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    double rnormReg = rnorm;
    if (rnorm < 1e-13) {
        answer = Eigen::Matrix3d::Zero();
        return;
    }
    auto part2 = rst * rst.transpose() / (rnormReg * rnormReg * rnormReg);
    auto part1 = Eigen::Matrix3d::Identity() / rnormReg;
    answer = part1 + part2;
}

// periodic in X direction for a unit cubic box [0,1)^3
inline void Gkernel1D(const Eigen::Vector3d &target, Eigen::Matrix3d &G) {
    const int Nsum = 10000;
    G.setZero();
    Eigen::Matrix3d Gtemp1;
    Eigen::Matrix3d Gtemp2;
    Gkernel(target, Eigen::Vector3d(0, 0, 0), G);
    for (int i = 1; i < Nsum + 1; i++) {
        Gkernel(target, Eigen::Vector3d(i, 0, 0), Gtemp1);
        Gkernel(target, Eigen::Vector3d(-i, 0, 0), Gtemp2);
        G += (Gtemp1 + Gtemp2);
    }
}

/*
 * def AEW(xi,rvec):
 r=np.sqrt(rvec.dot(rvec))
 A = 2*(xi*np.exp(-(xi**2)*(r**2))/(np.sqrt(np.pi)*r**2)+ss.erfc(xi*r)/(2*r**3))
 \
    *(r*r*np.identity(3)+np.outer(rvec,rvec)) -
 4*xi/np.sqrt(np.pi)*np.exp(-(xi**2)*(r**2))*np.identity(3) return A
 *
 * */
inline Eigen::Matrix3d AEW(const double xi, const Eigen::Vector3d &rvec) {
    const double r = rvec.norm();
    Eigen::Matrix3d A =
        2 *
            (xi * exp(-(xi * xi) * (r * r)) / (sqrt(PI314) * r * r) +
             erfc(xi * r) / (2 * r * r * r)) *
            (r * r * Eigen::Matrix3d::Identity() + (rvec * rvec.transpose())) -
        4 * xi / sqrt(PI314) * exp(-(xi * xi) * (r * r)) *
            Eigen::Matrix3d::Identity();
    return A;
}

/*
 *
 def BEW(xi,kvec):
 k=np.sqrt(kvec.dot(kvec))
 B =
 8*np.pi*(1+k*k/(4*(xi**2)))*((k**2)*np.identity(3)-np.outer(kvec,kvec))/(k**4)
 return B*np.exp(-k**2/(4*xi**2))
 *
 * */
inline Eigen::Matrix3d BEW(const double xi, const Eigen::Vector3d &kvec) {
    const double k = kvec.norm();
    Eigen::Matrix3d B =
        8 * PI314 * (1 + k * k / (4 * (xi * xi))) *
        ((k * k) * Eigen::Matrix3d::Identity() - (kvec * kvec.transpose())) /
        (k * k * k * k);
    B *= exp(-k * k / (4 * xi * xi));
    return B;
}

/*
 * def stokes3DEwald(rvec,force):
 xi = 2
 r=np.sqrt(rvec.dot(rvec))
 real = 0
 N=4
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 real = real + AEW(xi,rvec+1.0*np.array([i,j,k])).dot(force)
 wave = 0
 N=4
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 kvec=2*np.pi*np.array([i,j,k]) # L = 1
 if(i==0 and j==0 and k==0):
 continue
 else:
 wave = wave + BEW(xi,kvec).dot(force)*np.exp(-complex(0,1)*kvec.dot(rvec))

 return (np.real(wave)+real)

 * */
// periodic in XYZ direction for a unit cubic box [0,1)^3
inline void GkernelEwald3D(const Eigen::Vector3d &rvecIn, Eigen::Matrix3d &Gsum,
                           double box) {
    const double xi = 2;
    Eigen::Vector3d rvec = rvecIn;
    rvec[0] = rvec[0] - floor(rvec[0]);
    rvec[1] = rvec[1] - floor(rvec[1]);
    rvec[2] = rvec[2] - floor(rvec[2]);
    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 10;
    if (r < 1e-11) {
        auto Gself = -4 * xi / sqrt(PI314) *
                     Eigen::Matrix3d::Identity(); // the self term
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                for (int k = -N; k < N + 1; k++) {
                    if (i == 0 && j == 0 && k == 0) {
                        continue;
                    }
                    real =
                        real + AEW(xi, rvec + Eigen::Vector3d(i * box, j * box,
                                                              k * box));
                }
            }
        }
        real += Gself;
    } else {
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                for (int k = -N; k < N + 1; k++) {
                    real =
                        real + AEW(xi, rvec + Eigen::Vector3d(i * box, j * box,
                                                              k * box));
                }
            }
        }
    }
    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();

    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Eigen::Vector3d kvec(2 * PI314 * i / box, 2 * PI314 * j / box,
                                     2 * PI314 * k / box);
                if (i == 0 and j == 0 and k == 0) {
                    continue;
                } else {
                    wave = wave + BEW(xi, kvec) * cos(kvec.dot(rvec));
                }
            }
        }
    }
    Gsum = real + wave * (1 / (box * box * box));
}

/*
 *
 def stokes3DM2L(rvec,force):
 uEwald=stokes3DEwald(rvec,force)
 uNB=0
 N=3
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 uNB=uNB+Gkernel(rvec-np.array([i,j,k])).dot(force)
 return uEwald-uNB
 * */
// Out of Layer 1
inline void GkernelEwald3DFF(const Eigen::Vector3d &rvec,
                             Eigen::Matrix3d &GsumO1) {
    Eigen::Matrix3d Gfree = Eigen::Matrix3d::Zero();
    GkernelEwald3D(rvec, GsumO1, 1.0);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Gkernel(rvec, Eigen::Vector3d(i, j, k), Gfree);
                GsumO1 -= Gfree;
            }
        }
    }
}

inline double lbda(double k, double xi, double z) {
    return exp(-k * k / (4 * xi * xi) - (xi * xi) * (z * z));
}

inline double thetaplus(double k, double xi, double z) {
    return exp(k * z) * ERFC(k / (2 * xi) + xi * z);
}

inline double thetaminus(double k, double xi, double z) {
    return exp(-k * z) * ERFC(k / (2 * xi) - xi * z);
}

inline double J00(double k, double xi, double z) {
    return sqrt(PI314) * lbda(k, xi, z) * xi;
}

inline double J10(double k, double xi, double z) {
    return PI314 * (thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (4 * k);
}

inline double J20(double k, double xi, double z) {
    return sqrt(PI314) * lbda(k, xi, z) / (4 * k * k * xi) +
           PI314 *
               ((thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (8 * k * k * k) +
                (thetaminus(k, xi, z) - thetaplus(k, xi, z)) * z / (8 * k * k) -
                (thetaplus(k, xi, z) + thetaminus(k, xi, z)) /
                    (16 * k * (xi * xi)));
}

inline double J12(double k, double xi, double z) {
    return PI314 * (-thetaplus(k, xi, z) - thetaminus(k, xi, z)) * k / 4 +
           sqrt(PI314) * lbda(k, xi, z) * xi;
}

inline double J22(double k, double xi, double z) {
    return PI314 * ((thetaplus(k, xi, z) + thetaminus(k, xi, z)) * k /
                        (16 * xi * xi) +
                    (thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (8 * k) +
                    (thetaplus(k, xi, z) - thetaminus(k, xi, z)) * z / 8) -
           sqrt(PI314) * lbda(k, xi, z) / (4 * xi);
}

inline double K11(double k, double xi, double z) {
    return PI314 * ((thetaminus(k, xi, z) - thetaplus(k, xi, z))) / 4;
}

inline double K12(double k, double xi, double z) {
    return PI314 *
           ((thetaplus(k, xi, z) - thetaminus(k, xi, z)) / (16 * xi * xi) +
            (thetaminus(k, xi, z) + thetaplus(k, xi, z)) * z / (8 * k));
}

inline void QI(const Eigen::Vector3d &kvec, double xi, double z,
               Eigen::Matrix3d &QI) {
    // 3*3 tensor
    // kvec: np.array([k1,k2,0])
    double knorm = sqrt(kvec[0] * kvec[0] + kvec[1] * kvec[1]);
    QI = 2 * (J00(knorm, xi, z) / (4 * xi * xi) + J10(knorm, xi, z)) *
         Eigen::Matrix3d::Identity();
}

inline void Qkk(const Eigen::Vector3d &kvec, double xi, double z,
                Eigen::Matrix3d &Qreal, Eigen::Matrix3d &Qimg) {
    double k1 = kvec[0];
    double k2 = kvec[1];
    double knorm = sqrt(k1 * k1 + k2 * k2);
    auto j10 = J10(knorm, xi, z);
    auto j20 = J20(knorm, xi, z);
    auto j12 = J12(knorm, xi, z);
    auto j22 = J22(knorm, xi, z);

    auto k11 = K11(knorm, xi, z);
    auto k12 = K12(knorm, xi, z);
    Qreal.setZero();
    Qreal(0, 0) = k1 * k1;
    Qreal(1, 1) = k2 * k2;
    Qreal(0, 1) = k1 * k2;
    Qreal(1, 0) = k1 * k2;

    Qreal *= (j10 / (4 * (xi * xi)) + j20);
    Qreal(2, 2) = (j12 / (4 * xi * xi) + j22);
    Qreal *= -2;

    Qimg.setZero();
    Qimg(0, 2) = k1;
    Qimg(1, 2) = k2;
    Qimg(2, 0) = k1;
    Qimg(2, 1) = k2;
    // Qimg=np.array([[0,0,k1],[0,0,k2],[k1,k2,0]])*( k11/(4*xi**2) + k12 )
    Qimg *= (k11 / (4 * xi * xi) + k12);
    Qimg *= -2;
}

// inline Eigen::Matrix3d uFk0(double xi, double zmn) {
//	Eigen::Matrix3d wavek0;
//	wavek0 = -(4.0 / 1) * (PI314 * (zmn) * ERF(zmn * xi) + sqrt(PI314) / (2
//* xi) * exp(-zmn * zmn * xi * xi)); 	return wavek0;
//
//}
// periodic in XY direction for a unit cubic box [0,1)^3
inline void GkernelEwald2D(const Eigen::Vector3d &rvecIn,
                           Eigen::Matrix3d &Gsum) {
    const double xi = 2;
    Eigen::Vector3d rvec = rvecIn;
    rvec[0] = rvec[0] - floor(rvec[0]);
    rvec[1] = rvec[1] - floor(rvec[1]); // reset to a periodic cell

    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 5;
    if (r < 1e-14) {
        auto Gself = -4 * xi / sqrt(PI314) *
                     Eigen::Matrix3d::Identity(); // the self term
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                if (i == 0 && j == 0) {
                    continue;
                }
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, 0));
            }
        }
        real += Gself;
    } else {
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, 0));
            }
        }
    }

    // k
    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();

    double zmn = rvec[2];
    Eigen::Vector3d rhomn = rvec;
    rhomn[2] = 0;
    Eigen::Matrix3d Qreal;
    Eigen::Matrix3d Qimg;
    Eigen::Matrix3d QImat;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            Eigen::Vector3d kvec(2 * PI314 * i, 2 * PI314 * j, 0);
            if (i == 0 and j == 0) {
                continue;
            }
            Qkk(kvec, xi, zmn, Qreal, Qimg);
            QI(kvec, xi, zmn, QImat);
            wave = wave + (QImat + Qreal) * cos(kvec.dot(rhomn)) -
                   (Qimg)*sin(kvec.dot(rhomn));
        }
    }
    wave *= 4;

    // k=0
    Eigen::Matrix3d waveK0;
    waveK0.setZero();
    /*
     *   I2fn=force
     I2fn[2]=0
     wavek0=-(4/1)*(np.pi*(zmn)*ss.erf(zmn*xi)+np.sqrt(np.pi)/(2*xi)*np.exp(-zmn**2*xi**2))*I2fn
     *
     * */
    waveK0 = -(4 / 1.0) *
             (PI314 * (zmn)*ERF(zmn * xi) +
              sqrt(PI314) / (2 * xi) * exp(-zmn * zmn * xi * xi)) *
             Eigen::Matrix3d::Identity();
    waveK0(2, 2) = 0;

    Gsum = real + wave + waveK0;
}

#endif /* STOKES3D3D_EWALD_HPP_ */


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/utility/frank_matrix.hpp>
#include <boost/foreach.hpp>
#define BOOST_TEST_MODULE test_solver
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

template<typename MATRIX_MAJOR>
void test(int dim, std::string const& name) {
  rokko::serial_dense_solver solver(name);
  solver.initialize(boost::unit_test::framework::master_test_suite().argc,
                    boost::unit_test::framework::master_test_suite().argv);
  rokko::localized_matrix<double, MATRIX_MAJOR> mat(dim, dim);
  rokko::frank_matrix::generate(mat);
  rokko::localized_vector<double> eigval(dim);
  rokko::localized_matrix<double, MATRIX_MAJOR> eigvec(dim, dim);

  solver.diagonalize(mat, eigval, eigvec);

  double sum = 0;
  for(int i = 0; i < dim; ++i) sum += eigval[i];
  BOOST_CHECK_CLOSE(sum, dim * (dim+1) * 0.5, 10e-5);

  rokko::frank_matrix::generate(mat);
  for (int i = 0; i < dim; ++i) {
    double w = eigvec.col(i).transpose() * mat * eigvec.col(i);
    BOOST_CHECK_CLOSE(w, eigval[i], 10e-5);
  }

  solver.finalize();
}

BOOST_AUTO_TEST_CASE(test_solver) {
  const int dim = 100;
  std::cout << "dimension = " << dim << std::endl;

  std::vector<std::string> names;
  int argc = boost::unit_test::framework::master_test_suite().argc;
  if (argc == 1) {
    names = rokko::serial_dense_solver::solvers();
  } else {
    for (int num=1; num < argc; ++num) {
      names.push_back(boost::unit_test::framework::master_test_suite().argv[num]);
    }
  }

  BOOST_FOREACH(std::string name, names) {
    std::cout << "solver = " << name << std::endl;
    std::cout << "  test for row major" << std::endl;
    test<rokko::matrix_row_major>(dim, name);
    std::cout << "  test for column major" << std::endl;
    test<rokko::matrix_col_major>(dim, name);
  }
}


// Copyright (C) 2017 Minhyuk Sung <mhsung@cs.stanford.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
//

#include "LibiglMesh.h"
#include <Eigen/Geometry>
#include <utils/utils.h>

// Mesh processing.
DEFINE_bool(centerize, false, "");
DEFINE_bool(flip_x, false, "");
DEFINE_bool(flip_y, false, "");
DEFINE_bool(flip_z, false, "");
DEFINE_string(translation, "", "(tx, ty, tz)");
DEFINE_string(rotation, "", "(rx, ry, rz)");
DEFINE_string(transformation, "", "(rx, ry, rz, tx, ty, tz)");
DEFINE_string(inverse_transformation, "", "(rx, ry, rz, tx, ty, tz)");
DEFINE_bool(normalize_height, false, "");

// Point set processing.
DEFINE_bool(sample_points, false, "");
DEFINE_bool(with_normals, false, "");
DEFINE_int32(num_sample_points, 1024, "");
DEFINE_bool(normalize_point_set, false, "");
DEFINE_bool(centerize_point_set, false, "");
DEFINE_string(out_point_set_center, "", "");
DEFINE_bool(pca_align_point_set, false, "");
DEFINE_string(out_pca_transformation, "", "");

void LibiglMesh::mesh_processing() {
  bool mesh_modified = false;

  // Centerize.
  if (FLAGS_centerize) {
    V_ = V_.rowwise() - center_.transpose();
    mesh_modified = true;
  }

  // Mesh flipping.
  if (FLAGS_flip_x) { V_.col(0) = -V_.col(0); mesh_modified = true; }
  if (FLAGS_flip_y) { V_.col(1) = -V_.col(1); mesh_modified = true; }
  if (FLAGS_flip_z) { V_.col(2) = -V_.col(2); mesh_modified = true; }

  if (FLAGS_translation != "") {
    std::vector<std::string> strs = Utils::split_string(FLAGS_translation);
    CHECK_EQ(strs.size(), 3);
    Vector3d t;
    for (int i = 0; i < 3; ++i) t[i] = std::stod(strs[i]);
    translate_mesh(V_, t);
    mesh_modified = true;
  }

  if (FLAGS_rotation != "") {
    std::vector<std::string> strs = Utils::split_string(FLAGS_rotation);
    CHECK_EQ(strs.size(), 3);
    Matrix3d R = Matrix3d::Identity();
    for (int i = 0; i < 3; ++i) {
      const double angle = std::stod(strs[i]) / 180.0 * M_PI;
      const AngleAxisd axis_R(angle, Vector3d::Unit(i));
      R = axis_R.toRotationMatrix() * R;
    }
    const Eigen::Matrix<double, Dynamic, 3>& V_temp = V_;
    V_ = (R * V_temp.transpose()).transpose();
    mesh_modified = true;
  }

  if (FLAGS_transformation != "") {
    std::vector<std::string> strs = Utils::split_string(FLAGS_transformation);
    CHECK_EQ(strs.size(), 6);
    Vector3d r, t;
    for (int i = 0; i < 3; ++i) r[i] = std::stod(strs[0 + i]);
    for (int i = 0; i < 3; ++i) t[i] = std::stod(strs[3 + i]);
    transform_mesh(V_, r, t);
    mesh_modified = true;
  }

  if (FLAGS_inverse_transformation != "") {
    std::vector<std::string> strs = Utils::split_string(
        FLAGS_inverse_transformation);
    CHECK_EQ(strs.size(), 6);
    Vector3d r, t;
    for (int i = 0; i < 3; ++i) r[i] = std::stod(strs[0 + i]);
    for (int i = 0; i < 3; ++i) t[i] = std::stod(strs[3 + i]);
    inverse_transform_mesh(V_, r, t);
    mesh_modified = true;
  }

  if (FLAGS_normalize_height) {
    const double height = V_.col(1).maxCoeff() - V_.col(1).minCoeff();
    CHECK_GT(height, 1.0E-6);
    V_ /= height;
  }

  if (mesh_modified) {
    update_bounding_box();
    renderer_->set_mesh(V_, F_);
    renderer_->set_scene_pos(center_.cast<float>(), (float)radius_);
  }
}

void LibiglMesh::point_set_processing() {
  if (FLAGS_sample_points) {
    sample_points_on_mesh(FLAGS_num_sample_points, FLAGS_with_normals);
  }

  if (FLAGS_normalize_point_set) {
    normalize_points();
  }

  if (FLAGS_centerize_point_set ||
      FLAGS_out_point_set_center != "") {
    compute_point_set_center_and_area(
        FLAGS_out_point_set_center, FLAGS_centerize_point_set);
  }

  if (FLAGS_pca_align_point_set) {
    pca_align_points(FLAGS_out_pca_transformation);
  }
}

void LibiglMesh::processing() {
  mesh_processing();
  point_set_processing();
}