/*
 * Copyright (c) 2012 Hypertriton, Inc. <http://hypertriton.com/>
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <agar/core.h>
#include <agar/gui.h>

#include "mu.h"
#include "mu_placement.h"
#include "mu_gui.h"

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

int muInitedSubsystem = 0;

const char *muConstraintNames[] = {
	N_("Distance"),
	N_("Incidence"),
	N_("Angle"),
	N_("Perpendicular"),
	N_("Parallel"),
	N_("Tangent"),
};

void
MU_InitSubsystem(void)
{
	if (muInitedSubsystem++ > 0)
		return (0);

	AG_RegisterNamespace("Agar-MU", "MU_", "http://libagar.org/");

	/* Register our base Agar object classes. */
	AG_RegisterClass(&muClass);

	/* Initialize GUI facilities for Edit() */
	MU_InitGUI();
}

void
MU_DestroySubsystem(void)
{
	if (--muInitedSubsystem > 0) {
		return;
	}
	MU_DestroyGUI();
	AG_UnregisterNamespace("Agar-MU");
}

/* Create a new piece. */
MU *
MU_New(void *parent, const char *name)
{
	char nameGen[AG_OBJECT_NAME_MAX];
	MU *mu;
	
	if (name == NULL) {
		AG_ObjectGenName(parent, &muClass, nameGen, sizeof(nameGen));
	} else {
		if (parent != NULL &&
		    AG_ObjectFindChild(parent, name) != NULL) {
			AG_SetError(_("%s: Existing child object %s"),
			    OBJECT(parent)->name, name);
			return (NULL);
		}
	}
	mu = Malloc(sizeof(MU));
	AG_ObjectInit(mu, &muClass);
	AG_ObjectSetNameS(mu, (name != NULL) ? name : nameGen);
	AG_ObjectAttach(parent, mu);
	return (mu);
}

static void
Init(void *obj)
{
	MU *mu = obj;
	
	OBJECT(mu)->flags |= AG_OBJECT_REOPEN_ONLOAD;

	mu->flags = 0;
	mu->status = MU_WELL_CONSTRAINED;
	Strlcpy(mu->statusText, _("New piece"), sizeof(mu->statusText));
	mu->nSolutions = 0;
	MU_InitCluster(&mu->ctGraph, 0);

	TAILQ_INIT(&mu->parts);
	TAILQ_INIT(&mu->clusters);
	TAILQ_INIT(&mu->insns);
}

/* Create a new part */
MU_Part *
MU_PartNew(MU *mu, const char *name, const char *nameFull)
{
	MU_Part *part;

	part = Malloc(sizeof(MU_Part));
	part->mu = mu;
	Strlcpy(part->name, name, sizeof(part->name));
	Strlcpy(part->nameFull, nameFull, sizeof(part->nameFull));
	AG_TextInit(&part->descr, 500);
	TAILQ_INIT(&part->scores);
	return (part);
}

/* Create a new staff under a part */
MU_Score *
MU_ScoreNew(MU_Part *part, enum mu_clef clef)
{
	MU_Part *part;

	part = Malloc(sizeof(MU_Part));
	Strlcpy(part->name, name, sizeof(part->name));
	AG_TextInit(&part->longName, 100);
	AG_TextInit(&part->descr, 500);
	TAILQ_INIT(&part->scores);
	return (part);
}

MU_Note *
MU_NoteNew(MU *mu)
{
	MU_Note *note;
	Uint32 noteID = 1;

	while (MU_FindNote(mu, noteID, type) != NULL) {
		if (++noteID >= MU_NOTE_ID_MAX)
			AG_FatalError("Out of note IDs");
	}
	note = Malloc(sizeof(MU_Note));
	MU_NoteInit(note, &muNoteOps, noteID, 0);
	MU_NoteAttach(pNote, node);
	return (node);
}

void
MU_NoteInit(void *np, const void *ops, Uint32 noteID, Uint flags)
{
	MU_Note *n = np;

	n->ops = (const MU_NoteOps *)ops;
	n->id = noteID;
	AG_Snprintf(n->name, sizeof(n->name), "%s%u", n->ops->name, noteID);

	n->flags = flags;
	n->sk = NULL;
	n->pNote = NULL;
	n->cons = Malloc(sizeof(MU_Constraint *));
	n->nCons = 0;
	M_MatIdentity44v(&n->T);
	TAILQ_INIT(&n->cnodes);
}

/* Change the name string associated with a node. */
void
MU_NoteSetName(void *p, const char *fmt, ...)
{
	MU_Note *node = p;
	va_list ap;

	va_start(ap, fmt);
	AG_Vsnprintf(node->name, sizeof(node->name), fmt, ap);
	va_end(ap);
}

/* Free a node and detach/free any child nodes. */
static void
MU_FreeNote(SK *sk, MU_Note *node)
{
	MU_Note *cnode, *cnodeNext;

	for (cnode = TAILQ_FIRST(&node->cnodes);
	     cnode != TAILQ_END(&node->cnodes);
	     cnode = cnodeNext) {
		cnodeNext = TAILQ_NEXT(cnode, sknodes);
		TAILQ_REMOVE(&sk->nodes, cnode, nodes);
		MU_FreeNote(sk, cnode);
	}
	if (node->ops->destroy != NULL) {
		node->ops->destroy(node);
	}
	Free(node->cons);
	Free(node);
}

