#include <boost/test/unit_test.hpp>

#include "Werk/Math/SummaryStatistics.hpp"

BOOST_AUTO_TEST_SUITE(SummaryStatistics)

BOOST_AUTO_TEST_CASE(TestEmpty) {
	Werk::SummaryStatistics<double> s;
	BOOST_REQUIRE_EQUAL(s.count(),0);
}

BOOST_AUTO_TEST_CASE(TestBasic) {
	Werk::SummaryStatistics<double> s;
	s.sample(5.0);
	s.sample(1.0);
	BOOST_REQUIRE_EQUAL(s.count(), 2);
    BOOST_REQUIRE_EQUAL(s.sum(), 6.0);
    BOOST_REQUIRE_EQUAL(s.average(), 3.0);
    BOOST_REQUIRE_EQUAL(s.variance(), 4.0);
    BOOST_REQUIRE_EQUAL(s.stddev(), 2.0);
    s.reset();
    BOOST_REQUIRE_EQUAL(s.count(),0);
    const char* filename = "summary.txt";
    FILE* file = fopen(filename, "rb");
    s.writeJson(file);
}

BOOST_AUTO_TEST_SUITE_END()


#include <ql/quantlib.hpp>

#include <boost/make_shared.hpp>
#include <iostream>
#include <fstream>

using namespace QuantLib;

void outputBasket(std::vector<boost::shared_ptr<CalibrationHelper> > basket,
                  boost::shared_ptr<TermStructure> termStructure) {

    std::cout << "Calibration Basket:" << std::endl;
    std::cout << "expiry;maturityDate;expiryTime;maturityTime;nominal;rate;marketvol" << std::endl;
    for(Size j=0;j<basket.size();j++) {
        boost::shared_ptr<SwaptionHelper> helper = boost::dynamic_pointer_cast<SwaptionHelper>(basket[j]);
        Date endDate = helper->underlyingSwap()->fixedSchedule().dates().back();
        Real nominal = helper->underlyingSwap()->nominal();
        Real vol = helper->volatility()->value();
        Real rate = helper->underlyingSwap()->fixedRate();
        Date expiry = helper->swaption()->exercise()->date(0);
        Real expiryTime = termStructure->timeFromReference(expiry);
        Real endTime = termStructure->timeFromReference(endDate);
        // std::cout << expiry << ";" << endDate << ";" << expiryTime << ";" << endTime << ";" <<
        //     nominal << ";" << rate << ";" << vol << std::endl;
        std::cout << expiry << " & " << endDate << " & " << expiryTime << " & " << endTime << " & " <<
            nominal << " & " << rate << " \\\\" << std::endl;
    }

}

void outputModel(std::vector<Date>& expiries, boost::shared_ptr<Gsr> model) {

    std::cout << "Model parameters: ";
    std::cout << "expiry;volatility" << std::endl;
    for(Size i=0;i<expiries.size();i++) { // first parameters are the vols, after that the mean reversions follow
        //std::cout << expiries[i] << ";" << model->params()[i] << std::endl;
        std::cout << expiries[i] << " & " << model->volatility()[i] << " \\\\" << std::endl;
    }
    std::cout << std::endl;

}

int main(int, char* []) {

    Date refDate(17,June,2013);
    Settings::instance().evaluationDate() = refDate;
    Date effective = TARGET().advance(refDate,2*Days);
    Date maturity = TARGET().advance(effective,10*Years);

    // market data: flat yts 3%, flat vol 20%

    Real rateLevel = 0.03;
    Real volLevel = 0.20;

    boost::shared_ptr<Quote> ytsQuote0(new SimpleQuote(rateLevel));
    boost::shared_ptr<Quote> ytsQuote1(new SimpleQuote(rateLevel+0.0010)); // 10bp shift up for dv01, dv02 calculation
    boost::shared_ptr<Quote> ytsQuote2(new SimpleQuote(rateLevel-0.0010)); // 10bp shift down for dv01, dv02 calculation

    RelinkableHandle<Quote> ytsQuote(ytsQuote0);

    Handle<YieldTermStructure> yts( boost::shared_ptr<YieldTermStructure>(new FlatForward(0,TARGET(),ytsQuote,
                                                                                Actual365Fixed())));

    boost::shared_ptr<Quote> volQuote0(new SimpleQuote(volLevel));
    boost::shared_ptr<Quote> volQuote1(new SimpleQuote(volLevel+0.01));

    RelinkableHandle<Quote> volQuote(volQuote0);

    boost::shared_ptr<SwaptionVolatilityStructure> swaptionVol(new ConstantSwaptionVolatility(0,TARGET(),
                                                          ModifiedFollowing,volQuote,Actual365Fixed()));

    boost::shared_ptr<IborIndex> iborIndex(new Euribor(6*Months,yts));
    boost::shared_ptr<SwapIndex> standardSwapBase(new EuriborSwapIsdaFixA(10*Years,yts));

    // spread Quote

    Real spreadLevel = 0.0100;
    boost::shared_ptr<Quote> spreadQuote0(new SimpleQuote(spreadLevel));

    RelinkableHandle<Quote> spreadQuote(spreadQuote0);

    // non standard swaption instrument (10y, amortizing nominal and step up coupon, yearly exercise dates)
    // we use the nonstandard swap and the nonstandard swaption for bond pricing, i.e. set the floating side to zero
    // receiver swap means we are long the bond
    // we are short the call right, so the rebate is positive

    std::vector<Real> fixedNominal(10), floatingNominal(20,0.0), fixedRate(10);
    for(Size i=0;i<10;i++) {
        fixedNominal[i] = 100.0;//-i*10.0;//-i*5000000;//(i>0 ? fixedNominal[i-1] : 100000000.0 )*1.075;
        floatingNominal[2*i] = floatingNominal[2*i+1] = 0.0; //fixedNominal[i];
        fixedRate[i] = 0.035;//+0.0030*i;
    }

    Schedule fixedSchedule(effective,maturity,1*Years,TARGET(),ModifiedFollowing,ModifiedFollowing,
                           DateGeneration::Forward,false);
    Schedule floatingSchedule(effective,maturity,6*Months,TARGET(),ModifiedFollowing,ModifiedFollowing,
                           DateGeneration::Forward,false);

    boost::shared_ptr<NonstandardSwap> underlying(new NonstandardSwap(VanillaSwap::Receiver,fixedNominal,floatingNominal,
                                                                      fixedSchedule,fixedRate,Thirty360(),floatingSchedule,
                                                                      iborIndex,1.0,0.0,Actual360(),true,true));

    std::vector<Date> exerciseDates;
    std::vector<Real> rebates;
    for(Size i=1;i<10;i++) {
        exerciseDates.push_back(TARGET().advance(fixedSchedule[i],-2*Days));
        rebates.push_back(-100.0);
    }

    //BermudanExercise exerciseTmp(exerciseDates, false);
    boost::shared_ptr<RebatedExercise> exercise =
        boost::make_shared<RebatedExercise>(
            BermudanExercise(exerciseDates, false), rebates, 2, TARGET(),
            Following);

    boost::shared_ptr<NonstandardSwaption> swaption(
        new NonstandardSwaption(underlying, exercise));

    // pricing of vanilla bond part

    std::cout << "============================================================" << std::endl;
    std::cout << "Vanilla part pricing" << std::endl;
    std::cout << "============================================================" << std::endl;

    Leg vanilla = underlying->leg(0);
    Leg other = underlying->leg(1);

    Real vanillaNpv = CashFlows::npv(vanilla,*yts,spreadQuote->value(),yts->dayCounter(),Continuous,NoFrequency,false);
    Real otherNpv = CashFlows::npv(other,*yts,spreadQuote->value(),yts->dayCounter(),Continuous,NoFrequency,false);

    std::cout << "npv = " << vanillaNpv << " (other = " << otherNpv << ")" << std::endl;

    // gsr model (1% mean reversion, intially 1% vol)

    exerciseDates.pop_back();
    std::vector<Date> stepDates(exerciseDates);
    std::vector<Real> vols(exerciseDates.size()+1,0.01);
    std::vector<Real> reversions(exerciseDates.size()+1,0.01);

    boost::shared_ptr<Gsr> gsr(new Gsr(yts,stepDates,vols,reversions,50.0));

    // engines for nonstandard swaption and standard swaption

    // this engine is used for standard swaptions used in model calibration
    boost::shared_ptr<PricingEngine> standardEngine(new Gaussian1dSwaptionEngine(gsr));
    // this engine is used for the non standard swaption
    boost::shared_ptr<PricingEngine> nonStandardEngine(new Gaussian1dNonstandardSwaptionEngine(gsr,64,7.0,true,false,
                                                                                spreadQuote));

    swaption->setPricingEngine(nonStandardEngine);

    std::cout.precision(6);
    std::cout << std::fixed;

    std::cout << "============================================================" << std::endl;
    std::cout << "Model is not calibrated" << std::endl;
    std::cout << "============================================================" << std::endl;
    outputModel(stepDates,gsr);

    std::cout << "Calculate calibration basket" << std::endl;
    std::vector<boost::shared_ptr<CalibrationHelper> > basket = swaption->calibrationBasket(standardSwapBase,swaptionVol);
    for(Size i=0;i<basket.size();i++) basket[i]->setPricingEngine(standardEngine);
    outputBasket(basket,*gsr->termStructure());

    std::cout << "Calibrate the model to the initial basket" << std::endl;
    LevenbergMarquardt lm;
    EndCriteria ec(2000,200,1E-8,1E-8,1E-8);
    gsr->calibrateVolatilitiesIterative(basket,lm,ec);
    outputModel(stepDates,gsr);

    std::cout << "Calculate calibration basket" << std::endl;

    std::vector<boost::shared_ptr<CalibrationHelper> > basket2 = swaption->calibrationBasket(standardSwapBase,swaptionVol);
    for(Size i=0;i<basket2.size();i++) basket2[i]->setPricingEngine(standardEngine);
    outputBasket(basket2,*gsr->termStructure());

    std::cout << "Calibrate the model to the second basket" << std::endl;

    gsr->calibrateVolatilitiesIterative(basket2,lm,ec);
    outputModel(stepDates,gsr);

    std::cout << "Price the nonstandard swaption" << std::endl;

    Real npv0 = swaption->NPV();

    std::cout << "Shift the rate curve by 10bp up, down and reprice to compute delta, gamma" << std::endl;

    ytsQuote.linkTo(ytsQuote1);
    Real npv1 = swaption->NPV();
    ytsQuote.linkTo(ytsQuote2);
    Real npv2 = swaption->NPV();
    ytsQuote.linkTo(ytsQuote0);

    Real npv3 = swaption->NPV();

    std::cout << "NPV(-10bp) = " << npv2 << " NPV(0) = " << npv0 << " NPV(+10bp) = " << npv1 << std::endl;
    std::cout << "DV01 = " << (npv1-npv2) / 20.0 << std::endl;
    std::cout << "DV02 = " << (npv1-2.0*npv0+npv2) / 100.0 << std::endl;
    std::cout << "Vega = " << (npv3-npv0) << std::endl;

    std::cout << "============================================================" << std::endl;
    std::cout << "Compute delta, gamma with model recalibration" << std::endl;
    std::cout << "============================================================" << std::endl;

    gsr->calibrateVolatilitiesIterative(basket2,lm,ec);
    outputModel(stepDates,gsr);
    npv0 = swaption->NPV();

    ytsQuote.linkTo(ytsQuote1);
    gsr->calibrateVolatilitiesIterative(basket2,lm,ec); outputModel(stepDates,gsr);
    npv1 = swaption->NPV();
    ytsQuote.linkTo(ytsQuote2);
    gsr->calibrateVolatilitiesIterative(basket2,lm,ec); outputModel(stepDates,gsr);
    npv2 = swaption->NPV();
    ytsQuote.linkTo(ytsQuote0);

    for(Size i=0;i<basket2.size();i++) boost::dynamic_pointer_cast<SimpleQuote>(*basket2[i]->volatility())
                                           ->setValue(volQuote1->value());
    gsr->calibrateVolatilitiesIterative(basket2,lm,ec); outputModel(stepDates,gsr);
    npv3 = swaption->NPV();
    for(Size i=0;i<basket2.size();i++) boost::dynamic_pointer_cast<SimpleQuote>(*basket2[i]->volatility())
                                           ->setValue(volQuote0->value());

    std::cout << "NPV(-10bp) = " << npv2 << " NPV(0) = " << npv0 << " NPV(+10bp) = " << npv1 << std::endl;
    std::cout << "DV01 = " << (npv1-npv2) / 20.0 << std::endl;
    std::cout << "DV02 = " << (npv1-2.0*npv0+npv2) / 100.0 << std::endl;
    std::cout << "Vega = " << (npv3-npv0) << std::endl;

    std::cout << "============================================================" << std::endl;
    std::cout << "Compute delta, gamma with basket recalculation and recalibration" << std::endl;
    std::cout << "============================================================" << std::endl;

    gsr->calibrateVolatilitiesIterative(basket2,lm,ec);
    outputModel(stepDates,gsr);
    npv0 = swaption->NPV();

    ytsQuote.linkTo(ytsQuote1);
    gsr->calibrateVolatilitiesIterative(basket2,lm,ec); outputModel(stepDates,gsr);
    std::vector<boost::shared_ptr<CalibrationHelper> > basket3a = swaption->calibrationBasket(standardSwapBase,swaptionVol);
    for(Size i=0;i<basket3a.size();i++) basket3a[i]->setPricingEngine(standardEngine);
    outputBasket(basket3a,*gsr->termStructure());
    gsr->calibrateVolatilitiesIterative(basket3a,lm,ec); outputModel(stepDates,gsr);
    npv1 = swaption->NPV();

    ytsQuote.linkTo(ytsQuote2);

    gsr->calibrateVolatilitiesIterative(basket2,lm,ec); outputModel(stepDates,gsr);
    std::vector<boost::shared_ptr<CalibrationHelper> > basket3b = swaption->calibrationBasket(standardSwapBase,swaptionVol);
    for(Size i=0;i<basket3b.size();i++) basket3b[i]->setPricingEngine(standardEngine);
    outputBasket(basket3b,*gsr->termStructure());
    gsr->calibrateVolatilitiesIterative(basket3b,lm,ec); outputModel(stepDates,gsr);
    npv2 = swaption->NPV();

    ytsQuote.linkTo(ytsQuote0);

    for(Size i=0;i<basket2.size();i++) boost::dynamic_pointer_cast<SimpleQuote>(*basket2[i]->volatility())->
                                           setValue(volQuote1->value());
    volQuote.linkTo(volQuote1);
    gsr->calibrateVolatilitiesIterative(basket2,lm,ec); outputModel(stepDates,gsr);
    std::vector<boost::shared_ptr<CalibrationHelper> > basket3c = swaption->calibrationBasket(standardSwapBase,swaptionVol);
    for(Size i=0;i<basket3c.size();i++) basket3c[i]->setPricingEngine(standardEngine);
    outputBasket(basket3c,*gsr->termStructure());
    gsr->calibrateVolatilitiesIterative(basket3c,lm,ec); outputModel(stepDates,gsr);
    npv3 = swaption->NPV();
    for(Size i=0;i<basket2.size();i++) boost::dynamic_pointer_cast<SimpleQuote>(*basket2[i]->volatility())->
                                           setValue(volQuote0->value());
    volQuote.linkTo(volQuote0);

    std::cout << "NPV(-10bp) = " << npv2 << " NPV(0) = " << npv0 << " NPV(+10bp) = " << npv1 << std::endl;
    std::cout << "DV01 = " << (npv1-npv2) / 20.0 << std::endl;
    std::cout << "DV02 = " << (npv1-2.0*npv0+npv2) / 100.0 << std::endl;
    std::cout << "Vega = " << (npv3-npv0) << std::endl;

}


