/* Code for constructing a Wigner-Sietz cell, and for
 * keeping track of a convex polyhedron defined by consistent
 * lists of vertices, lines and half-spaces (planes)
 */

/* Copyright (c) 2023 MJ Rutter 
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, either version 3
 * of the Licence, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, see http://www.gnu.org/licenses/
 */ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include<c2xsf.h>
#include<string.h>

void init_poly(struct poly *p){
  p->nplanes=0;
  p->p=NULL;
  p->nverts=0;
  p->v=NULL;
  p->nlines=0;
  p->l=NULL;
}

/* A plane is defined by a unit normal and a closest distance from origin */
void init_plane(struct plane *p){
  p->valid=0;
  p->norm[0]=p->norm[1]=p->norm[2]=0;
  p->dist=0;
  p->l=NULL;
  p->n=0;
}

void init_vertex(struct vertex *v){
  v->valid=0;
  v->x[0]=v->x[1]=v->x[2]=0;
  v->pl=NULL;
  v->n=0;
}

/* Find intersecton point of three planes */
int intersect(struct plane *p1, struct plane *p2,
              struct plane *p3, double x[3]){
  double cross[3][3],dot;
  struct plane *pl[3];
  int i;

  pl[0]=p1;
  pl[1]=p2;
  pl[2]=p3;

  /*
  for(i=0;i<3;i++)
    printf("Plane %d: (%f %f %f) d=%f\n",i+1,pl[i]->norm[0],pl[i]->norm[1],
	   pl[i]->norm[2],pl[i]->dist);
  */
  for(i=0;i<3;i++)
    vcross(pl[(i+1)%3]->norm,pl[(i+2)%3]->norm,cross[i]);

  dot=0;
  for(i=0;i<3;i++)
    dot+=pl[0]->norm[i]*cross[0][i];

  if (fabs(dot)<tol) return 0;

  for(i=0;i<3;i++)
    x[i]=(pl[0]->dist*cross[0][i]+
          pl[1]->dist*cross[1][i]+
          pl[2]->dist*cross[2][i])/dot;

  //  printf("Intersect (%f %f %f)\n",x[0],x[1],x[2]);
  
  return 1;
}

int intersect_lp(struct plane *p, double p1[3], double p2[3],
                 double x[3]){
  double vtmp[3],lambda,d1,d2;
  int i;

  for(i=0;i<3;i++)
    vtmp[i]=p2[i]-p1[i];
  
  d1=0;
  for(i=0;i<3;i++)
    d1+=p1[i]*p->norm[i];

  d2=0;
  for(i=0;i<3;i++)
    d2+=vtmp[i]*p->norm[i];

  if (fabs(d2)<tol) return 0;

  lambda=(p->dist-d1)/d2;

  if ((lambda<-tol)||(lambda>1+tol)) return 0;

  for(i=0;i<3;i++)
    x[i]=p1[i]+lambda*vtmp[i];

  /*
  printf("Plane (%f %f %f) %f and line (%f %f %f) (%f %f %f)\n",
	 p->norm[0],p->norm[1],p->norm[2],p->dist,
	 p1[0],p1[1],p1[2],p2[0],p2[1],p2[2]);
  printf("Intersect (%f %f %f)\n",x[0],x[1],x[2]);
  */
  return 1;
}

/* Print functions for debugging */
#if 1
void pv(struct vertex *v, int n){
  int i;
  printf("Vertex %d (%f %f %f) planes \n",n,v->x[0],v->x[1],v->x[2]);
  for(i=0;i<v->n;i++) printf("%d ",v->pl[i]);
  printf("\n");
}

void pplane(struct poly *p, int nn){
  double vtmp[3],vtmp2[3],x,*len,*angle;
  int i,j,hit,k,n;
  struct plane *pl;
  struct vertex *v;
  struct line *l;

  pl=p->p+nn;
  v=p->v;
  l=p->l;

  n=0;
  for(i=0;i<pl->n;i++)
    if (l[pl->l[i]].valid) n++;
  printf("Plane has %d sides\n",n);

  len=malloc(pl->n*sizeof(double));
  angle=malloc(pl->n*sizeof(double));
  if ((!len)||(!angle)) error_exit("malloc error in pplane");
  for(i=0;i<pl->n;i++){
    if (!l[pl->l[i]].valid) continue;
    for(j=0;j<3;j++)
      vtmp[j]=v[l[pl->l[i]].v2].x[j]-v[l[pl->l[i]].v1].x[j];
    len[i]=sqrt(vmod2(vtmp));
    hit=0;
    k=-1;
    for(j=0;j<pl->n;j++){
    if (!l[pl->l[j]].valid) continue;
      if (j==i) continue;
      if ((l[pl->l[j]].v1==l[pl->l[i]].v1)||(l[pl->l[j]].v2==l[pl->l[i]].v1)){
	k=pl->l[j];
	hit=1;
	break;
      }
    }
    if (!hit){
      printf("Impossible\n");
      free(angle);
      free(len);
      return;
    }
    for(j=0;j<3;j++)
      vtmp2[j]=v[l[k].v2].x[j]-v[l[k].v1].x[j];
    x=0;
    for(j=0;j<3;j++)
      x+=vtmp[j]*vtmp2[j];
    x=x/sqrt(vmod2(vtmp)*vmod2(vtmp2));
    angle[i]=acos(x);
  }
  for(i=0;i<pl->n;i++)
    printf("len %f  angle %f\n",len[i],180*angle[i]/M_PI);
  free(angle);
  free(len);
}

