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

static const int debug_line_project = 0;
static const int debug_square_project = 0;
static const int debug_getd=0;
static const int debug_convert=0;
static const int vec_debug=0;


// Might cause trouble in efence uninitialized data
typedef struct {
  double x;
  double y;
} coord;


coord vec_add(coord a, coord b)
{
  coord retval;
  retval.x=a.x+b.x;
  retval.y=a.y+b.y;
  return retval;
}

coord vec_sub(coord a, coord b)
{
  coord retval;
  retval.x=a.x-b.x;
  retval.y=a.y-b.y;
  return retval;
}



double vec_len(coord a)
{
  double retval=sqrt(pow(a.x,2)+pow(a.y,2));

  if (vec_debug) printf("len of (%f,%f) is %f\n", a.x, a.y, retval);
  
  return retval;  
}

double vec_norm(coord pa, coord pb)
{
  double retval=vec_len(vec_sub(pb,pa));
  
  return retval;
}

double vec_scalar(coord a, coord b)
{
  double retval;
  retval=a.x*b.x+a.y*b.y;
  return retval;  
}

double vec_cosang(coord a, coord b)
{
  double retval;

  retval=vec_scalar(a, b) / (vec_len(a) * vec_len(b));

  if (vec_debug) printf("cosang=%f\n", retval);
  
  return retval;  
}

double vec_ang(coord a, coord b)
{
  double cos_ang=vec_cosang(a, b);

  double retval=acos(cos_ang);

  if (vec_debug) printf("ang=%f\n", retval);

  return retval;
}


// vector of length one going from pa towards pb
coord vec_unit(coord pa, coord pb)
{
  coord retval;

  double norm=vec_norm(pa, pb);

  retval.x=(pb.x-pa.x)/norm;
  retval.y=(pb.y-pa.y)/norm;
  
  return retval;
}

coord vec_scale(double k, coord v)
{
  coord retval;
  retval.x = k*v.x;
  retval.y = k*v.y;
  return retval;
}

void vec_print(const char* str, coord c)
{
  printf("%s(%f, %f)\n", str, c.x, c.y);
}

coord vec_intersect(coord p0, coord pvec, coord q0, coord qvec)
{
  // the intersection of the vector p0 towards pvec and
  // q0 towards qvec

  // TODO: detect parallel vectors and write a limerick
  // about Euclides, also need to handle the case where one
  // vector points in the y-axis direction (k=inf)

  // calculating the stretches line equations
  // y=k*x+m => m = y - k*x
  //

  // ungraceful error handler for now
  if ( (pvec.x==p0.x) || (qvec.x==q0.x) )
    {
      fprintf(stderr, "Division by zero in vec_intersect()");
      exit(1);
    }
  
  double k1 = (pvec.y - p0.y) / (pvec.x - p0.x);
  double k2 = (qvec.y - q0.y) / (qvec.x - q0.x);
  double m1 = p0.y - k1*p0.x;
  double m2 = q0.y - k2*q0.x;
  
  if (vec_debug)
    {
      printf("k1=%f, k2=%f, m1=%f, m2=%f\n", k1, k2, m1, m2);
    }
  
  // So the intersect point is given by the pair:
  //
  // y=k1*x+m1
  // y=k2*x+m2
  //
  // combined eliminating y
  // k1*x+m1 = k2*x+m2
  // x(k1-k2) = m2 - m1
  //
  // x = (m2-m1)/(k1-k2)
  //
  // substitution in one of the original equations yield:
  // y = k1*x+m1
  //

  coord retval;
  retval.x=(m2-m1)/(k1-k2);
  retval.y=k1*retval.x+m1;

  if (vec_debug) vec_print("intersect: ", retval);
  return retval;
}

int vec_equal(coord a, coord b)
{
  return ( (a.x==b.x) && (a.y==b.y) );
}

  
// The ambition is to have no composant operations below here