#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>

using namespace boost::multiprecision;
using namespace boost::random;

uint256_t rand_gen(){
	typedef independent_bits_engine<mt19937, 256, uint256_t> generator_type;
   generator_type gen;
   //
   // Generate some values:
   //
   //std::cout << std::hex << std::showbase;
   //for(unsigned i = 0; i < 10; ++i)
    //  std::cout << gen() << std::endl;
    return gen();
}


# include <vector>
# include <iostream>
# include <string>
# include <math.h>
# include <fstream>
# include <sstream>
# include <tuple>
# include <algorithm>
# include <random>
# include <chrono>
#include <unistd.h>

#include <armadillo>

using namespace std;

vector<string> split(string& input, char delimiter)
{
    istringstream stream(input);
    string field;
    vector<string> result;
    while (getline(stream, field, delimiter)) {
        result.push_back(field);
    }
    return result;
}

int get_dataset(const char* file_path, int*& ou_array, int*& ov_array, float*& y0_array)
{

	vector<int> ou;
	vector<int> ov;
	vector<float> y0;
	ifstream ifs(file_path);
	string line;
	while(getline(ifs,line)){

		std::vector<string> strvec = split(line, ' ');

		ou.push_back( stoi(strvec.at(0)) );
		ov.push_back( stoi(strvec.at(1)) );
		y0.push_back( stof(strvec.at(2)) );

	}
	int k = ou.size();
	ou_array = new int[k];
	ov_array = new int[k];
	y0_array = new float[k];
	for (int i = 0; i < k; ++i)
	{
		ou_array[i] = ou[i];
		ov_array[i] = ov[i];
		y0_array[i] = (float)y0[i];
	}

	return k;
}

void show_vec_array(const std::vector<std::vector<int> >& array)
{
	for (int i = 0; i < array.size(); ++i)
	{

		// std::vector<int> array_i = array[i];
		for (int j = 0; j < array[i].size(); ++j)
		{
			printf("%i ", array[i][j]);
		}
		printf("\n");
	}
}

void show_array(int N, int M, float** array)
{
	for (int i = 0; i < N; ++i)
	{
		for (int j = 0; j < M; ++j)
		{
			printf("%f ", array[i][j]);
		}
		printf("\n");
	}
}

int maximum_value(int* array, int size)
{
	int max = array[0];
	for (int i = 0; i < size; ++i)
	{
		if (max < array[i])
		{
			max = array[i];
		}
	}
	return max;
}

float mean_matrix(float** matrix, int N, int M)
{
	float sm = 0;
	for (int i = 0; i < N; ++i)
	{
		for (int j = 0; j < M; ++j)
		{
			sm += matrix[i][j];
		}
	}
	sm = sm/(float)(N*M);
	return sm;
}

float mean_array(float* array, int size)
{
	float sm = 0;
	for (int i = 0; i < size; ++i)
	{
		sm += array[i];
	}
	return sm/(float)size;
}

float std_array(float* array, int size, float mean)
{
	float sm = 0;
	for (int i = 0; i < size; ++i)
	{
		sm += pow(array[i]-mean, 2);
	}
	sm = sm / (float)size;
	sm = sqrt(sm);
	return sm;
}

tuple<  vector< vector<int> >, vector< vector<int> >, vector< vector<int> >, vector< vector<int> >  > getNb(int* ou, int* ov,int N,int M,int k)
{
	vector< vector<int> > nbu;
	vector< vector<int> > nbv;
	vector< vector<int> > nbul;
	vector< vector<int> > nbvl;

	nbu = vector<vector<int>>(N, vector<int>(0, 0));
	nbul = vector<vector<int>>(N, vector<int>(0, 0));
	nbv = vector<vector<int>>(M, vector<int>(0, 0));
	nbvl = vector<vector<int>>(M, vector<int>(0, 0));

	for (int i = 0; i < k; ++i)
	{
		int u_ind = ou[i];
		int v_ind = ov[i];

		nbu[u_ind].push_back(v_ind);
		nbul[u_ind].push_back(i);

		nbv[v_ind].push_back(u_ind);
		nbvl[v_ind].push_back(i);
	}

	return std::forward_as_tuple(nbu, nbv, nbul, nbvl);

}

tuple<  float**, float**, int, float*  > cbmf(int N, int M, int* ou, int* ov, const vector<vector<int>>& nbu, const vector<vector<int>>& nbv, const vector<vector<int>>& nbul, const vector<vector<int>>& nbvl, int k, float* y0, int maxCnt, float gam, double conv, int R, float lam, int* te_u, int* te_v, float* te_y0, int k_te, float mean, float std)
{

	std::random_device rd;
	std::mt19937 mt(rd());
	std::uniform_real_distribution<float> rdm(0.0, 1.0);

	float *rmse_arr = new float[maxCnt];

	// allocating memory
	float **a_hat = new float*[k];
	float **b_hat = new float*[k];
	float **c_hat = new float*[k];
	float **d_hat = new float*[k];
	float **alf = new float*[k];
	float **beta = new float*[k];
	float **gamma = new float*[k];
	float **delta = new float*[k];
	for (int i = 0; i < k; ++i)
	{
		a_hat[i] = new float[R];
		b_hat[i] = new float[R];
		c_hat[i] = new float[R];
		d_hat[i] = new float[R];
		alf[i] = new float[R];
		beta[i] = new float[R];
		gamma[i] = new float[R];
		delta[i] = new float[R];
	}

	float **a_m_hat = new float*[N];
	float **b_m_hat = new float*[N];
	float **c_m_hat = new float*[M];
	float **d_m_hat = new float*[M];
	for (int i = 0; i < N; ++i)
	{
		a_m_hat[i] = new float[R];
		b_m_hat[i] = new float[R];
	}
	for (int i = 0; i < M; ++i)
	{
		c_m_hat[i] = new float[R];
		d_m_hat[i] = new float[R];
	}

	float *alf_m = new float[k];
    float *beta_m = new float[k];
    float *gamma_m = new float[k];
    float *delta_m = new float[k];

    float **u = new float*[N];
    for (int i = 0; i < N; ++i)
    {
    	u[i] = new float[R];
    }
    float **v = new float*[M];
    for (int i = 0; i < M; ++i)
    {
    	v[i] = new float[R];
    }

    auto t0 = std::chrono::system_clock::now();

    // initialize u and v
	arma::sp_mat spmx(N,M);
    for (int i = 0; i < k; ++i)
    {
    	spmx(ou[i], ov[i]) = y0[i];
    }
    arma::mat U;
	arma::vec s;
	arma::mat V;

	svds(U, s, V, spmx, R);

	s = arma::sqrt(s);
	arma::mat S = diagmat(s);

	U = U*S;
	V = V*S;

    for (int i = 0; i < N; ++i)
    {
    	for (int r = 0; r < R; ++r)
    	{
    		u[i][r] = U(i,r);
    	}
    }
    for (int i = 0; i < M; ++i)
    {
    	for (int r = 0; r < R; ++r)
    	{
    		v[i][r] = V(i,r);
    	}
    }

	for (int i = 0; i < k; ++i)
	{
		for (int j = 0; j < R; ++j)
		{
			a_hat[i][j] = rdm(mt)*5;
			b_hat[i][j] = -a_hat[i][j]*u[ou[i]][j];

			c_hat[i][j] = rdm(mt)*5;
			d_hat[i][j] = -c_hat[i][j]*v[ov[i]][j];
		}
	}

	for (int i = 0; i < N; ++i)
	{
		for (int r = 0; r < R; ++r)
		{

			float a_sm = 0;
			float b_sm = 0;
			for (const auto& nb: nbul[i])
			{
				a_sm += a_hat[nb][r];
				b_sm += b_hat[nb][r];
			}
			a_m_hat[i][r] = a_sm + lam;
			b_m_hat[i][r] = b_sm;
		}
	}
	for (int i = 0; i < M; ++i)
	{
		for (int r = 0; r < R; ++r)
		{

			float c_sm = 0;
			float d_sm = 0;
			for (const auto& nb: nbvl[i])
			{
				c_sm += c_hat[nb][r];
				d_sm += d_hat[nb][r];
			}
			c_m_hat[i][r] = c_sm + lam;
			d_m_hat[i][r] = d_sm;
		}
	}

	for (int i = 0; i < k; ++i)
	{
		for (int r = 0; r < R; ++r)
		{
			float vov = v[ov[i]][r];
			float vov2 = pow(vov, 2);
			float uou = u[ou[i]][r];
    		float uou2 = pow(uou, 2);
			alf[i][r] = vov2 / ( a_m_hat[ou[i]][r] - a_hat[i][r] );
			beta[i][r] = ( b_m_hat[ou[i]][r] - b_hat[i][r] ) * vov / ( a_m_hat[ou[i]][r] - a_hat[i][r] );
			gamma[i][r] = uou2 /( c_m_hat[ov[i]][r] - c_hat[i][r] );
			delta[i][r] = ( d_m_hat[ov[i]][r] - d_hat[i][r] ) * uou / ( c_m_hat[ov[i]][r] - c_hat[i][r] );
		}
	}

	float sm1;
	float sm2;
	float sm3;
	float sm4;
	for (int i = 0; i < k; ++i)
	{
		sm1 = 0;
		sm2 = 0;
		sm3 = 0;
		sm4 = 0;
		for (int r = 0; r < R; ++r)
		{
			sm1 += alf[i][r];
			sm2 += beta[i][r];
			sm3 += gamma[i][r];
			sm4 += delta[i][r];
		}
		alf_m[i] = sm1 + 1;
		beta_m[i] = sm2;
		gamma_m[i] = sm3 + 1;
		delta_m[i] = sm4;
	}

	  ///////////////
     /* main loop */
    ///////////////
    int cnt;
    float rmse = 0.0;
    for (cnt = 0; cnt < maxCnt; ++cnt)
    {

    	/* u update */

    	// update alf and beta
    	float dm;
    	float vov;
    	float vov2;
    	for (int i = 0; i < k; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				vov = v[ov[i]][r];
				vov2 = pow(vov, 2);
				dm = a_m_hat[ou[i]][r] - a_hat[i][r];

				alf[i][r] = vov2 / dm;
				beta[i][r] = ( b_m_hat[ou[i]][r] - b_hat[i][r] ) * vov / dm;
			}
		}

		// update alf_m and beta_m
		float sm1;
		float sm2;
		for (int i = 0; i < k; ++i)
		{
			sm1 = 0;
			sm2 = 0;
			for (int r = 0; r < R; ++r)
			{
				sm1 += alf[i][r];
				sm2 += beta[i][r];
			}
			alf_m[i] = sm1 + 1;
			beta_m[i] = sm2 + y0[i];
		}

    	// update a and b hat
    	for (int i = 0; i < k; ++i)
    	{
    		for (int r = 0; r < R; ++r)
    		{
    			vov = v[ov[i]][r];
    			vov2 = pow(vov, 2);
    			dm = alf_m[i] - alf[i][r];

    			a_hat[i][r] = (1-gam)*a_hat[i][r] + gam*vov2/dm;
    			b_hat[i][r] = (1-gam)*b_hat[i][r] - gam*( beta_m[i] - beta[i][r] )*vov/dm;
    		}
    	}

    	// update a and b marginalized hat
    	float a_sm;
    	float b_sm;
    	for (int i = 0; i < N; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				a_sm = 0;
				b_sm = 0;
				for (const auto& nb: nbul[i])
				{
					a_sm += a_hat[nb][r];
					b_sm += b_hat[nb][r];
				}
				a_m_hat[i][r] = a_sm + lam;
				b_m_hat[i][r] = b_sm;
			}
		}

		// update u
		for (int i = 0; i < N; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				u[i][r] = -b_m_hat[i][r]/a_m_hat[i][r];
			}
		}

		/* update v */

		// update gamma and delta
		float uou;
		float uou2;
		for (int i = 0; i < k; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				uou = u[ou[i]][r];
	    		uou2 = pow(uou, 2);
	    		dm = c_m_hat[ov[i]][r] - c_hat[i][r];

				gamma[i][r] = uou2 /dm;
				delta[i][r] = ( d_m_hat[ov[i]][r] - d_hat[i][r] ) * uou / dm;
			}
		}

		// update gamma_m and delta_m
		for (int i = 0; i < k; ++i)
		{
			sm1 = 0.0;
			sm2 = 0.0;
			for (int r = 0; r < R; ++r)
			{
				sm1 += gamma[i][r];
				sm2 += delta[i][r];
			}
			gamma_m[i] = sm1 + 1;
			delta_m[i] = sm2 + y0[i];
		}

		// update c and d hat
    	for (int i = 0; i < k; ++i)
    	{
    		for (int r = 0; r < R; ++r)
    		{
    			uou = u[ou[i]][r];
    			uou2 = pow(uou, 2);
    			dm = gamma_m[i] - gamma[i][r];

    			c_hat[i][r] = (1-gam)*c_hat[i][r] + gam*uou2/dm;
    			d_hat[i][r] = (1-gam)*d_hat[i][r] - gam*( delta_m[i] - delta[i][r] )*uou/dm;
    		}
    	}

    	// update c and d marginalized hat
    	float c_sm;
    	float d_sm;
    	for (int i = 0; i < M; ++i)
		{
			for (int r = 0; r < R; ++r)
			{

				c_sm = 0;
				d_sm = 0;
				for (const auto& nb: nbvl[i])
				{
					c_sm += c_hat[nb][r];
					d_sm += d_hat[nb][r];
				}
				c_m_hat[i][r] = c_sm + lam;
				d_m_hat[i][r] = d_sm;
			}
		}

		// update v
		for (int i = 0; i < M; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				v[i][r] = -d_m_hat[i][r]/c_m_hat[i][r];
			}
		}

		// calulate test rmse
		float mse = 0;
		for (int i = 0; i < k_te; ++i)
		{

			float inf_y0 = 0;
			int u_i = te_u[i];
			int v_i = te_v[i];
			for (int r = 0; r < R; ++r)
			{
				inf_y0 += u[u_i][r]*v[v_i][r];
			}
			inf_y0 = inf_y0*std + mean;
			mse += pow(inf_y0 - te_y0[i], 2);

		}
		mse = mse / (float)k_te;
		float new_rmse = sqrt(mse);
		float dif = abs(new_rmse-rmse);

		rmse_arr[cnt] = new_rmse;

		if (cnt%1 == 0)
		{
			printf("iteration:%d ", cnt);
			printf("rmse:%f\n", new_rmse);
		}

		rmse = new_rmse;
		if (dif < conv || dif != dif)
		{
			break;
		}

    } // end main loop

    return std::forward_as_tuple(u, v, cnt, rmse_arr);

}

