#include <float.h>
#ifdef DOUBLE
#define MAX_FLOAT DBL_MAX
#else
#define MAX_FLOAT FLT_MAX
#endif

#define TRUE 1
#define FLASE 0
#define MIN(x,y)  ((x) < (y) ? (x) : (y))
#define MAX(x,y)  ((x) > (y) ? (x) : (y))

FLOAT calculate_alpha( pt1, pt2, pt3, edge1, edge2, edge3, newpoint )
point pt1,pt2,pt3;
struct edge edge1, edge2, edge3;
point newpoint;
{
    FLOAT k,p,l,q, term1,term2,len_edge2;
    FLOAT noncons2 = nonconstraint_edge_cost * nonconstraint_edge_cost;
    FLOAT alpha[2];
    FLOAT len_p1_to_e2;
    FLOAT len_along_e2;
    FLOAT cost_edge2;


    printf("\t\tp1=(%.3f,%.3f)\tp2=(%.3f,%.3f)\tp3=(%.3f,%.3f)\n",
                            pt1[0],pt1[1], pt2[0],pt2[1], pt3[0],pt3[1] );

    if (mark(edge1)!=FREEEDGE) { /* direct is constrained, so cheapest direct */
        return( MAX_FLOAT );
    }
    if (mark(edge2)==FREEEDGE) { /* opposite is also not constrained, so     */
        return( MAX_FLOAT );     /* cheaper to go direct                     */
    }

    k = pt2[0] - pt3[0];
    l = pt2[1] - pt3[1];
    p = pt1[0] - pt3[0];
    q = pt1[1] - pt3[1];

    len_edge2 = (k*k + l*l);

    cost_edge2 = (costassigned(edge2)) ?  /* this is a constraint edge       */
        cost(edge2) / sqrt(len_edge2) :
        cost(edge2);

    term1 = (k*p + l*q) / len_edge2;
    term2 = len_edge2 * sqrt( noncons2 - cost_edge2*cost_edge2 );
    term2 = (cost_edge2*(p*l - k*q)) / term2;

    alpha[0] = term1 + term2;
    alpha[1] = term1 - term2;

    printf("\t\ta1=%.3f\ta2=%.3f\n", alpha[0], alpha[1] );


    newpoint[3] = 1;

    /* if both alphas are 0<=alpha<=1, then choose closest to p2: the       */
    /* larger value                                                         */
    if ( (alpha[0] <= 1.0) && (alpha[0] >= 0.0) &&
         (alpha[1] <= 1.0) && (alpha[1] >= 0.0) ) {
        newpoint[0] = MAX(alpha[0],alpha[1]) * k + pt3[0];
        newpoint[1] = MAX(alpha[0],alpha[1]) * l + pt3[1];
    }
    /* if only1 alpha0 is 0<=alpha<=1, then choose it. */
    else if ( (alpha[0] <= 1.0) && (alpha[0] >= 0.0) ) {
        newpoint[0] = alpha[0] * k + pt3[0];
        newpoint[1] = alpha[0] * l + pt3[1];
    }
    /* if only1 alpha1 is 0<=alpha<=1, then choose it. */
    else if ( (alpha[1] <= 1.0) && (alpha[1] >= 0.0) ) {
        newpoint[0] = alpha[1] * k + pt3[0];
        newpoint[1] = alpha[1] * l + pt3[1];
    } else { /* otherwise go direct is cheapest */
        newpoint[0] = pt2[0]; 
        newpoint[1] = pt2[1];
	newpoint[3] = 0;
    }


    printf("\t\tnewx=%.3f\tnewy=%.3f\n", newpoint[0], newpoint[1] );


    len_p1_to_e2 = sqrt( (pt1[0]-newpoint[0])*(pt1[0]-newpoint[0]) +
                         (pt1[1]-newpoint[1])*(pt1[1]-newpoint[1]) );
    len_along_e2 = sqrt( (pt2[0]-newpoint[0])*(pt2[0]-newpoint[0]) +
                         (pt2[1]-newpoint[1])*(pt2[1]-newpoint[1]) );

    return( nonconstraint_edge_cost * len_p1_to_e2 +
            cost_edge2 * len_along_e2 );
}