void ppoly(struct poly *p){
  int i,j;

  printf("%d planes",p->nplanes);
  for(i=0;i<p->nplanes;i++){
    printf("\nPlane %d norm (%f %f %f) %svalid lines: ",
	   i,p->p[i].norm[0],
	   p->p[i].norm[1],p->p[i].norm[2],p->p[i].valid?"":"in");
    for(j=0;j<p->p[i].n;j++) printf("%d ",p->p[i].l[j]);
    if (debug>1) {printf("\n"); pplane(p,i);}
  }

  printf("\n\n%d lines",p->nlines);
  for(i=0;i<p->nlines;i++){
    printf("\nLine %d %svalid: v1=%d v2=%d p1=%d p2=%d",i,p->l[i].valid?"":"in",
	   p->l[i].v1,p->l[i].v2,p->l[i].p1,p->l[i].p2);
  }

  printf("\n%d vertices",p->nverts);
  for(i=0;i<p->nverts;i++){
    printf("\nVertex %d %svalid (%f %f %f) planes: ",i,p->v[i].valid?"":"in",
	   p->v[i].x[0],p->v[i].x[1],p->v[i].x[2]);
    for(j=0;j<p->v[i].n;j++)
      printf("%d ",p->v[i].pl[j]);
  }	     
  printf("\n");
}
#endif

/* inside = certainly inside, i.e. more than tol within boundary */
int inside(double x[3],struct plane *p, int nplanes){
  int i,j;
  double dot;

  for(i=0;i<nplanes;i++){
    if (p[i].valid==0) continue;
    dot=0;
    for(j=0;j<3;j++) dot+=x[j]*p[i].norm[j];
    if (dot>p[i].dist-tol) return 0;
  }
  return 1;
}

/* outside = certainly outside, i.e. more than tol outside boundary */
int outside(double x[3],struct plane *p, int nplanes){
  int i,j;
  double dot;

  for(i=0;i<nplanes;i++){
    if (p[i].valid==0) continue;
    dot=0;
    for(j=0;j<3;j++) dot+=x[j]*p[i].norm[j];
    //    fprintf(stderr,"p=%d dot=%d dist=%f\n",i,dot,p[i].dist);
    if (dot>p[i].dist+tol) return 1;
  }
  return 0;
}

double area_plane(int n, struct poly *p){
  int i,j,v[3],last;
  double vtmp1[3],vtmp2[3],vtmp3[3],x;
  struct plane *pl;
  struct line *l;

  pl=p->p+n;
  x=0;
  l=NULL;
  v[2]=-1;

  if (!pl->valid) return 0;
  if (pl->n<3) return 0;
  
  for(i=0;i<pl->n;i++){
    l=p->l+pl->l[i];
    if (l->valid) break;
  }
  if (!l->valid) return 0;
  v[0]=l->v1;
  v[1]=l->v2;
  last=v[0];
  
  for(i=0;i<pl->n;i++){
    if (!p->l[pl->l[i]].valid) continue;
    v[2]=-1;
    for(j=0;j<pl->n;j++){
      if (!p->l[pl->l[j]].valid) continue;
      if((p->l[pl->l[j]].v1==v[1])&&
	 (p->l[pl->l[j]].v2!=last)){
	v[2]=p->l[pl->l[j]].v2;
	break;
      }
      if((p->l[pl->l[j]].v2==v[1])&&
	 (p->l[pl->l[j]].v1!=last)){
	v[2]=p->l[pl->l[j]].v1;
	break;
      }
    }
    if (v[2]==-1) return 0;
    if (v[2]==v[0]) break;
    for(j=0;j<3;j++){
      vtmp1[j]=p->v[v[1]].x[j]-p->v[v[0]].x[j];
      vtmp2[j]=p->v[v[2]].x[j]-p->v[v[0]].x[j];
    }
    vcross(vtmp1,vtmp2,vtmp3);
    x+=0.5*sqrt(vmod2(vtmp3));
    last=v[1];
    v[1]=v[2];
  }

  if (v[2]!=v[0]) return 0;
  return x;
  
}

double vol_plane(int n, struct poly *p){
  return area_plane(n,p)*fabs(p->p[n].dist)/3;
}

double vol_poly(struct poly *p){
  int i;
  double x;

  x=0;
  for(i=0;i<p->nplanes;i++)
    if (p->p[i].valid) x+=vol_plane(i,p);

  return x;
}


/* Remove first element with value y from list x */
int delete_from_list(int *x, int y, int *n){
  int i,j;

  for(i=0;i<*n;i++)
    if (x[i]==y) break;

  if (i==*n) return 0; /* item not in list */

  j=i;
  for(i=j+1;i<*n;i++)
    x[j++]=x[i];
  (*n)--;
  return 1;
}

void delete_line(int n, struct poly *p){
  int i,itmp[2];
  struct plane *pl;

  if (!p->l[n].valid) return;

  //  printf("deleting line %d (planes %d and %d)\n",n,p->l[n].p1,p->l[n].p2);
  
 /* Remove reference to line from planes */

  itmp[0]=p->l[n].p1;
  itmp[1]=p->l[n].p2;
  
  for(i=0;i<2;i++){
    pl=p->p+itmp[i];
    if (delete_from_list(pl->l,n,&(pl->n))==0){
      printf("Failed to find line %d in plane %d\n",n,itmp[i]);
      ppoly(p);
    }
  }
  p->l[n].valid=0;
}

void delete_plane(int n, struct poly *p){
  int i;

  if (!p->p[n].valid) return;

  //  printf("deleting plane %d containing %d lines\n",n,p->p[n].n);

  /* Delete all lines in plane */
  for(i=0;i<p->p[n].n;i++)
    delete_line(p->p[n].l[i],p);

  if (p->p[n].n==0){
    if (p->p[n].l) free(p->p[n].l);
    p->p[n].l=NULL;
  }
  
  /* Remove plane from all plane lists in vertices */

  for(i=0;i<p->nverts;i++)
    delete_from_list(p->v[i].pl,n,&(p->v[i].n));
  
  //  printf("now contains %d lines\n",p->p[n].n);
  p->p[n].valid=0;

}