static void
Reset(void *obj)
{
	SK *sk = obj;

	if (sk->root != NULL) {
		MU_FreeNote(sk, sk->root);
		sk->root = NULL;
	}
	TAILQ_INIT(&sk->nodes);
	MU_InitRoot(sk);

	MU_FreeCluster(&sk->ctGraph);
	MU_FreeClusters(sk);
	MU_FreeInsns(sk);
}

static int
MU_NoteSaveData(SK *sk, MU_Note *node, AG_DataSource *buf)
{
	MU_Note *chldNote;

	TAILQ_FOREACH(chldNote, &node->cnodes, sknodes) {
		if (MU_NoteSaveData(sk, chldNote, buf) == -1)
			return (-1);
	}
	if (node->ops->save != NULL &&
	    node->ops->save(sk, node, buf) == -1) {
		AG_SetError("NoteSave: %s", AG_GetError());
		return (-1);
	}
	return (0);
}

static int
MU_SaveNoteGeneric(SK *sk, MU_Note *node, AG_DataSource *buf)
{
	MU_Note *cnode;
	Uint32 ncnodes;
	off_t bsize_offs, ncnodes_offs;

	/* Save generic node information. */
	AG_WriteString(buf, node->ops->name);

	bsize_offs = AG_Tell(buf);
	AG_Seek(buf, sizeof(Uint32), AG_SEEK_CUR);

	AG_WriteUint32(buf, node->handle);
	AG_WriteString(buf, node->name);
	AG_WriteUint16(buf, (Uint16)node->flags);
	M_WriteMatrix44(buf, &node->T);

	/* Save the child nodes recursively. */
	ncnodes_offs = AG_Tell(buf);
	ncnodes = 0;
	AG_Seek(buf, sizeof(Uint32), AG_SEEK_CUR);
	TAILQ_FOREACH(cnode, &node->cnodes, sknodes) {
		if (MU_SaveNoteGeneric(sk, cnode, buf) == -1) {
			return (-1);
		}
		ncnodes++;
	}
	AG_WriteUint32At(buf, ncnodes, ncnodes_offs);

	/* Save the total block size to allow the loader to skip. */
	AG_WriteUint32At(buf, AG_Tell(buf)-bsize_offs, bsize_offs);
	return (0);
}

static int
Save(void *obj, AG_DataSource *buf)
{
	SK *sk = obj;
	MU_Constraint *ct;
	Uint32 count;
	off_t offs;
	
	AG_MutexLock(&sk->lock);
	
	AG_WriteUint32(buf, sk->flags);

	/* Save the generic part of all nodes. */
	if (MU_SaveNoteGeneric(sk, sk->root, buf) == -1)
		goto fail;

	/* Save the data part of all nodes. */
	if (MU_NoteSaveData(sk, sk->root, buf) == -1)
		goto fail;

	/* Save the graph of geometric constraints. */
	offs = AG_Tell(buf);
	count = 0;
	AG_Seek(buf, sizeof(Uint32), AG_SEEK_CUR);
	TAILQ_FOREACH(ct, &sk->ctGraph.edges, constraints) {
		AG_WriteUint32(buf, (Uint32)ct->type);
		AG_WriteUint32(buf, (Uint32)ct->uType);
		MU_WriteRef(buf, ct->n1);
		MU_WriteRef(buf, ct->n2);
		switch (ct->type) {
		case MU_DISTANCE:
			M_WriteReal(buf, ct->ct_distance);
			break;
		case MU_ANGLE:
			M_WriteReal(buf, ct->ct_angle);
			break;
		default:
			break;
		}
		count++;
	}
	AG_WriteUint32At(buf, count, offs);
	AG_MutexUnlock(&sk->lock);
	return (0);
fail:
	AG_MutexUnlock(&sk->lock);
	return (-1);
}

/* Load the data part of a node. */
static int
MU_LoadNoteData(SK *sk, MU_Note *node, AG_DataSource *buf)
{
	MU_Note *chldNote;

	TAILQ_FOREACH(chldNote, &node->cnodes, sknodes) {
		if (MU_LoadNoteData(sk, chldNote, buf) == -1)
			return (-1);
	}
	if (node->ops->load != NULL &&
	    node->ops->load(sk, node, buf) == -1) {
		AG_SetError("%s: %s", node->name, AG_GetError());
		return (-1);
	}
	return (0);
}

