// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>

#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

std::string program_dir; // Ugly global variable !!!

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    std::cout << "\n" << name << "\n";
    A= mtl::mat::hilbert_matrix<>(4, 3);

    string fname( mtl::io::join( program_dir, string("matrix_market/write_test_3_") + string(name) + string(".mtx") ) );
    cout << "File name is " << fname << "\nA is\n" << A;

    mtl::io::matrix_market_ostream oms(fname);
    oms << A;
    oms.close();

    Matrix B, C;
    B= mtl::io::matrix_market(fname);
    cout << "\nRead back results in\n" << B;

    C= A - B;
    cout << "\nDifference is\n" << C << "\none_norm of it == " << one_norm(C) << '\n';
}

int main(int, char* argv[])
{
    using namespace mtl;

    compressed2D<double>                             cdr(4, 3);
    compressed2D<float>                              cfr(4, 3);
    // compressed2D<int>                                cir(4, 3); // a Hilbert matrix as int is nonsense
    compressed2D<std::complex<double> >              ccr(4, 3);

    program_dir= mtl::io::directory_name(argv[0]);
    test(cdr, "compressed2D_double");
    test(cfr, "compressed2D_float");
    test(ccr, "compressed2D_complex");

    return 0;
}


// BLAS level 2

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::cout;
using std::endl;

#ifndef F_USE_STD_VECTOR
typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double, ublas::row_major> m_t;
#else
typedef ublas::vector<double, std::vector<double> > vct_t;
typedef ublas::matrix<double, ublas::column_major, std::vector<double> > m_t;
#endif

int main() {

  cout << endl;

  m_t m (10, 8);
  init_m (m, times_plus<double> (10, 1, 1));
  print_m (m, "m");
  cout << endl;

  vct_t v (8);
  atlas::set (1., v);
  print_v (v, "v");
  cout << endl;
  vct_t vy (12); // vector size can be larger
                // than corresponding matrix size

  // vy = m v
  atlas::gemv (CblasNoTrans, 1.0, m, v, 0.0, vy);
  print_v (vy, "vy = m v");
  atlas::gemv (1.0, m, v, 0.0, vy);
  print_v (vy, "vy = m v");
  atlas::gemv (m, v, vy);
  print_v (vy, "vy = m v");
  cout << endl;

  atlas::set (1, vy);
  print_v (vy, "vy");

  // v = m^T vy
  atlas::gemv (CblasTrans, 1.0, m, vy, 0.0, v);
  print_v (v, "v = m^T vy");
  cout << endl;

  // vy = 2.0 m v + 0.5 vy
  atlas::set (1., v);
  print_v (v, "v");
  print_v (vy, "vy");
  atlas::gemv (CblasNoTrans, 2.0, m, v, 0.5, vy);
  print_v (vy, "vy = 2.0 m v + 0.5 vy");
  cout << endl;
  cout << endl;

  atlas::set (1, v);
  atlas::set (0, vy);
  print_v (v, "v");
  print_v (vy, "vy");
  cout << endl;

  // m[2..8][1..7]
  ublas::matrix_range<m_t> mr (m, ublas::range (2, 8), ublas::range (1, 7));
  print_m (mr, "mr = m[2..8][1..7]");
  cout << endl;

  // vy = m[2..8][1..7] v
  atlas::gemv (mr, v, vy);
  print_v (vy, "vy = mr v");
  cout << endl;

  // vy = m[2..8][1..7]^T v
  atlas::gemv (CblasTrans, 1.0, mr, v, 0.0, vy);
  print_v (vy, "vy = mr^T v");
  cout << endl;

  cout << endl;

  // mrr = (m[2..8][1..7])[1..4][2..5]
  ublas::matrix_range<ublas::matrix_range<m_t> >
    mrr (mr, ublas::range (1, 4), ublas::range (2, 5));
  print_m (mrr, "mrr = (m[2..8][1..7])[1..4][2..5]");
  cout << endl;

  // vy = mrr v
  atlas::set (0, vy);
  atlas::gemv (CblasNoTrans, 1.0, mrr, v, 0.0, vy);
  print_v (vy, "vy = mrr v");
  cout << endl;

#ifdef F_COMPILATION_FAILURE
  ublas::matrix_slice<m_t>
    ms (m, ublas::slice (2, 1, 4), ublas::slice (1, 2, 4));
  print_m (ms, "ms = m[2:1:4][1:2:4]");
  cout << endl;

  atlas::set (0, vy);
  atlas::gemv (CblasNoTrans, 1.0, ms, v, 0.0, vy);
  print_v (vy, "vy = ms v");
  cout << endl;
#endif

}


//
// Created by kevin on 3/12/18.
//
#include "dubins.h"

#include <queue>
#include <boost/math/constants/constants.hpp>

namespace {
    const double twopi = 2. * boost::math::constants::pi<double>();
    const double DUBINS_EPS = 1e-6;
    const double DUBINS_ZERO = -1e-7;

    inline double mod2pi(double x) {
        if (x < 0 && x > DUBINS_ZERO)
            return 0;
        double xm = x - twopi * floor(x / twopi);
        if (twopi - xm < .5 * DUBINS_EPS) xm = 0.;
        return xm;
    }