/* Remove items marked as invalid from internal data arrays */
void compact_poly(struct poly *p){
  int i,j,nmax,*map;

  nmax=max(max(p->nplanes,p->nlines),p->nverts);
  map=malloc(nmax*sizeof(int));
  if (!map) error_exit("malloc failure in compact_poly");

  if (debug)
    fprintf(stderr,"On entry to compact, %d planes, %d lines, %d vertices\n",
	    p->nplanes,p->nlines,p->nverts);
  
  /* compact vertex array */

  j=0;
  for(i=0;i<p->nverts;i++){
    if (p->v[i].valid){
      p->v[j]=p->v[i];
      map[i]=j;
      j++;
    }
    else {
      if (p->v[i].pl) {
	free(p->v[i].pl);
	p->v[i].pl=NULL;
      }
      map[i]=-1;
    }
  }
  p->nverts=j;
  for(i=0;i<p->nlines;i++){
    if (p->l[i].valid){
      p->l[i].v1=map[p->l[i].v1];
      p->l[i].v2=map[p->l[i].v2];
    }
  }
  p->v=realloc(p->v,p->nverts*sizeof(struct vertex));
  if (!p->v) error_exit("realloc error on shrinking array!");
  
  /* compact planes array */

  j=0;
  for(i=0;i<p->nplanes;i++){
    if ((p->p[i].valid)&&(p->p[i].n==0)) delete_plane(i,p);
    if (p->p[i].valid){
      p->p[j]=p->p[i];
      map[i]=j;
      j++;
    }
    else map[i]=-1;
  }
  p->nplanes=j;
  for(i=0;i<p->nlines;i++){
    if (p->l[i].valid){
      p->l[i].p1=map[p->l[i].p1];
      p->l[i].p2=map[p->l[i].p2];
    }
  }
  for(i=0;i<p->nverts;i++){
    for(j=0;j<p->v[i].n;j++)
      p->v[i].pl[j]=map[p->v[i].pl[j]];
  }
  p->p=realloc(p->p,p->nplanes*sizeof(struct plane));
  if (!p->p) error_exit("realloc error on shrinking array!");
  
  /* compact lines array */

  j=0;
  for(i=0;i<p->nlines;i++){
    if (p->l[i].valid){
      p->l[j]=p->l[i];
      map[i]=j;
      j++;
    }
    else map[i]=-1;
  }
  p->nlines=j;
  for(i=0;i<p->nplanes;i++){
    for(j=0;j<p->p[i].n;j++)
      p->p[i].l[j]=map[p->p[i].l[j]];
  }
  p->l=realloc(p->l,p->nlines*sizeof(struct line));
  if (!p->l) error_exit("realloc error on shrinking array!");

  free(map);

  if (debug)
    fprintf(stderr,"On exit from compact, %d planes, %d lines, %d vertices\n",
	    p->nplanes,p->nlines,p->nverts);
  
}

/* Move point inside a Wigner-Sietz cell. Poly must be a WS cell */
void poly_reduce_point(double x[3],struct poly *p){
  int i,j,iclose;
  double dot,p1[3],y[3],close;

  p1[0]=p1[1]=p1[2]=0;
  iclose=-1;
  close=0;
  
  for(i=0;i<p->nplanes;i++){
    if (!p->p[i].valid) continue;
    dot=0;
    for(j=0;j<3;j++) dot+=x[j]*p->p[i].norm[j];
    if (dot<p->p[i].dist+tol) continue;
    intersect_lp(p->p+i,p1,x,y);
    if ((iclose==-1)||(vmod2(y)<close)){
      iclose=i;
      close=vmod2(y);
    }
  }

  if (iclose!=-1)
    for(i=0;i<3;i++) x[i]-=2*p->p[iclose].dist*p->p[iclose].norm[i];
  
}

/* Point must lie within WS cell corresponding to irreducible cell given */
void ir_poly_reduce_point(double x[3], struct poly *p, struct symmetry *s){
  int i,j;
  double v[3];

  if (!outside(x,p->p,p->nplanes)) return;

  for(i=0;i<s->n;i++){
    for(j=0;j<3;j++)
      v[j]=s->ops[i].mat[j][0]*x[0]+
	s->ops[i].mat[j][1]*x[1]+
	s->ops[i].mat[j][2]*x[2];
    if (!outside(v,p->p,p->nplanes)){
      for(j=0;j<3;j++) x[j]=v[j];
      return;
    }
  }
  fprintf(stderr,"Warning: reduction of point to irreducible zone failed!\n");
}

void ir_poly_reduce_kpoints(struct kpts *k, struct unit_cell *c,
			    struct symmetry *s){
  int i,j,ii,hit;
  double x[3];
  //  return;
  if (debug) fprintf(stderr,"Reducing symmetrised kpoints to IBZ\n");

  if (!c->ibz){
    fprintf(stderr,"Cannot reduce kpts to IBZ with no IBZ calculated!\n");
    return;
  }
  
  addabs(k->kpts,k->n,c->recip);

  j=0;
  for(i=0;i<k->n;i++){
    for(ii=0;ii<3;ii++) x[ii]=k->kpts[i].abs[ii];
    ir_poly_reduce_point(x,c->ibz,s);
    hit=0;
    for(ii=0;ii<j;ii++){
      if ((aeq(x[0],k->kpts[ii].abs[0]))&&
	  (aeq(x[1],k->kpts[ii].abs[1]))&&
	  (aeq(x[2],k->kpts[ii].abs[2]))){
	k->kpts[ii].wt+=k->kpts[i].wt;
	hit=1;
      }
    }
    if (!hit){
      if (j!=i) k->kpts[j]=k->kpts[i];
      for(ii=0;ii<3;ii++) k->kpts[j].abs[ii]=x[ii];
      j++;
    }
  }

  if ((debug)&&(k->n!=j))
    fprintf(stderr,"Reduced %d kpoints to %d\n",k->n,j);
  
  k->n=j;

  addfrac(k->kpts,k->n,c->basis);
}