/* Load the generic part of a node. */
static int
MU_LoadNoteGeneric(SK *sk, MU_Note **rnode, AG_DataSource *buf)
{
	char type[MU_TYPE_NAME_MAX];
	MU_Note *node;
	Uint32 bsize, nchildren, j;
	Uint32 handle;
	Uint i;

	/* Load generic node information. */
	AG_CopyString(type, buf, sizeof(type));
	bsize = AG_ReadUint32(buf);
	for (i = 0; i < skElementsCnt; i++) {
		if (strcmp(skElements[i]->name, type) == 0)
			break;
	}
	if (i == skElementsCnt) {
		if (sk->flags & MU_SKIP_UNKNOWN_FEATURES) {
			fprintf(stderr, "%s: skipping feature (%s/%luB)\n",
			    OBJECT(sk)->name, type, (Ulong)bsize);
			AG_Seek(buf, bsize, AG_SEEK_CUR);
			*rnode = NULL;
			return (0);
		} else {
			AG_SetError("Unimplemented node class: %s (%luB)",
			    type, (Ulong)bsize);
			return (-1);
		}
	}
	node = Malloc(skElements[i]->size);
	handle = AG_ReadUint32(buf);
	skElements[i]->init(node, handle);
	node->sk = sk;

	AG_CopyString(node->name, buf, sizeof(node->name));
	node->flags = (Uint)AG_ReadUint16(buf);
	M_ReadMatrix44v(buf, &node->T);

	/* Load the child nodes recursively. */
	nchildren = AG_ReadUint32(buf);
	for (j = 0; j < nchildren; j++) {
		MU_Note *cnode;

		if (MU_LoadNoteGeneric(sk, &cnode, buf) == -1) {
			AG_SetError("subnode: %s", AG_GetError());
			return (-1);
		}
		if (cnode != NULL)
			MU_NoteAttach(node, cnode);
	}
	*rnode = node;
	return (0);
}

static int
Load(void *obj, AG_DataSource *buf, const AG_Version *ver)
{
	char unitKey[128];
	SK *sk = obj;
	Uint32 i, count;
	MU_Constraint *ct;
	
	AG_MutexLock(&sk->lock);
	sk->flags = (Uint)AG_ReadUint32(buf);
	AG_CopyString(unitKey, buf, sizeof(unitKey));

	/* Free the existing root node. */
	if (sk->root != NULL) {
		MU_FreeNote(sk, sk->root);
		sk->root = NULL;
	}
	TAILQ_INIT(&sk->nodes);

	/*
	 * Load the generic part of all nodes. We need to load the data
	 * afterwards to properly resolve interdependencies.
	 */
	if (MU_LoadNoteGeneric(sk, &sk->root, buf) == -1) {
		goto fail;
	}
	TAILQ_INSERT_HEAD(&sk->nodes, sk->root, nodes);

	/* Load the data part of all nodes. */
	if (MU_LoadNoteData(sk, sk->root, buf) == -1)
		goto fail;

	/* Load the geometric constraint data. */
	count = AG_ReadUint32(buf);
	for (i = 0; i < count; i++) {
		ct = AG_Malloc(sizeof(MU_Constraint));
		ct->type = (enum sk_constraint_type)AG_ReadUint32(buf);
		ct->uType = (enum sk_constraint_type)AG_ReadUint32(buf);
		ct->n1 = MU_ReadRef(buf, sk, NULL);
		ct->n2 = MU_ReadRef(buf, sk, NULL);
		switch (ct->type) {
		case MU_DISTANCE:
			ct->ct_distance = M_ReadReal(buf);
			break;
		case MU_ANGLE:
			ct->ct_angle = M_ReadReal(buf);
			break;
		default:
			break;
		}
		MU_NoteAddConstraint(ct->n1, ct);
		MU_NoteAddConstraint(ct->n2, ct);
		TAILQ_INSERT_TAIL(&sk->ctGraph.edges, ct, constraints);
	}
	MU_Update(sk);
	AG_MutexUnlock(&sk->lock);
	return (0);
fail:
	AG_MutexUnlock(&sk->lock);
	AG_ObjectReset(sk);
	return (-1);
}

/*
 * Compute the product of the transform matrices of the given node and its
 * parents in order. T is initialized to identity.
 */
void
MU_GetNoteTransform(void *p, M_Matrix44 *T)
{
	MU_Note *node = p;
	MU_Note *cnode = node;
	TAILQ_HEAD_(sk_node) rnodes = TAILQ_HEAD_INITIALIZER(rnodes);

	/*
	 * Build a list of parent nodes and multiply their matrices in order
	 * (ugly but faster than computing the product of their inverses).
	 */
	while (cnode != NULL) {
		TAILQ_INSERT_HEAD(&rnodes, cnode, rnodes);
		if (cnode->pNote == NULL) {
			break;
		}
		cnode = cnode->pNote;
	}
	M_MatIdentity44v(T);
	TAILQ_FOREACH(cnode, &rnodes, rnodes)
		M_MatMult44v(T, &cnode->T);
}

/*
 * Compute the product of the inverse transform matrices of the given node
 * and its parents.
 */
void
MU_GetNoteTransformInverse(void *p, M_Matrix44 *T)
{
	MU_Note *node = p;
	MU_Note *cnode = node;
	TAILQ_HEAD_(sk_node) rnodes = TAILQ_HEAD_INITIALIZER(rnodes);

	M_MatIdentity44v(T);

	while (cnode != NULL) {
		TAILQ_INSERT_TAIL(&rnodes, cnode, rnodes);
		if (cnode->pNote == NULL) {
			break;
		}
		cnode = cnode->pNote;
	}
	TAILQ_FOREACH(cnode, &rnodes, rnodes) {
		M_Matrix44 Tinv;

		Tinv = M_MatInvert44(cnode->T);
		M_MatMult44v(T, &Tinv);
	}
}

void
MU_NoteRedraw(void *p, MU_View *skv)
{
	MU_Note *node = p;

	if (node->ops->redraw != NULL)
		node->ops->redraw(node, skv);
}

