#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <gtk/gtk.h>

#include "callbacks.h"
#include "interface.h"
#include "support.h"

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

GtkWidget *theWidget = NULL;
GdkGC *redGC = NULL;
GdkColor red = { 0, 32768, 0, 0}; /* Grey */

GdkGC *greenGC = NULL;
GdkColor green = { 0, 0, 32768, 0}; /* Grey */



int pressed = FALSE;
static GdkPixmap *pixmap = NULL;
int wrappingPointIndex = -1;


typedef struct _line {
    GdkPoint *p1;
    GdkPoint *p2;
    int pulleyId;


} Line;

typedef struct pully {
    GdkPoint center;
    gint flag;
    int id;
    int radius;
    int dir;
    
    GdkPoint wrapStop;
    double wrapStartAngle;
    GdkPoint wrapStart;
    
    GdkPoint unwrapLineP1;
    GdkPoint unwrapLineP2;
    int unwrapFlag;

} Pulley;



GdkPoint nextPoint,lastPoint;
GSList *enteredPulleys = NULL;
GSList *wrappedPulleys = NULL;

//double currentUb;

double gdx,gdy;
int gflag;


//int STARTPULLEY = 4;
Pulley *currentPulley = NULL;

#define PULLEYCOUNT 6
#define PULLEYRADIUS 30


int pointCount = PULLEYCOUNT;


Pulley pulleys[PULLEYCOUNT] = {
    { 50, 50, 0,0, PULLEYRADIUS},
    { 100,100,0,1, PULLEYRADIUS},
    { 200,200,0,2, PULLEYRADIUS},
    { 200,100,0,3, PULLEYRADIUS},
    { 100,200,0,4, PULLEYRADIUS},
    { 300,200,0,5, PULLEYRADIUS}
};

#if 0
void angleTest(void)
{
    double dx,dy,l,psi;

    dx = nextPoint.x - pulleys[STARTPULLEY].center.x;
    dy = nextPoint.y - pulleys[STARTPULLEY].center.y;

    l = hypot(dx,dy);
    psi = atan2(dx,dy);

    dx /= l;
    dy /= l;

//    printf("(%3.3f,%3.3f) %3.3f\n",dx,dy,psi);
}

void angleTest2(double dist)
{
    double dx,dy,l1,l2,psi1,psi2,theta;
    double diff;

    dx = nextPoint.x - pulleys[STARTPULLEY].center.x;
    dy = nextPoint.y - pulleys[STARTPULLEY].center.y;

    l1 = hypot(dx,dy);
    psi1 = atan2(dx,dy);




    dx = pulleys[1].center.x -  pulleys[STARTPULLEY].center.x;
    dy = pulleys[1].center.y -  pulleys[STARTPULLEY].center.y;

    l2 = hypot(dx,dy);
    psi2 = atan2(dx,dy);

    theta = asin(PULLEYRADIUS/l2);


    if(dist > 0.0)
    {
	psi2 += M_PI + theta;
    }
    else
    {
	psi2 += -theta;
    }
//   diff = psi1 - psi2;
    
//    dx = 20.0 * sin(M_PI_2 + psi2 );
//    dy = 20.0 * cos(M_PI_2 + psi2 ); 

    dx = 20.0 * cos(psi2 );
    dy = 20.0 * sin(psi2 ); 
//    printf("l=%3.3f psi=%3.3f theta=%3.3f \n",l1,psi1,theta);

/*
    gflag = TRUE;
    gdx = dx;
    gdy = dy;
*/
}
#endif