tuple<  float**, float**, int, float*  > approx_cbmf(int N, int M, int* ou, int* ov, const vector<vector<int>>& nbu, const vector<vector<int>>& nbv, const vector<vector<int>>& nbul, const vector<vector<int>>& nbvl, int k, float* y0, int maxCnt, float gam, double conv, int R, float lam, int* te_u, int* te_v, float* te_y0, int k_te, float mean, float std)
{

	std::random_device rd;
	std::mt19937 mt(rd());
	std::uniform_real_distribution<float> rdm(0.0, 1.0);
	std::normal_distribution<float> g_rdm(0.0,1.0);

	float *rmse_arr = new float[maxCnt];

	// allocating memory
	float *a = new float[k];
	float *b = new float[k];
	float *c = new float[k];
	float *d = new float[k];

	float **a_hat = new float*[N];
	float **b_hat = new float*[N];
	float **u = new float*[N];
	float **c_hat = new float*[M];
	float **d_hat = new float*[M];
	float **v = new float*[M];
	for (int i = 0; i < N; ++i)
	{
		a_hat[i] = new float[R];
		b_hat[i] = new float[R];
		u[i] = new float[R];
	}
	for (int i = 0; i < M; ++i)
	{
		c_hat[i] = new float[R];
		d_hat[i] = new float[R];
		v[i] = new float[R];
	}

    arma::sp_mat spmx(N,M);
    for (int i = 0; i < k; ++i)
    {
    	spmx(ou[i], ov[i]) = y0[i];
    }
    arma::mat U;
	arma::vec s;
	arma::mat V;

	svds(U, s, V, spmx, R);

	s = arma::sqrt(s);
	arma::mat S = diagmat(s);

	U = U*S;
	V = V*S;

    for (int i = 0; i < N; ++i)
    {
    	for (int r = 0; r < R; ++r)
    	{
    		u[i][r] = U(i,r);
    	}
    }
    for (int i = 0; i < M; ++i)
    {
    	for (int r = 0; r < R; ++r)
    	{
    		v[i][r] = V(i,r);
    	}
    }

    // initialize a,b,c,d hat
	for (int i = 0; i < N; ++i)
	{
		for (int j = 0; j < R; ++j)
		{
			a_hat[i][j] = rdm(mt)+100;
			b_hat[i][j] = (a_hat[i][j])*u[i][j];
		}
	}
	for (int i = 0; i < M; ++i)
	{
		for (int j = 0; j < R; ++j)
		{
			c_hat[i][j] = rdm(mt)+100;
			d_hat[i][j] = (c_hat[i][j])*v[i][j];
		}
	}

	// initialize alf, beta, gamma, delta
	for (int i = 0; i < k; ++i)
	{
		a[i] = rdm(mt);
		b[i] = rdm(mt);
		c[i] = rdm(mt);
		d[i] = rdm(mt);
	}

	  ///////////////
     /* main loop */
    ///////////////
    printf("main loop\n");
    float vov;
	float vov2;
	float uou;
	float uou2;
    float sm1;
    float sm2;
    int cnt;
    float rmse = 0.0;
    for (cnt = 0; cnt < maxCnt; ++cnt)
    {

    	/* u update */

    	// update alf and beta
    	for (int i = 0; i < k; ++i)
		{
			sm1 = 0;
			sm2 = 0;
			for (int r = 0; r < R; ++r)
			{
				vov = v[ov[i]][r];
				uou = u[ou[i]][r];
				vov2 = pow(vov, 2);

				// a
				sm1 += vov2/(a_hat[ou[i]][r]+lam);
				// b
				sm2 += vov*uou;
			}
			b[i] = ( y0[i]-sm2+a[i]*b[i] ) / ( 1+a[i] );
			a[i] = sm1;

		}

    	// update a and b hat
    	for (int i = 0; i < N; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				sm1 = 0;
				sm2 = 0;
				for (const auto& nb: nbul[i])
				{
					vov = v[ov[nb]][r];
					vov2 = pow(vov, 2);
					uou = u[i][r];

					// a_hat
					sm1 += vov2/(1+a[nb]);

					// b_hat
					sm2 += b[nb]*vov;
					sm2 += uou*vov2/(1+a[nb]);

				}

				b_hat[i][r] = (1-gam)*b_hat[i][r] + gam*sm2;
				a_hat[i][r] = (1-gam)*a_hat[i][r] + gam*sm1;

				// update u
				u[i][r] = b_hat[i][r]/(a_hat[i][r]+lam);

			}
		}

		/* update v */

		// update c and d
    	for (int i = 0; i < k; ++i)
		{
			sm1 = 0;
			sm2 = 0;
			for (int r = 0; r < R; ++r)
			{
				vov = v[ov[i]][r];
				uou = u[ou[i]][r];
				uou2 = pow(uou, 2);

				// a
				sm1 += uou2/(c_hat[ov[i]][r]+lam);
				// b
				sm2 += vov*uou;
			}
			d[i] = ( y0[i]-sm2+c[i]*d[i] ) / ( 1+c[i] );
			c[i] = sm1;

		}

		// update c and d hat
    	for (int i = 0; i < M; ++i)
		{
			for (int r = 0; r < R; ++r)
			{
				sm1 = 0;
				sm2 = 0;
				for (const auto& nb: nbvl[i])
				{
					// vov = v[ov[nb]][r];
					vov = v[i][r];
					uou = u[ou[nb]][r];
					uou2 = pow(uou, 2);

					// a_hat
					sm1 += uou2/(1+c[nb]);

					// b_hat
					sm2 += d[nb]*uou;
					sm2 += vov*uou2/(1+c[nb]);

				}

				d_hat[i][r] = (1-gam)*d_hat[i][r] + gam*sm2;
				c_hat[i][r] = (1-gam)*c_hat[i][r] + gam*sm1;

				// update v
				v[i][r] = d_hat[i][r]/(c_hat[i][r]+lam);
			}
		}

		// calulate test rmse
		float mse = 0;
		for (int i = 0; i < k_te; ++i)
		{

			float inf_y0 = 0;
			int u_i = te_u[i];
			int v_i = te_v[i];
			for (int r = 0; r < R; ++r)
			{
				inf_y0 += u[u_i][r]*v[v_i][r];
			}
			inf_y0 = inf_y0*std + mean;

			mse += pow(inf_y0 - te_y0[i], 2);

		}
		mse = mse / (float)k_te;
		float new_rmse = sqrt(mse);
		float dif = abs(new_rmse-rmse);

		rmse_arr[cnt] = new_rmse;

		if (cnt%1 == 0)
		{
			printf("iteration:%d ", cnt);
			printf("rmse:%f\n", new_rmse);
		}

		rmse = new_rmse;
		if (dif < conv || dif != dif)
		{
			break;
		}

    } // end main loop

    return std::forward_as_tuple(u, v, cnt, rmse_arr);

}

void usage(string& doc)
{
	doc = "Usage:\n";
	doc += "   ./cbmf [-l learningrate] [-L lambda] [-R rank] [-m maxiteration] [-r trainfilename] [-t testfilename] [-o outpath] [-c convint] [-p] [-h] [-v]\n";
	doc += "\n";
	doc += "Options:\n";
	doc += "   -l   Learning rate. [default: 0.3]\n";
	doc += "   -L   Regularization parameter. [default: 3]\n";
	doc += "   -R   Rank. [default: 10]\n";
	doc += "   -m   Maximum number of iterations. [default: 100]\n";
	doc += "   -r   Filename of dataset for training. [default: dataset/ml_1m_train.txt]\n";
	doc += "   -t   Filename of dataset for test. [default: dataset/ml_1m_test.txt]\n";
	doc += "   -o   Where output files are to be placed. [default: output/]\n";
	doc += "   -c   Exponent of convergence condition. If RMSE < pow(10, convint) is satisfied, it is regarded as convergence. [default: -5]\n";
	doc += "   -p   If this option is set, ACBMF is to be performed. Without this option, CBMF is to be done.\n";
	doc += "   -h   Show help.\n";
	doc += "   -v   Show version.\n";
	doc += "\n";
	doc += "Examples:\n";
	doc += "Performing CBMF using 'dataset/ml_1m_train.txt' as training dataset and 'dataset/ml_1m_test.txt' as test dataset.\n";
	doc += "   ./cbmf -r dataset/ml_1m_train.txt -t dataset/ml_1m_test.txt\n";
	doc += "Performing ACBMF using 'dataset/ml_1m_train.txt' as training dataset and 'dataset/ml_1m_test.txt' as test dataset.\n";
	doc += "   ./cbmf -p -r dataset/ml_1m_train.txt -t dataset/ml_1m_test.txt\n";
	doc += "Showing help.\n";
	doc += "   ./cbmf -h\n";
	doc += "Showing version.\n";
	doc += "   ./cbmf -v";
}

void version(string& doc)
{
	doc = "cbmf v1.0";
}

int main(int argc, char **argv)
{
	float gam = 0.3;
	float lam = 3;
	int R = 10;
	int maxCnt = 100;
	const char* train_filename = "dataset/ml_1m_train.txt";
	const char* test_filename = "dataset/ml_1m_test.txt";
	const char* outpath = "output/";
	int is_approx = 0;
	int conv_int = -5;
	int opt;
	string doc = "";
	usage(doc);
	string vsn = "";
	version(vsn);
	while ((opt = getopt(argc, argv, "l:L:R:m:r:t:o:c:pshv")) != -1) {
        switch (opt) {
            case 'l': gam=atof(optarg); break;
            case 'L': lam=atof(optarg); break;
            case 'R': R=atoi(optarg); break;
            case 'm': maxCnt=atoi(optarg); break;
            case 'r': train_filename=optarg; break;
            case 't': test_filename=optarg; break;
            case 'o': outpath=optarg; break;
            case 'c': conv_int=atoi(optarg); break;
            case 'p': is_approx=1; break;
            case 'h':
            	printf("%s\n", doc.c_str());
            	return 0;
            case 'v':
            	printf("%s\n", vsn.c_str());
            	return 0;
            default:
            	printf("%s\n", doc.c_str());
            	return 1;
        }
    }
    double conv = pow(10, conv_int);
	int folds = 10;

	printf("learning rate:%f\n", gam);
	printf("lambda:%f\n", lam);
	printf("rank:%d\n", R);
	printf("maxCnt:%d\n", maxCnt);
	printf("is_approx:%d\n", is_approx);
	printf("conv:%f\n", conv);

	int* ou_te = NULL;
	int* ov_te = NULL;
	float* y0_te = NULL;
	int* ou_tr = NULL;
	int* ov_tr = NULL;
	float* y0_tr = NULL;
	int k_te = get_dataset(test_filename,ou_te, ov_te, y0_te);
	int k_tr = get_dataset(train_filename,ou_tr, ov_tr, y0_tr);

	printf("dataset loaded\n");

	int N = maximum_value(ou_tr, k_tr)+1;
	int M = maximum_value(ov_tr, k_tr)+1;
	int K = k_tr;
	printf("N:%d\n", N);
	printf("M:%d\n", M);
	printf("K:%d\n", K);

	float mean = mean_array(y0_tr, k_tr);
	float std = std_array(y0_tr, k_tr, mean);
	printf("mean:%f\n", mean);
	printf("std:%f\n", std);
	for (int i = 0; i < k_tr; ++i)
	{
		y0_tr[i] = (y0_tr[i]-mean)/std;
	}

	vector< vector<int> > nbu;
	vector< vector<int> > nbv;
	vector< vector<int> > nbul;
	vector< vector<int> > nbvl;
	std::tie(nbu, nbv, nbul, nbvl) = getNb(ou_tr, ov_tr, N, M, K);

	printf("got nb\n");

	float** u;
	float** v;
	int cnt;
	float* rmse_arr;

	auto start = std::chrono::system_clock::now();
	if (is_approx == 0)
	{
		std::tie(u, v, cnt, rmse_arr) = cbmf(N, M, ou_tr, ov_tr, nbu, nbv, nbul, nbvl, K, y0_tr, maxCnt, gam, conv, R, lam, ou_te, ov_te, y0_te, k_te, mean, std);
	}
	else
	{
		std::tie(u, v, cnt, rmse_arr) = approx_cbmf(N, M, ou_tr, ov_tr, nbu, nbv, nbul, nbvl, K, y0_tr, maxCnt, gam, conv, R, lam, ou_te, ov_te, y0_te, k_te, mean, std);
	}
	auto end = std::chrono::system_clock::now();

	char gam_char[8];
	sprintf(gam_char, "%.2f", gam);
	char lam_char[8];
	sprintf(lam_char, "%.5f", lam);
	std::string filename_base = "gam=" + string(gam_char) + "_lam=" + string(lam_char) + "_R=" + to_string(R) + "_is_approx=" + to_string(is_approx);
	ofstream rmse_f(outpath + filename_base + "_rmse_approx.txt");
	for (int i = 0; i < cnt+1; ++i)
	{
		char rmse_char[16];
		sprintf(rmse_char, "%.5f", rmse_arr[i]);
		rmse_f << rmse_char;
		rmse_f << '\n';
	}
	rmse_f.close();

	printf("main end\n");

	return 0;
}