coord vec_project
(
 coord p1,
 coord p2,
 coord p3,
 coord p4,
 coord p
 )
{
  coord retval;

  if (vec_debug) {
    vec_print("p1=",p1);
    vec_print("p2=",p2);
    vec_print("p3=",p3);
    vec_print("p4=",p4);
    vec_print("p=",p);
  }

  // Special case if p===p1 this will become the zero
  // vector which obviously lacks direction so "sign"
  // function below would return NaN

  // Just return vector of start of projection
  if (vec_equal(p1, p)) return p3;

  // These vectors should be parallel but can
  // be opposite or same directions
  double sign=vec_cosang(vec_sub(p2, p1),
			vec_sub(p, p1));
  
  double norm43=vec_norm(p4, p3);
  double norm21=vec_norm(p2, p1);

  if (vec_debug) printf("norm43=%f\n", norm43);
  if (vec_debug) printf("norm21=%f\n", norm21);
  
  double l1=vec_norm(p, p1);

  coord ev43=vec_unit(p3, p4);

  if (vec_debug) vec_print("ev43=", ev43);
  
  double scale=norm43/norm21;

  double l2=scale*l1;
  
  if (vec_debug) printf("l1=%f\n", l1);
  if (vec_debug) printf("scale=%f\n", scale);
  if (vec_debug) printf("l2=%f\n", l2);

  if (vec_debug) vec_print("scale vec=", vec_scale(scale, ev43));
  
  retval = vec_add(p3, vec_scale(sign*l2, ev43));

  if (vec_debug) vec_print("projection=", retval);
  
  double x_max=fmax(p1.x,p2.x);
  double x_min=fmin(p1.x,p2.x);
  
  return retval;
} 

void square_projection_vec
(
 double xmin, double xmax, double ymin, double ymax, // simple coordinates from cad/cam
 double* xcorner,
 double* ycorner,
 double xin, double yin,
 double *xoutp, double *youtp
 )
{
  // TODO have us called with coords instead?
  coord pa,pb,pc,pd,p,p1,p2;

  // Meassured points
  pa.x=xcorner[0];
  pa.y=ycorner[0];
  pb.x=xcorner[1];
  pb.y=ycorner[1];
  pc.x=xcorner[2];
  pc.y=ycorner[2];
  pd.x=xcorner[3];
  pd.y=ycorner[3];

  coord pab, pbc, pcd, pda;

  // Still here? Ok we now want to take the four corner positions
  // and for each deduct a position on four projected lines
  // Please note that the vectors must be in the following order:
  // [0]->[1] the vector projecting from xlow to xhigh at ylow
  // [1]->[2] the vector projecting from ylow to yhigh at xhigh
  // [2]->[3] the vector projecting from xlow to xhigh at yhigh
  // [3]->[0] the vector projecting from ylow to yhigh at xlow

  // Or expressed alternately we start at the low x and y and more around
  // counter-clockwise making a rectangle.
  
  // We lean against a home-cooked theorem that says that for any fixed point
  // we can obtain the projection against all four sides and then calculate the
  // intersection, it being the only possible projection point

  // We will end up with opposing sides on [0] - [2]
  // and [1] - [3] respectively

  p1.x=xmin;
  p1.y=0;
  p2.x=xmax;
  p2.y=0;
  p.x=xin;
  p.y=0;
  
  pab=vec_project(p1, p2, pa, pb, p);

  if (debug_square_project) vec_print("pab=", pab);
  
  // We use the x-axis of vec_project for our y-axis
  p1.x=ymin;
  p1.y=0;
  p2.x=ymax;
  p2.y=0;
  p.x=yin;
  p.y=0;
  pbc=vec_project(p1, p2, pb, pc, p);

  if (debug_square_project) vec_print("pbc=", pbc);
  
  p1.x=xmax;
  p1.y=0;
  p2.x=xmin;
  p2.y=0;
  p.x=xin;
  p.y=0;
  pcd=vec_project(p1, p2, pc, pd, p);

  if (debug_square_project) vec_print("pcd=", pcd);

  // We use the x-axis of vec_project for our y-axis
  p1.x=ymax;
  p1.y=0;
  p2.x=ymin;
  p2.y=0;
  p.x=yin;
  p.y=0;
  pda=vec_project(p1, p2, pd, pa, p);

  if (debug_square_project)
    {
      vec_print("pab=", pab);
      vec_print("pbc=", pbc);
      vec_print("pcd=", pcd);
      vec_print("pda=", pda);
    }

  coord retval=vec_intersect(pab, pcd, pbc, pda);

  // todo vector types
  *xoutp=retval.x;
  *youtp=retval.y;
}


