#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 <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


#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


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------

 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 */

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/symbolic_function.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_dgp.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_values_extractors.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>

using namespace dealii;

template <int dim>
class Step6
{
public:
  Step6();

  void
  run();

private:
  void
  setup_system();
  void
  assemble_system();
  void
  solve();
  void
  refine_grid();
  void
  output_results(const unsigned int cycle) const;

  Triangulation<dim> triangulation;

  FE_Q<dim>     fe_velocity;
  FE_DGP<dim>   fe_pressure;
  FESystem<dim> fe;

  FEValuesExtractors::Vector velocity;
  FEValuesExtractors::Scalar pressure;

  DoFHandler<dim> dof_handler;

  AffineConstraints<double> constraints;

  SparseMatrix<double> system_matrix;
  SparsityPattern      sparsity_pattern;

  Vector<double> solution;
  Vector<double> system_rhs;
};

template <int dim>
Step6<dim>::Step6()
  : fe_velocity(2)
  , fe_pressure(1)
  , fe(fe_velocity, dim, fe_pressure, 1)
  , velocity(0)
  , pressure(dim)
  , dof_handler(triangulation)
{}

template <int dim>
void
Step6<dim>::setup_system()
{
  dof_handler.distribute_dofs(fe);

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());

  constraints.clear();
  DoFTools::make_hanging_node_constraints(dof_handler, constraints);

  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<dim>(dim +
                                                                        1),
                                           constraints,
                                           fe.component_mask(velocity));

  constraints.close();

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler,
                                  dsp,
                                  constraints,
                                  /*keep_constrained_dofs = */ false);

  sparsity_pattern.copy_from(dsp);

  system_matrix.reinit(sparsity_pattern);
}

template <int dim>
void
Step6<dim>::assemble_system()
{
  const QGauss<dim> quadrature_formula(fe.degree + 1);

  FEValues<dim> fe_values(fe,
                          quadrature_formula,
                          update_values | update_gradients |
                            update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe.dofs_per_cell;

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  Functions::SymbolicFunction<dim> rhs_function(
    dim == 2 ?
      "x^2*y^2*(x - 1)^2*(2*y - 2) + 2*x^2*y*(x - 1)^2*(y - 1)^2;-x^2*y^2*(2*x - 2)*(y - 1)^2 - 2*x*y^2*(x - 1)^2*(y - 1)^2; 0" :
      "x^2*y^2*(x - 1)^2*(2*y - 2) + 2*x^2*y*(x - 1)^2*(y - 1)^2;-x^2*y^2*(2*x - 2)*(y - 1)^2 - 2*x*y^2*(x - 1)^2*(y - 1)^2; 0; 0");

  Vector<double> rhs_values(dim + 1);
  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      cell_matrix = 0;
      cell_rhs    = 0;

      fe_values.reinit(cell);

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          rhs_function.vector_value(fe_values.quadrature_point(q_index),
                                    rhs_values);
          for (const unsigned int i : fe_values.dof_indices())
            {
              auto v      = fe_values[velocity].value(i, q_index);
              auto div_v  = fe_values[velocity].divergence(i, q_index);
              auto grad_v = fe_values[velocity].gradient(i, q_index);
              auto q      = fe_values[pressure].value(i, q_index);

              for (const unsigned int j : fe_values.dof_indices())
                {
                  auto div_u  = fe_values[velocity].divergence(j, q_index);
                  auto grad_u = fe_values[velocity].gradient(j, q_index);
                  auto p      = fe_values[pressure].value(j, q_index);

                  cell_matrix(i, j) +=
                    (scalar_product(grad_u, grad_v) - div_v * p - div_u * q) *
                    fe_values.JxW(q_index); // dx
                }
              for (unsigned int d = 0; d < dim; ++d)
                cell_rhs(i) += (1000 * rhs_values[d] *   // f(x)
                                v[d] *                   // phi_i(x_q)
                                fe_values.JxW(q_index)); // dx
            }
        }

      cell->get_dof_indices(local_dof_indices);
      constraints.distribute_local_to_global(
        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
    }
}

template <int dim>
void
Step6<dim>::solve()
{
  SparseDirectUMFPACK inverse;
  inverse.initialize(system_matrix);

  // SolverControl            solver_control(1000, 1e-12);
  // SolverCG<Vector<double>> solver(solver_control);

  // PreconditionSSOR<SparseMatrix<double>> preconditioner;
  // preconditioner.initialize(system_matrix, 1.2);

  // solver.solve(system_matrix, solution, system_rhs, preconditioner);

  inverse.vmult(solution, system_rhs);
  constraints.distribute(solution);
}