void add_line(int v1, int v2, int p1, int p2, struct poly *p){
  int i;
  struct plane *pl[2];

  p->l=realloc(p->l,(p->nlines+1)*sizeof(struct line));
  if (!p->l) error_exit("realloc error in add_line");
  p->l[p->nlines].valid=1;
  p->l[p->nlines].v1=v1;
  p->l[p->nlines].v2=v2;
  p->l[p->nlines].p1=p1;
  p->l[p->nlines].p2=p2;

  pl[0]=p->p+p1;
  pl[1]=p->p+p2;
  for(i=0;i<2;i++){
    pl[i]->l=realloc(pl[i]->l,(pl[i]->n+1)*sizeof(int));
    if (!pl[i]->l) error_exit("realloc error in add_line");
    pl[i]->l[pl[i]->n]=p->nlines;
    pl[i]->n++;
  }
  
  p->nlines++;
}

void add_plane(struct plane *np, struct poly *p){
  int i,j,ii,jj,count,ptmp[3],idx,lidx,hit;
  int *sides,nverts_old,adjust;
  double vtmp1[3],vtmp2[3],dot;

  sides=malloc(p->nverts*sizeof(int));
  adjust=0;
  idx=0;

  for(i=0;i<p->nverts;i++){
    sides[i]=0;
    if (!p->v[i].valid) continue;
    if (inside(p->v[i].x,np,1)) sides[i]=1;
    if (outside(p->v[i].x,np,1)) {
      if (sides[i]==1){
        printf("Error: vertex %d both inside and outside!\n",i);
        exit(1);
      }
      sides[i]=-1;
      adjust=1;
    }
    //    printf("%d: %d\n",i,sides[i]);
  }

  if (!adjust){
    free(sides);
    return;
  }
  
  p->p=realloc(p->p,(p->nplanes+1)*sizeof(struct plane));
  if (!p->p) error_exit("realloc error in add_plane");
  p->p[p->nplanes]=*np;
  p->nplanes++;
  
  nverts_old=p->nverts;

  //  printf("nverts_old=%d\n", nverts_old);

  /* We are adding a plane, and if it passes through existing
   * vertices it must be added to their list of planes
   */
  
  for(i=0;i<nverts_old;i++){
    if (!p->v[i].valid) continue;
    if (sides[i]) continue;
    p->v[i].pl=realloc(p->v[i].pl,(p->v[i].n+1)*sizeof(int));
    if (!p->v[i].pl) error_exit("realloc error in add_plane");
    //    p->v[i].l=realloc(p->v[i].l,(p->v[i].n+1)*sizeof(int));
    p->v[i].pl[p->v[i].n]=p->nplanes-1;
    p->v[i].n++;
  }

  /* Some lines need to be removed or truncated.
   * Truncating lines forms new vertices
   */
  
  for(i=0;i<p->nlines;i++){
    /*
    printf("line %d (%svalid) vertices %d %d sides %d %d\n",i,
	   p->l[i].valid?"":"in",
	   p->l[i].v1,p->l[i].v2,
	   sides[p->l[i].v1],sides[p->l[i].v2]);
    */
    if (!p->l[i].valid) continue;
    /* Either both outside (-1+-1) or one outside and one on edge (-1+0) */
    if (sides[p->l[i].v1]+sides[p->l[i].v2]<=-1){
      //      printf("Removing line %d (vertices %d to %d)\n",i,
      //	     p->l[i].v1,p->l[i].v2);
      //      pv(p->v+p->l[i].v1,-1);
      //      pv(p->v+p->l[i].v2,-1);
      if (sides[p->l[i].v1]==-1) p->v[p->l[i].v1].valid=0;
      if (sides[p->l[i].v2]==-1) p->v[p->l[i].v2].valid=0;
      delete_line(i,p);;
    }
    if (sides[p->l[i].v1]*sides[p->l[i].v2]==-1){
      //      printf("Tuncating line %d (vertices %d to %d)\n",i,
      //	     p->l[i].v1,p->l[i].v2);
      p->v=realloc(p->v,(p->nverts+1)*sizeof(struct vertex));
      if (!p->v) error_exit("realloc error for vertex in add_plane");
      p->v[p->nverts].valid=1;
      intersect_lp(np,p->v[p->l[i].v1].x,p->v[p->l[i].v2].x,p->v[p->nverts].x);
      p->v[p->nverts].n=3;
      p->v[p->nverts].pl=malloc(3*sizeof(int));
      //      p->v[p->nverts].l=malloc(3*sizeof(int));
      p->v[p->nverts].pl[0]=p->l[i].p1;
      p->v[p->nverts].pl[1]=p->l[i].p2;
      p->v[p->nverts].pl[2]=p->nplanes-1;
      if (sides[p->l[i].v1]==-1){
	//	printf(" invalidated vertex %d\n",p->l[i].v1);
	//	pv(p->v+p->l[i].v1,-1);
        p->v[p->l[i].v1].valid=0;
        p->l[i].v1=p->nverts;
      }
      else if (sides[p->l[i].v2]==-1){
	//	printf(" invalidated vertex %d\n",p->l[i].v2);
	//	pv(p->v+p->l[i].v2,-1);
        p->v[p->l[i].v2].valid=0;
        p->l[i].v2=p->nverts;
      }
      else{
        fprintf(stderr,"Impossible\n");
        exit(1);
      }
      p->nverts++;
    }
  }

  for(i=0;i<p->nlines;i++){
    if (!p->l[i].valid) continue;
    if (p->v[p->l[i].v1].valid==0)
      printf("invalid vertex %d (%f %f %f) on line %d\n",
	     p->l[i].v1,p->v[p->l[i].v1].x[0],p->v[p->l[i].v1].x[1],
	     p->v[p->l[i].v1].x[2],i);
    if (p->v[p->l[i].v2].valid==0)
      printf("invalid vertex %d (%f %f %f) on line %d\n",
	     p->l[i].v2,p->v[p->l[i].v2].x[0],p->v[p->l[i].v2].x[1],
	     p->v[p->l[i].v2].x[2],i);
  }

  //  printf("%d new/moved vertices\n",p->nverts-nverts_old);
  
  
  /* Lines which lie wholly in the new plane need to be added
   * These will have both ends on either newly-added vertices,
   * or on old vertices which lie on the new plane
   */
  
  for(i=0;i<p->nverts;i++){
    if (!p->v[i].valid) continue;
    if ((i<nverts_old)&&(sides[i]!=0)) continue;
    for(j=i+1;j<p->nverts;j++){
      if (!p->v[j].valid) continue;
      if ((j<nverts_old)&&(sides[j]!=0)) continue;
      count=0;
      for(ii=0;ii<p->v[i].n;ii++){
	if (!p->p[p->v[i].pl[ii]].valid) continue;
        for(jj=0;jj<p->v[j].n;jj++){
	  if (!p->p[p->v[j].pl[jj]].valid) continue;
          if(p->v[i].pl[ii]==p->v[j].pl[jj]) {
            if (count==3){
              fprintf(stderr,"Impossible count of 3\n");
	      exit(1);
            }
	    else{
	      ptmp[count]=p->v[i].pl[ii];
	      count++;
	    }
          }
        }
      }
      if (count==3){
	/* Three planes meet at common line */
	/* Need to delete a plane, but which? */
	if (ptmp[2]!=p->nplanes-1) error_exit("confused in add_plane");
	vcross(p->p[ptmp[0]].norm,p->p[ptmp[1]].norm,vtmp1);
	vcross(p->p[ptmp[0]].norm,p->p[ptmp[2]].norm,vtmp2);
	dot=vtmp1[0]*vtmp2[0]+vtmp1[1]*vtmp2[1]+vtmp1[2]*vtmp2[2];
	//	printf("count=3, planes %d %d %d, dot %f nplanes %d\n",ptmp[0],ptmp[1],
	//	       ptmp[2],dot,p->nplanes);
	if (dot<0)
	  idx=0;
	else
	  idx=1;
	/* Try to find common line, then update to new plane */
	lidx=-1;
	for(ii=0;ii<p->p[ptmp[idx]].n;ii++){
	  jj=p->p[ptmp[idx]].l[ii];
	  if (((p->l[jj].v1==i)&&(p->l[jj].v2==j))||
	      ((p->l[jj].v2==i)&&(p->l[jj].v1==j))){
	    lidx=jj;
	    break;
	  }
	}
	delete_plane(ptmp[idx],p);
	if (lidx>=0){
	  hit=0;
	  if (p->l[lidx].p1==ptmp[idx]){
	    p->l[lidx].p1=ptmp[2];
	    hit=1;
	  }
	  else if (p->l[lidx].p2==ptmp[idx]){
	    p->l[lidx].p2=ptmp[2];
	    hit=1;
	  }
	  if (hit==1){
	    p->l[lidx].valid=1;
	    // printf("LINE SAVED! %d (%d --> %d)\n",lidx,ptmp[idx],ptmp[2]);
	    ptmp[idx]=ptmp[2];
	    p->p[ptmp[2]].l=realloc(p->p[ptmp[2]].l,
				    (p->p[ptmp[2]].n+1)*sizeof(int));
	    if (!p->p[ptmp[2]].l)
	      error_exit("realloc error in add_plane");
	    for(ii=0;ii<2;ii++){ /* no realloc needed as just deleted line */
	      p->p[ptmp[ii]].l[p->p[ptmp[ii]].n]=lidx;
	      p->p[ptmp[ii]].n++;
	    }
	  }
	  else{
	    count=2;
	    printf("Unexpected inability to find line(1)\n");
	  }
	}
	else{
	  count=2;
	  printf("Unexpected inability to find line(2) % d to %d\n",i,j);
	  ppoly(p);
	}
	ptmp[idx]=ptmp[2];
      }
      if (count==2){
	add_line(i,j,ptmp[0],ptmp[1],p);
      }
    }
  }

  free(sides);
}