/* Return the orientation vector of a node. */
M_Vector3
MU_NoteDir(void *p)
{
	MU_Note *node = p;
	M_Matrix44 T;
	M_Vector3 v = M_VecK3();				/* Convention */
	
	MU_GetNoteTransform(node, &T);
	M_MatMult44Vector3v(&v, &T);
	return (v);
}

/* Associate a constraint with a node. */
void
MU_NoteAddConstraint(void *pNote, MU_Constraint *ct)
{
	MU_Note *node = pNote;
	Uint i;

	for (i = 0; i < node->nCons; i++) {
		if (node->cons[i] == ct)
			return;
	}
	node->cons = Realloc(node->cons, (node->nCons+1) *
	                                 sizeof(MU_Constraint *));
	node->cons[node->nCons++] = ct;
}

/* Dissociate a constraint from a node. */
void
MU_NoteDelConstraint(void *pNote, MU_Constraint *ct)
{
	MU_Note *node = pNote;
	Uint i;

	for (i = 0; i < node->nCons; i++) {
		if (node->cons[i] != ct) {
			continue;
		}
		if (i < node->nCons-1) {
			memmove(&node->cons[i], &node->cons[i+1],
				(node->nCons-i-1)*sizeof(MU_Constraint *));
		}
		node->nCons--;
		break;
	}
}

/* Search a node by handle and class. */
void *
MU_FindNote(SK *sk, Uint32 handle, const char *type)
{
	MU_Note *node;

	TAILQ_FOREACH(node, &sk->nodes, nodes) {
		if (node->handle == handle &&
		    strcmp(node->ops->name, type) == 0)
			return (node);
	}
	AG_SetError("No such node: %u", handle);
	return (NULL);
}

/* Search a node by name only. */
void *
MU_FindNoteByName(SK *sk, const char *name)
{
	MU_Note *node;

	TAILQ_FOREACH(node, &sk->nodes, nodes) {
		if (strcmp(node->name, name) == 0)
			return (node);
	}
	AG_SetError("No such node: %s", name);
	return (NULL);
}

/* Attach a node to another node in the sketch. */
void
MU_NoteAttach(void *ppNote, void *pcNote)
{
	MU_Note *pNote = ppNote;
	MU_Note *cNote = pcNote;

	cNote->sk = pNote->sk;
	cNote->pNote = pNote;
	TAILQ_INSERT_TAIL(&pNote->cnodes, cNote, sknodes);
	TAILQ_INSERT_TAIL(&pNote->sk->nodes, cNote, nodes);
}

/* Detach a node from its parent in the sketch. */
void
MU_NoteDetach(void *ppNote, void *pcNote)
{
	MU_Note *pNote = ppNote;
	MU_Note *cNote = pcNote;
	MU_Note *subnode;
	SK *sk = pNote->sk;
	MU_Constraint *ct;

	while ((subnode = TAILQ_FIRST(&cNote->cnodes)) != NULL) {
		MU_NoteDetach(cNote, subnode);
	}
free_constraints:
	TAILQ_FOREACH(ct, &sk->ctGraph.edges, constraints) {
		if (ct->n1 == cNote || ct->n2 == cNote) {
			MU_DelConstraint(&sk->ctGraph, ct);
			goto free_constraints;
		}
	}
	TAILQ_REMOVE(&pNote->cnodes, cNote, sknodes);
	TAILQ_REMOVE(&sk->nodes, cNote, nodes);
	cNote->sk = NULL;
	cNote->pNote = NULL;
}

/* Move a node up in the order of rendering. */
void
MU_NoteMoveUp(void *p)
{
	MU_Note *node = p;
	MU_Note *prev;

	AG_ObjectLock(node->sk);
	if (node != TAILQ_FIRST(&node->pNote->cnodes)) {
		prev = TAILQ_PREV(node, sk_nodeq, sknodes);
		TAILQ_REMOVE(&node->pNote->cnodes, node, sknodes);
		TAILQ_INSERT_BEFORE(prev, node, sknodes);
	}
	AG_ObjectUnlock(node->sk);
}

/* Move a node down in the order of rendering. */
void
MU_NoteMoveDown(void *p)
{
	MU_Note *node = p;
	MU_Note *next = TAILQ_NEXT(node, sknodes);

	AG_ObjectLock(node->sk);
	if (next != NULL) {
		TAILQ_REMOVE(&node->pNote->cnodes, node, sknodes);
		TAILQ_INSERT_AFTER(&node->pNote->cnodes, next, node, sknodes);
	}
	AG_ObjectUnlock(node->sk);
}

/* Move a node to the tail of its parent's child list. */
void
MU_NoteMoveTail(void *p)
{
	MU_Note *n = p;

	AG_ObjectLock(n->sk);
	TAILQ_REMOVE(&n->pNote->cnodes, n, sknodes);
	TAILQ_INSERT_TAIL(&n->pNote->cnodes, n, sknodes);
	AG_ObjectUnlock(n->sk);
}

/* Move a node to the head of its parent's child list. */
void
MU_NoteMoveHead(void *p)
{
	MU_Note *n = p;

	AG_ObjectLock(n->sk);
	TAILQ_REMOVE(&n->pNote->cnodes, n, sknodes);
	TAILQ_INSERT_HEAD(&n->pNote->cnodes, n, sknodes);
	AG_ObjectUnlock(n->sk);
}

