#include "itensor/all.h"
#include <iostream>
#include <chrono>
#include <cstdlib>
#include <stdio.h>
#include <ctime>
#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(double *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(cicoeff[i],2);

        auto wf = ITensor(s1,s2);
        wf.set(s1(2),s2(3), cicoeff[pos]/sqrt(normalization));
        wf.set(s1(3),s2(2), cicoeff[pos+1]/sqrt(normalization));
        wf.set(s1(1),s2(4), cicoeff[pos+2]/sqrt(normalization));
        wf.set(s1(4),s2(1), 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(double G[8][8])
{
    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
                            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;

    double cicoeff[8];
    for (int i=0; i<8; i++)
    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
double G[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++) {
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 = G[2*p+pp][2*q+pp];
                if (abs(elem) > 1e-6) cout << elem << " " << p << " " << q << " " << pp << endl;
        }}}


  return 0;

}