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

#define errexit(s,i) (errno = EINVAL, perror(s), exit(i))

int main(int argc, char **argv)
{
    FILE *fpo = NULL;
    double x0, y0, x1, y1;
    double zsafe, zstock, z1, deltaz, last_deltaz;
    double xyfeed, zfeed, mill_dia, overlap, overlappc;
    char c;
    int nbpasses;
    double zstep;
    double x, y, z;
    double speed_height = 1;
    int xfirst;
    int ntrav;
    double l, dl;
    double rmill, adv;
    double xd, xa, yd, ya;
    int n, nz;
    char str[16];
    int ntrav_even;

    if(argc >1){
	if(!(fpo=fopen(argv[1], "w"))) errexit("Cant open output", 1);
    } else fpo=stdout;

    printf("Enter x0 y0: "); scanf("%lf %lf", &x0, &y0);
    printf("Enter x1 y1: "); scanf("%lf %lf", &x1, &y1);
    printf("Enter zsafe zstock z1: "); scanf("%lf %lf %lf", &zsafe, &zstock, &z1);
    printf("Enter deltaZ XYfeed Zfeed: "); scanf("%lf %lf %lf", &deltaz, &xyfeed, &zfeed);
    printf("Enter mill_diameter overlap%: "); scanf("%lf %lf", &mill_dia, &overlappc);

    printf("Doing pocket [%.3f, %.3f] [%.3f, %.3f]\n", x0, y0, x1, y1);
    printf("             zsafe:%.3f zstock:%.3f z1:%.3f\n", zsafe, zstock, z1);
    printf("             deltaz:%.3f XYfeed:%.3f Zfeed:%.3f\n", deltaz, xyfeed, zfeed);
    printf("             mill_diameter:%.3f overlap%:%.3f\n", mill_dia, overlappc);

    printf("OK (y/n)? "); scanf("%s", &str);
    if((*str != 'y') && (*str != 'Y'))exit(1); 

    if((x0 >= (x1- mill_dia)))errexit("x0 < x1-mill_diameter", 1);
    if((y0 >= (y1-mill_dia)))errexit("y0 < y1-mill_diameter", 1);
    if((zsafe < (zstock+speed_height)))errexit("zsafe < zstock+1", 1);
    if((zstock <= z1))errexit("zstock < z1", 1);

    // Header
    fprintf(fpo, "(Pocketing, generated automatically by pocket.c)\n");
    fprintf(fpo, "([%.3f, %.3f] [%.3f, %.3f])\n", x0, y0, x1, y1);
    fprintf(fpo, "(zsafe:%.3f zstock:%.3f z1:%.3f)\n", zsafe, zstock, z1);
    fprintf(fpo, "(deltaz:%.3f last_deltaz:%.3f)\n", deltaz, last_deltaz);
    fprintf(fpo, "(XYfeed:%.3f Zfeed:%.3f)\n", xyfeed, zfeed);
    fprintf(fpo, "(mill_diameter:%.3f overlap%:%.3f)\n", mill_dia, overlappc);

    // Nb passes
    z = (zstock - z1) / deltaz;
    if(((z - floor(z)) / z) >= 0.01) nbpasses = ceil(z);
    else nbpasses = floor(z);
    zstep = (zstock - z1) / nbpasses;
    fprintf(fpo, "(%d passes of %3g mm)\n", nbpasses, zstep);
    overlap = overlappc / 100;

    if((x1-x0) >= (y1-y0)) xfirst=1;
    else xfirst=0;

    if(xfirst) l = y1 - y0;
    else l = x1 - x0;

    // Nb traverses
    adv = mill_dia * (1 - overlap); // advance
    x = (l - mill_dia) / adv;
    if(((x-floor(x))/x) > 0.01) ntrav = ceil(x) + 1;
    else ntrav = floor(x) + 1;
    dl = (l - mill_dia) / (ntrav -1);
    rmill = mill_dia / 2;
    ntrav_even = ! (ntrav & 1);

    fprintf(fpo, "(ntrav:%d adv:%.3f mm dl:%.3f mm)\n", ntrav, adv, dl);
    fprintf(fpo,"\n");

    fprintf(fpo, "G17 G21 G40 G49 G54 G80 G90 G94\n");
    fprintf(fpo, "G0 Z%.3f\n", zsafe);

    if(xfirst){
	for(z = zstock-zstep, nz = 0; nz < nbpasses; nz++, z -= zstep){
	    for(n = 0; n < ntrav; n++) {
		if(! (n % 2)) { // goto the right
		    if(ntrav_even) {
			xd = x0 + rmill + adv * (ntrav/2 - n/2 - 1);
			xa = x1 - rmill - adv * (ntrav/2 - n/2 - 1);
			yd = y1 - rmill - dl * (ntrav/2 - n/2 - 1);
			ya = y0 + rmill + dl * (ntrav/2 - n/2 - 1);
		    }
		    else {
			xd = x0 + rmill + adv * (ntrav/2 - (n - 1)/2);
			xa = x1 - rmill - adv * (ntrav/2 - n/2 - 1);
			yd = y1 - rmill - dl * (ntrav/2 - n/2);
			ya = y0 + rmill + dl * (ntrav/2 - n/2 - 1);
		    }
		    if(n == (ntrav - 1)){
			ya = y0 + rmill;
			xa = x1 - rmill;
		    }
		} else { // goto the left
		    if(ntrav_even) {
			xd = x1 - rmill - adv * (ntrav/2 - (n + 1)/2);
			xa = x0 + rmill + adv * (ntrav/2 - (n + 1)/2 - 1);
			yd = y0 + rmill + dl * (ntrav/2 - (n + 1)/2);
			ya = y1 - rmill - dl * (ntrav/2 - (n + 1)/2 - 1);
		    }
		    else {
			xd = x1 - rmill - adv * (ntrav/2 - (n + 1)/2 - 1);
			xa = x0 + rmill + adv * (ntrav/2 - n/2 - 1);
			yd = y0 + rmill + dl * (ntrav/2 - (n + 1)/2 - 1);
			ya = y1 - rmill - dl * (ntrav/2 - n/2 - 1);
		    }
		    if(n == (ntrav - 1)){
			ya = y1 - rmill;
			xa = x0 + rmill;
		    }
		}
		if(!n){
		    if(! nz) {
			fprintf(fpo, "G0 X%.3f Y%.3f\n", xd, yd);
			fprintf(fpo, "G0 Z%.3f\n", zstock + speed_height);
		    }
		    else {
			fprintf(fpo, "G0 X%.3f Y%.3f\n", xd, yd);
		    }
		    fprintf(fpo, "G1 Z%.3f F%.3f\n", z, zfeed);
		    fprintf(fpo, "G4 P0.3\n", z, zfeed); // Dwell
		}
		fprintf(fpo, "G1 X%.3f F%.3f\n", xa, xyfeed);
		fprintf(fpo, "G1 Y%.3f\n", ya);
	    }
	}
    }
    else errexit("Y > X not implemented yet!", 1);
    fprintf(fpo, "G0 Z%.3f\n", zsafe);
    fprintf(fpo, "M2\n");
    fclose(fpo);
    return(0);
}