void SortTimes(double t_start1, int fls1, double t_end1, int fle1, double t_start2, int fls2, double t_end2, int fle2, function1D<double>& tm, function1D<int>& op);

class NOperators{
  const ClusterData& cluster;           // this is reference to the information about the cluster atomic states (to the object containing that information)
  int time_order_sign;                  // current sign due to time ordering of the operators
  function1D<double> time;              // stores time of all kinks
  function1D<int> index;                // Kinks are not stored in successive order due to efficiency reasons. Index is an array which sorts them according to their time.
  function1D<int> type;                 // is type 2*fl (for creation) or 2*fl+1 (for destruction) operator, where fl is the index of the operator type (for one band model either spin-up or spin-down)
  deque<int> Empty;                     // empty space in the container
  function1D<function2D<double> > Expn; // contains factors : -dt*E_m
  function1D<int> tr_index;             // index for trial moves (not yet accepted moves)
  // Kinks are stored in this class for computing the local trace. They are also stored in the intervals class.
  // However, there are more intervals - as many as the number of independent baths. Sometimes we need to link
  // a kink from this class with the same kink in the intervals class. The arrays below contain the information
  // which can be used to find the link in intervals provided we know the position of the same kink in operators
  function1D<IntervalIndex> p_ifl;      // link between kink in this class and kink in intervals
  int Empty_used;                       
  int maxi_, mini_;
public:
  NOperators(int N_max, const ClusterData& cluster_);
  
  pair<int,int> Add_Cd_C(int fls, double t_start, int fle, double t_end, const IntervalIndex& ifls, const IntervalIndex& ifle);
  bool Try_Add_Cd_C_(int fls, double t_start, int fle, double t_end, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right);
  pair<int,int> Try_Add_Cd_C(int fls, double t_start, int fle, double t_end);
  
  void Remove_C_Cd(int ipe, int ips);
  bool Try_Remove_C_Cd_(int fle, int ie, int fls, int is, int& ipe, int& ips, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right);
  void Try_Remove_C_Cd(int ipe, int ips);
  
  void Move(int ip_old, int ip_new);
  bool Try_Move_(int opera, double t_old, int i_old, double t_new, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right);
  void TryMove(int opera, double t_old, int i_old, int& ip_old, int& ipo, double t_new, int& ip_new);
    
  bool Try_Add_2_Cd_2_C_(int fls1, double t_start1, int fle1, double t_end1, int fls2, double t_start2, int fle2, double t_end2, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right);
  pair<int,int> Try_Add_2_Cd_2_C(int fls1, double t_start1, int fle1, double t_end1, int fls2, double t_start2, int fle2, double t_end2);
  bool Try_Remove_2_C_2_Cd_(int fle1, int ie1, int fls1, int is1, int fle2, int ie2, int fls2, int is2, int& ipe1, int& ips1, int& ipe2, int& ips2, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right);
  void Try_Remove_C_Cd(int ipe1, int ips1, int ipe2, int ips2);
  
  double t(int i) const { return time[index[i]];}
  int typ(int i) const {return type[index[i]];}
  const IntervalIndex& iifl(int i) const {return p_ifl[index[i]];}
  
  int typ_transpose(int i) const {int o = type[index[i]]; return (o%2==0) ? o+1 : o-1;}
  int sign() const {return time_order_sign;}
  bool empty() const {return size()==0;}
  double tr_t(int i) const {return time[tr_index[i]];  }
  int tr_typ(int i) const {return type[tr_index[i]];}
  int size() const {return index.size()-Empty.size();}
  bool full() const {return Empty.size()==6;}
  int max_size() const {return index.size();}
  int mini(){return mini_;}
  int maxi(){return maxi_;}
  const function2D<double>& exp_(int it) const{ int itt = it<size() ? index[it] : index.size(); return Expn[itt]; }
  const function2D<double>& exp_last() const { return Expn[index.size()];}
  const function2D<double>& tr_exp_(int it) const { return Expn[tr_index[it]];}
  const function2D<double>& tr_exp_last() const { return Expn[index.size()+1];}
  void GlobalFlipChange(const function<int>& gflip_fl, int ifa, int ifb);
  
  //  bool Slow_Try_Add_2_Cd_2_C_(int fls1, double t_start1, int fle1, double t_end1, int fls2, double t_start2, int fle2, double t_end2);
  //  bool Slow_Try_Remove_2_C_2_Cd_(int fle1, int ie1, int fls1, int is1, int fle2, int ie2, int fls2, int is2);
  //  void TryMoveCheck(double t_old, double t_new);
  //  void MoveCheck(double t_old, double t_new);
  //  bool Try_Move_Slow(int opera, double t_old, int i_old, double t_new, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right);
  

private:
  int FindSuccessive(int opera, int which, int istart);
  void ChangeEntry(int isort, double time_, int type_, double dt, int new_place, function1D<int>& x_index, const IntervalIndex& p_ifl_);
  void Add(int op, double t, int& is, int& count, const IntervalIndex& iflx);
  int Remove(int to_remove);
  void TryAdd_basic(int which, int op, double t, int& is);
  void TryRemove_basic(int which, int to_remove);
};