#include <NTL/LLL.h>

#include <NTL/new.h>

NTL_START_IMPL

static void ExactDiv(ZZ& qq, const ZZ& a, const ZZ& b)
{
   NTL_ZZRegister(q);
   NTL_ZZRegister(r);

   DivRem(q, r, a, b);
   if (!IsZero(r)) {
      cerr << "a = " << a << "\n";
      cerr << "b = " << b << "\n";
      LogicError("ExactDiv: nonzero remainder");
   }
   qq = q;
}

static void BalDiv(ZZ& q, const ZZ& a, const ZZ& d)

//  rounds a/d to nearest integer, breaking ties
//    by rounding towards zero.  Assumes d > 0.

{
   NTL_ZZRegister(r);
   DivRem(q, r, a, d);

   add(r, r, r);

   long cmp = compare(r, d);
   if (cmp > 0 || (cmp == 0 && q < 0))
      add(q, q, 1);
}

static void MulAddDiv(ZZ& c, const ZZ& c1, const ZZ& c2,
                      const ZZ& x, const ZZ& y, const ZZ& z)

// c = (x*c1 + y*c2)/z

{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);

   mul(t1, x, c1);
   mul(t2, y, c2);
   add(t1, t1, t2);
   ExactDiv(c, t1, z);
}

static void MulSubDiv(ZZ& c, const ZZ& c1, const ZZ& c2,
                      const ZZ& x, const ZZ& y, const ZZ& z)

// c = (x*c1 - y*c2)/z

{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);

   mul(t1, x, c1);
   mul(t2, y, c2);
   sub(t1, t1, t2);
   ExactDiv(c, t1, z);
}

#if 0

static void MulSubDiv(vec_ZZ& c, const vec_ZZ& c1, const vec_ZZ& c2,
                      const ZZ& x, const ZZ& y, const ZZ& z)

// c = (x*c1 + y*c2)/z

{
   long n = c1.length();
   if (c2.length() != n) LogicError("MulSubDiv: length mismatch");
   c.SetLength(n);

   long i;
   for (i = 1; i <= n; i++)
      MulSubDiv(c(i), c1(i), c2(i), x, y, z);
}

#endif

static void RowTransform(vec_ZZ& c1, vec_ZZ& c2,
                         const ZZ& x, const ZZ& y, const ZZ& u, const ZZ& v)

// (c1, c2) = (x*c1 + y*c2, u*c1 + v*c2)

{
   long n = c1.length();
   if (c2.length() != n) LogicError("MulSubDiv: length mismatch");
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);
   NTL_ZZRegister(t3);
   NTL_ZZRegister(t4);

   long i;
   for (i = 1; i <= n; i++) {
      mul(t1, x, c1(i));
      mul(t2, y, c2(i));
      add(t1, t1, t2);

      mul(t3, u, c1(i));
      mul(t4, v, c2(i));
      add(t3, t3, t4);

      c1(i) = t1;
      c2(i) = t3;
   }
}

static void RowTransform(ZZ& c1, ZZ& c2,
                         const ZZ& x, const ZZ& y, const ZZ& u, const ZZ& v)

// (c1, c2) = (x*c1 + y*c2, u*c1 + v*c2)

{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);
   NTL_ZZRegister(t3);
   NTL_ZZRegister(t4);

   mul(t1, x, c1);
   mul(t2, y, c2);
   add(t1, t1, t2);

   mul(t3, u, c1);
   mul(t4, v, c2);
   add(t3, t3, t4);

   c1 = t1;
   c2 = t3;
}

static void MulSubFrom(vec_ZZ& c, const vec_ZZ& c2, const ZZ& x)

// c = c - x*c2

{
   long n = c.length();
   if (c2.length() != n) LogicError("MulSubFrom: length mismatch");

   long i;
   for (i = 1; i <= n; i++)
      MulSubFrom(c(i), c2(i), x);
}

static void MulSubFrom(vec_ZZ& c, const vec_ZZ& c2, long x)

// c = c - x*c2

{
   long n = c.length();
   if (c2.length() != n) LogicError("MulSubFrom: length mismatch");

   long i;
   for (i = 1; i <= n; i++)
      MulSubFrom(c(i), c2(i), x);
}

static long SwapTest(const ZZ& d0, const ZZ& d1, const ZZ& d2, const ZZ& lam,
                     long a, long b)

// test if a*d1^2 > b*(d0*d2 + lam^2)

{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);

   mul(t1, d0, d2);
   sqr(t2, lam);
   add(t1, t1, t2);
   mul(t1, t1, b);

   sqr(t2, d1);
   mul(t2, t2, a);

   return t2 > t1;
}

static
void reduce(long k, long l,
            mat_ZZ& B, vec_long& P, vec_ZZ& D,
            vec_vec_ZZ& lam, mat_ZZ* U)
{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(r);

   if (P(l) == 0) return;
   add(t1, lam(k)(P(l)), lam(k)(P(l)));
   abs(t1, t1);
   if (t1 <= D[P(l)]) return;

   long j;
   long rr, small_r;

   BalDiv(r, lam(k)(P(l)), D[P(l)]);

   if (r.WideSinglePrecision()) {
      small_r = 1;
      rr = to_long(r);
   }
   else {
      small_r = 0;
   }

   if (small_r) {
      MulSubFrom(B(k), B(l), rr);

      if (U) MulSubFrom((*U)(k), (*U)(l), rr);

      for (j = 1; j <= l-1; j++)
         if (P(j) != 0)
            MulSubFrom(lam(k)(P(j)), lam(l)(P(j)), rr);
      MulSubFrom(lam(k)(P(l)), D[P(l)], rr);
   }
   else {
      MulSubFrom(B(k), B(l), r);

      if (U) MulSubFrom((*U)(k), (*U)(l), r);

      for (j = 1; j <= l-1; j++)
         if (P(j) != 0)
            MulSubFrom(lam(k)(P(j)), lam(l)(P(j)), r);
      MulSubFrom(lam(k)(P(l)), D[P(l)], r);
   }

}

static
long swap(long k, mat_ZZ& B, vec_long& P, vec_ZZ& D,
          vec_vec_ZZ& lam, mat_ZZ* U, long m, long verbose)

// swaps vectors k-1 and k;  assumes P(k-1) != 0
// returns 1 if vector k-1 need to be reduced after the swap...
//    this only occurs in 'case 2' when there are linear dependencies

{
   long i, j;
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);
   NTL_ZZRegister(t3);
   NTL_ZZRegister(e);
   NTL_ZZRegister(x);
   NTL_ZZRegister(y);

   if (P(k) != 0) {
      if (verbose) cerr << "swap case 1: " << k << "\n";

      swap(B(k-1), B(k));
      if (U) swap((*U)(k-1), (*U)(k));

      for (j = 1; j <= k-2; j++)
         if (P(j) != 0)
            swap(lam(k-1)(P(j)), lam(k)(P(j)));

      for (i = k+1; i <= m; i++) {
         MulAddDiv(t1, lam(i)(P(k)-1), lam(i)(P(k)),
                   lam(k)(P(k)-1), D[P(k)-2], D[P(k)-1]);
         MulSubDiv(t2, lam(i)(P(k)-1), lam(i)(P(k)),
                   D[P(k)], lam(k)(P(k)-1), D[P(k)-1]);
         lam(i)(P(k)-1) = t1;
         lam(i)(P(k)) = t2;
      }

      MulAddDiv(D[P(k)-1], D[P(k)], lam(k)(P(k)-1),
                D[P(k)-2], lam(k)(P(k)-1), D[P(k)-1]);

      return 0;
   }
   else if (!IsZero(lam(k)(P(k-1)))) {
      if (verbose) cerr << "swap case 2: " << k << "\n";
      XGCD(e, x, y, lam(k)(P(k-1)), D[P(k-1)]);

      ExactDiv(t1, lam(k)(P(k-1)), e);
      ExactDiv(t2, D[P(k-1)], e);

      t3 = t2;
      negate(t2, t2);
      RowTransform(B(k-1), B(k), t1, t2, y, x);
      if (U) RowTransform((*U)(k-1), (*U)(k), t1, t2, y, x);
      for (j = 1; j <= k-2; j++)
         if (P(j) != 0)
            RowTransform(lam(k-1)(P(j)), lam(k)(P(j)), t1, t2, y, x);

      sqr(t2, t2);
      ExactDiv(D[P(k-1)], D[P(k-1)], t2);

      for (i = k+1; i <= m; i++)
         if (P(i) != 0) {
            ExactDiv(D[P(i)], D[P(i)], t2);
            for (j = i+1; j <= m; j++) {
               ExactDiv(lam(j)(P(i)), lam(j)(P(i)), t2);
            }
         }

      for (i = k+1; i <= m; i++) {
         ExactDiv(lam(i)(P(k-1)), lam(i)(P(k-1)), t3);
      }

      swap(P(k-1), P(k));

      return 1;
   }
   else {
      if (verbose) cerr << "swap case 3: " << k << "\n";

      swap(B(k-1), B(k));
      if (U) swap((*U)(k-1), (*U)(k));

      for (j = 1; j <= k-2; j++)
         if (P(j) != 0)
            swap(lam(k-1)(P(j)), lam(k)(P(j)));

      swap(P(k-1), P(k));

      return 0;
   }
}

static
void IncrementalGS(mat_ZZ& B, vec_long& P, vec_ZZ& D, vec_vec_ZZ& lam,
                   long& s, long k)
{
   long n = B.NumCols();
   long m = B.NumRows();

   NTL_ZZRegister(u);
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);

   long i, j;

   for (j = 1; j <= k-1; j++) {
      long posj = P(j);
      if (posj == 0) continue;

      InnerProduct(u, B(k), B(j));
      for (i = 1; i <= posj-1; i++) {
         mul(t1, D[i], u);
         mul(t2, lam(k)(i), lam(j)(i));
         sub(t1, t1, t2);
         div(t1, t1, D[i-1]);
         u = t1;
      }

      lam(k)(posj) = u;
   }

   InnerProduct(u, B(k), B(k));
   for (i = 1; i <= s; i++) {
      mul(t1, D[i], u);
      mul(t2, lam(k)(i), lam(k)(i));
      sub(t1, t1, t2);
      div(t1, t1, D[i-1]);
      u = t1;
   }

   if (u == 0) {
      P(k) = 0;
   }
   else {
      s++;
      P(k) = s;
      D[s] = u;
   }
}

static
long LLL(vec_ZZ& D, mat_ZZ& B, mat_ZZ* U, long a, long b, long verbose)
{
   long m = B.NumRows();
   long n = B.NumCols();

   long force_reduce = 1;

   vec_long P;
   P.SetLength(m);

   D.SetLength(m+1);
   D[0] = 1;

   vec_vec_ZZ lam;

   lam.SetLength(m);

   long j;
   for (j = 1; j <= m; j++)
      lam(j).SetLength(m);

   if (U) ident(*U, m);

   long s = 0;

   long k = 1;
   long max_k = 0;

   while (k <= m) {
      if (k > max_k) {
         IncrementalGS(B, P, D, lam, s, k);
         max_k = k;
      }

      if (k == 1) {
         force_reduce = 1;
         k++;
         continue;
      }

      if (force_reduce)
         for (j = k-1; j >= 1; j--)
            reduce(k, j, B, P, D, lam, U);

      if (P(k-1) != 0 &&
          (P(k) == 0 ||
           SwapTest(D[P(k)], D[P(k)-1], D[P(k)-2], lam(k)(P(k)-1), a, b))) {
         force_reduce = swap(k, B, P, D, lam, U, max_k, verbose);
         k--;
      }
      else {
         force_reduce = 1;
         k++;
      }
   }

   D.SetLength(s+1);
   return s;
}

static
long image(ZZ& det, mat_ZZ& B, mat_ZZ* U, long verbose)
{
   long m = B.NumRows();
   long n = B.NumCols();

   long force_reduce = 1;

   vec_long P;
   P.SetLength(m);

   vec_ZZ D;
   D.SetLength(m+1);
   D[0] = 1;

   vec_vec_ZZ lam;

   lam.SetLength(m);

   long j;
   for (j = 1; j <= m; j++)
      lam(j).SetLength(m);

   if (U) ident(*U, m);

   long s = 0;

   long k = 1;
   long max_k = 0;

   while (k <= m) {
      if (k > max_k) {
         IncrementalGS(B, P, D, lam, s, k);
         max_k = k;
      }

      if (k == 1) {
         force_reduce = 1;
         k++;
         continue;
      }

      if (force_reduce)
         for (j = k-1; j >= 1; j--)
            reduce(k, j, B, P, D, lam, U);

      if (P(k-1) != 0 && P(k) == 0) {
         force_reduce = swap(k, B, P, D, lam, U, max_k, verbose);
         k--;
      }
      else {
         force_reduce = 1;
         k++;
      }
   }

   det = D[s];
   return s;
}

long LLL(ZZ& det, mat_ZZ& B, mat_ZZ& U, long verbose)
{
   vec_ZZ D;
   long s;
   s = LLL(D, B, &U, 3, 4, verbose);
   det = D[s];
   return s;
}

long LLL(ZZ& det, mat_ZZ& B, long verbose)
{
   vec_ZZ D;
   long s;
   s = LLL(D, B, 0, 3, 4, verbose);
   det = D[s];
   return s;
}

long LLL(ZZ& det, mat_ZZ& B, mat_ZZ& U, long a, long b, long verbose)
{
   if (a <= 0 || b <= 0 || a > b || b/4 >= a) LogicError("LLL: bad args");

   vec_ZZ D;
   long s;
   s = LLL(D, B, &U, a, b, verbose);
   det = D[s];
   return s;
}