void zone1(double basis_in[3][3], struct poly *p){
  double vtmp[3],basis[3][3],x,vol_init,vol_final;
  struct plane np;
  int i,j,k,ii,jj,count,ptmp[2];

  for(i=0;i<3;i++)
    for(j=0;j<3;j++)
      basis[i][j]=basis_in[i][j];

  vol_init=0;
  vcross(basis[0],basis[1],vtmp);
  for(i=0;i<3;i++)
    vol_init+=vtmp[i]*basis[2][i];
  vol_init=fabs(vol_init);
  
  niggli_basis(basis);
  
  /* initialise poly with the six planes formed by the basis vectors */

  p->p=malloc(6*sizeof(struct plane));
  p->v=malloc(8*sizeof(struct vertex));
  if ((!p->p)||(!p->v)) error_exit("malloc error in zone1");
  for(i=0;i<6;i++) init_plane(p->p+i);
  for(i=0;i<8;i++) init_vertex(p->v+i);
  
  for(i=0;i<3;i++){
    x=sqrt(vmod2(basis[i]));
    for(j=0;j<3;j++){
      p->p[2*i].norm[j]=basis[i][j]/x;
      p->p[2*i+1].norm[j]=-basis[i][j]/x;
    }
    p->p[2*i].dist=0.5*x;
    p->p[2*i+1].dist=0.5*x;
    p->p[2*i].valid=1;
    p->p[2*i+1].valid=1;
  }
  p->nplanes=6;

  /* Now find the vertices from those planes, rather generally,
     save that we know that each vertex has precisely three planes meeting */

  for(i=0;i<p->nplanes;i++){
    for(j=i+1;j<p->nplanes;j++){
      vcross(p->p[i].norm,p->p[j].norm,vtmp);
      if (vmod2(vtmp)<tol*tol) continue;
      for(k=j+1;k<p->nplanes;k++){
        vcross(p->p[i].norm,p->p[k].norm,vtmp);
        if (vmod2(vtmp)<tol*tol) continue;
        vcross(p->p[j].norm,p->p[k].norm,vtmp);
        if (vmod2(vtmp)<tol*tol) continue;
	//        printf("Triplet %d %d %d\n",i,j,k);
        if (intersect(p->p+i,p->p+j,p->p+k,vtmp)){
          for(ii=0;ii<3;ii++) p->v[p->nverts].x[ii]=vtmp[ii];
          p->v[p->nverts].pl=malloc(3*sizeof(int));
	  //      p->v[p->nverts].l=malloc(3*sizeof(int));
	  if (!p->v[p->nverts].pl)
	    error_exit("malloc error in zone1");
          p->v[p->nverts].pl[0]=i;
          p->v[p->nverts].pl[1]=j;
          p->v[p->nverts].pl[2]=k;
          p->v[p->nverts].n=3;
          p->v[p->nverts].valid=1;
          p->nverts++;
        }
      }
    }
  }
  /*
  for(i=0;i<p->nverts;i++){
    printf("%d: ",i);
    for(j=0;j<p->v[i].n;j++){
      printf("%d ",p->v[i].pl[j]);
    }
    for(j=0;j<p->v[i].n;j++){
      printf("%f ",p->v[i].x[j]);
    }
    printf("\n");
  }
  */
  /* Now add lines arising from those vertices, again rather generally */

  for(i=0;i<p->nverts;i++){
    for(j=i+1;j<p->nverts;j++){
      count=0;
      for(ii=0;ii<p->v[i].n;ii++){
        for(jj=0;jj<p->v[j].n;jj++){
          if(p->v[i].pl[ii]==p->v[j].pl[jj]) {
            if (count==2){
              fprintf(stderr,"Impossible count of 2\n");
              exit(1);
            }
            ptmp[count]=p->v[i].pl[ii];
            count++;
          }
        }
      }
      if (count==2){
	add_line(i,j,ptmp[0],ptmp[1],p);
      }
    }
  }

  /* End initialisation */

  init_plane(&np);
  np.valid=1;

#define SCAN 1
  
  for(i=-SCAN;i<=SCAN;i++){
    for(j=-SCAN;j<=SCAN;j++){
      for(k=-SCAN;k<=SCAN;k++){
	if ((i==0)&&(j==0)&&(k==0)) continue;
	if (abs(i)+abs(j)+abs(k)==1) continue;
	for(ii=0;ii<3;ii++)
	  vtmp[ii]=i*basis[0][ii]+j*basis[1][ii]+k*basis[2][ii];
	x=sqrt(vmod2(vtmp));
	for(ii=0;ii<3;ii++)
	  np.norm[ii]=vtmp[ii]/x;
	np.dist=0.5*x;
	add_plane(&np,p);
      }
    }
  }
#undef SCAN

  compact_poly(p);

  vol_final=vol_poly(p);

  if (!aeq(vol_init,vol_final))
    fprintf(stderr,"Error: initial volume %f, final %f, in zone1\n",
	    vol_init,vol_final);

  if (debug)
    fprintf(stderr,
	    "Initial volume %f, final %f, in zone1 (should be identical)\n",
	    vol_init,vol_final);

  if (debug>1) ppoly(p);
  
}

