#include "itensor/all.h"
#include <iostream>
#include <chrono>
#include <cstdlib>
#include <stdio.h>
#include <ctime>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_complex_math.h>
#include <string>
#include <set>
#include <vector>
#include <functional>
#include <algorithm>

using namespace itensor;
using namespace std;
using namespace std::chrono;

#define Nspins 4

auto sites = Hubbard(Nspins);
MPO H; //The hubbard hamiltonian
MPS State; //The state for which I want to compute the matrix elements in question


void create_state(const gsl_vector *cicoeff)
{

    auto psi = MPS(sites);
    double normalization;

    for(int n = 1; n <= Nspins; n += 2)
        {

        auto s1 = sites(n);
        auto s2 = sites(n+1);

        normalization = 0;
        int pos = n/8;
        for (int i=pos; i<=pos+4-1; i++)
          normalization += pow(gsl_vector_get(cicoeff,i),2);

        auto wf = ITensor(s1,s2);
        wf.set(s1(2),s2(3), gsl_vector_get(cicoeff, pos)/sqrt(normalization));
        wf.set(s1(3),s2(2), gsl_vector_get(cicoeff, pos+1)/sqrt(normalization));
        wf.set(s1(1),s2(4), gsl_vector_get(cicoeff, pos+2)/sqrt(normalization));
        wf.set(s1(4),s2(1), gsl_vector_get(cicoeff, pos+3)/sqrt(normalization));

        psi.ref(n) = ITensor(s1);
        psi.ref(n+1) = ITensor(s2);
        auto [U,S,V] = svd(wf,{inds(psi.ref(n))},{"Cutoff=",1E-8});
        psi.ref(n) = U;
        psi.ref(n+1) = S*V;

      }

      State = psi;

}

string convertToString(char* a)
{
    int i, size=strlen(a);
    string s = "";
    for (i = 0; i < size; i++) {
        s = s + a[i];
    }
    return s;
}

void generate_H()
{

    double U = 1.0;
    auto ampo = AutoMPO(sites);

        for(int b = 1; b < Nspins; ++b)
            {
                ampo += -1.0,"Cdagup",b,"Cup",b+1; //Hopping for spin up
                ampo += -1.0,"Cdagup",b+1,"Cup",b; // <-
                ampo += -1.0,"Cdagdn",b,"Cdn",b+1; //Hopping for spin down
                ampo += -1.0,"Cdagdn",b+1,"Cdn",b;// <-
                ampo += U, "Nup", b, "Ndn", b;
            }
                ampo += U, "Nup", Nspins, "Ndn", Nspins;
                ampo += -1.0,"Cdagup",1,"Cup",Nspins; //Hopping for spin up
                ampo += -1.0,"Cdagup",Nspins,"Cup",1; // <-
                ampo += -1.0,"Cdagdn",1,"Cdn",Nspins; //Hopping for spin down
                ampo += -1.0,"Cdagdn",Nspins,"Cdn",1;// <-
    H = toMPO(ampo);

}


void computeG(gsl_matrix *G)
{
    for (int i=1; i<=Nspins; i++)
    {
        for (int j=1; j<=Nspins; j++)
        {
            for (int sgi=0; sgi<=1; sgi+=1) { //Loop for up/dn
                for (int sgj=0; sgj<=1; sgj+=1) { //Loop for up/dn
                     char Cd[7] = "Cdag"; char C[4] = "C";
                        char Sgi[3], Sgj[3];
        if (sgi == 0) {
            strcpy(Sgi,"dn");
        }
        if (sgj == 0) {
            strcpy(Sgj,"dn");
        }
        if (sgi == 1) {
            strcpy(Sgi,"up");
        }
        if (sgj == 1) {
            strcpy(Sgj, "up");
        }
                    MPS Phi_0 = State;//Storing State to Phi_0
                    MPO h = H; //Storing the Hamiltonian to h
                            auto ampo = AutoMPO(sites);
                    if (sgi == sgj && i != j) {
                    ampo += convertToString(Cd)+convertToString(Sgi), i, convertToString(C)+convertToString(Sgj), j;//This line basically generates the operator C!_i*C_j with up/down depending on sgi and sgj
                    MPO cc = toMPO(ampo);
                                        MPS h_mps = applyMPO(h,Phi_0); MPS cc_mps = applyMPO(cc,Phi_0); //We apply the cc and the h

                    double elem = -(inner(Phi_0,h,cc_mps) - inner(Phi_0,cc, h_mps));//Computing the elements
                    PAUSE
                            gsl_matrix_set(G, 2*(i-1)+sgi,2*(j-1)+sgj,elem);
                            }
                                }
                        }
        }
    }
}

int main()
{
    generate_H(); //Storing the Hubbard Hamiltonian to the public variable "H"
        double cicoeffs[8]; //These are some coefficients for creating the MPS "State"
    cicoeffs[0] = 0.55727;
    cicoeffs[1] = -0.557423;
    cicoeffs[2] = 0.43516;
    cicoeffs[3] = 0.435161;
    cicoeffs[4] = 0.557338;
    cicoeffs[5] = -0.557365;
    cicoeffs[6] = 0.43516;
    cicoeffs[7] = 0.43515;

    gsl_vector *cicoeff; cicoeff = gsl_vector_alloc(8);
    for (int i=0; i<8; i++)
    gsl_vector_set(cicoeff, i, cicoeffs[i]);

    create_state(cicoeff); //Creating "State" which is a product of dimers

    cout << inner(State,H,State) << endl; //Computing the energy of state


//----------------------------------------
//And here comes the "problematic code"
//Initializing the matrix in which we store the matrix elements in question
gsl_matrix *G; G = gsl_matrix_alloc(Nspins*2,Nspins*2);
for (int p=0; p<Nspins; p++){
        for (int q=0; q<Nspins; q++)
        {for (int pp=0; pp<=1; pp++) { for (int qq=0;qq<=1;qq++) {
gsl_matrix_set(G,2*p+pp,2*q+qq,0);}}}}
//Finished initializing the matrix

computeG(G);//Computing the commutator of the question

//Printing the elements (it never reaches here, except if we use removeQNs)
for (int p=0; p<Nspins; p++){
        for (int q=0; q<Nspins; q++)
        {for (int pp=0; pp<=1; pp++)  {
                double elem =gsl_matrix_get(G,2*p+pp,2*q+pp);
                if (abs(elem) > 1e-6) cout << elem << " " << p << " " << q << " " << pp << endl;
        }}}


  return 0;

}