#include "ElectricalData.h"
#include "AdmittanceNetwork.h"
#include "ComplexMatrix_LAPACK.h"
#include <iostream>
#include <cassert>
using namespace std;

static const double pi = 3.14159265358979;

static const double phase_angle[3] =
{
	0.0,
	2.0*pi/3.0,
	4.0*pi/3.0
};

ElectricalData::ElectricalData(double freq):
	freq(freq)
{
}

ElectricalData::~ElectricalData()
{
}

void ElectricalData::configureGenerator(genr_t& genr_data)
{
	/**
	 * Inertia estimates are calculated from data in Table 5.1 of
	 * "Computer-Aided Power System Analysis" by Ramasamy Natarajan,
	 * published by Marcel Dekker, Inc., c. 2002. Define:
	 */
	if (genr_data.genr_mw * SBase > 600.0) // Big nuke plant
	{
		genr_data.M = 3.5*600.0/SBase;
		genr_data.Lg = 0.26/(2.0*pi*freq);
	}
	else if (genr_data.genr_mw * SBase > 300.0) // Big fossil plant
	{
		genr_data.M = 4.0*300.0/SBase;
		genr_data.Lg = 0.25/(2.0*pi*freq);
	}

	else if (genr_data.genr_mw * SBase > 120.0) // Smaller steam plant
	{
		genr_data.M = 3.3*120.0/SBase;
		genr_data.Lg = 0.26/(2.0*pi*freq);
	}
	else 
	{
		genr_data.M = 6.0*50.0/SBase;
		genr_data.Lg = 0.2/(2.0*pi*freq);
	}
	// Try to get close to an appropriate set of initial conditions
	genr_data.w0 = 0.0;
	genr_data.Tspd1 = 10.0;
	genr_data.Tspd2 = 1.0;
	genr_data.R = 1.0/0.05;
	genr_data.Agc = genr_data.R*0.1;
	genr_data.D = 1.0;
	genr_data.G = 400.0;
	genr_data.Rg = 0.0;	
}