/* Move a node to a different parent node. */
void
MU_NoteMoveToParent(void *pNote, void *pParent)
{
	MU_Note *n = pNote;
	MU_Note *pOld = n->pNote;
	MU_Note *pNew = pParent;

	AG_ObjectLock(n->sk);
	TAILQ_REMOVE(&pOld->cnodes, n, sknodes);
	TAILQ_INSERT_TAIL(&pNew->cnodes, n, sknodes);
	AG_ObjectUnlock(n->sk);
}

/*
 * Detach and free a node (and its children) from the sketch. Annotations
 * and constraints referencing the node are automatically deleted as well.
 */
int
MU_NoteDel(void *p)
{
	MU_Note *node = p;
	SK *sk = node->sk;
	MU_Constraint *ct;

	if (node == sk->root) {
		AG_SetError("Cannot delete root node");
		return (-1);
	}
	if (!MU_NoteOfClass(node, "Annot:Dimension:*")) {
		MU_Dimension *dim;
deldims:
		MU_FOREACH_NODE_CLASS(dim, sk, sk_dimension,
		    "Annot:Dimension:*") {
			if (dim->n1 == node || dim->n2 == node) {
				SKNODE(dim)->ops->del(dim);
				goto deldims;
			}
		}
	}
delcts:
	TAILQ_FOREACH(ct, &sk->ctGraph.edges, constraints) {
		if (ct->n1 == node || ct->n2 == node) {
			MU_NoteDelConstraint(ct->n1, ct);
			MU_NoteDelConstraint(ct->n2, ct);
			MU_DelConstraint(&sk->ctGraph, ct);
			goto delcts;
		}
	}
	MU_NoteDetach(node->pNote, node);
	MU_FreeNote(sk, node);
	return (0);
}

void
MU_WriteRef(AG_DataSource *buf, void *pNote)
{
	MU_Note *node = pNote;

	AG_WriteString(buf, node->ops->name);
	AG_WriteUint32(buf, node->handle);
}

void *
MU_ReadRef(AG_DataSource *buf, SK *sk, const char *expType)
{
	char rType[MU_TYPE_NAME_MAX];

	AG_CopyString(rType, buf, sizeof(rType));
	if (expType != NULL) {
		if (strcmp(rType, expType) != 0) {
			AG_FatalError("Unexpected reference type: `%s' "
			              "(expecting %s)", rType, expType);
		}
	}
	return (MU_FindNote(sk, AG_ReadUint32(buf), rType));
}

/*
 * Perform a proximity query with the given vector against all elements
 * of the given type (or all elements if type is NULL), and return the
 * closest item.
 *
 * The closest point of the closest item is also returned in vC.
 */
void *
MU_ProximitySearch(SK *sk, const char *type, const M_Vector3 *v, M_Vector3 *vC,
    void *nodeIgnore)
{
	MU_Note *node, *nClosest = NULL;
	M_Real rClosest = M_INFINITY, p;
	M_Vector3 vClosest = M_VecGet3(M_INFINITY, M_INFINITY, 0.0);

	TAILQ_FOREACH(node, &sk->nodes, nodes) {
		if (node == nodeIgnore ||
		    node->ops->proximity == NULL) {
			continue;
		}
		if (type != NULL &&
		    strcmp(node->ops->name, type) != 0) {
			continue;
		}
		p = node->ops->proximity(node, v, vC);
		if (p < rClosest) {
			rClosest = p;
			nClosest = node;
			vClosest.x = vC->x;
			vClosest.y = vC->y;
		}
	}
	vC->x = vClosest.x;
	vC->y = vClosest.y;
	vC->z = 0.0;
	return (nClosest);
}

/* Search a node by name in a sketch. */
/* XXX use a hash table */
MU_Cluster *
MU_FindCluster(SK *sk, Uint32 name)
{
	MU_Cluster *cl;

	TAILQ_FOREACH(cl, &sk->clusters, clusters) {
		if (cl->name == name)
			return (cl);
	}
	AG_SetError("No such cluster: %u", name);
	return (NULL);
}

/* Allocate a new cluster name. */
Uint
MU_GenClusterName(SK *sk)
{
	Uint name = 1;

	while (MU_FindCluster(sk, name) != NULL) {
		if (++name >= MU_CLUSTER_ID_MAX)
			AG_FatalError("Out of cluster IDs");
	}
	return (name);
}

void
MU_InitCluster(MU_Cluster *cl, Uint32 name)
{
	cl->name = name;
	TAILQ_INIT(&cl->edges);
}

void
MU_CopyCluster(const MU_Cluster *clSrc, MU_Cluster *clDst)
{
	MU_Constraint *ct;

	TAILQ_FOREACH(ct, &clSrc->edges, constraints)
		MU_AddConstraintCopy(clDst, ct);
}

void
MU_FreeCluster(MU_Cluster *cl)
{
	MU_Constraint *ct;

	while ((ct = TAILQ_FIRST(&cl->edges)) != NULL) {
		TAILQ_REMOVE(&cl->edges, ct, constraints);
		Free(ct);
	}
}

void
MU_FreeClusters(SK *sk)
{
	MU_Cluster *cl;

	while ((cl = TAILQ_FIRST(&sk->clusters)) != NULL) {
		TAILQ_REMOVE(&sk->clusters, cl, clusters);
		MU_FreeCluster(cl);
		Free(cl);
	}
}

