#include "polygon.hpp"

Coefficients Polygon::coefficients;

Polygon::Polygon(string str){
  Vertex v(0,0);
  indices.insert({v,0});
  vertices.push_back(v);
  length=0;
  size_t i=0;
  size_t l=str.size();
  if(l==0) return;
  char c=str[0];
  while(i<l){
    char s=c;
    int64 xs=0;
    int64 ys=0;
    switch(s){
    case 'l':
      xs=-1;
      break;
    case 'r':
      xs=1;
      break;
    case 'u':
      ys=1;
      break;
    case 'd':
      ys=-1;
      break;
    default:
      Error::string_error("Invalid step",str,i);
      break;
    };
    size_t j=++i;
    size_t n=0;
    if(i<l){
      c=str[i];
      while(i<l and '0'<=c and c<='9'){
        n=n*10+c-'0';
        if(++i==l) break;
        c=str[i];
      }
    }
    if(i==j) n=1; // No digits after step s
    for(size_t k=0;k<n;++k){
      v.x+=xs;
      v.y+=ys;
      ++length;
      auto res=indices.insert({v,length});
      if(!res.second and (i<l or k<n-1)) Error::string_error("Not self avoiding",str,j-1);
      if(res.second) vertices.push_back(v);
    }
  }
  if(v.x!=0 or v.y!=0) Error::error("Not a polygon");
  compute_B();
  compute_C();
  compute_M();
  compute_fp();
}

void Polygon::add_vertex(const int32& x,const int32& y){
  Vertex w(x,y);
  if(indices.find(w)==indices.end()){
    indices.insert({w,vertices.size()});
    vertices.push_back(w);
  }
}

void
Polygon::add_neighbours(const int32& x,const int32& y){
  add_vertex(x,y+1);
  add_vertex(x,y-1);
  add_vertex(x-1,y);
  add_vertex(x+1,y);
}

void
Polygon::compute_B(){
  for(size_t i=0;i<length;++i){
    int32 x=vertices[i].x;
    int32 y=vertices[i].y;
    add_neighbours(x,y);
  }
  B.init(vertices.size());
  B.clear();
  for(size_t i=0;i<length;++i){
    Vertex& v=vertices[i];
    size_t r=indices[Vertex(v.x+1,v.y)];
    size_t l=indices[Vertex(v.x-1,v.y)];
    size_t u=indices[Vertex(v.x,v.y+1)];
    size_t d=indices[Vertex(v.x,v.y-1)];
    B.get(i,r)=1;
    B.get(r,i)=1;
    B.get(i,l)=1;
    B.get(l,i)=1;
    B.get(i,u)=1;
    B.get(u,i)=1;
    B.get(i,d)=1;
    B.get(d,i)=1;
  }
}

void
Polygon::compute_C(){
  C.init(vertices.size());
  size_t n=vertices.size();
  for(size_t i=0;i<n;++i){
    for(size_t j=0;j<n;++j){
      Vertex& vi=vertices[i];
      Vertex& vj=vertices[j];
      double dx=abs(vi.x-vj.x);
      double dy=abs(vi.y-vj.y);
      C.get(i,j)=coefficients.get(dx,dy);
    }
  }
}

void
Polygon::compute_M(){
  //M=[I+1/4*C*B|1]=[I+1/4*C*tB|1]
  size_t n=vertices.size();
  M.init(n,n+1);
  size_t n_avx=(n-1)/4+1;
  AvxBlock b;
  for(size_t i=0;i<n;++i){
    for(size_t j=0;j<n;++j){
      __m256d* avx_C=C.get_avx_row(i);
      __m256d* avx_B=B.get_avx_row(j);
      b.avx=zeros;
      for(size_t k=0;k<n_avx;++k){
        b.avx=_mm256_fmadd_pd(*avx_C,*avx_B,b.avx);
        ++avx_C;
        ++avx_B;
      }
      M.get(i,j)=(0.25)*(b.data[0]+b.data[1]+b.data[2]+b.data[3]);
    }
    M.get(i,i)+=1;
    M.get(i,n)=1;
  }
}

void
Polygon::compute_fp(){
  size_t n=vertices.size();
  double det=M.Gauss();
  cout<<"Det = "<<det<<endl;
  fp=0;
  for(size_t i=0;i<n;++i){
    fp+=B.get_diag_square_sym(i)*M.get(i,n);
  }
  fp*=(det*0.25);
}