template <int dim>
void
Step6<dim>::refine_grid()
{
  Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

  KellyErrorEstimator<dim>::estimate(dof_handler,
                                     QGauss<dim - 1>(fe.degree + 1),
                                     {},
                                     solution,
                                     estimated_error_per_cell,
                                     fe.component_mask(velocity));

  GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                  estimated_error_per_cell,
                                                  0.3,
                                                  0.03);

  triangulation.execute_coarsening_and_refinement();
}

template <int dim>
void
Step6<dim>::output_results(const unsigned int cycle) const
{
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");
    DataOutBase::VtkFlags flags;
    flags.write_higher_order_cells = true;

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);

    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);
    DataOut<dim> data_out;
    data_out.set_flags(flags);

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches(fe.degree);

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
    data_out.write_vtu(output);
  }
}

template <int dim>
void
Step6<dim>::run()
{
  for (unsigned int cycle = 0; cycle < 4; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle == 0)
        {
          GridGenerator::hyper_cube(triangulation);
          triangulation.refine_global(4);
        }
      else
        refine_grid();

      std::cout << "   Number of active cells:       "
                << triangulation.n_active_cells() << std::endl;

      setup_system();

      std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                << std::endl;

      assemble_system();
      solve();
      output_results(cycle);
    }
}

int
main()
{
  try
    {
      Step6<2> laplace_problem_2d;
      laplace_problem_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


 #include <boost/spirit.hpp>
 #include <boost/spirit/tree/ast.hpp>
 #include <string>
 #include <cassert>
 #include <iostream>
 #include <istream>
 #include <ostream>

 using boost::spirit::rule;
 using boost::spirit::parser_tag;
 using boost::spirit::ch_p;
 using boost::spirit::real_p;

 using boost::spirit::tree_node;
 using boost::spirit::node_val_data;

 // The grammar
 struct parser: public boost::spirit::grammar<parser>
 {
   enum rule_ids { addsub_id, multdiv_id, value_id, real_id };

   struct set_value
   {
     set_value(parser const& p): self(p) {}
     void operator()(tree_node<node_val_data<std::string::iterator,
                                             double> >& node,
                     std::string::iterator begin,
                     std::string::iterator end) const
     {
       node.value.value(self.tmp);
     }
     parser const& self;
   };

   mutable double tmp;

   template<typename Scanner> struct definition
   {
     rule<Scanner, parser_tag<addsub_id> > addsub;
     rule<Scanner, parser_tag<multdiv_id> > multdiv;
     rule<Scanner, parser_tag<value_id> > value;
     rule<Scanner, parser_tag<real_id> > real;

     definition(parser const& self)
     {
       using namespace boost::spirit;
       addsub = multdiv
         >> *((root_node_d[ch_p('+')] | root_node_d[ch_p('-')]) >> multdiv);
       multdiv = value
         >> *((root_node_d[ch_p('*')] | root_node_d[ch_p('/')]) >> value);
       value = real | inner_node_d[('(' >> addsub >> ')')];
       real = leaf_node_d[access_node_d[real_p[assign_a(self.tmp)]][set_value(self)]];
     }

     rule<Scanner, parser_tag<addsub_id> > const& start() const
     {
       return addsub;
     }
   };
 };

 template<typename TreeIter>
 double evaluate(TreeIter const& i)
 {
   double op1, op2;
   switch (i->value.id().to_long())
   {
   case parser::real_id:
     return i->value.value();
   case parser::value_id:
   case parser::addsub_id:
   case parser::multdiv_id:
     op1 = evaluate(i->children.begin());
     op2 = evaluate(i->children.begin()+1);
     switch(*i->value.begin())
     {
     case '+':
       return op1 + op2;
     case '-':
       return op1 - op2;
     case '*':
       return op1 * op2;
     case '/':
       return op1 / op2;
     default:
       assert(!"Should not happen");
     }
   default:
     assert(!"Should not happen");
   }
   return 0;
 }

 // the read/eval/write loop
 int main()
 {
   parser eval;
   std::string line;
   while (std::cout << "Expression: "
          && std::getline(std::cin, line)
          && !line.empty())
   {
     typedef boost::spirit::node_val_data_factory<double> factory_t;
     boost::spirit::tree_parse_info<std::string::iterator, factory_t> info =
       boost::spirit::ast_parse<factory_t>(line.begin(), line.end(),
                                           eval, boost::spirit::space_p);
     if (info.full)
     {
       std::cout << "Result: " << evaluate(info.trees.begin()) << std::endl;
     }
     else
     {
       std::cout << "Error in expression." << std::endl;
     }
   }
 };


// Copyright (c) 2020 Sabar Nimmagadda. All rights reserved.

#include "matrix_app.h"

#include <cinder/app/App.h>
#include <Eigen/Dense>
#include <cinder/gl/gl.h>
#include <cinder/Font.h>
#include <cinder/Text.h>
#include <cinder/Vector.h>
#include <cinder/gl/draw.h>
#include <Linear Algebra/computations.h>
#include <iostream>
#include <utility>
#include "CinderImGui.h"
using Eigen::MatrixXd;
namespace matrixapp {
using cinder::app::KeyEvent;
using cinder::Color;
using cinder::ColorA;
using cinder::Rectf;
using cinder::TextBox;
using std::string;
const char kNormalFont[] = "Arial";

MatrixApp::MatrixApp()
    : state_{AppState::kSelecting} {}

void MatrixApp::setup() {
    ui::initialize();
    test_mat << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;
}

void MatrixApp::update() {
    if (state_ == AppState::kInputtingData) {
        InputMatrix();
        String_To_Matrix();
    }
}

void MatrixApp::draw() {
    DrawBackground();
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 50};
    const Color color = Color::white();
    if (state_ == AppState::kSelecting) {
        PrintText("WELCOME", color, size, {100, 50});
        CreateMenu();
    }
    if (state_ == AppState::kSolved) {
        DrawAnswer();
    }
}

void MatrixApp::CreateMenu() {
    ui::ScopedWindow window( "Choose problem", ImGuiWindowFlags_MenuBar );
    if( ui::BeginMenuBar() ){
        if( ui::BeginMenu( "Problem Type" )) {
            if (ui::MenuItem( "RREF" )) {
                problemType = ProblemType::RREF;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Row Space" )) {
                problemType = ProblemType::RowSpace;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Column Space")) {
                problemType = ProblemType::ColumnSpace;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem("LU Decomposition")) {
                problemType = ProblemType::LUDecomposition;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Permutation Matrix")) {
                problemType = ProblemType::PermutationMatrix;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Inverse")) {
                problemType = ProblemType::Inverse;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Matrix Multiplication")) {
                problemType = ProblemType::MatrixMultiplication;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("QR Decomposition")) {
                problemType = ProblemType::QRDecomposition;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Dot Product")) {
                problemType = ProblemType::DotProduct;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Eigen Vectors")) {
                problemType = ProblemType::EigenVectors;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Eigen Values")) {
                problemType = ProblemType::EigenValues;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Determinant")) {
                problemType = ProblemType::Determinant;
                state_ = AppState::kInputtingData;
            }
            ui::EndMenu();
        }
        ui::EndMenuBar();
    }
    const ImVec2 vec2(500, 500);
    ui::SetWindowSize("Choose problem", vec2);
}

void MatrixApp::DrawAnswer() {
    if (problemType == ProblemType::QRDecomposition) {
        DrawQRAnswer(in_mat1);
    }
    if (problemType == ProblemType::LUDecomposition) {
        DrawLUAnswer(in_mat1);
    }
    if (problemType == ProblemType::PermutationMatrix) {
        DrawPermutationAnswer(in_mat1);
    }
    if (problemType == ProblemType::RREF) {
        DrawRREFAnswer(in_mat1);
    }
    if (problemType == ProblemType::EigenValues) {
        DrawEigenValuesAnswer(in_mat1);
    }
    if (problemType == ProblemType::Inverse) {
        DrawInverseAnswer(in_mat1);
    }
    if (problemType == ProblemType::EigenVectors) {
        DrawEigenVectorsAnswer(in_mat1);
    }
    if (problemType == ProblemType::DotProduct) {
        DrawDotProductAnswer(in_mat1, in_mat2);
    }
    if (problemType == ProblemType::MatrixMultiplication) {
        DrawMultiplicationAnswer(in_mat1, in_mat2);
    }
    if (problemType == ProblemType::Determinant) {
        DrawDeterminantAnswer(in_mat1);
    }
    if (problemType == ProblemType::RowSpace) {
        DrawRowSpaceAnswer(in_mat1);
    }
    if (problemType == ProblemType::ColumnSpace) {
        DrawColSpaceAnswer(in_mat1);
    }
}

void MatrixApp::keyDown(KeyEvent event) {
}

void MatrixApp::PrintText(const string& text, const Color color, const cinder::ivec2& size,
               const cinder::vec2& loc) {
    cinder::gl::color(color);
    auto box = TextBox()
            .alignment(TextBox::CENTER)
            .font(cinder::Font(kNormalFont, 30))
            .size(size)
            .color(color)
            .backgroundColor(ColorA(0, 0, 1, 0))
            .text(text);

    const auto box_size = box.getSize();
    const cinder::vec2 locp = {loc.x - box_size.x / 2, loc.y - box_size.y / 2};
    const auto surface = box.render();
    const auto texture = cinder::gl::Texture::create(surface);
    cinder::gl::draw(texture, locp);
}
void MatrixApp::DrawBackground() const {
    cinder::gl::clear(Color(0, 0, 0));
}
void MatrixApp::DrawLUAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeL(matrix);
    PrintText("Your L Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    std::stringstream st;
    st << Computations::ComputeU(matrix);
    PrintText("Your U Matrix is",color,{500,500},{center.x-50,center.y + 150});
    PrintText(st.str(), color, size, {center.x, center.y + 200});
    BackToMenu();
}

void MatrixApp::DrawDotProductAnswer(MatrixXd matrix1, MatrixXd matrix2) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeDotProduct(std::move(matrix1), std::move(matrix2), kDimension);
    PrintText("Your Dot Product is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::DrawPermutationAnswer(MatrixXd matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputePermutationMatrix(std::move(matrix));
    PrintText("Your Permutation Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawRREFAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeRREF(matrix);
    PrintText("Your Row Reduced Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawMultiplicationAnswer(MatrixXd matrix1, MatrixXd matrix2) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeMultiply(std::move(matrix1), std::move(matrix2));
    PrintText("The product Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawInverseAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeInverse(matrix);
    PrintText("The Inverse Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawQRAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeQ(matrix);
    PrintText("Your Q Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    std::stringstream st;
    st << Computations::ComputeR(matrix);
    PrintText("Your R Matrix is",color,{500,500},{center.x-50,center.y + 150});
    PrintText(st.str(), color, size, {center.x, center.y + 200});
    BackToMenu();
}

void MatrixApp::DrawEigenVectorsAnswer(MatrixXd matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenVectors(matrix);
    PrintText("The Matrix of EigenVectors is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawEigenValuesAnswer(MatrixXd matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenValues(matrix);
    PrintText("The Eigenvalues are",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawDeterminantAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeDeterminant(matrix);
    PrintText("Your determinant is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::DrawColSpaceAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenVectors(matrix.inverse());
    PrintText("Your Column Space Matrix is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::DrawRowSpaceAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenVectors(matrix);
    PrintText("Your Row Space Matrix is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::InputMatrix() {
    if (problemType != ProblemType::DotProduct && problemType != ProblemType::MatrixMultiplication) {
        ui::InputInt("Enter dimension",  &kDimension);
        ui::InputText("Input matrix", &input_string);
    } else {
        ui::InputInt("Enter dimension",  &kDimension);
        ui::InputText("Input first matrix", &input_string);
        ui::InputText("Input second matrix", &input_string2);
        str_mat2 = input_string2;
    }
    str_mat = input_string;
}

void MatrixApp::String_To_Matrix() {
    int mat_size = kDimension *kDimension;
    if ( problemType != ProblemType::DotProduct
     && problemType != ProblemType::MatrixMultiplication
     && str_mat.size() == mat_size * 2 && mat_size != 0) { // <= size * 2
        //When the computation only needs one matrix.
        std::istringstream ss(str_mat);
        for (int r = 0; r < kDimension; r++) {
            for (int c = 0; c < kDimension; c++) {
                int elem;
                ss >> elem;
                in_mat1(r, c) = elem;
            }
        }
        state_ = AppState::kSolved;
    } else if (str_mat.size() == mat_size * 2 && str_mat2.size() == mat_size * 2 && mat_size != 0){
        //Made else if instead of else, because size cannot be zero ever.
        std::istringstream ss1(str_mat);
        std::istringstream ss2(str_mat2);
        for (int r = 0; r < kDimension; r++) {
            for (int c = 0; c < kDimension; c++) {
                int elem1;
                int elem2;
                ss1 >> elem1;
                ss2 >> elem2;
                in_mat1(r, c) = elem1;
                in_mat2(r, c) = elem2;
            }
        }
        state_ = AppState::kSolved;
    }
}

void MatrixApp::BackToMenu() {
    const cinder::ivec2 button_size = {500, 50};
    if (ui::Button("BACK TO MAIN MENU", button_size)) {
        state_ = AppState::kSelecting;
        input_string = "";
        input_string2 = "";
        kDimension = 0;
    }
}

}  // namespace myapp


#include "vc.h"

#include "cropper.h"
#include "fft_plan.h"
#include "tensorOps.h"

#include <Eigen/SVD>

Cx4 VBC(Cx4 &maps, Log &log)
{
  Index const nc = maps.dimension(0);
  Index const nx = maps.dimension(1);
  Index const ny = maps.dimension(2);
  Index const nz = maps.dimension(3);

  Eigen::Map<Eigen::MatrixXcf const> mat(maps.data(), nc, nx * ny * nz);
  log.info("VBC SVD size {}x{}", mat.rows(), mat.cols());
  auto const svd = mat.bdcSvd(Eigen::ComputeThinV);
  Cx4 body(maps.dimensions());
  Eigen::Map<Eigen::MatrixXcf> bodymat(body.data(), nc, nx * ny * nz);
  bodymat = svd.matrixV().transpose();
  log.image(maps, "vbc-maps.nii");
  log.image(body, "vbc-body.nii");
  bodymat = bodymat.array().conjugate() / bodymat.array().abs();
  return body;
}

void VCC(Cx4 &data, Log &log)
{
  Index const nc = data.dimension(0);
  Index const nx = data.dimension(1);
  Index const ny = data.dimension(2);
  Index const nz = data.dimension(3);

  // Assemble our virtual conjugate channels
  Cx4 cdata(nc, nx, ny, nz);
  FFT::Planned<5, 3> fft(cdata, log);
  cdata = data;
  log.image(cdata, "vcc-cdata.nii");
  fft.forward(cdata);
  log.image(cdata, "vcc-cdata-ks.nii");
  Cx4 rdata = cdata.slice(Sz4{0, 1, 1, 1}, Sz4{nc, nx - 1, ny - 1, nz - 1})
                .reverse(Eigen::array<bool, 4>({false, true, true, true}))
                .conjugate();
  cdata.setZero();
  cdata.slice(Sz4{0, 1, 1, 1}, Sz4{nc, nx - 1, ny - 1, nz - 1}) = rdata;
  log.image(cdata, "vcc-cdata-conj-ks.nii");
  fft.reverse(cdata);
  log.image(cdata, "vcc-cdata-conj.nii");

  Cx3 phase(nx, ny, nz);
  phase.setZero();
  for (Index iz = 1; iz < nz; iz++) {
    for (Index iy = 1; iy < ny; iy++) {
      for (Index ix = 1; ix < nx; ix++) {
        Cx1 const vals = data.chip(iz, 3).chip(iy, 2).chip(ix, 1);
        Cx1 const cvals = cdata.chip(iz, 3).chip(iy, 2).chip(ix, 1).conjugate(); // Dot has a conj
        float const p = std::log(Dot(cvals, vals)).imag() / 2.f;
        phase(ix, iy, iz) = std::polar(1.f, -p);
      }
    }
  }
  log.image(phase, "vcc-correction.nii");
  log.info("Applying Virtual Conjugate Coil phase correction");
  data = data * Tile(phase, nc);
  log.image(data, "vcc-corrected.nii");
}

Cx3 Hammond(Cx4 const &maps, Log &log)
{
  Index const nc = maps.dimension(0);
  Index const nx = maps.dimension(1);
  Index const ny = maps.dimension(2);
  Index const nz = maps.dimension(3);
  log.info("Combining images via the Hammond method");

  Index const refSz = 9;
  Cropper refCrop(Sz3{nx, ny, nz}, Sz3{refSz, refSz, refSz}, log);
  Cx1 const ref =
    refCrop.crop4(maps).sum(Sz3{1, 2, 3}).conjugate() / refCrop.crop4(maps).sum(Sz3{1, 2, 3}).abs();

  using FixedOne = Eigen::type2index<1>;
  Eigen::IndexList<int, FixedOne, FixedOne, FixedOne> rsh;
  rsh.set(0, nc);
  Eigen::IndexList<FixedOne, int, int, int> brd;
  brd.set(1, nx);
  brd.set(2, ny);
  brd.set(3, nz);
  auto const broadcasted = ref.reshape(rsh).broadcast(brd);
  Cx3 const combined = (maps * broadcasted).sum(Sz1{0});
  Cx3 const rss = maps.square().sum(Sz1{0}).sqrt();
  log.image(combined, "hammond-combined.nii");
  log.image(rss, "hammond-rss.nii");

  return combined;
}


// Copyright 2008 Chung-Lin Wen.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/*************************************************************************************************/

#ifndef GIL_LAB_H
#define GIL_LAB_H

////////////////////////////////////////////////////////////////////////////////////////
/// \file
/// \brief Support for CIE Lab color space
/// \author Chung-Lin Wen \n
////////////////////////////////////////////////////////////////////////////////////////

#include <boost/cast.hpp>
#include <boost/gil/gil_all.hpp>

namespace boost { namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace lab_color_space
{
/// \brief Luminance
struct luminance_t {};
/// \brief a Color Component
struct a_color_opponent_t {};
/// \brief b Color Component
struct b_color_opponent_t {};
}
/// \}

/// \ingroup ColorSpaceModel
typedef mpl::vector3< lab_color_space::luminance_t
                    , lab_color_space::a_color_opponent_t
                    , lab_color_space::b_color_opponent_t
                    > lab_t;

/// \ingroup LayoutModel
typedef layout<lab_t> lab_layout_t;

GIL_DEFINE_ALL_TYPEDEFS( 32f, lab );

/// \ingroup ColorConvert
/// \brief RGB to LAB
template <>
struct default_color_converter_impl< rgb_t, lab_t >
{
   template <typename P1, typename P2>
   void operator()( const P1& src, P2& dst ) const
   {
      using namespace lab_color_space;

      // only bits32f for lab is supported
      bits32f temp_red   = channel_convert<bits32f>( get_color( src, red_t()   ));
      bits32f temp_green = channel_convert<bits32f>( get_color( src, green_t() ));
      bits32f temp_blue  = channel_convert<bits32f>( get_color( src, blue_t()  ));

      // first, transfer to xyz color space
      bits32f normalized_r = temp_red / 255.f;
      bits32f normalized_g = temp_green / 255.f;
      bits32f normalized_b = temp_blue / 255.f;

      if( normalized_r > 0.04045f )
      {
	  	   normalized_r = pow( (( normalized_r + 0.055f ) / 1.055f ), 2.4f );
      }
	   else
	   {
	  	   normalized_r /= 12.92f;
      }

      if( normalized_g > 0.04045f )
      {
	  	   normalized_g = pow((( normalized_g + 0.055f ) / 1.055f ), 2.4f );
      }
	   else
	   {
	  	   normalized_g /= 12.92f;
      }

      if( normalized_b > 0.04045f )
      {
	  	   normalized_b = pow( (( normalized_b + 0.055f ) / 1.055f ), 2.4f );
      }
	   else
	   {
	  	   normalized_b /= 12.92f;
      }

	  normalized_r *= 100.f;
	  normalized_g *= 100.f;
	  normalized_b *= 100.f;

      bits32f x, y, z;
      x = normalized_r * 0.4124f + normalized_g * 0.3576f + normalized_b * 0.1805f;
      y = normalized_r * 0.2126f + normalized_g * 0.7152f + normalized_b * 0.0722f;
      z = normalized_r * 0.0193f + normalized_g * 0.1192f + normalized_b * 0.9505f;

      // then, transfer to lab color space
      bits32f ref_x = 95.047f;
      bits32f ref_y = 100.000f;
      bits32f ref_z = 108.883f;
      bits32f normalized_x = x / ref_x;
      bits32f normalized_y = y / ref_y;
      bits32f normalized_z = z / ref_z;

      if( normalized_x > 0.008856f )
      {
         normalized_x = pow( normalized_x, 0.333f );
      }
	   else
	   {
	  	   normalized_x = (7.787f * normalized_x) + ( 16.f/116.f );
      }

      if( normalized_y > 0.008856f )
      {
	  	   normalized_y = pow( normalized_y, 0.333f );
      }
	   else
	   {
	  	   normalized_y = (7.787f * normalized_y) + ( 16.f/116.f );
      }

	   if( normalized_z > 0.008856f )
	   {
	  	   normalized_z = pow( normalized_z, 0.333f );
      }
	   else
	   {
	  	   normalized_z = ( 7.787f * normalized_z ) + ( 16.f/116.f );
      }

      bits32f luminance, a_color_opponent, b_color_opponent;
      luminance = ( 116.f * normalized_y ) - 16.f;
      a_color_opponent = 500.f * ( normalized_x - normalized_y );
      b_color_opponent = 200.f * ( normalized_y - normalized_z );

      get_color( dst, luminance_t() ) = luminance;
      get_color( dst, a_color_opponent_t() ) = a_color_opponent;
      get_color( dst, b_color_opponent_t() ) = b_color_opponent;
   }
};

/// \ingroup ColorConvert
/// \brief LAB to RGB
template <>
struct default_color_converter_impl<lab_t,rgb_t>
{
   template <typename P1, typename P2>
   void operator()( const P1& src, P2& dst) const
   {
      using namespace lab_color_space;

      bits32f luminance = get_color( src, luminance_t() );
      bits32f a_color_opponent = get_color( src, a_color_opponent_t() );
      bits32f b_color_opponent = get_color( src, b_color_opponent_t() );

      // first, transfer to xyz color space
      bits32f normalized_y = ( luminance + 16.f ) / 116.f;
      bits32f normalized_x = ( a_color_opponent / 500.f ) + normalized_y;
      bits32f normalized_z = normalized_y - ( b_color_opponent / 200.f );

      if( pow( normalized_y, 3.f ) > 0.008856f )
      {
         normalized_y = pow( normalized_y, 3.f );
      }
      else
      {
         normalized_y = ( normalized_y - 16.f / 116.f ) / 7.787f;
      }

      if( pow( normalized_x, 3.f ) > 0.008856f )
      {
         normalized_x = pow( normalized_x, 3.f );
      }
      else
      {
         normalized_x = ( normalized_x - 16.f / 116.f ) / 7.787f;
      }

      if( pow( normalized_z, 3.f ) > 0.008856f )
      {
         normalized_z = pow( normalized_z, 3.f );
      }
      else
      {
         normalized_z = ( normalized_z - 16.f / 116.f ) / 7.787f;
      }

      bits32f reference_x = 95.047f;
      bits32f reference_y = 100.000f;
      bits32f reference_z = 108.883f;
      bits32f x, y, z;
      x = reference_x * normalized_x;
      y = reference_y * normalized_y;
      z = reference_z * normalized_z;

      // then, transfer to rgb color space
      normalized_x = x / 100.f;
      normalized_y = y / 100.f;
      normalized_z = z / 100.f;

      bits32f result_r = normalized_x *  3.2406f + normalized_y * -1.5372f + normalized_z * -0.4986f;
      bits32f result_g = normalized_x * -0.9689f + normalized_y *  1.8758f + normalized_z *  0.0415f;
      bits32f result_b = normalized_x *  0.0557f + normalized_y * -0.2040f + normalized_z *  1.0570f;

      if( result_r > 0.0031308f )
      {
         result_r = 1.055f * pow( result_r, 1.f/2.4f ) - 0.055f;
      }
      else
      {
         result_r = 12.92f * result_r;
      }

      if( result_g > 0.0031308f )
      {
         result_g = 1.055f * pow( result_g, 1.f/2.4f ) - 0.055f;
      }
      else
      {
         result_g = 12.92f * result_g;
      }

      if( result_b > 0.0031308f )
      {
         result_b = 1.055f * pow( result_b, 1.f/2.4f ) - 0.055f;
      }
      else
      {
         result_b = 12.92f * result_b;
      }

      bits32f red, green, blue;
      red   = result_r * 255.f;
      green = result_g * 255.f;
      blue  = result_b * 255.f;

      get_color(dst,red_t())  =
         channel_convert<typename color_element_type<P2, red_t>::type>( red );
      get_color(dst,green_t())=
         channel_convert<typename color_element_type<P2, green_t>::type>( green );
      get_color(dst,blue_t()) =
         channel_convert<typename color_element_type<P2, blue_t>::type>( blue );
   }
};

} }  // namespace boost::gil

#endif // GIL_LAB_H