void
MU_FreeInsns(SK *sk)
{
	MU_Insn *si;

	while ((si = TAILQ_FIRST(&sk->insns)) != NULL) {
		TAILQ_REMOVE(&sk->insns, si, insns);
		Free(si);
	}
}

/* Create a new constraint edge in the given constraint graph. */
MU_Constraint *
MU_AddConstraint(MU_Cluster *cl, void *node1, void *node2,
    enum sk_constraint_type type, ...)
{
	MU_Constraint *ct;
	va_list ap;

	if (MU_FindConstraint(cl, MU_CONSTRAINT_ANY, node1, node2) != NULL) {
		AG_SetError(_("Existing constraint; new %s constraint would "
		              "overconstraint sketch."),
			      skConstraintNames[type]);
		return (NULL);
	}
	ct = Malloc(sizeof(MU_Constraint));
	ct->uType = type;
	ct->n1 = node1;
	ct->n2 = node2;

	va_start(ap, type);
	switch (type) {
	case MU_DISTANCE:
		ct->ct_distance = (M_Real)va_arg(ap, double);
		break;
	case MU_ANGLE:
		ct->ct_angle = (M_Real)va_arg(ap, double);
		break;
	default:
		break;
	}
	va_end(ap);

	switch (type) {
	case MU_INCIDENT:
		ct->type = MU_DISTANCE;
		ct->ct_distance = 0.0;
		break;
	case MU_PERPENDICULAR:
		ct->type = MU_ANGLE;
		ct->ct_angle = M_PI/2.0;
		break;
	case MU_PARALLEL:
		ct->type = MU_ANGLE;
		ct->ct_angle = 0.0;
		break;
	default:
		ct->type = ct->uType;
		break;
	}
	TAILQ_INSERT_TAIL(&cl->edges, ct, constraints);
	return (ct);
}

/* Duplicate a constraint. */
MU_Constraint *
MU_DupConstraint(const MU_Constraint *ct1)
{
	MU_Constraint *ct2;

	ct2 = Malloc(sizeof(MU_Constraint));
	ct2->type = ct1->type;
	ct2->uType = ct1->uType;
	ct2->n1 = ct1->n1;
	ct2->n2 = ct1->n2;
	switch (ct1->type) {
	case MU_DISTANCE:
		ct2->ct_distance = ct1->ct_distance;
		break;
	case MU_ANGLE:
		ct2->ct_angle = ct1->ct_angle;
		break;
	default:
		break;
	}
	return (ct2);
}

/* Duplicate a constraint edge. */
MU_Constraint *
MU_AddConstraintCopy(MU_Cluster *clDst, const MU_Constraint *ct)
{
	switch (ct->type) {
	case MU_DISTANCE:
		return MU_AddConstraint(clDst, ct->n1, ct->n2, MU_DISTANCE,
		                        ct->ct_distance);
	case MU_ANGLE:
		return MU_AddConstraint(clDst, ct->n1, ct->n2, MU_ANGLE,
		                        ct->ct_angle);
	default:
		break;
	}
	return MU_AddConstraint(clDst, ct->n1, ct->n2, ct->type);
}

/* Destroy a constraint edge. */
void
MU_DelConstraint(MU_Cluster *cl, MU_Constraint *ct)
{
	TAILQ_REMOVE(&cl->edges, ct, constraints);
	Free(ct);
}

/* Destroy a constraint edge matching the argument. */
int
MU_DelSimilarConstraint(MU_Cluster *cl, const MU_Constraint *ctRef)
{
	MU_Constraint *ct;

	if ((ct = MU_FindSimilarConstraint(cl, ctRef)) == NULL) {
		AG_SetError("No matching constraint");
		return (-1);
	}
	MU_DelConstraint(cl, ct);
	return (0);
}

int
MU_CompareConstraints(const MU_Constraint *ct1, const MU_Constraint *ct2)
{
	if (ct1->type == ct2->type &&
	    ((ct1->n1 == ct2->n1 && ct1->n2 == ct2->n2) ||
	     (ct1->n1 == ct2->n2 && ct1->n2 == ct2->n1))) {
#if 1
		switch (ct1->type) {
		case MU_DISTANCE:
			return (int)(ct1->ct_distance - ct2->ct_distance);
		case MU_ANGLE:
			return (int)(ct1->ct_angle - ct2->ct_angle);
		default:
			return (0);
		}
#else
		return (0);
#endif
	}
	return (1);
}

/*
 * Search a constraint graph for any constraint of a given type between
 * two given nodes.
 */
MU_Constraint *
MU_FindConstraint(const MU_Cluster *cl, enum sk_constraint_type type,
    void *n1, void *n2)
{
	MU_Constraint *ct;

	TAILQ_FOREACH(ct, &cl->edges, constraints) {
		if ((ct->type == type || type == MU_CONSTRAINT_ANY) &&
		    ((ct->n1 == n1 && ct->n2 == n2) ||
		     (ct->n1 == n2 && ct->n2 == n1)))
			return (ct);
	}
	return (NULL);
}

/* Search a constraint graph for a constraint matching the argument. */
MU_Constraint *
MU_FindSimilarConstraint(const MU_Cluster *cl, const MU_Constraint *ct)
{
	MU_Constraint *ct2;

	TAILQ_FOREACH(ct2, &cl->edges, constraints) {
		if (MU_CompareConstraints(ct2, ct) == 0)
			return (ct2);
	}
	return (NULL);
}