#define VMD_DRAW_LINE(x,y) fprintf(outfile, \
				   "draw line {%f %f %f} {%f %f %f}\n", \
				   x[0],x[1],x[2],y[0],y[1],y[2]);

void vmd_recip_write(FILE* outfile, struct unit_cell *c, struct kpts *k){
  int i,j,kk;
  double p1[3],p2[3],p3[3];
  struct poly *p;

  /* write basis */

  if (volume(c->recip)>tol){
    fprintf(outfile,"# Basis\n");
    fprintf(outfile,"draw color yellow\n");
    for(i=0;i<3;i++)
      p3[i]=c->recip[0][i]+c->recip[1][i]+c->recip[2][i];
    
    for(i=0;i<3;i++){
      p1[0]=p1[1]=p1[2]=0;
      VMD_DRAW_LINE(p1,c->recip[i]);
      for(kk=0;kk<3;kk++) p1[kk]=c->recip[i][kk];
      for(j=0;j<3;j++){
	if (i==j) continue;
	for(kk=0;kk<3;kk++) p2[kk]=p1[kk]+c->recip[j][kk];
	VMD_DRAW_LINE(p1,p2);
	VMD_DRAW_LINE(p2,p3);
      }
    }
    fprintf(outfile,"draw text {0 0 0} \"O\"\n");
    for(i=0;i<3;i++)
      fprintf(outfile,"draw text {%f %f %f} \"%c*\"\n",
	      0.75*c->recip[i][0],0.75*c->recip[i][1],
	      0.75*c->recip[i][2],i+'a');
  }
  else{ /* mark Gamma point */
    fprintf(outfile,"draw color white\n");
    fprintf(outfile,"draw sphere {0 0 0} radius 0.05\n");
  }

  p=c->fbz;
  fprintf(outfile,"draw color red\n");
  while (p){
    for(i=0;i<p->nverts;i++){
      if (p->v[i].valid)
	fprintf(outfile,"draw sphere {%f %f %f} radius 0.02\n",
		p->v[i].x[0],p->v[i].x[1],p->v[i].x[2]);
    }

    for(i=0;i<p->nlines;i++){
      if (p->l[i].valid){
	if ((p->v[p->l[i].v1].valid==0)||(p->v[p->l[i].v1].valid==0))
	  fprintf(outfile,"# invalid vertex %d (%f %f %f) on line\n",
		  p->l[i].v1,p->v[p->l[i].v1].x[0],p->v[p->l[i].v1].x[1],
		  p->v[p->l[i].v1].x[2]);
	fprintf(outfile,"draw line {%f %f %f} {%f %f %f}\n",
		p->v[p->l[i].v1].x[0],p->v[p->l[i].v1].x[1],
		p->v[p->l[i].v1].x[2],
		p->v[p->l[i].v2].x[0],p->v[p->l[i].v2].x[1],
		p->v[p->l[i].v2].x[2]);
      }
    }
    if ((p==c->fbz)&&(c->ibz)&&(c->ibz->nverts)){
      fprintf(outfile,"draw color green\n");
      p=c->ibz;
    }
    else p=NULL;
  }

  if ((k)&&(k->n)){
    fprintf(outfile,"# kpoints\n");
    addabs(k->kpts,k->n,c->recip);
    fprintf(outfile,"draw color cyan\n");
    for(i=0;i<k->n;i++)
      fprintf(outfile,"draw sphere {%f %f %f} radius 0.02\n",
	      k->kpts[i].abs[0],k->kpts[i].abs[1],k->kpts[i].abs[2]);
  }

}