double pointToLine(GdkPoint *point, Pulley *pulley)
{
   double denom,A,B,C,Ua,Ub,Uc,dx,dy,angle;
    GdkPoint *P1,*P2,*P3,*P4,p4;

    if(!pulley->unwrapFlag) return;

    P1 = &pulley->unwrapLineP1;
    P2 = &pulley->unwrapLineP2;
  
    P3 = point;
    P4 = &p4;
   
    denom = hypot((P2->x - P1->x),(P2->y - P1->y));
    
    C = ((P2->x - P1->x) * (P1->y - P3->y)) - 
	((P1->x - P3->x) * (P2->y - P1->y));
    
    Uc = C / denom;

    printf("P1(%d,%d) P2(%d,%d) ",P1->x,P1->y,P2->x,P2->y);

//    dx = P2->y - P1->y;
//    dy = -(P2->x - P1->x);;

    dx = -(P2->y - P1->y) * PULLEYRADIUS / denom;
    dy =  (P2->x - P1->x) * PULLEYRADIUS / denom;

    angle = atan2(dx,dy);
    
 
    
    p4.x = dx + P3->x;
    p4.y = dy + P3->y;

    denom = ((P4->y - P3->y) * (P2->x - P1->x)) - 
	((P4->x - P3->x) * (P2->y - P1->y));

    A = ((P4->x - P3->x) * (P1->y - P3->y)) -
	((P4->y - P3->y) * (P1->x - P3->x));

    B = ((P2->x - P1->x) * (P1->y - P3->y)) -
	((P2->y - P1->y) * (P1->x - P3->x));


    Ua = A / denom;
    Ub = B / denom;

    if(pulley->dir) Uc = -Uc;

    if(Ua < 0.0) Uc = 0.0;
    printf("*%2d Ua=%3.3f Ub=%3.3f Uc=%3.3f\n",pulley->id,Ua,Ub,Uc);
//    printf("Ua=%1.3f Ub=%1.3f Uc=%1.3f\n",Ua,Ub,Uc);

    return(Uc);

}


double pulleyToLine(Pulley *pulley, Line *line)
{
    double denom,A,B,C,Ua,Ub,Uc,dx,dy,angle;
    GdkPoint *P1,*P2,*P3,*P4,p4;

    P1 = line->p1;
    P2 = line->p2;
  
    P3 = &pulley->center;
    P4 = &p4;
   
    denom = hypot((P2->x - P1->x),(P2->y - P1->y));
    
    C = ((P2->x - P1->x) * (P1->y - P3->y)) - 
	((P1->x - P3->x) * (P2->y - P1->y));
    
    Uc = C / denom;


    printf("P1(%d,%d) P2(%d,%d) ",P1->x,P1->y,P2->x,P2->y);

//    dx = P2->y - P1->y;
//    dy = -(P2->x - P1->x);;

    dx = -(P2->y - P1->y) * PULLEYRADIUS / denom;
    dy =  (P2->x - P1->x) * PULLEYRADIUS / denom;

    angle = atan2(dx,dy);
    
 
    
    p4.x = dx + P3->x;
    p4.y = dy + P3->y;

    denom = ((P4->y - P3->y) * (P2->x - P1->x)) - 
	((P4->x - P3->x) * (P2->y - P1->y));

    A = ((P4->x - P3->x) * (P1->y - P3->y)) -
	((P4->y - P3->y) * (P1->x - P3->x));

    B = ((P2->x - P1->x) * (P1->y - P3->y)) -
	((P2->y - P1->y) * (P1->x - P3->x));


    Ua = A / denom;
    Ub = B / denom;

   
    printf(" %2d Ua=%3.3f Ub=%3.3f Uc=%3.3f\n",pulley->id,Ua,Ub,Uc);


#if 0   
    gdk_draw_line (pixmap,
		   greenGC,
		   P1->x,P1->y,
		   P3->x,P3->y);
#endif
#if 1
    gdk_draw_line (pixmap,
		   theWidget->style->black_gc,
		   pulley->center.x,pulley->center.y,
		   dx + pulley->center.x, 
		   dy + pulley->center.y);
#endif
 

    if((fabs(Uc) <= PULLEYRADIUS) && (Ua < 1.0) && (Ua > 0.0)) 
    {
	if(pulley->flag == FALSE)
	{	    
	    printf("-%2d Ua=%3.3f Ub=%3.3f Uc=%3.3f angle=%1.3f\n",pulley->id,Ua,Ub,Uc,angle);

	    pulley->flag = TRUE; 
	  
	    if(Uc > 0.0)
	    {
		pulley->dir = TRUE;
	    }
	    else
	    {
		angle += M_PI;
		if(angle > (2.0 * M_PI)) angle -= (2.0 * M_PI);
		pulley->dir = FALSE;
		dx = -dx;
		dy = -dy;
	    }
	    enteredPulleys = g_slist_prepend(enteredPulleys,(gpointer) pulley);

	    if(angle > M_PI) angle -= 2.0 * M_PI;
	    if(angle < -M_PI) angle += 2.0 * M_PI;


	    pulley->wrapStartAngle = angle;
	    pulley->wrapStart.x = dx + pulley->center.x;
	    pulley->wrapStart.y = dy + pulley->center.y;
	    pulley->unwrapLineP1 = *line->p1;
	    pulley->unwrapLineP2 = *line->p2;
	    pulley->unwrapFlag = TRUE;

	}



//	angleTest2(Uc);
    }
    else
    {
	pulley->flag = FALSE;
    }

    return(Uc);


}

