eq.c

circuit calculation program

#include <stdlib.h>
#include "eq.h"

typedef struct EQ_list_def {
	EQ_item_t *eq;
	float_t constant;
	struct EQ_list_def *next;
} EQ_list;

static EQ_list *eq_list;

int EQ_start ()
{
	EQ_list *p;	
	
	p = (EQ_list *) malloc (sizeof (EQ_list));
	if (!p) {error (NO_MEM); }
	else {
		p->next = eq_list;
		p->eq = 0;
		p->constant = 0;

		eq_list = p;
	}

	return !!p;
}	
/* add an item to equation: f(X) = Y */
int EQ_add_const (float_t constant)
{
	EQ_list *p = eq_list;
	assert (p);
	
	p->constant -= constant;   /* merge constant item */
}

/* add an item to equation: f(X) = Y */
int EQ_add (float_t constant, EQ_item_t *item)
{
	EQ_list *p = eq_list;
	EQ_item_t *q,*r;
	
	assert (p);
	assert (item->var_index>=0);
	
	p->constant -= constant;   /* merge constant item */
	
	r = NULL;
	q = p->eq;
	while (q && q->var_index < item->var_index) {r=q; q=q->next;}
	
	if ( q && q->var_index == item->var_index ) {
	/* merge items of the same vars */
		q->coef += item->coef;
		EQ_item_free (item);
	} else {
	/* add to the link */
		item->next = q;
		if (r) r->next = item;
		else p->eq = item;
	}
}


int EQ_end ()
{
}

/* keep the eq list in ascending order of lowerest var index */
int EQ_sort ()
{
	EQ_list *p = eq_list,*q,*r;
	
	assert (p);
	assert (p->eq);
	
	r = NULL;
	q = eq_list->next;  /* scan from the second eq */
	while (q) {
		assert (q->eq);
		
		if (q->eq->var_index < p->eq->var_index) {r=q; q=q->next;}
		else break;
	}
	
	if (r) { /* should insert somewhere between r and q */
		eq_list = eq_list->next;  /* remove it first */
		
		r->next = p;
		p->next = q;
	}
}	

int EQ_reset()
{
	EQ_list *p;
	EQ_item_t *q,*r;
	
	while (p = eq_list) {
		r = p->eq;
		
		while (q=r) {r = q->next; EQ_item_free (q);}
		
		eq_list = eq_list->next;
		free (p);
	}
}

int dump_EQ()
{
	EQ_list *p;
	EQ_item_t *q,*r;
	int  i;
	
	i = 0;	
	p = eq_list;
	while (p) {
		q = p->eq;
		
		printf ("\n%f =", - p->constant);
		while (q) {
			printf ("%f x%d +", q->coef, q->var_index );
			q = q->next; 
		}
		
		p = p->next;
		i ++;
	}
	printf ("\nTotalling equations: %d\n",i );
}	

EQ_item_t *EQ_item_alloc()
{
	return (EQ_item_t *) malloc (sizeof (EQ_item_t));
}	

int EQ_item_free (EQ_item_t *q)
{
	free (q);
	return 0;
}	

/* 
 * find the next eq for var reduction
 *
 */
int EQ_find_var()
{
	EQ_list *p,*t,*p_prev,*t_prev;

	int var_index;

	p_prev = eq_list;
	p = eq_list->next;	
	
	t_prev = NULL;
	t = eq_list;
	
	assert (eq_list->eq);
	var_index = eq_list->eq->var_index;

	while (p) {
		assert (p->eq);
		if (p->eq->var_index < var_index 
		    ||(p->eq->var_index == var_index 
		       && fabs (t->eq->coef) < FLOAT_BCEILING 
  		       && fabs (p->eq->coef) > fabs (t->eq->coef)) ) {
  		       	
			var_index = p->eq->var_index;
			t_prev = p_prev;
			t = p;
		}
		p_prev = p;
		p = p->next;
	}
	
	/* 
	 *move the selected t to the to top
	 */
  	if (t != eq_list ){
  		EQ_list *temp;
		/* remove first */	
		temp = t->next;	
		t_prev->next = temp;
		
		/* insert after x_prev */
		t->next = eq_list;	
		eq_list = t;
		
		return 1;
	}	
	
	return 0;
	
}
/*
 * The first eq is mul-ed and added to the rest eq's
 */