void test_line_project()
{
  double xq, yq;

  double fi;

  coord p1,p2,p3,p4,p;

  p1.x=1.0;
  p1.y=0.0;

  p2.x=11.0;
  p2.y=0.0;

  // Just some random uneveness
  p3.x=1.2;
  p3.y=0.5;
  p4.x=11.0;
  p4.y=-0.2;
  
  p.y=0.0;
  
  for (fi=1.0;fi<=11.0; fi+=0.5)
    {
      p.x=fi;
      coord rpy=vec_project(p1,p2,p3,p4,p);
      printf("fi=%f, xq=%f, yq=%f\n", fi, rpy.x, rpy.y);
    }
}

void test_square_project()
{
  // We assume a board with x-corner holes at 10 and 100 mm
  // and y-corner at -40 and +40 mm
  double xlow=10.0;
  double xhigh=100.0;
  double ylow=-40.0;
  double yhigh=40.0;

  // Then we introduce some skew, in a real world case we let the cnc machine
  // move to this position and use those coordinates as input here...
  double xcorner[4];
  double ycorner[4];

  xcorner[0]=xlow+0.1;
  ycorner[0]=ylow-0.05;

  xcorner[1]=xhigh+1.0;
  ycorner[1]=ylow-0.42;
 
  xcorner[2]=xhigh-0.99;
  ycorner[2]=yhigh+0.01;
 
  xcorner[3]=xlow-0.05;
  ycorner[3]=yhigh+0.034;

  
  double xout, yout;

  double xin=50.0;
  double yin=10.0;
  
  square_projection_vec
  (
     xlow, xhigh, ylow, yhigh,
     xcorner, ycorner,
     xin, yin,
     &xout, &yout
     );

  printf("xin: %f, yin: %f, xout: %f, yout: %f\n",
	 xin, yin, xout, yout);
  
}


int getd(const char* str, double* retval)
{
  char buf[4096];

  printf("%s: ", str);
  fflush(stdout);

  fgets(buf, 4095, stdin);
  
  if (buf[0] == 'q')
    {
      if (debug_getd) printf("User requested quit interactive mode...\n");
      return 0;
    }
  
  sscanf(buf, "%lf", retval);
  if (debug_getd) printf("%s: %f\n", str, *retval);

  return 1;
}


// Filled in by main once.
double xlow;
double xhigh;
double ylow;
double yhigh;

double xcorner[4];
double ycorner[4];