long LLL(ZZ& det, mat_ZZ& B, long a, long b, long verbose)
{
   if (a <= 0 || b <= 0 || a > b || b/4 >= a) LogicError("LLL: bad args");

   vec_ZZ D;
   long s;
   s = LLL(D, B, 0, a, b, verbose);
   det = D[s];
   return s;
}

long LLL_plus(vec_ZZ& D_out, mat_ZZ& B, mat_ZZ& U, long verbose)
{
   vec_ZZ D;
   long s;
   s = LLL(D, B, &U, 3, 4, verbose);
   D_out = D;
   return s;
}

long LLL_plus(vec_ZZ& D_out, mat_ZZ& B, long verbose)
{
   vec_ZZ D;
   long s;
   s = LLL(D, B, 0, 3, 4, verbose);
   D_out = D;
   return s;
}

long LLL_plus(vec_ZZ& D_out, mat_ZZ& B, mat_ZZ& U, long a, long b, long verbose)
{
   if (a <= 0 || b <= 0 || a > b || b/4 >= a) LogicError("LLL_plus: bad args");

   vec_ZZ D;
   long s;
   s = LLL(D, B, &U, a, b, verbose);
   D_out = D;
   return s;
}

long LLL_plus(vec_ZZ& D_out, mat_ZZ& B, long a, long b, long verbose)
{
   if (a <= 0 || b <= 0 || a > b || b/4 >= a) LogicError("LLL_plus: bad args");

   vec_ZZ D;
   long s;
   s = LLL(D, B, 0, a, b, verbose);
   D_out = D;
   return s;
}

long image(ZZ& det, mat_ZZ& B, mat_ZZ& U, long verbose)
{
   return image(det, B, &U, verbose);
}

long image(ZZ& det, mat_ZZ& B, long verbose)
{
   return image(det, B, 0, verbose);
}

long LatticeSolve(vec_ZZ& x, const mat_ZZ& A, const vec_ZZ& y, long reduce)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (y.length() != m)
      LogicError("LatticeSolve: dimension mismatch");

   if (reduce < 0 || reduce > 2)
      LogicError("LatticeSolve: bad reduce parameter");

   if (IsZero(y)) {
      x.SetLength(n);
      clear(x);
      return 1;
   }

   mat_ZZ A1, U1;
   ZZ det2;
   long im_rank, ker_rank;

   A1 = A;

   im_rank = image(det2, A1, U1);
   ker_rank = n - im_rank;

   mat_ZZ A2, U2;
   long new_rank;
   long i;

   A2.SetDims(im_rank + 1, m);
   for (i = 1; i <= im_rank; i++)
      A2(i) = A1(ker_rank + i);

   A2(im_rank + 1) = y;

   new_rank = image(det2, A2, U2);

   if (new_rank != im_rank ||
      (U2(1)(im_rank+1) != 1  && U2(1)(im_rank+1) != -1))
      return 0;

   vec_ZZ x1;
   x1.SetLength(im_rank);

   for (i = 1; i <= im_rank; i++)
      x1(i) = U2(1)(i);

   if (U2(1)(im_rank+1) == 1)
      negate(x1, x1);

   vec_ZZ x2, tmp;
   x2.SetLength(n);
   clear(x2);
   tmp.SetLength(n);

   for (i = 1; i <= im_rank; i++) {
      mul(tmp, U1(ker_rank+i), x1(i));
      add(x2, x2, tmp);
   }

   if (reduce == 0) {
      x = x2;
      return 1;
   }
   else if (reduce == 1) {
      U1.SetDims(ker_rank+1, n);
      U1(ker_rank+1) = x2;
      image(det2, U1);

      x = U1(ker_rank + 1);
      return 1;
   }
   else if (reduce == 2) {
      U1.SetDims(ker_rank, n);
      LLL(det2, U1);
      U1.SetDims(ker_rank+1, n);
      U1(ker_rank+1) = x2;
      image(det2, U1);

      x = U1(ker_rank + 1);
      return 1;
   }

   return 0;
}

NTL_END_IMPL


// In debug builds define
// #define GSL_THROW_ON_CONTRACT_VIOLATION
// In release builds define
// #define GSL_UNENFORCED_ON_CONTRACT_VIOLATION    // Segmentation fault on GCC
// or use assert() and define / undefine NDEBUG
#include <iostream>
#include <chrono>
#include <string>
#include <typeinfo>
#include <cassert>
#include <boost/type_index.hpp>
#if !defined(NDEBUG)
#define GSL_THROW_ON_CONTRACT_VIOLATION
#include <gsl/gsl>
#else
#define Expects(cond)
#define Ensures(cond)
#endif

using std::cout; using std::wcout; using std::endl;
using namespace std::string_literals;

// User-defined literals
class Probability
{
public:
    constexpr explicit Probability(long double v)
        : value(v)
        {
            Expects(v < 1.0 && 0.0 <= v);
            // assert(v < 1.0 && 0.0 <= v);
        }
private:
    long double value;

    friend constexpr Probability operator "" _prob(long double v);
    friend std::ostream& operator<<(std::ostream& os, const Probability& prob)
        { return os << "Probability{" << prob.value << "}"; }
};

class BadProbability : public std::logic_error
{
public:
    explicit BadProbability(long double v)
        : std::logic_error( "BadProbability exception, got value: " + std::to_string(v) )
        { }
};

constexpr Probability operator "" _prob(long double v)
{
    return 1.0 < v ? throw BadProbability{v} : Probability{v};
    // literals never represent negative values - no need to check for v < 0.0
}

using boost::typeindex::type_id_with_cvr;
int main()
try{
    // std::basic_string<wchar_t>
    auto s1 = "String literal"s;        // this is a std::string;
    auto s2 = L"Wide string literal"s;  // this is a std::wstring;
    auto s3 = std::basic_string<wchar_t>(L"This is a very explicit wstring");
    cout << "s1:\n\t" << s1 << "\n\ttypeid " << type_id_with_cvr<decltype(s1)>().pretty_name() << endl;
    wcout << L"s2:\n\t" << s2 << L"\n\ttypeid ";
    cout << type_id_with_cvr<decltype(s2)>().pretty_name() << endl;
    wcout << L"s3:\n\t" << s3 << L"\n\ttypeid ";
    cout << type_id_with_cvr<decltype(s3)>().pretty_name() << endl;

    cout << "\nUser defined literals\n" << "Probability\n" << endl;

    // constexpr auto prob1 = 1.2_prob;   // compiletime error;

    // auto prob2 = 1.2_prob;  // NOT a compiletimer error - throws at runtime

    constexpr auto prob3 = 0.3_prob;   // OK
        cout << prob3 << endl;

    // constexpr auto prob4 = Probability{1.2}; // Compiletime error

    constexpr auto prob5 = Probability{0.5};
        cout << prob5 << endl;

    auto prob6 = 0.6_prob;
        cout << prob6 << endl;

    // auto prob7 = Probability{1.7}; // runtime error
    //     cout << prob7 << endl;
}
catch( const std::exception& e )
{
    std::cerr << e.what () << endl;
}


#include "generator.hpp"

#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/normal.hpp>

#include <boost/random/variate_generator.hpp>

namespace crossbow {

CrossbowRandomGenerator::CrossbowRandomGenerator (unsigned int seed) {

	this->seed = seed;
	this->rng  = new rng_t(seed);
}

float CrossbowRandomGenerator::nextafter (const float value) {

	return boost::math::nextafter<float>(value, std::numeric_limits<float>::max());
}

void CrossbowRandomGenerator::randomUniformFill (float *buffer, const int count, const float start, const float end) {

	if (buffer == NULL) {
		fprintf(stderr, "error: buffer to fill must not be null\n");
		exit (1);
	}

	if (count < 0) {
		fprintf(stderr, "error: number of buffer elements to fill must be greater or equal to 0\n");
		exit (1);
	}

	if (start > end) {
		fprintf(stderr, "error: invalid uniform random distribution specification\n");
		exit (1);
	}

	boost::uniform_real<float> dist (start, nextafter (end));
	boost::variate_generator<crossbow::rng_t *, boost::uniform_real<float> > variate_generator (this->rng, dist);

	for (int i = 0; i < count; ++i)
		buffer [i] = variate_generator ();

	return;
}

void CrossbowRandomGenerator::randomGaussianFill (float *buffer, const int count, const float mean, const float std, const int truncate) {

	if (buffer == NULL) {
		fprintf(stderr, "error: buffer to fill must not be null\n");
		exit (1);
	}

	if (count < 0) {
		fprintf(stderr, "error: number of buffer elements to fill must be greater or equal to 0\n");
		exit (1);
	}

	if (std <= 0) {
		fprintf(stderr, "error: invalid normal distribution specification\n");
		exit (1);
	}

	boost::normal_distribution<float> dist (mean, std);
	boost::variate_generator<crossbow::rng_t *, boost::normal_distribution<float> > variate_generator (this->rng, dist);

	/* float checksum = 0; */

	if (truncate) {

		/*
		 * Added on 14 Apr 2018: Hard-coded truncated version:
		 *
		 * Values whose magnitude is more than 2 standard deviations
		 * from the mean are dropped and re-picked.
		 */
		float min = mean - (2 * std);
		float max = mean + (2 * std);
		int maxiterations = 100;

		float sample;
		int correct = 0;
		for (int i = 0; i < count; ++i) {
			for (int j = 0; j < maxiterations; ++j) {
				sample = variate_generator ();
				if ((sample > min) && (sample < max)) {
					correct ++;
					break;
				}
			}
			buffer [i] = sample;
			/* checksum += buffer [i]; */
		}
		if (correct < count)
			fprintf(stderr, "warning: only %d out of %d values truncated\n", correct, count);
	}
	else {
		for (int i = 0; i < count; ++i) {
			buffer [i] = variate_generator ();
			/* checksum += buffer [i]; */
		}
	}
	/*
	 * fprintf(stdout, "[DBG] checksum is %.5f\n", checksum);
	 * fflush (stdout);
	 */

	return;
}

void CrossbowRandomGenerator::dump () {

	fprintf(stdout, "CrossbowRandom (%du)\n", seed);
	fflush (stdout);
}

}  /* namespace crossbow */


#include "third_party/frc971/control_loops/paths/path.h"
#include <Eigen/Geometry>

#define _USE_MATH_DEFINES
#include <cmath>

#include "muan/logging/logger.h"