void drawFreeEnd(void)
{
    GdkPoint *P1,*P2,*P3,p3;
    double dx,dy,denom,angle,rdx,rdy,f;
loop:
    P3 = &p3;
//    P1 = &pulleys[STARTPULLEY].center;
    P1 = &currentPulley->center;
    P2 = &nextPoint;


//    printf("drawFreeEnd %d\n",pulleys[STARTPULLEY].id);


    denom = hypot((P2->x - P1->x),(P2->y - P1->y));
    
    f = PULLEYRADIUS / denom;

    if(f <= 1.0)
    {
	angle = asin(PULLEYRADIUS / denom);
    }
    else
    {
	angle = 0.0;
    }
    

//    if(pulleys[STARTPULLEY].dir)
    if(currentPulley->dir)
    {
	dx = -(P2->y - P1->y) * PULLEYRADIUS / denom;
	dy =  (P2->x - P1->x) * PULLEYRADIUS / denom;
    }
    else
    {
	dx =  (P2->y - P1->y) * PULLEYRADIUS / denom;
	dy = -(P2->x - P1->x) * PULLEYRADIUS / denom;
	angle = -angle; 
    }

    rdx = ( cos(angle) * dx) + (sin(angle) * dy);
    rdy = (-sin(angle) * dx) + (cos(angle) * dy);

    p3.x = rdx + P1->x;
    p3.y = rdy + P1->y;
/*
    p3.x += (PULLEYRADIUS * sin(angle));
    p3.y += (PULLEYRADIUS * cos(angle));
*/

//    P1 = &pulleys[STARTPULLEY].center;
    P1 = &currentPulley->center;
    gdk_draw_line (pixmap,
		   redGC, //theWidget->style->black_gc,
		   P2->x,P2->y,
		   p3.x,p3.y);
#if 1    
//    if(!pulleys[STARTPULLEY].dir) angle += M_PI ;
//    angle += atan2((P2->x - P1->x),(P2->y - P1->y));

    angle += atan2(dx,dy);

    if(angle > M_PI) angle -= 2.0 * M_PI;
    if(angle < -M_PI) angle += 2.0 * M_PI;


    dx = 35.0 * sin(angle);
    dy = 35.0 * cos(angle);
    gdk_draw_line (pixmap,
		   greenGC, //theWidget->style->black_gc,
		   P1->x + dx,P1->y + dy,
		   P1->x,P1->y);

//    dx = 35.0 * sin(pulleys[STARTPULLEY].wrapStartAngle);
//    dy = 35.0 * cos(pulleys[STARTPULLEY].wrapStartAngle);

    dx = 35.0 * sin(currentPulley->wrapStartAngle);
    dy = 35.0 * cos(currentPulley->wrapStartAngle);
  
    gdk_draw_line (pixmap,
		   greenGC, //theWidget->style->black_gc,
		   P1->x + dx,P1->y + dy,
		   P1->x,P1->y);

   
 



    f = angle - currentPulley->wrapStartAngle;
    if(f > M_PI) f -= 2.0 * M_PI;
    if(f < -M_PI) f += 2.0 * M_PI;

    currentPulley->wrapStop = p3;

//    printf("%1.3f %1.3f %1.5f\n",angle,currentPulley->wrapStartAngle,f);

#if 1
    if( (pointToLine(&nextPoint,currentPulley) > 0.001) && wrappedPulleys)
    {
	
	printf("UNWRAP1 \007 \n");

	if(g_slist_length(wrappedPulleys) > 1)
	{
		wrappedPulleys = g_slist_remove_link(wrappedPulleys,wrappedPulleys);	
		currentPulley = (Pulley *)wrappedPulleys->data;
	}
    }
#endif


#if 0
    if(fabs(f) < M_PI_4)
    {
	if(currentPulley->dir)
	{
	    if(f < -0.01)
	    {
		printf("UNWRAP1 \007 \n");

		wrappedPulleys = g_slist_remove_link(wrappedPulleys,wrappedPulleys);	
		STARTPULLEY = ((Pulley *)wrappedPulleys->data)->id;
//		goto loop;
	    }
	}
	else
	{
	    if(f > 0.01)
	    {
		printf("UNWRAP2 \007 \n");	
	
		wrappedPulleys = g_slist_remove_link(wrappedPulleys,wrappedPulleys);
		STARTPULLEY = ((Pulley *)wrappedPulleys->data)->id;
//		goto loop;
	    }
	}

    }
#endif
    




#endif
    


   
}