void convert(const char* ins, char* outs)
{
  if (debug_convert) printf("Attempting to convert: ``%s''\n", ins);
  
  // Position just after "X" 
  char ch;
  int i,j;

  double x,y;
  
  i=0;
  while (1)
    {
      ch=ins[i];

      if (ch=='\0')
	{
	  fprintf(stderr, "Cant parse line: `´%s''\n", ins);
	  exit(1);
	}
	
      if (ch=='X')
	{
	  sscanf(&ins[i+1], "%lf", &x);
	  break;
	}

      // Just echo otherwise
      outs[i]=ins[i];
      
      i++;
    }

  // We keep i since it point to where we are to sprintf the converted
  // X and Y
  j=i;
  while (1)
    {
      ch=ins[j];

      if (ch=='\0')
	{
	  fprintf(stderr, "Cant parse line: `´%s''\n", ins);
	  exit(1);
	}
	
      if (ch=='Y')
	{
	  sscanf(&ins[j+1], "%lf", &y);
	  break;
	}

      j++;
    }

  double xout, yout;
  
  // Gotten here we should have both X and Y
  square_projection_vec
    (
     xlow, xhigh, ylow, yhigh,
     xcorner, ycorner,
     x, y,
     &xout, &yout
     );
  
  if (debug_convert) printf("converting: (%f, %f) to (%f, %f)\n",
			 x,y,xout,yout);

  // Assuming DOS format
  sprintf(&outs[i], "X%f Y%f\r\n", xout, yout);
  if (debug_convert) printf("Converting to: ``%s''\n", outs);
}



int main()
{

  //  test_line_project();
  
  //  test_square_project();
  
  double xout, yout;

  double xin;
  double yin;
  
  printf("Please first input the x and y min and max corner coords of the board from the gcode\n");
  
  getd("xlow", &xlow);
  getd("xhigh", &xhigh);
  getd("ylow", &ylow);
  getd("yhigh", &yhigh);

  printf("Now use the machine and position it on the lower left corner marker\n");
  
  getd("X", &xcorner[0]);
  getd("Y", &ycorner[0]);

  printf("Now use the machine and position it on the lower right corner marker\n");
  
  getd("X", &xcorner[1]);
  getd("Y", &ycorner[1]);

  printf("Now use the machine and position it on the upper right corner marker\n");
  
  getd("X", &xcorner[2]);
  getd("Y", &ycorner[2]);

  printf("Now use the machine and position it on the upper left corner marker\n");
  
  getd("X", &xcorner[3]);
  getd("Y", &ycorner[3]);


  // This section can be used as interactive then after 'q'
  // the batch process begins
  while (1)
    {
      
      printf("Now please input the gcode coordinates of the position we like to get the machine coords for\n");

      int status;
      
      status=getd("xin", &xin);
      if (status == 0) break;
      
      status=getd("yin", &yin);
      if (status == 0) break;
      
      square_projection_vec
	(
	 xlow, xhigh, ylow, yhigh,
	 xcorner, ycorner,
	 xin, yin,
	 &xout, &yout
	 );
      
      printf("If you position the machine at X: %f, Y: %f it should stand on the desired gcode object\n",
	     xin, yin);
      printf("G00 X%f Y%f\n", xout, yout);
    }

  // Now process the .nc file for now this is not a very forgiving filter
  char fname_in[4096];
  char fname_out[4096];
  char buf[4096];

  printf("Please enter infile name: ");
  fgets(buf, 4095, stdin);
  sscanf(buf, "%s", fname_in);

  printf("Please enter outfile name: ");
  fgets(buf, 4095, stdin);
  sscanf(buf, "%s", fname_out);

  FILE *fin, *fout;

  fin=fopen(fname_in, "r");
  if (NULL == fin)
    {
      fprintf(stderr, "cant find file: ``%s''\n", fname_in);
      exit(1);
    }
  
  fout=fopen(fname_out, "w");
  if (NULL == fout)
    {
      fprintf(stderr, "cant find file: ``%s''\n", fname_out);
      exit(1);
    }

  char bufout[4096];
  
  while (1)
    {
      fgets(buf, 4095, fin);

      if (!strncmp(buf, "G00 X", 5))
	{
	  convert(buf, bufout);
	  fprintf(fout, "%s", bufout);
	}
      else if (!strncmp(buf, "G01 X", 5))
	{
	  convert(buf, bufout);
	  fprintf(fout, "%s", bufout);
	}
      else
	{
	  // No conv just echo line 
	  fprintf(fout, "%s", buf);
	}

      if (feof(fin)) break;
    }

  fclose(fin);
  fclose(fout);
  
}