/*
 * Check if the given two nodes share a constraint edge in the given
 * constraint graph.
 */
MU_Constraint *
MU_ConstrainedNotes(const MU_Cluster *cl, const MU_Note *n1, const MU_Note *n2)
{
	MU_Constraint *ct;

	TAILQ_FOREACH(ct, &cl->edges, constraints) {
		if ((ct->n1 == n1 && ct->n2 == n2) ||
		    (ct->n1 == n2 && ct->n2 == n1))
			return (ct);
	}
	return (NULL);
}

/*
 * Return the number of constraints for the given node, not counting incidence
 * constraints between line segments and their endpoints.
 */
Uint
MU_NoteConstraintCount(const MU_Cluster *cl, void *node)
{
	MU_Note *nOther;
	MU_Constraint *ct;
	Uint count = 0;

	/* TODO use nCons */
	TAILQ_FOREACH(ct, &cl->edges, constraints) {
		if (ct->n1 != node && ct->n2 != node) {
			continue;
		}
		nOther = (ct->n1 == node) ? ct->n2 : ct->n1;
		if (ct->type == MU_DISTANCE && ct->ct_distance == 0.0) {
			if (MU_NoteOfClass(node, "Point:*") &&
			    MU_NoteOfClass(nOther, "Line:*")) {
				continue;
			}
			if (MU_NoteOfClass(node, "Line:*") &&
			    MU_NoteOfClass(nOther, "Point:*")) {
				continue;
			}
		}
		count++;
	}
	return (count);
}

/* Evaluate whether the given node is in the given constraint graph. */
/* XXX optimize */
int
MU_NoteInCluster(const MU_Note *node, const MU_Cluster *cl)
{
	MU_Constraint *ct;

	TAILQ_FOREACH(ct, &cl->edges, constraints) {
		if (ct->n1 == node || ct->n2 == node)
			return (1);
	}
	return (0);
}

/*
 * Count the number of edges between a given node and any other node
 * in the given subgraph clSub of graph cl.
 */
Uint
MU_ConstraintsToSubgraph(const MU_Cluster *clOrig, const MU_Note *node,
    const MU_Cluster *clSub, MU_Constraint *rv[2])
{
	MU_Constraint *ct;
	Uint count = 0;

	TAILQ_FOREACH(ct, &clOrig->edges, constraints) {
		if ((ct->n1 == node && MU_NoteInCluster(ct->n2, clSub)) ||
		    (ct->n2 == node && MU_NoteInCluster(ct->n1, clSub))) {
			if (count < 2) {
				rv[count] = ct;
			}
			count++;
		}
	}
	return (count);
}

/* Add a construction step for the construction phase of the solver. */
MU_Insn *
MU_AddInsn(SK *sk, enum sk_insn_type type, ...)
{
	MU_Insn *si;
	va_list ap;

	si = Malloc(sizeof(MU_Insn));
	si->type = type;

	va_start(ap, type);
	switch (type) {
	case MU_COMPOSE_PAIR:
		si->n[0] = va_arg(ap, void *);
		si->n[1] = va_arg(ap, void *);
		si->ct01 = va_arg(ap, void *);
#ifdef SG_DEBUG
		if (si->n[0] == NULL || si->n[1] == NULL || si->ct01 == NULL ||
		    si->ct01->type >= MU_CONSTRAINT_LAST)
			AG_FatalError("Bad args");
#endif
		Debug(sk, "+ COMPOSE_PAIR(%s,%s)\n",
		    si->n[0]->name, si->n[1]->name);
		break;
	case MU_COMPOSE_RING:
		si->n[0] = va_arg(ap, void *);
		si->n[1] = va_arg(ap, void *);
		si->n[2] = va_arg(ap, void *);
		si->ct01 = va_arg(ap, void *);
		si->ct02 = va_arg(ap, void *);
#ifdef SG_DEBUG
		if (si->n[0] == NULL || si->n[1] == NULL || si->n[2] == NULL ||
		    si->ct01 == NULL || si->ct02 == NULL ||
		    si->ct01->type >= MU_CONSTRAINT_LAST ||
		    si->ct02->type >= MU_CONSTRAINT_LAST)
			AG_FatalError("Bad args");
#endif
		Debug(sk, "+ COMPOSE_RING(%s,%s,%s)\n",
		    si->n[0]->name, si->n[1]->name, si->n[2]->name);
		break;
	}
	va_end(ap);

	TAILQ_INSERT_TAIL(&sk->insns, si, insns);
	return (si);
}