int EQ_reduce_var(EQ_list *p)
{
	EQ_list *t;
	EQ_item_t *q,*r,*s,*r_prev;
	int var_index;

	float_t a,b;

        /* if this eq is null, it means this eq is reduntant 
         * we assure that eq is not null by checking it in the caller
         */
	assert (p->eq);

	q = p->eq;
	a = q->coef;
	var_index = q->var_index;
		
	t = p->next;
	/* reduce the first variable in p */
	while (t) {
			assert (t->eq);	

			assert (t!=p);			
			r = t->eq;
			/* because eq list is sorted, so when no var can be
			 * reduced, stop
			 */
			if (r->var_index > var_index ) goto next; 
			
			assert (r->var_index == var_index );
			assert (fabs (a) > FLOAT_BCEILING 
  			        || fabs (r->coef) < fabs (a) );
	
  			/*
  			 * now b is expected to have a normal value
  			 * see EQ_find_var ()
  			 */ 
			b = -r->coef/a;
		
			/* constant item */
			t->constant += p->constant * b;
			
			/* drop r */
			s = r;
			r = t->eq = s->next;
			EQ_item_free (s);
			
			/* then add each item in q to r */
			r_prev = NULL;
			s = q->next;
			while (s) {
				/* items in an eq are in ascending order of var index */
				while ( r && r->var_index < s->var_index ) { r_prev = r; r = r->next; }
				
				if (r && r->var_index == s->var_index) {
					r->coef = r->coef + s->coef*b;
						
						/* don't check whether the coef is near to 0 now*/
				} else { /* add a new item */
					EQ_item_t *n;
					
					n = EQ_item_alloc ();
					if (!n) {
						error (__FILE__,__LINE__,ENO_MEM);
						return ENO_MEM;
					}
						
					n->var_index = s->var_index;
					n->coef = s->coef*b;
					n->next = r;
						
					if (r_prev) r_prev->next = n;
					else t->eq = n;
				}
				
				s = s->next;
			}
next:
			t = t->next;
	}
	
	return 0;
}	

int EQ_resolve ()
{
	EQ_list *p,*q;
	EQ_item_t *r,*s;
	int i;
	/* step 1. reduce vars /	
	 * processed eq are queued to p 
	 */
	p = NULL;	

	while(eq_list) {
		/* check redundant eq */
		if (!eq_list->eq) {
			q = eq_list;
			eq_list = eq_list->next;
			free (q);
			continue;
		}
		
		/*
		 * when eq_list->next, EQ_xx will return directly
		 */
		EQ_find_var ();
		
		if ( EQ_reduce_var (eq_list)) {
			/* failed. */
			return ENO_MEM;	
		}
		
		/* remove the top eq */
		q = eq_list;
		eq_list = eq_list->next;
		/* move the top eq to p */
		q->next = p;
		p = q;		
	}
	/* step 2. calculate vars 
	 *  Now, p is a triangle like:
	 *              A00*x0                 = y0
	 *              A10*x0 +A11*x1         = y1
	 *              A20*x0 +A21*x1 +A32*X2 = y1
	 */
	
	dump_EQ ();	

	i = 0;	
	while (q) {
		int j;
		
		p = q;	
		q = p->next;
		
		assert (p->eq);   /* all redundant eq's have been removed */		
		
		r = p->eq;
		s = r->next;
	
		
		/* bring in all known vars */
		j = 0;
		while (s ) {
			EQ_item_t *temp;
			
			
			p->constant += s->coef * vars[ s->var_index ];
			
			temp = s;
			s = s->next;
			EQ_item_free (temp);
			
			j ++;
		}

		if (j>i) /* there are more than i vars in the first eq*/
			return ENO_SOLUTION;
				
		/* calc next vars */
		vars[ r->var_index ] = -p->constant / r->coef;	
		
		WATCH (	r->var_index );
		WATCHF (	vars[ r->var_index ] );
		
		EQ_item_free (r);				
		i ++;  /* a new vars is known */
		
		free (p);
	}
	eq_list = 0;
}