#include "circuit.h"
#include "matrix.h"

// defined in ludcmp.cpp
int ludcmp(Matrix &a, int order[]);
void solvlu(const Matrix &a, const Vec &b, Vec &x, const int order[]);

void nmAcc(Matrix &G,int i, int j, double val);
void srcAcc(Vec &J, int i, int j, double val);
void mnaVSrc(Matrix &G,int n, int i, int j);
void gmAcc(Matrix &G, int j, int jp, int k, int kp, double value);


void eCircuit::sysbuild()
{
	int k;
	int nbr_nodes = ncount;
	const int devR = *(int *)(Resistor::code());
	const int devI = *(int *)(CurrentSource::code());
	const int devV = *(int *)(VoltageSource::code());
	const int devC = *(int *)(Capacitor::code());
	const int devL = *(int *)(Inductor::code());
	const int devVCCS = *(int *)(VCCS::code());
	int nC = 0;
	int nL = 0;
	int nV = 0;
	for (k=0; k<ndevices; k++) {
		eDevice &dev = *devices[k];
		int code = dev.getCode();
		if (code==devC) nC++;
		else if (code==devL) nL++;
		else if (code==devV) nV++;
	}
	int nrows = nbr_nodes + nV + nL;
	Vec J(nrows);
	Matrix G(nrows,nrows);
	Matrix B(nrows,nrows);
	J.set(0.0);
	G.set(0.0);
	B.set(0.0);
	for (k=0; k<ndevices; k++) {
		eDevice &dev = *devices[k];
		double v = dev.value;
		int code = dev.getCode();
		if (code==devR) nmAcc(G,dev.ndx[0],dev.ndx[1],1/v);
		else if (code==devI) srcAcc(J,dev.ndx[1],dev.ndx[0],v);
		else if (code==devC) nmAcc(B,dev.ndx[0],dev.ndx[1],v);
		else if (code==devVCCS) gmAcc(G,dev.ndx[2],dev.ndx[3],dev.ndx[0],dev.ndx[1],v);
	}
	// do voltage sources and inductors.
	int m = nbr_nodes;
	for (k=0; k<ndevices; k++) {
		eDevice &dev = *devices[k];
		int code = dev.getCode();
		if (code==devV) {
			mnaVSrc(G,m,dev.ndx[0],dev.ndx[1]);
			J[m] = dev.value;
			m++;
		}
		else if (code==devL) {
			mnaVSrc(G,m,dev.ndx[0],dev.ndx[1]);
			B[m][m] = -dev.value;
			m++;
		}
	}
	printf("\nG matrix:\n");
	G.print();
	if ((nC+nL)>0) {
		printf("\nB matrix:\n");
		B.print();
	}
	if (J.norm()==0) return;
	printf("\nJ vector:\n");
	J.print();
	Matrix a = G;
	Vec V(nrows);
	int *ipvt = new int[nrows];
	ludcmp(a,ipvt);
	solvlu(a,J,V,ipvt);
	printf("\nV vector:\n");
	V.print();
}
	
	


//! Accumulate g = (1/R) or R values onto G Matrix
//  i and j are node (mesh) indices.

void nmAcc(Matrix &G,int i, int j, double val)
{
	if ((i<0) && (j<0)) return; // trivial case, both indices equal reference node
	if (i>=0) G[i][i] += val;
	if (j>=0) G[j][j] += val;
	if ( (i>=0) && (j>=0) ) {
		G[i][j] -= val;
		G[j][i] -= val;
	}
}

//!   Accumulates Is or Vs values onto source Matrix J.
/*    Index i is "from" node (drop mesh).
*     Index j is "to" node (rise mesh).
*/
void srcAcc(Vec &J, int i, int j, double val)
{
	if (j>=0) J[j] += val;
	if (i>=0) J[i] -= val;
}


//function amat = mnaVSrc(m,i,j,bmat)
/*
 * Accumulation of Vsrc onto bmat for MNA analysis.
*   m is the index into the additional CE row.
 *   i and j are the + and - nodes of the voltage source.
 */
void mnaVSrc(Matrix &G,int m, int i, int j)
{
	if (i>=0) {
		G[m][i] = 1.0;
		G[i][m] = 1.0;
	}
	if (j>=0) {
		G[m][j] = -1.0;
		G[j][m] = -1.0;
	}
}

/* Voltage-Controlled Current Source (G source)
 *  j and jp are + and i control nodes
 *  k and kp are from and to nodes
 */

void gmAcc(Matrix &G, int j, int jp, int k, int kp, double value)
{
	if ((k>=0) && (j>=0)) G[k][j] += value;
	if ((kp>=0) && (jp>=0)) G[kp][jp] += value;
	if ((k>=0) && (jp>=0)) G[k][jp] -= value;
	if ((kp>=0) && (j>=0)) G[kp][j] -= value;
}
/* Voltage-Controlled Voltage Source (E)
 *   m is the index of the added constitutive equation row
 *   j and jp are the + and - controlling nodes.
 *   k and kp are the + and - nodes of the E source
*/
void mnaESrc(Matrix &G,int m, int j, int jp, int k, int kp,double value)
{
	if (k>=0) {
		G[k][m] = 1.0;
		G[m][k] = 1.0;
	}
	if (kp>=0) {
		G[kp][m] = -1.0;
		G[m][kp] = -1.0;
	}
	if (j>=0) G[m][j] -= value;
	if (jp>=0) G[m][jp] += value;
}

/* Current-Controlled Current Source (F) 
 *  m is the index of the added consitutive equation row
 *  j and jp are the from and to controlling nodes
 *  k and kp are the from and to nodes of the F source
 */
void mnaFSrc(Matrix &G,int m, int j, int jp, int k, int kp,double value)
{
	if (j>=0) {
		G[j][m] = 1.0;
		G[m][j] = 1.0;
	}
	if (jp>=0) {
		G[jp][m] = -1.0;
		G[m][jp] = -1.0;
	}
	if (k>=0) G[k][m] += value;
	if (kp>=0) G[kp][m] -= value;
}

/* Current-Controlled Voltage Source (H)
 * m and m+1 are rows for constutive equations
 * j and jp are + and - controlling nodes
 * k and kp are + and - nodes for H source
 */
void mnaHSrc(Matrix &G, int m, int j, int jp, int k, int kp, double value)
{
	if (j>=0) {
		G[j][m] = 1.0;
		G[m][j] = 1.0;
	}
	if (jp>=0) {
		G[jp][m] = -1.0;
		G[m][jp] = -1.0;
	}
	int mp1 = m+1;
	if (k>=0) {
		G[k][mp1] = 1.0;
		G[mp1][k] = 1.0;
	}
	if (kp>=0) {
		G[kp][mp1] = -1.0;
		G[mp1][kp] = -1.0;
	}
	G[mp1][m] = -value;
}

/* ideal op-amp
 *  m is the index of the added row.
 *  j and jp are the noninverting and inverting op-amp input nodes.
 *  k is the output node;
 */
void mnaOA(Matrix &G, int m, int j, int jp, int k)
{
	if (j>=0) G[m][j] = 1.0;
	if (jp>=0) G[m][jp] = -1.0;
	if (k>=0) G[k][m] = 1.0;
}