static int
MU_ComposePair(SK *sk, const MU_Insn *insn)
{
	MU_Note *n, *n1;
	MU_Constraint *ct = insn->ct01;
	int i, rv = -1;

	if (MU_FIXED(insn->n[0]) && MU_FIXED(insn->n[1])) {
		AG_SetError("Attempt to place two fixed entities");
		return (-1);
	}
	if (MU_FIXED(insn->n[0])) {
		n =  insn->n[1];
		n1 = insn->n[0];
	} else if (MU_FIXED(insn->n[1])) {
		n =  insn->n[0];
		n1 = insn->n[1];
	} else {
		if (MU_MOVED(insn->n[0])) {
			n =  insn->n[1];
			n1 = insn->n[0];
		} else if (MU_MOVED(insn->n[1])) {
			n =  insn->n[0];
			n1 = insn->n[1];
		} else {
			n =  insn->n[0];
			n1 = insn->n[1];
		}
	}
	
	Debug(sk, "Constructing: %s(%s) => %s\n",
	    n1->name, skConstraintNames[ct->type], n->name);

	for (i = 0; i < skConstraintPairFnCount; i++) {
		const MU_ConstraintPairFn *fn = &skConstraintPairFns[i];

		if (fn->ctType != ct->type) {
			continue;
		}
		if (MU_NoteOfClass(n,  fn->type1) &&
		    MU_NoteOfClass(n1, fn->type2)) {
			rv = fn->fn(ct, (void *)n, (void *)n1);
			break;
		} else
		if (MU_NoteOfClass(n1, fn->type1) &&
		    MU_NoteOfClass(n,  fn->type2)) {
			rv = fn->fn(ct, (void *)n1, (void *)n);
			break;
		}
	}
	if (i == skConstraintPairFnCount) {
		AG_SetError("Illegal case: %s(%s,%s)",
		    skConstraintNames[ct->type], n->ops->name, n1->ops->name);
		return (-1);
	}
	return (rv);
}

static int
MU_ComposeRing(SK *sk, const MU_Insn *insn)
{
	MU_Note *n = insn->n[0];
	MU_Note *n1 = insn->n[1];
	MU_Note *n2 = insn->n[2];
	MU_Constraint *ct1 = insn->ct01;
	MU_Constraint *ct2 = insn->ct02;
	int i, rv = 0;
	
	if (MU_FIXED(n)) {
		AG_SetError("Attempt to place fixed entity");
		return (-1);
	}
	
	Debug(sk, "Constructing: %s,%s(%s,%s) => %s\n",
	    n1->name, n2->name,
	    skConstraintNames[ct1->type],
	    skConstraintNames[ct2->type],
	    n->name);

	for (i = 0; i < skConstraintRingFnCount; i++) {
		const MU_ConstraintRingFn *fn = &skConstraintRingFns[i];

		if ((fn->ctType1 == ct1->type ||
		     fn->ctType1 == MU_CONSTRAINT_ANY) &&
		    (fn->ctType2 == ct2->type ||
		     fn->ctType2 == MU_CONSTRAINT_ANY) &&
		    MU_NoteOfClass(n, fn->type1) &&
		    MU_NoteOfClass(n1, fn->type2) &&
		    MU_NoteOfClass(n2, fn->type3)) {
			rv = fn->fn(n, ct1, n1, ct2, n2);
			if (rv == 0) {
				n->flags |= MU_NOTE_KNOWN;
			}
			break;
		} else if
		   ((fn->ctType1 == ct2->type ||
		     fn->ctType1 == MU_CONSTRAINT_ANY) &&
		    (fn->ctType2 == ct1->type ||
		     fn->ctType2 == MU_CONSTRAINT_ANY) &&
		    MU_NoteOfClass(n, fn->type1) &&
		    MU_NoteOfClass(n2, fn->type2) &&
		    MU_NoteOfClass(n1, fn->type3)) {
			rv = fn->fn(n, ct2, n2, ct1, n1);
			if (rv == 0) {
				n->flags |= MU_NOTE_KNOWN;
			}
			break;
		}
	}
	if (i == skConstraintRingFnCount) {
		AG_SetError("Illegal case: %s([%s:%s], [%s:%s])",
		    n->ops->name,
		    skConstraintNames[ct1->type],
		    n1->ops->name,
		    skConstraintNames[ct2->type],
		    n2->ops->name);
		return (-1);
	}
	return (rv);
}

int
MU_ExecInsn(SK *sk, const MU_Insn *insn)
{
	switch (insn->type) {
	case MU_COMPOSE_PAIR:
		return MU_ComposePair(sk, insn);
	case MU_COMPOSE_RING:
		return MU_ComposeRing(sk, insn);
	default:
		AG_SetError("Illegal instruction: 0x%x", insn->type);
		return (-1);
	}
}

void
MU_ClearProgramState(SK *sk)
{
	MU_Note *node;

	sk->nSolutions = 0;
	TAILQ_FOREACH(node, &sk->nodes, nodes)
		sk->flags &= ~(MU_NOTE_KNOWN);
}

int
MU_ExecProgram(SK *sk)
{
	MU_Insn *si;
	Uint i = 0;

	TAILQ_FOREACH(si, &sk->insns, insns) {
		if (MU_ExecInsn(sk, si) == -1) {
			MU_SetStatus(sk, MU_INVALID, _("Error(%u): %s"),
			    i, AG_GetError());
			return (-1);
		}
		i++;
	}
	return (0);
}

void
MU_SetStatus(SK *sk, enum mu_solver_status status, const char *fmt, ...)
{
	va_list ap;

	sk->status = status;
	va_start(ap, fmt);
	AG_Vsnprintf(sk->statusText, sizeof(sk->statusText), fmt, ap);
	va_end(ap);
}

AG_ObjectClass muClass = {
	"MU",
	sizeof(MU),
	{ 0,0 },
	Init,
	Reset,
	NULL,		/* destroy */
	Load,
	Save,
	MU_Edit
};