namespace frc971 {
namespace control_loops {
namespace paths {

Eigen::Vector2d Projection(Eigen::Vector2d a, Eigen::Vector2d direction) {
  return a.dot(direction) * direction.dot(direction) * direction;
}

Position FromMagDirection(double magnitude, double direction) {
  return magnitude * (Position() << ::std::cos(direction), ::std::sin(direction)).finished();
}

Pose::Pose(Eigen::Vector3d values) : values_(values) {}

Pose::Pose(Position pos, double theta) {
  values_.block<2, 1>(0, 0) = pos;
  values_(2) = remainder(theta, 2 * M_PI);
}

Pose Pose::operator+(const Pose &other) const {
  Eigen::Vector3d new_values = values_ + other.values_;

  // Wrap the heading into [-pi, pi]
  new_values(2) = remainder(new_values(2), 2 * M_PI);

  return Pose(new_values);
}

Pose Pose::TranslateBy(const Position &delta) const {
  Eigen::Vector3d new_values = values_;
  new_values.block<2, 1>(0, 0) += delta;
  return Pose(new_values);
}

Pose Pose::RotateBy(double theta) const {
  Eigen::Vector3d new_values = values_;
  new_values.block<2, 1>(0, 0) = Eigen::Rotation2D<double>(theta) * new_values.block<2, 1>(0, 0);

  // Wrap the heading into [-pi, pi]
  new_values(2) = remainder(new_values(2) + theta, 2 * M_PI);

  return Pose(new_values);
}

Pose Pose::operator-(const Pose &other) const {
  Eigen::Vector3d new_values = values_ - other.values_;

  // Wrap the heading into [-pi, pi]
  new_values(2) = remainder(new_values(2), 2 * M_PI);
  return Pose(new_values);
}

Pose Pose::Compose(const Pose &other) const { return other.RotateBy(heading()).TranslateBy(translational()); }

HermitePath::HermitePath(Pose initial, Pose final,
                         double initial_velocity, double final_velocity, bool backwards,
                         double extra_distance_initial, double extra_distance_final,
                         double initial_angular_velocity,
                         double final_angular_velocity)
    : HermitePath(initial.translational(),
                  FromMagDirection(1, initial.heading()),
                  final.translational(),
                  FromMagDirection(1, final.heading()),
                  initial_velocity, final_velocity, backwards,
                  extra_distance_initial, extra_distance_final,
                  initial_angular_velocity, final_angular_velocity) {}

HermitePath::HermitePath(Position initial_position, Eigen::Vector2d initial_tangent,
                         Position final_position, Eigen::Vector2d final_tangent,
                         double initial_velocity, double final_velocity, bool backwards,
                         double extra_distance_initial, double extra_distance_final,
                         double initial_angular_velocity,
                         double final_angular_velocity) {
  backwards_ = backwards;

  Eigen::Vector2d initial_derivative_basis;
  Eigen::Vector2d final_derivative_basis;
  double initial_deriv_magnitude;
  double final_deriv_magnitude;

  {
    Eigen::Vector2d distance = final_position - initial_position;
    // How far to the side are we driving, relative to initial state?
    Eigen::Vector2d sideways = distance - Projection(distance, initial_tangent);
    // How far to the front is the sideways vector, relative to final state?
    double forwards2 = sideways.dot(final_tangent);
    if (backwards) {
      forwards2 *= -1;
    }
    // Rough estimate of the curvature, this should be approximately
    // correct as long as the curvature doesn't have excessively large changes.
    double approx_curve = (sideways.norm() * 2 - forwards2) / distance.norm();
    // Standard hermite spline uses tangent * |distance|, but initial velocity
    // should be taken into account as well, although only sigmificantly if
    // distance is short and initial velocity is high. This formula was found
    // experimentally.
    initial_deriv_magnitude = distance.norm() + extra_distance_initial * 5.0 +
        initial_velocity * initial_velocity * 0.5 * approx_curve * approx_curve;
    final_deriv_magnitude = distance.norm() + extra_distance_final * 5.0 +
        final_velocity * final_velocity * 0.5 * approx_curve * approx_curve;
  }

  initial_derivative_basis = initial_tangent * initial_deriv_magnitude;
  final_derivative_basis = final_tangent * final_deriv_magnitude;

  if (backwards_) {
    initial_derivative_basis *= -1;
    final_derivative_basis *= -1;
  }

  Eigen::Vector2d initial_acceleration = Eigen::Vector2d::Zero();
  if (initial_velocity > 0.01 || initial_velocity < -0.01) {
    initial_acceleration =
          (Eigen::Vector2d() << -initial_tangent(1), initial_tangent(0)).finished() *
          initial_deriv_magnitude * final_deriv_magnitude *
          initial_angular_velocity / initial_velocity;
  } else if (initial_angular_velocity > 0.01 || initial_angular_velocity < -0.01) {
    LOG(WARNING, "Initial velocity required if initial angular velocity present"
                 " (v_0 = %f, omega_0 = %f, cutoff = 0.01)",
        initial_velocity, initial_angular_velocity);
  }

  Eigen::Vector2d final_acceleration = Eigen::Vector2d::Zero();
  if (final_velocity > 0.01 || final_velocity < -0.01) {
    final_acceleration =
          (Eigen::Vector2d() << -final_tangent(1), final_tangent(0)).finished() *
          final_deriv_magnitude * final_deriv_magnitude *
          final_angular_velocity / final_velocity;
  } else if (final_angular_velocity > 0.01 || final_angular_velocity < -0.01) {
    LOG(WARNING, "Final velocity required if final angular velocity present"
                 " (v_f = %f, omega_f = %f, cutoff = 0.01)",
        final_velocity, final_angular_velocity);
  }

  coefficients_ = Eigen::Matrix<double, 4, 6>::Zero();
  coefficients_.block<2, 1>(0, 0) = initial_position;
  coefficients_.block<2, 1>(0, 1) = initial_derivative_basis;
  coefficients_.block<2, 1>(0, 2) = 0.5 * initial_acceleration;
  coefficients_.block<2, 1>(0, 3) =
          -10 * initial_position - 6 * initial_derivative_basis +
          -1.5 * initial_acceleration + 0.5 * final_acceleration +
          -4 * final_derivative_basis + 10 * final_position;
  coefficients_.block<2, 1>(0, 4) =
          15 * initial_position + 8 * initial_derivative_basis +
          1.5 * initial_acceleration - 1 * final_acceleration +
          7 * final_derivative_basis - 15 * final_position;
  coefficients_.block<2, 1>(0, 5) =
          -6 * initial_position - 3 * initial_derivative_basis +
          -0.5 * initial_acceleration + 0.5 * final_acceleration +
          -3 * final_derivative_basis + 6 * final_position;

  for (int i = 0; i < 6; i++) {
    coefficients_.block<2, 1>(2, i) = coefficients_.block<2, 1>(0, i) * i;
  }

  initial_heading_ = remainder(::std::atan2(initial_tangent(1), initial_tangent(0)), 2 * M_PI);
}

void HermitePath::Populate(double s_min, double s_max, Pose *pose_arr, size_t arr_len) const {
  Eigen::Matrix<double, 6, 1> s_polynomial_bases;
  double step = (s_max - s_min) / (arr_len - 1);
  for (size_t i = 0; i < arr_len; i++) {
    double s = s_min + i * step;
    s_polynomial_bases << 1.0, s, s * s, s * s * s, s * s * s * s, s * s * s * s * s;

    Eigen::Vector4d combined = coefficients_ * s_polynomial_bases;

    double theta;
    if (s == 0) {
      // When s is _exactly_ zero, we can't get the heading directly from
      // the derivative (because it collapses to zero)! Let's use the cached
      // value instead.
      theta = initial_heading_;
    } else {
      theta = ::std::atan2(combined(3), combined(2));
      if (backwards_) {
        if (theta > 0) {
          theta -= M_PI;
        } else {
          theta += M_PI;
        }
      }
    }

    pose_arr[i] = Pose(combined.block<2, 1>(0, 0), theta);
  }
}

}  // namespace paths
}  // namespace control_loops
}  // namespace frc971


/*
BSD 3-Clause License

Copyright (c) 2020, The Regents of the University of Minnesota

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <queue>
#include <math.h>
#include <cmath>
#include <iostream>
#include <stdlib.h>
#include <map>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <time.h>
#include <sstream>
#include <iterator>
#include <string>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include "opendb/db.h"
#include "ir_solver.h"
#include "node.h"
#include "gmat.h"
#include "get_power.h"
#include "openroad/Error.hh"

using ord::error;
using ord::warn;
using odb::dbBlock;
using odb::dbBox;
using odb::dbChip;
using odb::dbDatabase;
using odb::dbInst;
using odb::dbNet;
using odb::dbSBox;
using odb::dbSet;
using odb::dbSigType;
using odb::dbSWire;
using odb::dbTech;
using odb::dbTechLayer;
using odb::dbTechLayerDir;
using odb::dbVia;
using odb::dbViaParams;

using namespace std;
using std::vector;
using Eigen::Map;
using Eigen::VectorXd;
using Eigen::SparseMatrix;
using Eigen::SparseLU;
using Eigen::Success;

//! Returns the created G matrix for the design
/*
 * \return G Matrix
 */
GMat* IRSolver::GetGMat()
{
  return m_Gmat;
}

//! Returns current map represented as a 1D vector
/*
 * \return J vector
 */
vector<double> IRSolver::GetJ()
{
  return m_J;
}

//! Function to solve for voltage using SparseLU
void IRSolver::SolveIR()
{
  if(!m_connection) {
    cout<<"WARNING: Powergrid is not connected to all instances,"<<
          "IR Solver may not be accurate, LVS may also fail."<<endl;
  }
  int unit_micron = (m_db->getTech())->getDbUnitsPerMicron();
  clock_t t1, t2;
  CscMatrix*        Gmat = m_Gmat->GetGMat();
// fill A
  int     nnz     = Gmat->nnz;
  int               m    = Gmat->num_rows;
  int               n    = Gmat->num_cols;
  double* values  = &(Gmat->values[0]);
  int*    row_idx = &(Gmat->row_idx[0]);
  int*    col_ptr = &(Gmat->col_ptr[0]);
  Map<SparseMatrix<double> > A( Gmat->num_rows,
                                Gmat->num_cols,
                                Gmat->nnz,
                                col_ptr, // read-write
                                row_idx,
                                values);

  vector<double>    J = GetJ();
  Map<VectorXd> b(J.data(),J.size());
  VectorXd x;
  SparseLU<SparseMatrix<double> > solver;
  cout << "INFO: Factorizing G" << endl;
  solver.compute(A);
  if(solver.info()!=Success) {
    // decomposition failed
    cout<<"Error: LU factorization of GMatrix failed"<<endl;
    return;
  }
  cout << "INFO: Solving GV=J" << endl;
  cout << "INFO: SparseLU begin solving" << endl;
  x = solver.solve(b);
  cout << "INFO: SparseLU finished solving" << endl;
  if(solver.info()!=Success) {
    // solving failed
    cout<<"Error: Solving V = inv(G)*J failed"<<endl;
    return;
  }
  ofstream ir_report;
  ir_report.open (m_out_file);
  ir_report<<"Instance name, "<<" X location, "<<" Y location, "<<" Voltage "<<"\n";
  int num_nodes         = m_Gmat->GetNumNodes();
  int node_num =0;
  double       sum_volt = 0;
  wc_voltage            = vdd;
  while(node_num < num_nodes) {
    Node* node = m_Gmat->GetNode(node_num);
    double volt = x(node_num);
    sum_volt   = sum_volt + volt;
    if (volt < wc_voltage) {
      wc_voltage = volt;
    }
    node->SetVoltage(volt);
    node_num++;
    if(node->HasInstances()) {
      NodeLoc node_loc = node->GetLoc();
      float loc_x = ((float)node_loc.first)/((float)unit_micron);
      float loc_y = ((float)node_loc.second)/((float)unit_micron);
      std::vector<dbInst*> insts = node->GetInstances();
      std::vector<dbInst*>::iterator inst_it;
      if (m_out_file != "") {
        for(inst_it = insts.begin();inst_it!=insts.end();inst_it++) {
          ir_report<<(*inst_it)->getName()<<", "<<loc_x<<", " <<loc_y<<", "<<setprecision(10)<<volt<<"\n";
        }
      }
    }
  }
  ir_report<<endl;
  ir_report.close();
  avg_voltage = sum_volt / num_nodes;
}

//! Function to add C4 bumps to the G matrix
bool IRSolver::AddC4Bump()
{
  if (m_C4Bumps.size() == 0) {
    cout << "ERROR: Invalid number of voltage sources" << endl;
    return false;
  }
  for (unsigned int it = 0; it < m_C4Nodes.size(); ++it) {
    NodeIdx node_loc = m_C4Nodes[it].first;
    double  voltage  = m_C4Nodes[it].second;
    m_Gmat->AddC4Bump(node_loc, it);  // add the 0th bump
    m_J.push_back(voltage);           // push back first vdd
    vdd = voltage;
  }
  return true;
}

//! Function that parses the Vsrc file
void IRSolver::ReadC4Data()
{
  int unit_micron = (m_db->getTech())->getDbUnitsPerMicron();
  cout << "INFO: Reading location of VDD and VSS sources " << endl;
  if(m_vsrc_file != "") {
  std::ifstream file(m_vsrc_file);
    std::string line = "";
  // Iterate through each line and split the content using delimiter
  while (getline(file, line)) {
    tuple<int, int, int, double> c4_bump;
    int                          first, second, size;
    double                       voltage;
    stringstream                 X(line);
    string                       val;
    for (int i = 0; i < 4; ++i) {
      getline(X, val, ',');
      if (i == 0) {
        first = (int) (unit_micron * stod(val));
      } else if (i == 1) {
        second = (int) (unit_micron * stod(val));
      } else if (i == 2) {
        size = (int) (unit_micron * stod(val));
      } else {
        voltage = stod(val);
      }
    }
    m_C4Bumps.push_back(make_tuple(first, second, size, voltage));
  }
  file.close();
  }
  else {
    cout << "Warning: Voltage pad location file not spcified, defaulting pad location to origin" << endl;
    m_C4Bumps.push_back(make_tuple(0,0,0,0));
  }
}

//! Function that parses the Vsrc file
/*void IRSolver::ReadResData()
{
  cout << "Default resistance file" << m_def_res << endl;
  cout << "INFO: Reading resistance of layers and vias " << endl;
  std::ifstream file(m_def_res);
  std::string line = "";
  int line_num = 0;
  // Iterate through each line and split the content using delimiter
  while (getline(file, line)) {
    line_num ++;
    if (line_num == 1) {
      continue;
    }
    //tuple<int, double, double> layer_res;
    int                          routing_level;
    double                       res_per_unit;
    double                       res_via;
    stringstream                 X(line);
    string                       val;
    for (int i = 0; i < 3; ++i) {
      getline(X, val, ',');
      if (i == 0) {
        routing_level = stoi(val);
      } else if (i == 1) {
        res_per_unit = stod(val);
      } else {
        res_via = stod(val);
      }
    }
    m_layer_res.push_back(make_tuple(routing_level, res_per_unit, res_via));
  }
  file.close();
}
*/

//! Function to create a J vector from the current map
bool IRSolver::CreateJ()
{  // take current_map as an input?
  int num_nodes = m_Gmat->GetNumNodes();
  m_J.resize(num_nodes, 0);

  vector<pair<string, double>> power_report = GetPower();
  dbChip*                      chip         = m_db->getChip();
  dbBlock*                     block        = chip->getBlock();
  for (vector<pair<string, double>>::iterator it = power_report.begin();
       it != power_report.end();
       ++it) {
    dbInst* inst = block->findInst(it->first.c_str());
    if (inst == NULL) {
      cout << "Warning instance " << it->first << " not found within database"
           << endl;
      continue;
    }
    int x, y;
    inst->getLocation(x, y);
    //cout << "Got location" <<endl;
    int   l      = m_bottom_layer;  // atach to the bottom most routing layer
    Node* node_J = m_Gmat->GetNode(x, y, l,true);
    NodeLoc node_loc = node_J->GetLoc();
    if( abs(node_loc.first - x) > m_node_density || abs(node_loc.second - y) > m_node_density ){
      cout<<"WARNING: Instance current node at "<<node_loc.first<<" "<<node_loc.second<<" layer "<<l<<" moved from "<<x<<" "<<y<<endl;
      cout<<"Instance: " << it->first <<endl;
    }
    //TODO modify for ground network
    node_J->AddCurrentSrc(it->second);
    node_J->AddInstance(inst);
  }
  for (int i = 0; i < num_nodes; ++i) {
    Node* node_J = m_Gmat->GetNode(i);
    m_J[i] = -1 * (node_J->GetCurrent());  // as MNA needs negative
    // cout << m_J[i] <<endl;
  }
  cout << "INFO: Created J vector" << endl;
  return true;
}