void assignedgecosts()
{
  struct edge edge[3];
  point pt[3], newpt1, newpt2, minpt, directpt;
  FLOAT len_edge;
  FLOAT cost_direct, cost_indirect1, cost_indirect2, temp;
  int i;

  newpt1 = pointalloc();
  newpt2 = pointalloc();

  printf("Assigning costs\n");

  quadpathinit();
  edge[0].orient = 0;
  edge[0].quad = followquadpath();
  while (edge[0].quad != (quadedge *) NULL) {
    org(edge[0], pt[0]);
    dest(edge[0], pt[1]);
    if ((pt[0] < pt[1])) {
      sym(edge[0], edge[0]);
      pt[0] = pt[1];
    }
    if (pt[0] != (point) NULL) {
      lnext(edge[0], edge[1]);
      org(edge[1], pt[1]);
      lnext(edge[1], edge[2]);
      org(edge[2], pt[2]);
      if ((pt[0] > pt[1]) && (pt[0] > pt[2])) {
        /* calculate edge cost */

        printf("New triangle\n");

        for (i=0; i<3; i++) { /* for all edges */
            newpt1[3] = 0;
            newpt2[3] = 0;

            printf( "(%.3f,%.3f) to ", pt[i][0], pt[i][1] );
            printf( "(%.3f,%.3f)\n", pt[(i+1)%3][0], pt[(i+1)%3][1]);

            len_edge = sqrt(
                        (pt[(i+1)%3][0]-pt[i][0])*(pt[(i+1)%3][0]-pt[i][0]) +
                        (pt[(i+1)%3][1]-pt[i][1])*(pt[(i+1)%3][1]-pt[i][1]) );

            /* direct is constrained, so cheapest direct */
            if (mark(edge[i])==FREEEDGE) {
                printf("\t\te1:%d e2:%d e3:%d\n",
                        mark(edge[i])==FREEEDGE,
                        mark(edge[(i+1)%3])==FREEEDGE,
                        mark(edge[(i+2)%3])==FREEEDGE);
                cost_indirect1 = calculate_alpha(
                                    pt[(i+1)%3], pt[i], pt[(i+2)%3],
                                    edge[i], edge[(i+2)%3], edge[(i+1)%3],
                                    newpt1 );
                if (newpt1[3]) {
                    printf( "\tvia (%.3f,%.3f)", newpt1[0], newpt1[1] );
                    printf( "\tCost: %.3f", cost_indirect1 );
                    printf( "\n");
                }
                cost_indirect2 = calculate_alpha(
                                    pt[i], pt[(i+1)%3], pt[(i+2)%3],
                                    edge[i], edge[(i+1)%3], edge[(i+2)%3],
                                    newpt2 );
                if (newpt2[3]) {
                    printf( "\tvia (%.3f,%.3f)", newpt2[0], newpt2[1] );
                    printf( "\tCost: %.3f", cost_indirect2 );
                    printf( "\n");
                }
            } else {
                cost_indirect1 = cost_indirect2 = MAX_FLOAT;
            }

            if (cost_indirect1 < cost_indirect2) {
                temp = cost_indirect1;
                minpt = newpt1;
            } else {
                temp = cost_indirect2;
                minpt = newpt2;
            }

	    directpt = NULL;
            if (! costassigned(edge[i])) {
                cost_direct = (mark(edge[i]) == FREEEDGE) ?
                    (len_edge * nonconstraint_edge_cost) :
                    (len_edge * cost(edge[i]));
                printf( "\tDirect:\tCost: %.3f", cost_direct );
                printf( "\n");
            } else {
                cost_direct = cost(edge[i]); /* really min of other triangle */
                directpt = joinpt(edge[i]);

                if (directpt) {
                    printf( "\tvia (%.3f,%.3f)", directpt[0], directpt[1] );
                } else {
                    printf( "\tdirect         ");
                }
                printf( "\tCost: %.3f", cost_direct );
                printf( "\n");
            }

            if (cost_direct < temp) {
                temp = cost_direct;
		minpt = directpt;
            }

            if (minpt && minpt[3]) {
                printf( "\tChoosing via (%.3f,%.3f)", minpt[0], minpt[1] );
                printf( "\tCost: %.3f", temp );
            } else {
                printf( "\tChoosing direct root ");
            }
            printf( "\n\n");

            setcost( edge[i], temp);
	    setjoinpt( edge[i], minpt);
            setcostassigned( edge[i], TRUE );

/*
            printf("\t Cost Direct   = %.3f\n", cost_direct );
            printf("\t Cost InDirect1 = %.3f\n", cost_indirect1 );
            printf("\t Cost InDirect2 = %.3f\n", cost_indirect2 );
            printf("\n");
*/
        }

/*
        printf("\tAfter set:");
        printf("\t%.3f", cost(edge[0]) );
        printf("\t%.3f", cost(edge[1]) );
        printf("\t%.3f", cost(edge[2]) );
        printf("\n\n");
*/
      }
    }
    edge[0].quad = followquadpath();
  }
  pointdealloc(newpt1);
  pointdealloc(newpt2);
}