    DubinsStateSpace::DubinsPath dubinsLSL(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
        if (tmp >= DUBINS_ZERO) {
            double theta = atan2(cb - ca, d + sa - sb);
            double t = mod2pi(-alpha + theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha + t) - sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsRSR(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
        if (tmp >= DUBINS_ZERO) {
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha - t) + sa - sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[1], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsRSL(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
        if (tmp >= DUBINS_ZERO) {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
            double t = mod2pi(alpha - theta);
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[2], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsLSR(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
        if (tmp >= DUBINS_ZERO) {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
            double t = mod2pi(-alpha + theta);
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[3], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsRLR(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
        if (fabs(tmp) < 1.) {
            double p = twopi - acos(tmp);
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta + .5 * p);
            double q = mod2pi(alpha - beta - t + p);
            assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
            assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[4], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsLRL(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
        if (fabs(tmp) < 1.) {
            double p = twopi - acos(tmp);
            double theta = atan2(-ca + cb, d + sa - sb);
            double t = mod2pi(-alpha + theta + .5 * p);
            double q = mod2pi(beta - alpha - t + p);
            assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
            assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[5], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubins(double d, double alpha, double beta) {
        if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], 0, d, 0);

        DubinsStateSpace::DubinsPath path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
        double len, minLength = path.length();

        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRSL(d, alpha, beta);
        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLSR(d, alpha, beta);
        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRLR(d, alpha, beta);
        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLRL(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
            path = tmp;
        return path;
    }
}

const DubinsStateSpace::DubinsPathSegmentType DubinsStateSpace::dubinsPathType[6][3] = {
        {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT},
        {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT},
        {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT},
        {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT},
        {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT},
        {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}
};

double DubinsStateSpace::distance(double q0[3], double q1[3]) {
    return rho_ * dubins(q0, q1).length();
}

DubinsStateSpace::DubinsPath DubinsStateSpace::dubins(double q0[3], double q1[3]) {
    double x1 = q0[0], y1 = q0[1], th1 = q0[2];
    double x2 = q1[0], y2 = q1[1], th2 = q1[2];
    double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
    double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
    return ::dubins(d, alpha, beta);
}

void DubinsStateSpace::interpolate(double q0[3], DubinsPath &path, double seg, double s[3]) {

    if (seg < 0.0) seg = 0.0;
    if (seg > path.length()) seg = path.length();

    double phi, v;

    s[0] = s[1] = 0.0;
    s[2] = q0[2];

    for (unsigned int i = 0; i < 3 && seg > 0; ++i) {
        v = std::min(seg, path.length_[i]);
        seg -= v;
        phi = s[2];
        switch (path.type_[i]) {
            case DUBINS_LEFT:
                s[0] += ( sin(phi+v) - sin(phi));
                s[1] += (-cos(phi+v) + cos(phi));
                s[2] = phi + v;
                break;
            case DUBINS_RIGHT:
                s[0] += (-sin(phi-v) + sin(phi));
                s[1] += ( cos(phi-v) - cos(phi));
                s[2] = phi - v;
                break;
            case DUBINS_STRAIGHT:
                s[0] += (v * cos(phi));
                s[1] += (v * sin(phi));
                break;
        }
    }

    s[0] = s[0] * rho_ + q0[0];
    s[1] = s[1] * rho_ + q0[1];

}

void DubinsStateSpace::sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points) {
    DubinsPath path = dubins(q0, q1);
    length = rho_ * path.length();

    for (double seg=0.0; seg<=length; seg+=step_size){
        double qnew[3] = {};
        interpolate(q0, path, seg/rho_, qnew);
        std::vector<double> v(qnew, qnew + sizeof qnew / sizeof qnew[0]);
        points.push_back(v);
    }
    return;
}


//
//  multiply.cpp
//  BGV-Adder
//
//  Created by Hindrik Stegenga on 27/10/2020.
//  Copyright © 2020 RUG. All rights reserved.
//

#include "multiply.hpp"
#include <helib/FHE.h>
#include <NTL/ZZX.h>
#include <NTL/tools.h>

using helib::Ctxt;

long compute_multiplication(i16 lhs, i16 rhs) {
    long k = 128; // Security parameter
    long L = 16; // Number of levels in the modulus default is 16
    long c = 3; // Nr of columns in key switch matrix.
    long w = 64; // secret key hamming weight

    long p = 1021; // plaintext base default = 1021
    long d = 0; // Degree of field extension
    long r = 1; // hensel lifting

    // Determine a value for m
    auto m = helib::FindM(k, L, c, p, d, 0, 0);
    // Setup context
    auto context = helib::Context(m, p, r);
    // Build mod chain
    helib::buildModChain(context, L, c);

    // Generating secret key and public key
    NTL::ZZX encryption_polynomial = context.alMod.getFactorsOverZZ()[0];
    auto secretKey = helib::SecKey(context);
    secretKey.GenSecKey();
    const helib::PubKey& publicKey = secretKey;

    // Initialize ciphertexts
    Ctxt lhs_ciphertext(publicKey), rhs_ciphertext(publicKey);

    // Encrypt them using the public key
    publicKey.Encrypt(lhs_ciphertext, NTL::ZZX(lhs));
    publicKey.Encrypt(rhs_ciphertext, NTL::ZZX(rhs));

    // Compute multiplication of a and b
    lhs_ciphertext *= rhs_ciphertext;

    Ctxt cipher_result = lhs_ciphertext;

    // Decrypt the results using secret key and convert back from
    // polynomial representation to numeric.
    long return_value = 0;
    NTL::ZZX plaintext_result;
    // Decrypt using secret key
    secretKey.Decrypt(plaintext_result, cipher_result);

    // Compensate for negative numbers by checking if it's larger than p/2,
    // In such case it wrapped around due to negative numbers
    conv(return_value, plaintext_result[0]);
    if (return_value > p / 2) {
        return_value += (-1 * p);
    }

    return return_value;
}