inline NOperators::NOperators(int N_max, const ClusterData& cluster_) :
  cluster(cluster_), time_order_sign(1), time(N_max+2), index(N_max), type(N_max+2), Empty(N_max),
  Expn(N_max+2), tr_index(N_max),
  p_ifl(N_max+2)
{
  for (int i=0; i<N_max; i++) Empty[i]=i;
  
  for (int i=0; i<Expn.size(); i++){
    Expn[i].resize(cluster.nsize+1,cluster.max_size);
    for (int j=1; j<Expn[i].size_N(); j++) Expn[i][j].resize(cluster.msize(j));
  }
  for (int i=1; i<Expn[N_max].size_N(); i++){
    for (int m=0; m<Expn[N_max][i].size(); m++)
      Expn[N_max](i,m) = -cluster.Ene(i,m)*common::beta;
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////
/// Some auxiliary routines needed by the main routines below. They are located here          ///
/// because the compiler can inline them only if they are defined before they are used.       ///
/////////////////////////////////////////////////////////////////////////////////////////////////
inline int NOperators::FindSuccessive(int opera, int which, int istart=0)
{
  int nsize = size();
  int i=0;
  int it=0;
  while (i<nsize){
    if (type[index[i]]==opera){
      it++;
      if (it==which+1) goto label;
    }
    i++;
  }
  if (i==nsize){cout<<"Could not find in FindSuccessive!! "<<nsize<<" "<<opera<<" "<<which<<" "<<istart<<endl;}
 label:
  return i;
}

inline void NOperators::ChangeEntry(int isort, double time_, int type_, double dt, int new_place, function1D<int>& x_index, const IntervalIndex& p_ifl_)
{
  tr_index[isort] = new_place;
  time[new_place] = time_;
  type[new_place] = type_;
  p_ifl[new_place] = p_ifl_;
  for (int i=1; i<Expn[0].size_N(); i++){
    for (int m=0; m<Expn[0][i].size(); m++){
      Expn[new_place](i,m) = -cluster.Ene(i,m)*dt;
    }
  }
}



////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////// Routines to add kinks /////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
inline pair<int,int> NOperators::Add_Cd_C(int fls, double t_start, int fle, double t_end, const IntervalIndex& ifls, const IntervalIndex& ifle)
{ // The step to add two kinks succeded -> actually insert the two kinks
  int is1, is2;       // place to insert the new kink
  int count1, count2; // size of the rest of the interval
  Add(2*fls, t_start, is1, count1, ifls);
  Add(2*fle+1, t_end, is2, count2, ifle);
  if (t_end<t_start) is1++;
  time_order_sign *= 1-2*((count1+count2)%2);
  return make_pair(is1,is2);
}

inline bool NOperators::Try_Add_Cd_C_(int fls, double t_start, int fle, double t_end, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right)
{ // Only trial step // migh succeed or might not. This step should not modify the status of the system
  // This is very optimized routine which is called at every trial move when adding a kink.
  // With simple lookups in the table tries to determin if the matrix element is zero or not.
  double t0, t1; int op0, op1;
  if (t_start<t_end){
    t0 = t_start;
    op0 = 2*fls;
    t1 = t_end;
    op1 = 2*fle+1;
  }else{
    t1 = t_start;
    op1 = 2*fls;
    t0 = t_end;
    op0 = 2*fle+1;
  }
  int nsize = size();
  int i0=0;
  while (i0<nsize && time[index[i0]]<=t0) i0++;
  int i1=i0;
  while (i1<nsize && time[index[i1]]<=t1) i1++;
  bool succn;
  for (int i=0; i<cluster.size(); i++){
    int st0 = cluster.praState(i);
    succn = true;
    int stn=st0;
    if (i0>0){
      if (state_evolution_left[i].size()<i0) {succn=false; goto loops_out;}
      stn = state_evolution_left(i,i0-1).istate;
      if (stn==0) {succn=false; goto loops_out;}
    }
    if (i1<nsize && state_evolution_right[i].size()<=nsize-1-i1) {succn=false; goto loops_out;}
    
    stn = cluster.Fi(op0,stn);
    if (stn==0) {succn=false; goto loops_out;}
    for (int l=i0; l<i1; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    stn = cluster.Fi(op1,stn);
    if (stn==0) {succn=false; goto loops_out;}
    
    if (i1<nsize && stn!=state_evolution_right[i][nsize-1-i1].istate) {succn=false; goto loops_out;}
    if (succn) break;
  loops_out:;
  }
  return succn;
}
inline pair<int,int> NOperators::Try_Add_Cd_C(int fls, double t_start, int fle, double t_end)
{ // Only trial step 
  // After the lookups were successful, the matrix element is probably nonzero and the move has a finite probability.
  // This is still a trial step and therefore should not modify the status of the system. However, it has to
  // give the matrix element of the trial "new" configuration.
  int is1, ie1;
  TryAdd_basic(0, 2*fls, t_start, is1);
  TryAdd_basic(1, 2*fle+1, t_end, ie1);
  if (t_end<t_start) is1++;
  return make_pair(is1,ie1);
}



////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////// Routines to remove kinks //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
inline void NOperators::Remove_C_Cd(int ipe, int ips)
{// The step to remove two kinks succeded -> actually remove the two kinks
  int count1 = Remove(ipe);
  int count2 = Remove(ips);
  time_order_sign *= 1-2*((count1+count2)%2);
}

inline bool NOperators::Try_Remove_C_Cd_(int fle, int ie, int fls, int is, int& ipe, int& ips, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right)
{// Only trial step // migh succeed or might not. This step should not modify the status of the system
  // This is very optimized routine which is called at every trial move when removing a kink.
  // With simple lookups in the table tries to determin if the matrix element is zero or not.
  ipe = FindSuccessive(2*fle+1, ie);
  ips = FindSuccessive(2*fls,   is);
  int i0, i1;
  if (ips<ipe){
    i0 = ips;
    i1 = ipe;
  }else{
    i1 = ips;
    i0 = ipe;
  }
  int nsize = size();
  bool succn = true;
  int st0;
  for (int i=0; i<cluster.size(); i++){
    st0 = cluster.praState(i);
    succn=true;
    int stn=st0;
    if (i0>0){
      if (state_evolution_left[i].size()<i0) {succn=false; goto loops_out;}
      stn = state_evolution_left(i,i0-1).istate;
      if (stn==0) {succn=false; goto loops_out;}
    }
    if (i1<nsize-1 && state_evolution_right[i].size()<=nsize-2-i1) {succn=false; goto loops_out;}
    
    for (int l=i0+1; l<i1; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    if (i1<nsize-1){
      if (stn!=state_evolution_right[i][nsize-2-i1].istate) {succn=false; goto loops_out;}
    }else{
      if (stn!=st0) {succn=false; goto loops_out;}
    }
    if (succn) break;
  loops_out:;
  }
  maxi_ = max(ips,ipe);
  if (ips>ipe) ips--;
  mini_ = min(ips,ipe);
  return succn;
}

inline void NOperators::Try_Remove_C_Cd(int ipe, int ips)
{ // Only trial step 
  // After the lookup was successful, the matrix element is probably nonzero and the move has a finite probability.
  // This is still a trial step and therefore should not modify the status of the system. However, it has to
  // give the matrix element of the trial "new" configuration.
  TryRemove_basic(0, ipe);
  TryRemove_basic(1, ips);
}


////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////// Routines to move a kinks //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////

inline void NOperators::Move(int ip_old, int ip_new)
{ // Routine which actually moves the kink
  // after the step is accepted
  int nsize = size();
  for (int i=0; i<Empty_used; i++) Empty.pop_front();
  for (int i=0; i<nsize; i++){
    int ii = index[i]; bool used=false;
    for (int j=0; j<nsize; j++) // is this index still used?
      if (ii==tr_index[j]){ used=true; break;}
    if (!used) Empty.push_back(ii);
  }
  if (size()!=nsize) cerr<<"Did not correctly fill up Empty array!"<<endl;
  for (int i=0; i<nsize; i++) index[i] = tr_index[i];
  ChangeEntry(nsize+1,  common::beta,  -1,  common::beta-time[index[nsize-1]], index.size(), index, -1);
  time_order_sign *= 1-2*((ip_new+ip_old)%2);
}

inline bool NOperators::Try_Move_(int opera, double t_old, int i_old, double t_new, const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right)
{// Only trial step // migh succeed or might not. This step should not modify the status of the system
  // This is very optimized routine which is called at every trial to move a kink.
  // With simple lookups in the table tries to determin if the matrix element is zero or not.
  int ip_old = FindSuccessive(opera, i_old);
  if (time[index[ip_old]]!=t_old) cerr<<"TryMove_basic failed, did not find the right time "<<t_old<<" "<<time[index[ip_old]]<<endl;
  
  int nsize = size();
  int in=0;
  while (in<nsize && time[index[in]]<=t_new) in++;
  bool succn;
  if (t_new>t_old){
    for (int i=0; i<cluster.size(); i++){
      int st0 = cluster.praState(i);
      succn = true;
      int stn=st0;
      if (ip_old>0){
	if (state_evolution_left[i].size()<ip_old) {succn=false; goto loops_out1;}
	stn = state_evolution_left(i,ip_old-1).istate;
	if (stn==0) {succn=false; goto loops_out1;}
      }
      if (in<nsize && state_evolution_right[i].size()<=nsize-1-in) {succn=false; goto loops_out1;}
      for (int l=ip_old+1; l<in; l++){
	int op = type[index[l]];
	stn = cluster.Fi(op,stn);
	if (stn==0) {succn=false; goto loops_out1;}
      }
      stn = cluster.Fi(opera,stn);
      if (stn==0) {succn=false; goto loops_out1;}

      if (in<nsize && stn!=state_evolution_right[i][nsize-1-in].istate) {succn=false; goto loops_out1;}
      if (succn) break;
    loops_out1:;
    }
  }else{
    for (int i=0; i<cluster.size(); i++){
      int st0 = cluster.praState(i);
      succn = true;
      int stn=st0;
      if (in>0){
	if (state_evolution_left[i].size()<in) {succn=false; goto loops_out2;}
	stn = state_evolution_left(i,in-1).istate;
	if (stn==0) {succn=false; goto loops_out2;}
      }
      if (state_evolution_right[i].size()<nsize-1-ip_old) {succn=false; goto loops_out2;}
	
      stn = cluster.Fi(opera,stn);
      if (stn==0) {succn=false; goto loops_out2;}
      for (int l=in; l<ip_old; l++){
	int op = type[index[l]];
	stn = cluster.Fi(op,stn);
	if (stn==0) {succn=false; goto loops_out2;}
      }
      if (ip_old<nsize-1 && stn!=state_evolution_right[i][nsize-2-ip_old].istate) {succn=false; goto loops_out2;}
      if (succn) break;
    loops_out2:;
    }
  }
  return succn;
}

inline void NOperators::TryMove(int opera, double t_old, int i_old, int& ip_old, int& ipo, double t_new, int& ip_new)
{ // Only trial step 
  // After the lookup was successful, the matrix element is probably nonzero and the move has a finite probability.
  // This is still a trial step and therefore should not modify the status of the system. However, it has to
  // give the matrix element of the trial "new" configuration.
  ip_old = FindSuccessive(opera, i_old);
  if (time[index[ip_old]]!=t_old){
    cerr<<"TryMove_basic failed, did not find the right time "<<t_old<<" "<<time[index[ip_old]]<<endl;
  }
    
  Empty_used=0;
  int nsize = size();
  // prepare index array where the choose operator (ip_old) is missing
  for (int i=0; i<ip_old; i++) tr_index[i] = index[i];
  for (int i=ip_old+1; i<nsize; i++) tr_index[i-1] = index[i];
  // finds the new place for this operator according to new time t_new
  int is=0;
  while (is<nsize-1 && time[tr_index[is]]<t_new) is++;
  // makes place for the new operator to insert
  for (int i=nsize-1; i>is; i--) tr_index[i] = tr_index[i-1];
  // changes the entry for this particular moved operator
  double dt = is>0 ? t_new - time[tr_index[is-1]] : t_new;
  ChangeEntry(is, t_new, opera, dt, Empty[Empty_used++], tr_index, p_ifl[index[ip_old]]);
  // its neighbor needs to be changes as well. Makes a copy of time and type, and changes exponents
  if (is<nsize-1)
    ChangeEntry(is+1, time[tr_index[is+1]], type[tr_index[is+1]], time[tr_index[is+1]]-t_new, Empty[Empty_used++], tr_index, p_ifl[tr_index[is+1]]);
  ipo = ip_old;
  if (ip_old<nsize-1){      // the operator closest to the old place needs to be changed as well      
    ipo = ip_old>1 ? ip_old-2 : 0;
    while (!(time[tr_index[ipo]]==time[index[ip_old+1]] && type[tr_index[ipo]]==type[index[ip_old+1]])) ipo++;
    if (ipo!=is && ipo!=is+1){
      double dt = ipo>0 ? time[tr_index[ipo]]-time[tr_index[ipo-1]] : time[tr_index[ipo]];
      ChangeEntry(ipo,  time[tr_index[ipo]],  type[tr_index[ipo]], dt, Empty[Empty_used++], tr_index, p_ifl[tr_index[ipo]]);
    }
  }
  ChangeEntry(nsize+1,  common::beta,  -1,  common::beta-time[tr_index[nsize-1]], index.size()+1, tr_index, -1);

  ip_new = is;
  //    ip_old = ipo;
}



////////////////////////////////////////////////////////////////////////////////////////
/// Lower level routines which peform some operation and are called by above routines //
////////////////////////////////////////////////////////////////////////////////////////

inline void NOperators::Add(int op, double t, int& is, int& count, const IntervalIndex& iflx)
{
  int nsize = size();
  int to_insert = Empty.front(); // first empty space in the array
  time[to_insert] = t;           // just insert into container in the first empy space
  type[to_insert] = op;          // will reorder index rather than data
  p_ifl[to_insert] = iflx;       // save the index to the same kink in the intervals
  is=0;                          // because index rearangment is cheap, data rearangmenet is expensive
  while (is<nsize && time[index[is]]<=t) is++; // these times are before the current time and are already ordered
  count = (nsize-is);            // found the place where the order will need to be changed
  for (int i=nsize; i>is; i--) index[i] = index[i-1]; // first make a room for new fellow
  index[is] = to_insert;         // and finally insert it in.
  Empty.pop_front();             // now mark that this place in the container is occupied
  
  bool last = is == nsize;       // we need now the kink before this kink and the kink after this kink
  double dt1 = !last ? time[index[is+1]]-time[index[is]] : common::beta-time[index[is]]; // if the kink is the last, next time is beta
  double dt0 = (is>0) ? time[index[is]]-time[index[is-1]] : time[index[is]]; // if kink is the first, previous time is zero
  // We need to modify two time differences: [tau_new,t_previous] and [tau_next,tau_new]
  // In most of the cases, we modify the content of Exp[t_new]==Exp[index[is]] and Exp[t_next]==Exp[index[is+1]]
  // If this new kink is the last kink, we need to modify the last time interval which is stored in
  // index.size()
  int istore = !last ? index[is+1] : index.size(); 
    
  for (int i=1; i<Expn[0].size_N(); i++){
    for (int m=0; m<Expn[0][i].size(); m++){
      Expn[index[is]](i,m) = -cluster.Ene(i,m)*dt0;
      Expn[istore](i,m) = -cluster.Ene(i,m)*dt1;
    }
  }
}

inline int NOperators::Remove(int to_remove)
{
  int nsize = size();
  Empty.push_back(index[to_remove]);
  for (int i=to_remove; i<nsize-1; i++) index[i] = index[i+1];
  int count = nsize-to_remove-1;

  bool last = to_remove==nsize-1;
  double t1 = !last ? time[index[to_remove]] : common::beta;
  double t0 = to_remove>0 ? time[index[to_remove-1]] : 0;
  int istore = !last ? index[to_remove] : index.size();
    
  for (int i=1; i<Expn[0].size_N(); i++){
    for (int m=0; m<Expn[0][i].size(); m++)
      Expn[istore](i,m) = -cluster.Ene(i,m)*(t1-t0);
  }
  return count;
}

inline void NOperators::TryAdd_basic(int which, int op, double t, int& is)
{
  int nsize = size();
  nsize += which;
  int to_insert = Empty[2*which];
    
  time[to_insert] = t;
  type[to_insert] = op;    
  is=0;
  if (which==0){
    while (is<nsize && time[index[is]]<=t) { tr_index[is]=index[is]; is++;}
    for (int i=nsize; i>is; i--) tr_index[i] = index[i-1];
  }else{
    while (is<nsize && time[tr_index[is]]<t) is++;
    for (int i=nsize; i>is; i--) tr_index[i] = tr_index[i-1];
  }
  tr_index[is] = to_insert;
  int to_neigh = Empty[2*which+1];
  bool last = is == nsize;
  double dt1 = !last ? time[tr_index[is+1]]-time[tr_index[is]] : common::beta-time[tr_index[is]];
  double dt0 = (is>0) ? time[tr_index[is]]-time[tr_index[is-1]] : time[tr_index[is]];
  int istore = to_neigh;
  if (!last){
    time[to_neigh] = time[tr_index[is+1]];
    type[to_neigh] = type[tr_index[is+1]];
    tr_index[is+1] = to_neigh;
  }else{
    istore = index.size()+1;
  }
  for (int i=1; i<Expn[0].size_N(); i++){
    for (int m=0; m<Expn[0][i].size(); m++){
      Expn[tr_index[is]](i,m) = -cluster.Ene(i,m)*dt0;
      Expn[istore](i,m) = -cluster.Ene(i,m)*dt1;
    }
  }
  if (!last && which==0){
    for (int i=1; i<Expn[0].size_N(); i++){
      for (int m=0; m<Expn[0][i].size(); m++){
	Expn[index.size()+1](i,m) = Expn[index.size()](i,m);
      }
    }
  }
}

inline void NOperators::TryRemove_basic(int which, int to_remove)
{
  int nsize = size();
  nsize -= which;
  if (which==0){
    for (int i=0; i<to_remove; i++) tr_index[i] = index[i];
    for (int i=to_remove; i<nsize-1; i++) tr_index[i] = index[i+1];
  }else{
    for (int i=to_remove; i<nsize-1; i++) tr_index[i] = tr_index[i+1];
  }

  int to_insert = Empty[which];
    
  bool last = to_remove==nsize-1;
  double t1 = !last ? time[tr_index[to_remove]] : common::beta;
  double t0 = to_remove>0 ? time[tr_index[to_remove-1]] : 0;
  int istore = !last ? tr_index[to_remove] : tr_index.size()+1;
  if (!last){
    istore = to_insert;
    time[istore] = time[tr_index[to_remove]];
    type[istore] = type[tr_index[to_remove]];
    tr_index[to_remove] = to_insert;
  }
  for (int i=1; i<Expn[0].size_N(); i++){
    for (int m=0; m<Expn[0][i].size(); m++){
      Expn[istore](i,m) = -cluster.Ene(i,m)*(t1-t0);
    }
  }
  if (!last && which==0){
    for (int i=1; i<Expn[0].size_N(); i++){
      for (int m=0; m<Expn[0][i].size(); m++){
	Expn[index.size()+1](i,m) = Expn[index.size()](i,m);
      }
    }
  }
}


// inline void NOperators::TryMoveCheck(double t_old, double t_new)
// {
//   int nsize = size();

//   for (int i=0; i<nsize-1; i++)
//     if (time[tr_index[i]]>time[tr_index[i+1]]) cerr<<"Times are not sorted!"<<endl;

//   bool found=false;
//   for (int i=0; i<nsize; i++)
//     if (time[tr_index[i]]==t_new){found=true; break;}
//   if (!found)cerr<<"Could not find new time in the index!"<<endl;
    
//   found=false;
//   for (int i=0; i<nsize; i++)
//     if (time[tr_index[i]]==t_old){found=true; break;}
//   if (found)cerr<<"Found old time in the index!"<<endl;
      
//   double diff=0;
//   for (int i=1; i<Expn[0].size_N(); i++){
//     for (int m=0; m<Expn[0][i].size(); m++){
//       double Ene = cluster.Ene(i,m);
//       diff += fabs(Expn[tr_index[0]](i,m)+Ene*(time[tr_index[0]]-0.0));
//     }
//   }
//   if (diff>1e-10) cerr<<"Found problems at 0!";
//   for (int it=1; it<nsize; it++){
//     for (int i=1; i<Expn[0].size_N(); i++){
//       for (int m=0; m<Expn[0][i].size(); m++){
// 	double Ene = cluster.Ene(i,m);
// 	diff += fabs(Expn[tr_index[it]](i,m)+Ene*(time[tr_index[it]]-time[tr_index[it-1]]));
//       }
//     }
//     if (diff>1e-10) cerr<<"Found problems at "<<it<<"!"<<endl;
//   }
//   for (int i=1; i<Expn[0].size_N(); i++){
//     for (int m=0; m<Expn[0][i].size(); m++){
//       double Ene = cluster.Ene(i,m);
//       diff += fabs(Expn[index.size()+1](i,m)+Ene*(common::beta-time[tr_index[nsize-1]]));
//     }
//   }
//   if (diff>1e-10) cerr<<"Found problems at last!"<<endl;
// }

// inline void NOperators::MoveCheck(double t_old, double t_new)
// {
//   int nsize = size();
  
//   for (int i=0; i<nsize-1; i++)
//     if (time[index[i]]>time[index[i+1]]) cerr<<"Times are not sorted!"<<endl;
  
//   bool found=false;
//   for (int i=0; i<nsize; i++)
//     if (time[index[i]]==t_new){found=true; break;}
//   if (!found)cerr<<"Could not find new time in the index!"<<endl;
  
//   found=false;
//   for (int i=0; i<nsize; i++)
//     if (time[index[i]]==t_old){found=true; break;}
//   if (found)cerr<<"Found old time in the index!"<<endl;
  
//   double diff=0;
//   for (int i=1; i<Expn[0].size_N(); i++){
//     for (int m=0; m<Expn[0][i].size(); m++){
//       double Ene = cluster.Ene(i,m);
//       diff += fabs(Expn[index[0]](i,m)+Ene*(time[index[0]]-0.0));
//     }
//   }
//   if (diff>1e-10) cerr<<"Found problems at 0!";
//   for (int it=1; it<nsize; it++){
//     for (int i=1; i<Expn[0].size_N(); i++){
//       for (int m=0; m<Expn[0][i].size(); m++){
// 	double Ene = cluster.Ene(i,m);
// 	diff += fabs(Expn[index[it]](i,m)+Ene*(time[index[it]]-time[index[it-1]]));
//       }
//     }
//     if (diff>1e-10) cerr<<"Found problems at "<<it<<"!"<<endl;
//   }
//   for (int i=1; i<Expn[0].size_N(); i++){
//     for (int m=0; m<Expn[0][i].size(); m++){
//       double Ene = cluster.Ene(i,m);
//       diff += fabs(Expn[index.size()](i,m)+Ene*(common::beta-time[index[nsize-1]]));
//     }
//   }
//   if (diff>1e-10) cerr<<"Found problems at last!"<<endl;
// }

// inline bool NOperators::Try_Move_Slow(int opera, double t_old, int i_old, double t_new,	const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right)
// {
//   int ip_old = FindSuccessive(opera, i_old);
//   if (time[index[ip_old]]!=t_old) cerr<<"TryMove_basic failed, did not find the right time "<<t_old<<" "<<time[index[ip_old]]<<endl;
    
//   int nsize = size();
//   int in=0;
//   while (in<nsize && time[index[in]]<=t_new) in++;
//   bool succn;
//   //    if (in==ip_old) return true;
    
//   for (int i=0; i<cluster.size(); i++){
//     int st0 = cluster.praState(i);
//     succn = true;
//     int stn=st0;
//     for (int l=0; l<nsize; l++){
//       if (l==in){
// 	stn = cluster.Fi(opera,stn);
// 	if (stn==0) {succn=false; goto loops_out3;}
//       }
//       if (l==ip_old) continue;
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out3;}
//     }
//     if (in==nsize) stn = cluster.Fi(opera,stn);
//     if (stn!=st0) {succn=false; goto loops_out3;}
//     if (succn) break;
//   loops_out3:;
//   }
//   return succn;
// }

inline pair<int,int> NOperators::Try_Add_2_Cd_2_C(int fls1, double t_start1, int fle1, double t_end1, int fls2, double t_start2, int fle2, double t_end2)
{
  vector<int> isx(4);
  TryAdd_basic(0, 2*fls1, t_start1, isx[0]);
  TryAdd_basic(1, 2*fle1+1, t_end1, isx[1]);
  if (t_end1<t_start1) isx[0]++;
  TryAdd_basic(2, 2*fls2, t_start2, isx[2]);
  if (t_start2<t_start1) isx[0]++;
  if (t_start2<t_end1) isx[1]++;
  TryAdd_basic(3, 2*fle2+1, t_end2, isx[3]);
  if (t_end2<t_start1) isx[0]++;
  if (t_end2<t_end1) isx[1]++;
  if (t_end2<t_start2) isx[2]++;
  sort(isx.begin(), isx.end());
  return make_pair(isx[0],isx[3]);
}

/////////////////////////////////////////////////////////////////////////////////////
class Cmp{
  const function1D<double>& tm;
public:
  Cmp(const function1D<double>& tm_) : tm(tm_){};
  bool operator()(int a, int b)
  { return tm[a] < tm[b];}
};

void SortTimes(double t_start1, int fls1, double t_end1, int fle1, double t_start2, int fls2, double t_end2, int fle2,
	       function1D<double>& tm, function1D<int>& op)
{
  static function1D<double> tm_o(4);
  static function1D<int> op_o(4);
  static function1D<int> index(4);
  tm.resize(4); op.resize(4);
  
  for (int i=0; i<index.size(); i++) index[i]=i;
  tm_o[0] = t_start1; op_o[0] = fls1;
  tm_o[1] = t_end1;   op_o[1] = fle1;
  tm_o[2] = t_start2; op_o[2] = fls2;
  tm_o[3] = t_end2;   op_o[3] = fle2;
  
  Cmp cmp(tm_o);
  sort(index.begin(),index.end(), cmp);
  for (int i=0; i<index.size(); i++){
    tm[i] = tm_o[index[i]];
    op[i] = op_o[index[i]];
  }
  
}

bool NOperators::Try_Add_2_Cd_2_C_(int fls1, double t_start1, int fle1, double t_end1, int fls2, double t_start2, int fle2, double t_end2,
				   const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right)
{
  static function1D<double> tm(4);
  static function1D<int> op(4);
  SortTimes(t_start1, 2*fls1, t_end1, 2*fle1+1, t_start2, 2*fls2, t_end2, 2*fle2+1, tm, op);
    
  int nsize = size();
  static function1D<int> isx(4);
  for (int l=0; l<4; l++){
    int ii = l>0 ? isx[l-1] : 0;
    while (ii<nsize && time[index[ii]]<=tm[l]) ii++;
    isx[l] = ii;
  }
    
  bool succn;
  for (int i=0; i<cluster.size(); i++){
    int st0 = cluster.praState(i);
    succn = true;
    int stn=st0;
    if (isx[0]>0){
      if (state_evolution_left[i].size()<isx[0]) {succn=false; goto loops_out;}
      stn = state_evolution_left(i,isx[0]-1).istate;
      if (stn==0) {succn=false; goto loops_out;}
    }
    if (isx[3]<nsize && state_evolution_right[i].size()<=nsize-1-isx[3]) {succn=false; goto loops_out;}
	
    stn = cluster.Fi(op[0],stn);
    if (stn==0) {succn=false; goto loops_out;}
    for (int l=isx[0]; l<isx[1]; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    stn = cluster.Fi(op[1],stn);
    if (stn==0) {succn=false; goto loops_out;}
    for (int l=isx[1]; l<isx[2]; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    stn = cluster.Fi(op[2],stn);
    if (stn==0) {succn=false; goto loops_out;}
    for (int l=isx[2]; l<isx[3]; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    stn = cluster.Fi(op[3],stn);
    if (stn==0) {succn=false; goto loops_out;}

    if (isx[3]<nsize && stn!=state_evolution_right[i][nsize-1-isx[3]].istate) {succn=false; goto loops_out;}
    if (succn) break;
  loops_out:;
  }
  return succn;
}

bool NOperators::Try_Remove_2_C_2_Cd_(int fle1, int ie1, int fls1, int is1, int fle2, int ie2, int fls2, int is2,
				      int& ipe1, int& ips1, int& ipe2, int& ips2,
				      const function2D<NState>& state_evolution_left, const function2D<NState>& state_evolution_right)
{
  ipe1 = FindSuccessive(2*fle1+1, ie1);
  ips1 = FindSuccessive(2*fls1,   is1);
  ipe2 = FindSuccessive(2*fle2+1, ie2);
  ips2 = FindSuccessive(2*fls2,   is2);
  static function1D<int> it(4);
  it[0]=ipe1; it[1]=ips1; it[2]=ipe2; it[3]=ips2;
  sort(it.begin(),it.end());
  
  int nsize = size();
  bool succn = true;
  int st0;
  for (int i=0; i<cluster.size(); i++){
    st0 = cluster.praState(i);
    succn=true;
    int stn=st0;
    if (it[0]>0){
      if (state_evolution_left[i].size()<it[0]) {succn=false; goto loops_out;}
      stn = state_evolution_left(i,it[0]-1).istate;
      if (stn==0) {succn=false; goto loops_out;}
    }
    if (it[3]<nsize-1 && state_evolution_right[i].size()<=nsize-2-it[3]) {succn=false; goto loops_out;}
	
    for (int l=it[0]+1; l<it[1]; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    for (int l=it[1]+1; l<it[2]; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    for (int l=it[2]+1; l<it[3]; l++){
      int op = type[index[l]];
      stn = cluster.Fi(op,stn);
      if (stn==0) {succn=false; goto loops_out;}
    }
    if (it[3]<nsize-1){
      if (stn!=state_evolution_right[i][nsize-2-it[3]].istate) {succn=false; goto loops_out;}
    }else{
      if (stn!=st0) {succn=false; goto loops_out;}
    }
    if (succn) break;
  loops_out:;
  }
  maxi_ = max(max(ips1,ipe1),max(ips2,ipe2));
  if (ips1>ipe1) ips1--;
  if (ipe2>ipe1) ipe2--;
  if (ips2>ipe1) ips2--;
  if (ipe2>ips1) ipe2--;
  if (ips2>ips1) ips2--;
  if (ips2>ipe2) ips2--;
  mini_ = min(min(ips1,ipe1),min(ips2,ipe2));
  return succn;
}

// bool NOperators::Slow_Try_Add_2_Cd_2_C_(int fls1, double t_start1, int fle1, double t_end1, int fls2, double t_start2, int fle2, double t_end2)
// {
//   static function1D<double> tm(4);
//   static function1D<int> op(4);
//   SortTimes(t_start1, 2*fls1, t_end1, 2*fle1+1, t_start2, 2*fls2, t_end2, 2*fle2+1, tm, op);
    
//   int nsize = size();
//   static function1D<int> isx(4);
//   for (int l=0; l<4; l++){
//     int ii = l>0 ? isx[l-1] : 0;
//     while (ii<nsize && time[index[ii]]<=tm[l]) ii++;
//     isx[l] = ii;
//   }
    
//   bool succn;
//   for (int i=0; i<cluster.size(); i++){
//     int st0 = cluster.praState(i);
//     succn = true;
//     int stn=st0;
//     for (int l=0; l<isx[0]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     stn = cluster.Fi(op[0],stn);
//     if (stn==0) {succn=false; goto loops_out;}
//     for (int l=isx[0]; l<isx[1]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     stn = cluster.Fi(op[1],stn);
//     if (stn==0) {succn=false; goto loops_out;}
//     for (int l=isx[1]; l<isx[2]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     stn = cluster.Fi(op[2],stn);
//     if (stn==0) {succn=false; goto loops_out;}
//     for (int l=isx[2]; l<isx[3]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     stn = cluster.Fi(op[3],stn);
//     if (stn==0) {succn=false; goto loops_out;}
//     for (int l=isx[3]; l<nsize; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     if (stn!=st0) {succn=false; goto loops_out;}
//     if (succn) break;
//   loops_out:;
//   }
//   return succn;
// }

// bool NOperators::Slow_Try_Remove_2_C_2_Cd_(int fle1, int ie1, int fls1, int is1, int fle2, int ie2, int fls2, int is2)
// {
//   static function1D<int> it(4);
//   it[0] = FindSuccessive(2*fle1+1, ie1);
//   it[1] = FindSuccessive(2*fls1,   is1);
//   it[2] = FindSuccessive(2*fle2+1, ie2);
//   it[3] = FindSuccessive(2*fls2,   is2);
//   sort(it.begin(),it.end());
  
//   int nsize = size();
//   bool succn = true;
//   for (int i=0; i<cluster.size(); i++){
//     int st0 = cluster.praState(i);
//     succn=true;
//     int stn=st0;
//     for (int l=0; l<it[0]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     for (int l=it[0]+1; l<it[1]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     for (int l=it[1]+1; l<it[2]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     for (int l=it[2]+1; l<it[3]; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     for (int l=it[3]+1; l<nsize; l++){
//       int op = type[index[l]];
//       stn = cluster.Fi(op,stn);
//       if (stn==0) {succn=false; goto loops_out;}
//     }
//     if (stn!=st0) {succn=false; goto loops_out;}
//     if (succn) break;
//   loops_out:;
//   }
//   return succn;
// }

void NOperators::Try_Remove_C_Cd(int ipe1, int ips1, int ipe2, int ips2)
{
  TryRemove_basic(0, ipe1);
  TryRemove_basic(1, ips1);
  TryRemove_basic(2, ipe2);
  TryRemove_basic(3, ips2);
}

void NOperators::GlobalFlipChange(const function<int>& gflip_fl, int ifa, int ifb)
{
  for (int i=0; i<size(); i++){
    int iop = type[index[i]];
    int nfl = gflip_fl[iop/2];
    int nop = 2*nfl + iop%2;
    type[index[i]] = nop;

    if(p_ifl[index[i]].ifl == ifa){
      p_ifl[index[i]].ifl = ifb;
    } else if (p_ifl[index[i]].ifl == ifb){
      p_ifl[index[i]].ifl = ifa;
    }
  }
}