/* Draw a rectangle on the screen */
static int
draw_string (GtkWidget *widget,
	     gdouble    x,
	     gdouble    y)
{
    int count,n,flag;
    GSList *start,*end;
    GdkPoint *p1,*p2;
    int done;
    Pulley *pp;
    GSList *prevPulley,*nextPulley;

    done = FALSE;

   
    /* Draw the fixed pulleys as circles */
    for(n=0;n<PULLEYCOUNT;n++)
    {
	pp = &pulleys[n];
	p1 = &pp->center;


	gdk_draw_arc(pixmap,
		     // widget->style->black_gc,
		     //pulleys[n].flag,
		     pulleys[n].flag ? redGC : greenGC,
		     FALSE,
		     p1->x - pp->radius,
		     p1->y - pp->radius,
		     (2 * pp->radius)-1,
		     (2 * pp->radius)-1,
		     0,360*64);
		     
    }

/*
    if(gflag)
    {
	gdk_draw_point(pixmap,
		       widget->style->black_gc,
		       pulleys[1].center.x + gdx,pulleys[1].center.y + gdy);
    }
*/
    prevPulley = NULL;
    for(nextPulley = wrappedPulleys; 
	nextPulley; 
	nextPulley = g_slist_next(nextPulley) )
    {
	
	if(prevPulley)
	{
	    p1 = &((Pulley *)prevPulley->data)->wrapStart;
	    p2 = &((Pulley *)nextPulley->data)->wrapStop;

	    gdk_draw_line (pixmap,
			   greenGC, //theWidget->style->black_gc,
			   p1->x,p1->y,
			   p2->x,p2->y);
	}


	prevPulley = nextPulley;
    }
  

 






   /* Now invalidate the affected region of the drawing area. */
  
   gdk_window_invalidate_rect (widget->window,
				&widget->allocation,
				FALSE);
  
 

    return FALSE;

}

gboolean on_drawingarea1_button_press_event     (GtkWidget       *widget,
                                        GdkEventButton  *event,
                                        gpointer         user_data)
{
    gdouble x,y;
   
    if(!gdk_event_get_coords(event,&x,&y))
    {
	printf("on_drawingarea1_button_press_event no corrds !\n");
    }
    else
    {
	printf("on_drawingarea1_button_press_event\n");
	pressed = TRUE;	
	
	nextPoint.x = x;
	nextPoint.y = y;
    }

    return FALSE;
}


gboolean on_drawingarea1_button_release_event   (GtkWidget       *widget,
                                        GdkEventButton  *event,
                                        gpointer         user_data)
{
    printf("on_drawingarea1_button_release_event\n");
    pressed = FALSE;
  return FALSE;
}