void jmol_recip_write(FILE* outfile, struct unit_cell *c, struct kpts *k){
  int i;
  char *colour,*name;
  struct poly *p;

  /* write basis */

  if (volume(c->recip)>tol){
    fprintf(outfile,"# Basis\n");
    fprintf(outfile,"unitcell [{0 0 0}");
    for(i=0;i<3;i++)
      fprintf(outfile," {%f %f %f}",c->recip[i][0],
	      c->recip[i][1],c->recip[i][2]);
    fprintf(outfile,"]\n");
  }
  
  p=c->fbz;
  colour="red";
  name="fbz";
  while (p){

    for(i=0;i<p->nlines;i++){
      if (p->l[i].valid){
	if ((p->v[p->l[i].v1].valid==0)||(p->v[p->l[i].v1].valid==0))
	  fprintf(outfile,"# invalid vertex %d (%f %f %f) on line\n",
		  p->l[i].v1,p->v[p->l[i].v1].x[0],p->v[p->l[i].v1].x[1],
		  p->v[p->l[i].v1].x[2]);
	fprintf(outfile,"draw %sl%d color %s line {%f %f %f} {%f %f %f}\n",
		name,i,colour,
		p->v[p->l[i].v1].x[0],p->v[p->l[i].v1].x[1],
		p->v[p->l[i].v1].x[2],
		p->v[p->l[i].v2].x[0],p->v[p->l[i].v2].x[1],
		p->v[p->l[i].v2].x[2]);
      }
    }
    if ((p==c->fbz)&&(c->ibz)&&(c->ibz->nverts)){
      colour="green";
      name="ibz";
      p=c->ibz;
    }
    else p=NULL;
  }

  if ((k)&&(k->n)){
    fprintf(outfile,"# kpoints\n");
    addabs(k->kpts,k->n,c->recip);
    for(i=0;i<k->n;i++)
      fprintf(outfile,"draw p%d color cyan points [{%f %f %f}]\n",
	      i,k->kpts[i].abs[0],k->kpts[i].abs[1],k->kpts[i].abs[2]);
  }

}


/* Deep copy to dest of source */
void poly_copy(struct poly *s, struct poly *d){
  int i;
  
  d->nplanes=s->nplanes;
  d->p=malloc(d->nplanes*sizeof(struct plane));
  if (!d->p) error_exit("malloc error in poly_copy");
  memcpy(d->p,s->p,d->nplanes*sizeof(struct plane));
  for(i=0;i<d->nplanes;i++){
    d->p[i].l=malloc(d->p[i].n*sizeof(int));
    if (!d->p[i].l) error_exit("malloc error in poly_copy");
    memcpy(d->p[i].l,s->p[i].l,d->p[i].n*sizeof(int));
  }

  d->nverts=s->nverts;
  d->v=malloc(d->nverts*sizeof(struct vertex));
  if (!d->p) error_exit("malloc error in poly_copy");
  memcpy(d->v,s->v,d->nverts*sizeof(struct vertex));
  for(i=0;i<d->nverts;i++){
    d->v[i].pl=malloc(d->v[i].n*sizeof(int));
    if (!d->v[i].pl) error_exit("malloc error in poly_copy");
    memcpy(d->v[i].pl,s->v[i].pl,d->v[i].n*sizeof(int));
  }

  d->nlines=s->nlines;
  d->l=malloc(d->nlines*sizeof(struct line));
  if (!d->l) error_exit("malloc error in poly_copy");
  memcpy(d->l,s->l,d->nlines*sizeof(struct line));
  
}

static double orient(double v[3],double b[3][3]){
  int i;
  double x;

  x=0;
  for(i=0;i<3;i++)
    x+=1.2*v[i]*b[0][i];

  for(i=0;i<3;i++)
    x+=1.1*v[i]*b[1][i];

  for(i=0;i<3;i++)
    x+=v[i]*b[2][i];

  return(x);
  
}