//! Function to create a G matrix using the nodes
bool IRSolver::CreateGmat(bool connection_only)
{
  cout<<"Creating G Matrix"<<endl;
  std::vector<Node*> node_vector;
  dbTech*                      tech   = m_db->getTech();
  //dbSet<dbTechLayer>           layers = tech->getLayers();
  dbSet<dbTechLayer>::iterator litr;
  int unit_micron = tech->getDbUnitsPerMicron();
  int num_routing_layers = tech->getRoutingLayerCount();

  m_Gmat                    = new GMat(num_routing_layers);
  dbChip*             chip  = m_db->getChip();
  dbBlock*            block = chip->getBlock();
  dbSet<dbNet>        nets  = block->getNets();
  std::vector<dbNet*> vdd_nets;
  std::vector<dbNet*> gnd_nets;
  std::vector<dbNet*> power_nets;
  int num_wires =0;
  cout << "Extracting power stripes on net " << m_power_net <<endl;
  dbSet<dbNet>::iterator nIter;
  for (nIter = nets.begin(); nIter != nets.end(); ++nIter) {
    dbNet* curDnet = *nIter;
    dbSigType nType = curDnet->getSigType();
    if(m_power_net == "VSS") {
      if (nType == dbSigType::GROUND) {
        power_nets.push_back(curDnet);
      } else {
        continue;
      }
    } else if(m_power_net == "VDD") {
      if (nType == dbSigType::POWER) {
        power_nets.push_back(curDnet);
      } else {
        continue;
      }
    } else {
      cout << "Warning: Net not specifed as VDD or VSS. Power grid checker is not run." <<endl;
      return false;
    }
  }
  if(power_nets.size() == 0) {
    cout<<"Warning:No power stipes found in design. Power grid checker is not run."<<endl;
    return false;
  }
  std::vector<dbNet*>::iterator vIter;
  for (vIter = power_nets.begin(); vIter != power_nets.end(); ++vIter) {
    dbNet*                   curDnet = *vIter;
    dbSet<dbSWire>           swires  = curDnet->getSWires();
    dbSet<dbSWire>::iterator sIter;
    for (sIter = swires.begin(); sIter != swires.end(); ++sIter) {
      dbSWire*                curSWire = *sIter;
      dbSet<dbSBox>           wires    = curSWire->getWires();
      dbSet<dbSBox>::iterator wIter;
      for (wIter = wires.begin(); wIter != wires.end(); ++wIter) {
        num_wires++;
        dbSBox* curWire = *wIter;
        int l;
        dbTechLayerDir::Value layer_dir;
        if (curWire->isVia()) {
          dbVia* via      = curWire->getBlockVia();
          dbTechLayer* via_layer = via->getTopLayer();
          l = via_layer->getRoutingLevel();
          layer_dir = via_layer->getDirection();
        } else {
          dbTechLayer* wire_layer = curWire->getTechLayer();
          l = wire_layer->getRoutingLevel();
          layer_dir = wire_layer->getDirection();
          if (l < m_bottom_layer) {
            m_bottom_layer = l ;
            m_bottom_layer_dir = layer_dir;
          }
        }
        if (l > m_top_layer) {
          m_top_layer = l ;
          m_top_layer_dir = layer_dir;
        }
      }
    }
  }
  cout<<"Creating Nodes:     ";
  int progress_wires=0;
  int progress_percent=1;
  for (vIter = power_nets.begin(); vIter != power_nets.end(); ++vIter) {
    dbNet*                   curDnet = *vIter;
    dbSet<dbSWire>           swires  = curDnet->getSWires();
    dbSet<dbSWire>::iterator sIter;
    for (sIter = swires.begin(); sIter != swires.end(); ++sIter) {
      dbSWire*                curSWire = *sIter;
      dbSet<dbSBox>           wires    = curSWire->getWires();
      dbSet<dbSBox>::iterator wIter;
      for (wIter = wires.begin(); wIter != wires.end(); ++wIter) {
        if(progress_wires >= ((progress_percent/100.0)*num_wires)-1.0 ){
          cout<<"\b\b\b\b"<<setw(3)<<progress_percent++<<"%"<< std::flush;
        }
        progress_wires++;
        dbSBox* curWire = *wIter;
        if (curWire->isVia()) {
          dbVia* via      = curWire->getBlockVia();
          dbBox* via_bBox = via->getBBox();
          int check_params = via->hasParams();
          int x_cut_size = 0;
          int y_cut_size = 0;
          int x_bottom_enclosure = 0;
          int y_bottom_enclosure = 0;
          int x_top_enclosure = 0;
          int y_top_enclosure = 0;
		  if(check_params == 1) {
		    dbViaParams params;
			via->getViaParams(params);
		    x_cut_size = params.getXCutSize();
		    y_cut_size = params.getYCutSize();
            x_bottom_enclosure = params.getXBottomEnclosure();
            y_bottom_enclosure = params.getYBottomEnclosure();
            x_top_enclosure = params.getXTopEnclosure();
            y_top_enclosure = params.getYTopEnclosure();
		  }
          BBox   bBox = make_pair((via_bBox->getDX()) / 2, (via_bBox->getDY()) / 2);
          int x, y;
          curWire->getViaXY(x, y);
          dbTechLayer* via_layer = via->getBottomLayer();
          dbTechLayerDir::Value layer_dir  = via_layer->getDirection();
          int          l         = via_layer->getRoutingLevel();
          int x_loc1,x_loc2,y_loc1,y_loc2;
          if (m_bottom_layer != l && l != m_top_layer) {//do not set for top and bottom layers
            if (layer_dir == dbTechLayerDir::Value::HORIZONTAL) {
              y_loc1 = y;
              y_loc2 = y;
              x_loc1 = x - (x_bottom_enclosure+x_cut_size/2);
              x_loc2 = x + (x_bottom_enclosure+x_cut_size/2);
            } else {
              y_loc1 = y - (y_bottom_enclosure+y_cut_size/2);
              y_loc2 = y + (y_bottom_enclosure+y_cut_size/2);
              x_loc1 = x;
              x_loc2 = x;
            }
            m_Gmat->SetNode(x_loc1, y_loc1, l, make_pair(0, 0));
            m_Gmat->SetNode(x_loc2, y_loc2, l, make_pair(0, 0));
            m_Gmat->SetNode(x, y, l, bBox);
          }
          via_layer = via->getTopLayer();
          l         = via_layer->getRoutingLevel();

          //TODO this may count the stripe conductance twice but is needed to
          //fix a staggered stacked via
          layer_dir  = via_layer->getDirection();
          if (m_bottom_layer != l && l != m_top_layer) {//do not set for top and bottom layers
            if (layer_dir == dbTechLayerDir::Value::HORIZONTAL) {
              y_loc1 = y;
              y_loc2 = y;
              x_loc1 = x - (x_top_enclosure+x_cut_size/2);
              x_loc2 = x + (x_top_enclosure+x_cut_size/2);
            } else {
              y_loc1 = y - (y_top_enclosure+y_cut_size/2);
              y_loc2 = y + (y_top_enclosure+y_cut_size/2);
              x_loc1 = x;
              x_loc2 = x;
            }
            m_Gmat->SetNode(x_loc1, y_loc1, l, make_pair(0, 0));
            m_Gmat->SetNode(x_loc2, y_loc2, l, make_pair(0, 0));
            m_Gmat->SetNode(x, y, l, bBox);
          }
        } else {
          int                   x_loc1, x_loc2, y_loc1, y_loc2;
          dbTechLayer*          wire_layer = curWire->getTechLayer();
          int                   l          = wire_layer->getRoutingLevel();
          dbTechLayerDir::Value layer_dir  = wire_layer->getDirection();
          if(l == m_bottom_layer){
            layer_dir = dbTechLayerDir::Value::HORIZONTAL;
          }
          if (layer_dir == dbTechLayerDir::Value::HORIZONTAL) {
            y_loc1 = (curWire->yMin() + curWire->yMax()) / 2;
            y_loc2 = (curWire->yMin() + curWire->yMax()) / 2;
            x_loc1 = curWire->xMin();
            x_loc2 = curWire->xMax();
          } else {
            x_loc1 = (curWire->xMin() + curWire->xMax()) / 2;
            x_loc2 = (curWire->xMin() + curWire->xMax()) / 2;
            y_loc1 = curWire->yMin();
            y_loc2 = curWire->yMax();
          }
          if (l == m_bottom_layer || l == m_top_layer) {  // special case for bottom and top layers we design a dense grid
            if (layer_dir == dbTechLayerDir::Value::HORIZONTAL ) {
              int x_i;
              x_loc1 = (x_loc1/m_node_density)*m_node_density; //quantize the horizontal direction
              x_loc2 = (x_loc2/m_node_density)*m_node_density; //quantize the horizontal direction
              for (x_i = x_loc1; x_i <= x_loc2; x_i = x_i + m_node_density) {
                m_Gmat->SetNode(x_i, y_loc1, l, make_pair(0, 0));
              }
            } else {
              y_loc1 = (y_loc1/m_node_density)*m_node_density; //quantize the vertical direction
              y_loc2 = (y_loc2/m_node_density)*m_node_density; //quantize the vertical direction
              int y_i;
              for (y_i = y_loc1; y_i <= y_loc2; y_i = y_i + m_node_density) {
                m_Gmat->SetNode(x_loc1, y_i, l, make_pair(0, 0));
              }
            }
          } else {  // add end nodes
            m_Gmat->SetNode(x_loc1, y_loc1, l, make_pair(0, 0));
            m_Gmat->SetNode(x_loc2, y_loc2, l, make_pair(0, 0));
          }
        }
      }
    }
  }
  cout<<endl;
  progress_wires =0;
  progress_percent =1;
  // insert c4 bumps as nodes
  int num_C4 =0;
  for (unsigned int it = 0; it < m_C4Bumps.size(); ++it) {
    int x = get<0>(m_C4Bumps[it]);
    int y = get<1>(m_C4Bumps[it]);
    int size = get<2>(m_C4Bumps[it]);
    double v  = get<3>(m_C4Bumps[it]);
    std::vector<Node*> RDL_nodes;
    RDL_nodes = m_Gmat->GetRDLNodes(m_top_layer,
                                    m_top_layer_dir,
                                    x-size/2,
                                    x+size/2,
                                    y-size/2,
                                    y+size/2);
    if (RDL_nodes.empty() == true) {
      Node* node = m_Gmat->GetNode(x,y,m_top_layer,true);
      NodeLoc node_loc = node->GetLoc();
      double new_loc1 = ((double)node_loc.first) /((double) unit_micron);
      double new_loc2 = ((double)node_loc.second) /((double) unit_micron);
      double old_loc1 = ((double)x) /((double) unit_micron);
      double old_loc2 = ((double)y) /((double) unit_micron);
      double old_size = ((double)size) /((double) unit_micron);
      cout<<"WARNING: Vsrc location at x="<<std::setprecision(3)<<old_loc1<<"um , y="<<old_loc2
          <<"um and size ="<<old_size<<"um,  is not located on a power stripe."<<endl;
      cout<<"         Moving to closest stripe at x="<<std::setprecision(3)<<new_loc1<<"um , y="<<new_loc2<<"um"<<endl;
      RDL_nodes = m_Gmat->GetRDLNodes(m_top_layer,
                                      m_top_layer_dir,
                                      node_loc.first-size/2,
                                      node_loc.first+size/2,
                                      node_loc.second-size/2,
                                      node_loc.second+size/2);

    }
    vector<Node*>::iterator node_it;
    for(node_it = RDL_nodes.begin(); node_it != RDL_nodes.end(); ++node_it) {
      Node* node = *node_it;
      m_C4Nodes.push_back(make_pair(node->GetGLoc(),v));
      num_C4++;
    }
  }
  // All new nodes must be inserted by this point
  // initialize G Matrix

  cout << "INFO: Number of nodes on net " << m_power_net <<" =" << m_Gmat->GetNumNodes() << endl;
  cout << "Creating Connections:     ";
  m_Gmat->InitializeGmatDok(num_C4);
  int err_flag_via = 1;
  int err_flag_layer = 1;
  for (vIter = power_nets.begin(); vIter != power_nets.end();
       ++vIter) {  // only 1 is expected?
    dbNet*                   curDnet = *vIter;
    dbSet<dbSWire>           swires  = curDnet->getSWires();
    dbSet<dbSWire>::iterator sIter;
    for (sIter = swires.begin(); sIter != swires.end();
         ++sIter) {  // only 1 is expected?
      dbSWire*                curSWire = *sIter;
      dbSet<dbSBox>           wires    = curSWire->getWires();
      dbSet<dbSBox>::iterator wIter;
      for (wIter = wires.begin(); wIter != wires.end(); ++wIter) {
        if(progress_wires >= ((progress_percent/100.0)*num_wires)-1.0 ){
          cout<<"\b\b\b\b"<<setw(3)<<progress_percent++<<"%"<< std::flush;
        }
        progress_wires++;
        dbSBox* curWire = *wIter;
        if (curWire->isVia()) {
          dbVia* via      = curWire->getBlockVia();
          int num_via_rows = 1;
          int num_via_cols = 1;
          int check_params = via->hasParams();
          int x_cut_size = 0;
          int y_cut_size = 0;
          int x_bottom_enclosure = 0;
          int y_bottom_enclosure = 0;
          int x_top_enclosure = 0;
          int y_top_enclosure = 0;
		  if(check_params == 1) {
		    dbViaParams params;
			via->getViaParams(params);
		    num_via_rows = params.getNumCutRows();
		    num_via_cols = params.getNumCutCols();
		    x_cut_size = params.getXCutSize();
		    y_cut_size = params.getYCutSize();
            x_bottom_enclosure = params.getXBottomEnclosure();
            y_bottom_enclosure = params.getYBottomEnclosure();
            x_top_enclosure = params.getXTopEnclosure();
            y_top_enclosure = params.getYTopEnclosure();
		  }
          dbBox* via_bBox = via->getBBox();
          BBox   bBox
              = make_pair((via_bBox->getDX()) / 2, (via_bBox->getDY()) / 2);
          int x, y;
          curWire->getViaXY(x, y);
          dbTechLayer* via_layer = via->getBottomLayer();
          int          l         = via_layer->getRoutingLevel();

          double R = via_layer->getUpperLayer()->getResistance();
          R = R/(num_via_rows * num_via_cols);
          if (R == 0.0) {
            err_flag_via = 0;
            //R = get<2>(m_layer_res[l]); /// Must figure out via resistance value
            //cout << "Via Resistance" << R << endl;
          }
          bool top_or_bottom = ((l == m_bottom_layer) || (l == m_top_layer));
          Node* node_bot = m_Gmat->GetNode(x, y, l,top_or_bottom);
          NodeLoc node_loc = node_bot->GetLoc();
          if( abs(node_loc.first - x) > m_node_density || abs(node_loc.second - y) > m_node_density ){
            cout<<"WARNING: Node at "<<node_loc.first<<" "<<node_loc.second<<" layer "<<l<<" moved from "<<x<<" "<<y<<endl;
          }

          via_layer      = via->getTopLayer();
          l              = via_layer->getRoutingLevel();
          top_or_bottom = ((l == m_bottom_layer) || (l == m_top_layer));
          Node* node_top = m_Gmat->GetNode(x, y, l,top_or_bottom);
          node_loc = node_top->GetLoc();
          if( abs(node_loc.first - x) > m_node_density || abs(node_loc.second - y) > m_node_density ){
            cout<<"WARNING: Node at "<<node_loc.first<<" "<<node_loc.second<<" layer "<<l<<" moved from "<<x<<" "<<y<<endl;
          }

          if (node_bot == nullptr || node_top == nullptr) {
            cout << "ERROR: null pointer received for expected node. Code may "
                    "fail ahead."<<endl;
            return false;
          } else {
            if(R <= 1e-12){ //if the resitance was not set.
                m_Gmat->SetConductance(node_bot, node_top, 0);
            } else {
                m_Gmat->SetConductance(node_bot, node_top, 1 / R);
            }
          }

          via_layer = via->getBottomLayer();
          dbTechLayerDir::Value layer_dir  = via_layer->getDirection();
          l         = via_layer->getRoutingLevel();
          if(l != m_bottom_layer) {
            double       rho        = via_layer->getResistance()
                         * double(via_layer->getWidth())
                         / double(unit_micron);
            if (rho <= 1e-12) {
              rho = 0;
              err_flag_layer = 0;
            }
            int x_loc1,x_loc2,y_loc1,y_loc2;
            if (layer_dir == dbTechLayerDir::Value::HORIZONTAL) {
              y_loc1 = y - y_cut_size/2;
              y_loc2 = y + y_cut_size/2;
              x_loc1 = x - (x_bottom_enclosure+x_cut_size/2);
              x_loc2 = x + (x_bottom_enclosure+x_cut_size/2);
            } else {
              y_loc1 = y - (y_bottom_enclosure+y_cut_size/2);
              y_loc2 = y + (y_bottom_enclosure+y_cut_size/2);
              x_loc1 = x - x_cut_size/2;
              x_loc2 = x + x_cut_size/2;
            }
            m_Gmat->GenerateStripeConductance(via_layer->getRoutingLevel(),
                                              layer_dir,
                                              x_loc1,
                                              x_loc2,
                                              y_loc1,
                                              y_loc2,
                                              rho);
          }
          via_layer = via->getTopLayer();
          layer_dir  = via_layer->getDirection();
          l         = via_layer->getRoutingLevel();
          if(l != m_top_layer) {
            double       rho        = via_layer->getResistance()
                         * double(via_layer->getWidth())
                         / double(unit_micron);
            if (rho <= 1e-12) {
              rho = 0;
              err_flag_layer = 0;
            }
            int x_loc1,x_loc2,y_loc1,y_loc2;
            if (layer_dir == dbTechLayerDir::Value::HORIZONTAL) {
              y_loc1 = y - y_cut_size/2;
              y_loc2 = y + y_cut_size/2;
              x_loc1 = x - (x_top_enclosure+x_cut_size/2);
              x_loc2 = x + (x_top_enclosure+x_cut_size/2);
            } else {
              y_loc1 = y - (y_top_enclosure+y_cut_size/2);
              y_loc2 = y + (y_top_enclosure+y_cut_size/2);
              x_loc1 = x - x_cut_size/2;
              x_loc2 = x + x_cut_size/2;
            }
            m_Gmat->GenerateStripeConductance(via_layer->getRoutingLevel(),
                                              layer_dir,
                                              x_loc1,
                                              x_loc2,
                                              y_loc1,
                                              y_loc2,
                                              rho);
          }

        } else {
          dbTechLayer* wire_layer = curWire->getTechLayer();
          int l  = wire_layer->getRoutingLevel();
          double       rho        = wire_layer->getResistance()
                       * double(wire_layer->getWidth())
                       / double(unit_micron);
          if (rho <= 1e-12) {
            rho = 0;
            err_flag_layer = 0;
          }
          dbTechLayerDir::Value layer_dir = wire_layer->getDirection();
          if (l == m_bottom_layer){//ensure that the bootom layer(rail) is horizontal
            layer_dir = dbTechLayerDir::Value::HORIZONTAL;
          }
          int x_loc1 = curWire->xMin();
          int x_loc2 = curWire->xMax();
          int y_loc1 = curWire->yMin();
          int y_loc2 = curWire->yMax();
          if (l == m_bottom_layer || l == m_top_layer) {  // special case for bottom and top layers we design a dense grid
            if (layer_dir == dbTechLayerDir::Value::HORIZONTAL ) {
              x_loc1 = (x_loc1/m_node_density)*m_node_density; //quantize the horizontal direction
              x_loc2 = (x_loc2/m_node_density)*m_node_density; //quantize the horizontal direction
            } else {
              y_loc1 = (y_loc1/m_node_density)*m_node_density; //quantize the vertical direction
              y_loc2 = (y_loc2/m_node_density)*m_node_density; //quantize the vertical direction
            }
          }
          m_Gmat->GenerateStripeConductance(wire_layer->getRoutingLevel(),
                                            layer_dir,
                                            x_loc1,
                                            x_loc2,
                                            y_loc1,
                                            y_loc2,
                                            rho);
        }
      }
    }
  }
  cout<<endl;
  cout << "INFO: G matrix created " << endl;
  if (err_flag_via == 0 && !connection_only) {
    cout << "Error: Atleast one via resistance not found in DB. Check the LEF or set it with a odb::setResistance command"<<endl;
    return false;
  }
  if (err_flag_layer == 0 && !connection_only) {
    cout << "Error: Atleast one layer per unit resistance not found in DB. Check the LEF or set it with a odb::setResistance command"<<endl;
    return false;
  }
  return true;
}