gboolean on_drawingarea1_motion_notify_event    (GtkWidget       *widget,
						 GdkEventMotion  *event,
						 gpointer         user_data)
{
    int x, y,nn;
    GdkModifierType state;
    static int n = 0;
    static int skip = FALSE;
//    Line *newTrackLine,*theLine;
    Pulley *newPulley;
    GdkPoint *point;
    Line freeLine;
    double dist;

//    printf("on_drawingarea1_motion_notify_event %d\n",pressed);


   

    if(pressed)
    {

  

	lastPoint.x = nextPoint.x;
	lastPoint.y = nextPoint.y;

	gdk_window_get_pointer (event->window, &x, &y, &state);
    	nextPoint.x = x;
	nextPoint.y = y;

    loop:

	freeLine.p1 = &currentPulley->wrapStop;
	freeLine.p2 = &nextPoint;

	gdk_draw_rectangle (pixmap,
			widget->style->white_gc,
			TRUE,
			0, 0,
			widget->allocation.width,
			widget->allocation.height);
	

	/* Draw current free end segment */
	drawFreeEnd();

	g_slist_free(enteredPulleys);
	enteredPulleys = NULL;

	for(n=0;n<PULLEYCOUNT;n++)
	{
//	    if(n == STARTPULLEY) continue;
	    if(n == currentPulley->id) continue;
	    pulleys[n].flag = FALSE;
	    dist = pulleyToLine(&pulleys[n],&freeLine);
//	    printf("dist = %3.3f\n",dist);
	}
	
	if(enteredPulleys != NULL)
	{
/*
	    if(wrappedPulleys == NULL)
	    { 
		newPulley = (Pulley *) malloc(sizeof(Pulley));
		*newPulley = pulleys[STARTPULLEY];
		
		wrappedPulleys = g_slist_prepend(wrappedPulleys,
						 (gpointer) newPulley);
	    }
*/

//	    ((Pulley *)wrappedPulleys->data)->wrapStop =
//		currentPulley->wrapStop;
		
	    

//	    STARTPULLEY = ((Pulley *)enteredPulleys->data)->id;

	    newPulley = (Pulley *) malloc(sizeof(Pulley));
	    *newPulley = *((Pulley *)enteredPulleys->data);

	    wrappedPulleys = g_slist_prepend(wrappedPulleys,
					     (gpointer) newPulley);
	
	    currentPulley = newPulley;
    
	    goto loop;

	}


	draw_string(widget, x, y);

    }

  return FALSE;
}


void
on_button1_clicked                     (GtkButton       *button,
                                        gpointer         user_data)
{
    gtk_main_quit();
}



gboolean on_drawingarea1_configure_event        (GtkWidget       *widget,
						 GdkEventConfigure *event,
						 gpointer         user_data)
{
    GdkColormap* cm;
    theWidget = widget;
    redGC = gdk_gc_new(widget->window);
    greenGC = gdk_gc_new(widget->window);
    
    cm = gdk_colormap_get_system();
    gdk_colormap_alloc_color(cm,&red,TRUE,TRUE);
    gdk_colormap_alloc_color(cm,&green,TRUE,TRUE);
    
    gdk_gc_set_foreground(redGC,&red);
    gdk_gc_set_foreground(greenGC,&green);
    gdk_gc_set_line_attributes(greenGC,3,GDK_LINE_SOLID,GDK_CAP_NOT_LAST,
			       GDK_JOIN_MITER);


    if (pixmap)
	g_object_unref (pixmap);

    pixmap = gdk_pixmap_new (widget->window,
			   widget->allocation.width,
			   widget->allocation.height,
			   -1);
    
    /* Initialize the pixmap to white */
    gdk_draw_rectangle (pixmap,
			widget->style->white_gc,
			TRUE,
			0, 0,
			widget->allocation.width,
			widget->allocation.height);



    currentPulley = &pulleys[4];
       wrappedPulleys = g_slist_prepend(wrappedPulleys,
					     (gpointer) currentPulley);


    /* Initialise the threading data */

    //   if(trackLines == NULL) 
//	trackLines = g_slist_append(trackLines,(gpointer) &points[STARTPOINT]);


    //   if(lineList == NULL) 
//	MakeLineList(&points[STARTPOINT],TRUE);




    return FALSE;
}


gboolean
on_drawingarea1_expose_event           (GtkWidget       *widget,
                                        GdkEventExpose  *event,
                                        gpointer         user_data)
{
 gdk_draw_drawable (widget->window,
		     widget->style->fg_gc[GTK_WIDGET_STATE (widget)],
		     pixmap,
		     /* Only copy the area that was exposed. */
		     event->area.x, event->area.y,
		     event->area.x, event->area.y,
		     event->area.width, event->area.height);
  return FALSE;
}