void ElectricalData::initialize_circuit()
{
	map<int,int> node_to_idx;
	// Move line charging to shunt capacitances at the busses and
	// calculate the line electrical parameters
	for (unsigned i = 0; i < lines.size(); i++)
	{
		assert(real(lines[i].B) == 0.0);
		// Some capacitance belongs to every line
		if (lines[i].B == Complex(0.0,0.0))
		{
			double L = imag(1.0/lines[i].y)/(2.0*pi*freq);
			double C = L*1E-2;
			lines[i].B = Complex(0.0,C*2.0*pi*freq);
		}
		// Some resistance belongs to every line
		if (real(lines[i].y) == 0.0)
		{
			lines[i].Z = 1.0/lines[i].y;
			lines[i].Z = Complex(imag(lines[i].Z)/(6.0*pi*freq),imag(lines[i].Z));
			lines[i].y = 1.0/lines[i].Z;
		}
		double C = imag(lines[i].B)/(2.0*pi*freq);
		for (unsigned j = 0; j < nodes.size(); j++)
		{
			if (lines[i].from == nodes[j].ID)
				nodes[j].Cshunt += C/2.0;
			else if (lines[i].to == nodes[j].ID)
				nodes[j].Cshunt += C/2.0;
		}
		// Calculate line parameters
		lines[i].B = Complex(0.0,0.0);
		lines[i].Z = 1.0/lines[i].y;
		lines[i].R = real(lines[i].Z);
		assert(imag(lines[i].Z) > 0.0);
		lines[i].L = imag(lines[i].Z)/(2.0*pi*freq);
	}
	// Calculate load parameters
	for (unsigned i = 0; i < nodes.size(); i++)
	{
		node_to_idx[nodes[i].ID] = i;
		nodes[i].y = nodes[i].Z = Complex(0.0,0.0);
		if (nodes[i].B != Complex(0.0,0.0))
		{
			assert(real(nodes[i].B) == 0.0);
			if (imag(nodes[i].B) < 0.0)
				nodes[i].y += nodes[i].B;
			// Charging as capacitance
			else
				nodes[i].Cshunt += imag(nodes[i].B)/(2.0*pi*freq);
		}
		Complex V = polar(nodes[i].vamp,nodes[i].theta);
		Complex S(nodes[i].load_mw,nodes[i].load_mvar);
		if (S != Complex(0.0,0.0))
		{
			Complex I = conj(S/V);
			nodes[i].Z = V/I;
			nodes[i].y += 1.0/nodes[i].Z;
			nodes[i].Z = 1.0/nodes[i].y;
		}
		nodes[i].R = real(nodes[i].Z);
		if (imag(nodes[i].Z) > 0.0)
			nodes[i].L = imag(nodes[i].Z)/(2.0*pi*freq);
		else if (imag(nodes[i].Z) < 0.0)
		{
			nodes[i].C = -1.0/(2.0*pi*freq*imag(nodes[i].Z));
			assert(nodes[i].C > 0.0);
		}
		// Lump a purely capacitive load into the shunt capacitor or, 
		// if there is no load, add a small load that will dissipate
		// high frequency signals
		if (nodes[i].R == 0.0 && nodes[i].L == 0.0)
		{
			nodes[i].Cshunt += nodes[i].C;
			nodes[i].C = 0.0;
			nodes[i].y = Complex(0.0,0.0);
			nodes[i].Z = 1.0/nodes[i].y;
		}
		if (nodes[i].Cshunt == 0.0)
			nodes[i].Cshunt = 1E-5;
		nodes[i].B = Complex(0,2.0*pi*freq*nodes[i].Cshunt);
	}
	// Find initial generator currents and voltages
	AdmittanceNetwork* Y = new AdmittanceNetwork(new ComplexMatrix_LAPACK(nodes.size()));
	Complex *Vsrc = new Complex[nodes.size()];
	Complex *Vnode = new Complex[nodes.size()];
	// Node to ground currents or source voltages for generators
	for (unsigned i = 0; i < nodes.size(); i++)
	{
		if (nodes[i].genr.genr_mw != 0.0 || nodes[i].genr.genr_mvar != 0.0)
		{
			Vsrc[i] = polar(nodes[i].vamp,nodes[i].theta);
			Y->add_self(i,1.0);
		}
		else
		{
			Vsrc[i] = Complex(0,0);
			Y->add_self(i,nodes[i].y+nodes[i].B);
		}
	}
	// Node to node currents
	for (auto line: lines)
	{
		int to_idx = node_to_idx[line.to];
		int from_idx = node_to_idx[line.from];
		bool to_genr = nodes[to_idx].genr.genr_mw != 0.0 || nodes[to_idx].genr.genr_mvar != 0.0;
		bool from_genr = nodes[from_idx].genr.genr_mw != 0.0 || nodes[from_idx].genr.genr_mvar != 0.0;
		if (!to_genr)
		{
			if (line.turns == 0.0)
				Y->add_line(to_idx,from_idx,line.y);
			else
				Y->add_line(to_idx,from_idx,line.y,line.turns,0.0,line.tside);
		}
		if (!from_genr)
		{
			if (line.turns == 0.0)
				Y->add_line(from_idx,to_idx,line.y);
			else
				Y->add_line(from_idx,to_idx,line.y,line.turns,0.0,line.tside);
		}
	}
	Y->solve(Vsrc,Vnode);
	// Node initial conditions
	for (auto entry: node_to_idx)
	{
		nodes[entry.second].V = Vnode[entry.second];
		nodes[entry.second].vamp = abs(Vnode[entry.second]);
		nodes[entry.second].theta = arg(Vnode[entry.second]);
		Complex S = Vnode[entry.second]*conj(Vnode[entry.second]*nodes[entry.second].y);
		nodes[entry.second].load_mw = real(S);
		nodes[entry.second].load_mvar = imag(S);
		for (int i = 0; i < 3; i++)
		{
			nodes[entry.second].iload[i] = nodes[entry.second].vload[i] = 0.0;
			nodes[entry.second].v[i] = nodes[entry.second].vamp*cos(phase_angle[i]+nodes[entry.second].theta);
		}
		nodes[entry.second].Iload = nodes[entry.second].Vload = Complex(0,0);
		if (nodes[entry.second].C > 0.0)
		{
			assert(nodes[entry.second].L == 0.0);
			Complex Iload = Vnode[entry.second]*nodes[entry.second].y;
			nodes[entry.second].Vload = Iload*Complex(0.0,imag(nodes[entry.second].Z));
			for (int i = 0; i < 3; i++)
			{
				nodes[entry.second].vload[i] =
					abs(nodes[entry.second].Vload)*cos(arg(nodes[entry.second].Vload)+phase_angle[i]);
			}
		}
		else if (nodes[entry.second].L > 0.0)
		{
			assert(nodes[entry.second].C == 0.0);
			nodes[entry.second].Iload = Vnode[entry.second]*nodes[entry.second].y;
			for (int i = 0; i < 3; i++)
				nodes[entry.second].iload[i] =
					abs(nodes[entry.second].Iload)*cos(arg(nodes[entry.second].Iload)+phase_angle[i]);
		}
	}
	for (unsigned i = 0; i < lines.size(); i++)
	{
		lines[i].number = i;
		Complex va = nodes[node_to_idx[lines[i].from]].V;
		Complex vb = nodes[node_to_idx[lines[i].to]].V;
		lines[i].I = (va-vb)*lines[i].y;
		for (int j = 0; j < 3; j++)
			lines[i].i[j] =
				abs(lines[i].I)*cos(arg(lines[i].I)+phase_angle[j]);
	}
	// Calculate generator initial conditions
	for (auto entry: node_to_idx)
	{
		if (nodes[entry.second].genr.genr_mw == 0.0 && nodes[entry.second].genr.genr_mvar == 0.0)
			continue;
		configureGenerator(nodes[entry.second].genr);
		Complex Igenr = nodes[entry.second].V*(nodes[entry.second].B+nodes[entry.second].y);
		for (auto line: lines)
		{
			if (line.from == nodes[entry.second].ID)
			{
				Igenr += line.I;
			}
			else if (line.to == nodes[entry.second].ID)
			{
				Igenr -= line.I;
			}
		}
		Complex S = Vnode[entry.second]*conj(Igenr);
		nodes[entry.second].genr.S = S;
		nodes[entry.second].genr.genr_mw = real(S);
		nodes[entry.second].genr.genr_mvar = imag(S);
		nodes[entry.second].genr.Pm0 = 0.0;
		nodes[entry.second].genr.Rg = 0.0;
		// Critically damp the generator
		Complex X(nodes[entry.second].genr.Rg,nodes[entry.second].genr.Lg*2.0*pi*freq);
		Complex Vgenr = Igenr*X+nodes[entry.second].V;
		for (int i = 0; i < 3; i++)
		{
			double ig = nodes[entry.second].genr.i[i] = abs(Igenr)*cos(arg(Igenr)+phase_angle[i]);
			double vg = abs(Vgenr)*cos(arg(Vgenr)+phase_angle[i]);
			nodes[entry.second].genr.Pm0 += ig*vg;
		}
		nodes[entry.second].genr.Eamp = abs(Vgenr);
		nodes[entry.second].genr.Etheta = arg(Vgenr);
	}
	delete Y;
	delete [] Vsrc;
	delete [] Vnode;
}