bool IRSolver::CheckConnectivity()
{
  std::vector<std::pair<NodeIdx,double>>::iterator c4_node_it;
  int x,y;
  CscMatrix*        Amat = m_Gmat->GetAMat();
  int num_nodes = m_Gmat->GetNumNodes();

  dbTech* tech   = m_db->getTech();
  int unit_micron = tech->getDbUnitsPerMicron();

  for(c4_node_it = m_C4Nodes.begin(); c4_node_it != m_C4Nodes.end() ; c4_node_it++){
    Node* c4_node = m_Gmat->GetNode((*c4_node_it).first);
    std::queue<Node*> node_q;
    node_q.push(c4_node);
    while(!node_q.empty()) {
      NodeIdx col_loc, n_col_loc;
      Node* node = node_q.front();
      node_q.pop();
      node->SetConnected();
      NodeIdx col_num = node->GetGLoc();
      col_loc  = Amat->col_ptr[col_num];
      if(col_num < Amat->col_ptr.size()-1) {
        n_col_loc  = Amat->col_ptr[col_num+1];
      } else {
        n_col_loc  = Amat->row_idx.size() ;
      }
      std::vector<NodeIdx> col_vec(Amat->row_idx.begin()+col_loc,
                                   Amat->row_idx.begin()+n_col_loc);

      std::vector<NodeIdx>::iterator col_vec_it;
      for(col_vec_it = col_vec.begin(); col_vec_it != col_vec.end(); col_vec_it++){
        if(*col_vec_it<num_nodes) {
          Node* node_next = m_Gmat->GetNode(*col_vec_it);
          if(!(node_next->GetConnected())) {
            node_q.push(node_next);
          }
        }
      }
    }
  }
  int uncon_err_cnt = 0;
  int uncon_err_flag = 0;
  int uncon_inst_cnt = 0;
  int uncon_inst_flag = 0;
  std::vector<Node*> node_list = m_Gmat->GetAllNodes();
  std::vector<Node*>::iterator node_list_it;
  bool unconnected_node =false;
  for(node_list_it = node_list.begin(); node_list_it != node_list.end(); node_list_it++){
    if(!(*node_list_it)->GetConnected()){
      uncon_err_cnt++;
      NodeLoc node_loc = (*node_list_it)->GetLoc();
      float loc_x = ((float)node_loc.first)/((float)unit_micron);
      float loc_y = ((float)node_loc.second)/((float)unit_micron);

      //if(uncon_err_cnt>25 && uncon_err_flag ==0 ) {
      //  uncon_err_flag =1;
      //  cout<<"Error display limit reached, suppressing further unconnected node error messages"<<endl;
      //} else if( uncon_err_flag ==0) {
        //cout<<"node_not_connected ================================="<<endl;
        unconnected_node =true;
        cout<<"Warning: Unconnected PDN node on net " << m_power_net<<" at location x:"<<loc_x<<"um, y:"
            <<loc_y<<"um ,layer: "<<(*node_list_it)->GetLayerNum()<<endl;
      //}
      //if(uncon_inst_cnt>25 && uncon_inst_flag ==0 ) {
      //  uncon_inst_flag =1;
      //  cout<<"Error display limit reached, suppressing further unconnected instance error messages"<<endl;
      //} else if( uncon_inst_flag ==0) {
        if((*node_list_it)->HasInstances()){
          std::vector<dbInst*> insts = (*node_list_it)->GetInstances();
          std::vector<dbInst*>::iterator inst_it;
          for(inst_it = insts.begin();inst_it!=insts.end();inst_it++) {
            uncon_inst_cnt++;
            cout<<"Warning: Instance: "<< (*inst_it)->getName() <<"at location x:"<<loc_x<<"um, y:"
              <<loc_y<<"um ,layer: "<<(*node_list_it)->GetLayerNum()<<endl;
          }
        }
      //}
    }
  }
  if(unconnected_node == false){
    cout<<"INFO: No dangling stripe found on net "<<m_power_net<< endl;
    cout <<"INFO: Connection between all PDN nodes established on net "<<m_power_net <<endl;
  }
  return !unconnected_node;
}

int IRSolver::GetConnectionTest(){
  if(m_connection){
    return 1;
  } else {
    return 0;
  }
}

//! Function to get the power value from OpenSTA
/*
 *\return vector of pairs of instance name
 and its corresponding power value
*/
vector<pair<string, double>> IRSolver::GetPower()
{
  PowerInst                    power_inst;
  vector<pair<string, double>> power_report = power_inst.executePowerPerInst(
      m_sta);

  return power_report;
}

bool IRSolver::GetResult(){
  return m_result;
}

int IRSolver::PrintSpice() {
  DokMatrix*        Gmat = m_Gmat->GetGMatDOK();
  map<GMatLoc, double>::iterator it;

  ofstream pdnsim_spice_file;
  pdnsim_spice_file.open (m_spice_out_file);
  if (!pdnsim_spice_file.is_open()) {
    cout << "File did not open" << endl;
    return 0;
  }
  vector<double>    J = GetJ();
  int num_nodes = m_Gmat->GetNumNodes();
  int resistance_number = 0;
  int voltage_number = 0;
  int current_number = 0;

  NodeLoc node_loc;
  for(it = Gmat->values.begin(); it!= Gmat->values.end(); it++){
    NodeIdx col = (it->first).first;
    NodeIdx row = (it->first).second;
    if(col <= row) {
      continue; //ignore lower half and diagonal as matrix is symmetric
    }
    double cond = it->second;           // get cond value
    if(abs(cond) < 1e-15){            //ignore if an empty cell
      continue;
    }

    string net_name = "vdd";
    if(col < num_nodes) { //resistances
      double resistance = -1/cond;

      Node* node1 = m_Gmat->GetNode(col);
      Node* node2 = m_Gmat->GetNode(row);
      node_loc = node1->GetLoc();
      int x1 = node_loc.first;
      int y1 = node_loc.second;
      int l1 = node1->GetLayerNum();
      string node1_name = net_name + "_" + to_string(x1) + "_" + to_string(y1) + "_" + to_string(l1);

      node_loc = node2->GetLoc();
      int x2 = node_loc.first;
      int y2 = node_loc.second;
      int l2 = node2->GetLayerNum();
      string node2_name = net_name + "_" + to_string(x2) + "_" + to_string(y2) + "_" + to_string(l2);

      string resistance_name = "R" + to_string(resistance_number);
      resistance_number++;

      pdnsim_spice_file<< resistance_name <<" "<< node1_name << " " << node2_name <<" "<< to_string(resistance) <<endl;

      double current = node1->GetCurrent();
      string current_name = "I" + to_string(current_number);
      if(abs(current)> 1e-18) {
        pdnsim_spice_file<< current_name <<" "<< node1_name << " " << 0 <<" "<< current <<endl;
        current_number++;
      }

    } else { //voltage
      Node* node1 = m_Gmat->GetNode(row); //VDD location
      node_loc = node1->GetLoc();
      double voltage = J[col];
      int x1 = node_loc.first;
      int y1 = node_loc.second;
      int l1 = node1->GetLayerNum();
      string node1_name = net_name + "_" + to_string(x1) + "_" + to_string(y1) + "_" + to_string(l1);
      string voltage_name = "V" + to_string(voltage_number);
      voltage_number++;
      pdnsim_spice_file<< voltage_name <<" "<< node1_name << " 0 " << to_string(voltage) <<endl;
    }
  }

  pdnsim_spice_file<<".OPTION NUMDGT=6"<<endl;
  pdnsim_spice_file<<".OP"<<endl;
  pdnsim_spice_file<<".END"<<endl;
  pdnsim_spice_file<<endl;
  pdnsim_spice_file.close();
  return 1;
}

bool IRSolver::Build() {
  bool res = true;
  ReadC4Data();
  if(res) {
    res = CreateGmat();
  }
  if(res) {
    res = CreateJ();
  }
  if(res) {
    res = AddC4Bump();
  }
  if(res) {
    res = m_Gmat->GenerateCSCMatrix();
    res = m_Gmat->GenerateACSCMatrix();
  }
  if(res) {
    m_connection = CheckConnectivity();
    res = m_connection;
  }
  m_result = res;
  return m_result;
}

bool IRSolver::BuildConnection() {
  bool res = true;
  ReadC4Data();
  if(res) {
    res = CreateGmat(true);
  }
  if(res) {
    res = AddC4Bump();
  }
  if(res) {
    res = m_Gmat->GenerateACSCMatrix();
  }
  if(res) {
    m_connection = CheckConnectivity();
    res = m_connection;
  }
  m_result = res;
  return m_result;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing dot capabilities

    returns the dot product of the two vector arguments

    @par Semantic:

    For every parameters of type T:

    @code
    scalar_of_t<T> r = dot(x,y);
    @endcode

    is similar to:

    @code
    scalar_of_t<T> r = sum(x*conj(y));
    @endcode

  **/
  const boost::dispatch::functor<tag::dot_> dot = {};
} }
#endif

#include <boost/simd/function/scalar/dot.hpp>
#include <boost/simd/function/simd/dot.hpp>

#endif