#ifdef HAVE_STDLIB_H
#include <stdlib.h>
#endif
#include <stdio.h>
#include <memory.h>
#include <errno.h>
#include "yagi.h"
extern int errno;

#define MAX_NON_ITERATIVE 100000

void solve_equations(double frequency, int driven, int parasitic, double **driven_data, double **parasitic_data, double *v, double **z, double *pin, struct FCOMPLEX *voltage, struct FCOMPLEX *current, struct FCOMPLEX *input_impedance, struct element_data *coordinates, double **A, double *b, int *indx)
{
	int elements, element_number, i, j;
	double d, **A_pre_LUdcmp, *b_copy, t1, t2;
	elements=driven+parasitic;
	fill_z_matrix(frequency,driven,parasitic,driven_data,parasitic_data, z);
	/* Now we must fill the V vector, which gives the voltage at the centre of
	each driven element. Since we know the magnitude of the voltage and phase,
	we can calculate the real and imaginary components of voltage. NB */
	fill_v_vector(driven, parasitic, driven_data, v);

	/* We now have the voltage vector V, and the impedance matrix Z. All
	we need to do now is solve a set of NxN equations, where N is the
	number of elements, to get N values of current, which we can put in
	the I matrix.
	Unfortunately, this is not trivual, by any standards!!!  It is
	complicated by the fact the the N equations are all complex. 
	The method used us a brute-force approach mentioned by Press
	in their 2nd Edition of the Numerical Recipes in C book. Here we
	put the data in the form:
	
	|Zr -Zi|   |Ir|   = |Vr|   which we will call A.x=b
	|Zi  Zr|   |Ii|     |Vi| 
		
	so the Z data goes now in a 2Nx2N matrix. This is a bit wasteful of space
	and time, but it will do here. */
	/* Copy impedance data from 'z' to matrix A */
	copy_complex_data_to_real_matrix(elements,z,A);
	/* The following function prints to stantard output the z matrix of the
	antenna. It is really only used during debugging, so it can be commented
	out normally. */
	/* print_z_matrix(frequency,elements,z);  */
	/* read voltage data from v into b */
	for(i=1;i<=elements;++i)
	{
		b[i]= v[2*i-1];           /* real data */
		b[i+elements]=v[2*i];    /* imaginary data */
		voltage[i].r=v[2*i-1];
		voltage[i].i=v[2*i];
	}
	/* If there are a lot of elements, its possible that rounding errors
	will destroy the accuracy of the results. If there are more than 
	MAX_NON_ITERATIVE elements, we do the usual LU decompositioon and
	back-substitution, then do an iterative improvement of the 
	solution */
	if(elements > MAX_NON_ITERATIVE)
	{
		A_pre_LUdcmp=dmatrix(1L, 2L*(long)elements, 1L, 2L*(long) elements);
		b_copy=dvector(1L, 2L*(long)elements);
		/* I had troubled using memcpy to copy matrices - probably becuase
		they are offset by  1 */
		copy_matrix(2*elements, 2*elements, A_pre_LUdcmp, A);
		/* copy vector b */
		for(j=1;j<=2*elements;++j)
			b_copy[j]=b[j];
	}
	/* Perform a LU decompositon of A */
	ludcmp(A, elements*2, indx, &d);
	/* We now have the voltages in b. After lubksb is run, we get the 
	currents in b . A contains an LU decomposed version of the original A*/
	lubksb(A, 2*elements, indx, b); /* current's in b after lubksb has run*/
	if(elements>MAX_NON_ITERATIVE)
	{
		t1=b[1];
		mprove(A_pre_LUdcmp, A, 2*elements, indx, b_copy, v);
		free_dmatrix(A_pre_LUdcmp,1L, 2L*(long)elements, 1L, 2L*(long) elements);
		/* free_dvector(bcopy,1L,2L*elements); */
	   t2=b[1];
		if(t1!=t2)
		{
			printf("b[1]'s differ before and after mprove: before = %.16f after =%.16f\n", t1, t2);
			exit(1);
		}
	}
	/* Put currents an FCOMPLEX matrix current[element] 
	currents are stored in b as r,r,r,r ..... i,i,i,i */
	for(element_number=1;element_number<=elements;element_number++) 
	{
		current[element_number].r=b[element_number];
		current[element_number].i=b[element_number+elements];
	} 
	z_input(voltage[1], current[1], input_impedance);
	*pin=calculate_power_input(input_impedance->r,current[1]); /* in Watts */
	/* Sometimes the input power goes < 0. I'm not sure why this occurs, but
	the implication is that the antenna is a source of energy, which is silly.
	The gain is negative then (not in dB), so taking the log to put in dB
	would cause a numerical error. When this occurs, we print a message to
	stderr, then write the data in a file "problem". The user may then 
	investigate what went wrong. */
	if(*pin <= 0.0)
	{
		/* fprintf(stderr,"Input power less than 0! Undesirable, but non-fatal error.\n"); */
		*pin=1e20; /* Force it to a large value */
   /* do_since_better(0,"problem","problem",*input_impedance, \
	rubbish,flag, "This causes Pin < 0.0 - findout why!",frequency, frequency, \
	frequency,frequency/10.0, elements, driven, parasitic, \
	180.0, driven_data, parasitic_data, 1.0, 0.0); */
	} 
	for(i=1;i<=driven;++i)
	{
		coordinates[i].x=driven_data[i][X];
		coordinates[i].y=driven_data[i][Y];
		coordinates[i].length=driven_data[i][LENGTH];
	}
	for(i=driven+1;i<=elements;++i)
	{
		coordinates[i].x=parasitic_data[i-driven][X];
		coordinates[i].y=parasitic_data[i-driven][Y];
		coordinates[i].length=parasitic_data[i-driven][LENGTH];
	}

#ifdef DEBUG
	if(errno)
	{
	fprintf(stderr,"Errno =%d in solve.c\n", errno);
	exit(1);
	}
#endif
}