void orient_zone(struct poly *p, struct symmetry *s, double basis[3][3]){
  int i,j,best_i;
  double mid[3],v[3],x,best;

  /* Create unnormalised vector pointing to mid-point of poly */
  for(i=0;i<3;i++) mid[i]=0;
  for(i=0;i<p->nverts;i++)
    if (p->v[i].valid)
      for(j=0;j<3;j++)
	mid[j]+=p->v[i].x[j];

  best=orient(mid,basis);
  best_i=-1;
  
  for(i=0;i<s->n;i++){
    for(j=0;j<3;j++)
	v[j]=s->ops[i].mat[j][0]*mid[0]+
	  s->ops[i].mat[j][1]*mid[1]+
	  s->ops[i].mat[j][2]*mid[2];
    x=orient(v,basis);
    if (x>best){
      best=x;
      best_i=i;
    }
  }

  if (best_i==-1) return;
  //  ppoly(p);

  
  if (debug) fprintf(stderr,"Re-orienting IBZ\n");
  
  for(i=0;i<p->nverts;i++){
    for(j=0;j<3;j++)
	v[j]=s->ops[best_i].mat[j][0]*p->v[i].x[0]+
	  s->ops[best_i].mat[j][1]*p->v[i].x[1]+
	  s->ops[best_i].mat[j][2]*p->v[i].x[2];
    for(j=0;j<3;j++)
      p->v[i].x[j]=v[j];
  }

  for(i=0;i<p->nplanes;i++){
    for(j=0;j<3;j++)
	v[j]=s->ops[best_i].mat[j][0]*p->p[i].norm[0]+
	  s->ops[best_i].mat[j][1]*p->p[i].norm[1]+
	  s->ops[best_i].mat[j][2]*p->p[i].norm[2];
    for(j=0;j<3;j++)
      p->p[i].norm[j]=v[j];
  }

  //  ppoly(p);
  
}

/* See https://doi.org/10.4208/cicp.OA-2021-0094
 * Commun. Comput. Phys., 31 (2022), pg 495
 * Jorgensen et al
 * for IBZ-finding algorithm below */

void ibz(struct unit_cell *c, struct symmetry *s_in){
  int i,j,k,*valid;
  double v[3],mid[3],x,y;
  struct plane *pl,plane;
  struct symmetry *s,sym;
  struct poly *p;

  if (!c->fbz){
    c->fbz=malloc(sizeof(struct poly));
    if (!c->fbz) error_exit("malloc error for FBZ");
    init_poly(c->fbz);
    zone1(c->recip,c->fbz);
  }
  
  c->ibz=malloc(sizeof(struct poly));
  if (!c->ibz) error_exit("malloc error for FBZ");
  init_poly(c->ibz);
  
  s=&sym;
  init_sym(s);

  pl=&plane;
  init_plane(pl);
  
  poly_copy(c->fbz,c->ibz);
  p=c->ibz;

  if ((s_in==NULL)||(s_in->n==0))
    fprintf(stderr,"IBZ requested, but no symmetry information present."
	    " Was --sym forgotten?\n");
  
  sym2ksym(s_in,s);

  valid=malloc(s->n*sizeof(int));
  if (!valid) error_exit("malloc error in ibz");
  for(i=0;i<s->n;i++){
    if (is_identity(s->ops[i].mat))
      valid[i]=0;
    else
      valid[i]=1;
  }

  for(i=0;i<p->nverts;i++){
    if (!p->v[i].valid) continue;
    for(j=0;j<s->n;j++){
      if (!valid[j]) continue;
      for(k=0;k<3;k++)
	v[k]=s->ops[j].mat[k][0]*p->v[i].x[0]+
	  s->ops[j].mat[k][1]*p->v[i].x[1]+
	  s->ops[j].mat[k][2]*p->v[i].x[2];
      for(k=0;k<3;k++)
	mid[k]=0.5*(v[k]+p->v[i].x[k]);
      for(k=0;k<3;k++)
	v[k]-=p->v[i].x[k];
      x=vmod2(v);
      if (x<tol*tol) continue;
      x=sqrt(x);
      for(k=0;k<3;k++)
	pl->norm[k]=v[k]/x;
      pl->valid=1;
      x=0;
      for(k=0;k<3;k++)
	x+=pl->norm[k]*mid[k];
      pl->dist=x;
      add_plane(pl,p);
      valid[j]=0;
    }
  }
  free(valid);

  compact_poly(p);
  if (volume(c->recip)>tol) orient_zone(p,s,c->recip);
  
  x=vol_poly(c->fbz);
  y=vol_poly(c->ibz);
  if (debug){
    fprintf(stderr,"BZ vol=%f, ",x);
    fprintf(stderr,"expected IBZ vol=%f, ",x/s->n);
    fprintf(stderr,"actual IBZ vol=%f\n",y);
  }
  if (!aeq(x/s->n,y))
    fprintf(stderr,"\n*** PROBLEM *** IBZ has unexpected volume!\n\n");

  free_sym(s,1);
  
}

void vert_write(FILE* outfile, struct poly *p){
  int i;
  char *fmt;

  if (flags&HIPREC)
    fmt="% 19.15f % 19.15f % 19.15f\n";
  else
    fmt="% 11.7f % 11.7f % 11.7f\n";
  
  for(i=0;i<p->nverts;i++)
    if (p->v[i].valid)
      fprintf(outfile,fmt,p->v[i].x[0],p->v[i].x[1],p->v[i].x[2]);

}

void vert_frac_write(FILE* outfile, struct poly *p, double basis[3][3]){
  int i,j;
  char *fmt;
  struct atom a;

  init_atoms(&a,1);
  
  if (flags&HIPREC)
    fmt="% 18.15f % 18.15f % 18.15f\n";
  else
    fmt="% 10.7f % 10.7f % 10.7f\n";
  
  for(i=0;i<p->nverts;i++){
    if (p->v[i].valid){
      for(j=0;j<3;j++)
	a.abs[j]=p->v[i].x[j];
      addfrac(&a,1,basis);
      fprintf(outfile,fmt,a.frac[0],a.frac[1],a.frac[2]);
    }
  }
}
