#include <iostream>
#include <Eigen/Dense>
#define M_PI       3.14159265358979323846   // pi

using namespace Eigen;

#define STATE_VARS 3*2					// Сами переменные и их проивзодные
#define PHI 5							// Количество узлов в схеме
#define EDS 2							// Количество источиков напряжения
#define MATRIX_DIM STATE_VARS+PHI+EDS	// Размерность якобиана

const double TK = 1e-3; // время расчета
const double T_Start = 1e-10; // начальный шаг
const double SMN = 1e-15; // минимальный шаг
const double ACR = 1e-2; // точность

// Параметры модели
double TAU = 5e-9;
double MFt = 0.026;

double C1 = 0.1;
double Cb = 2e-12;

double L = 0.001;

double R1 = 1000;
double R2 = 10000;
double Ru = 1e+6;
double Rb = 20;

double It = 1e-12;

double IL_prev = 0;
double UC1_prev = 0;
double UCb_prev = 0;

double E2 = 5.;
// Синусоидальный источник тока
double E1(double t) {
	const double amplitude = 10.0;
	const double period = 1e-4;
	const double frequency = 2 * M_PI / period;
	const double phase = 0.0;
	return amplitude * sin(frequency * t + phase);
}

// Якобиан
MatrixXd Y = MatrixXd::Zero(MATRIX_DIM, MATRIX_DIM);
// Вектор невязок
VectorXd N = VectorXd::Zero(MATRIX_DIM);
// Вектор поправок
VectorXd dX = VectorXd::Zero(MATRIX_DIM);
// Вектор фазовых переменных
VectorXd X = VectorXd::Zero(MATRIX_DIM);
// Копия вектора фазовых переменных (для обработки случая неудачного шага)
VectorXd X_backup = VectorXd::Zero(MATRIX_DIM);

void Jacobian(double dt) {
	Y(0, 0) = 1, Y(0, 3) = -1. / dt;
	Y(1, 1) = 1, Y(1, 4) = -1. / dt;
	Y(2, 2) = 1, Y(2, 5) = -1. / dt;

	Y(3, 3) = 1., Y(3, 0) = -1.;
	Y(4, 4) = 1., Y(4, 8) = -1., Y(4, 9) = 1.;
	Y(5, 2) = L, Y(5, 7) = -1., Y(5, 10) = 1.;

	Y(6, 6) = 1. / R1, Y(6, 7) - 1. / R1, Y(6, 11) = -1.;
	Y(7, 5) = 1., Y(7, 6) = -1. / R1, Y(7, 7) = 1. / R1 + 1. / Rb, Y(7, 8) = -1. / Rb;
	Y(8, 1) = Cb, Y(8, 7) = -1. / Rb, Y(8, 8) = 1. / Rb + 1. / Ru, Y(8, 9) = -1. / Ru;
	Y(9, 1) = -Cb, Y(9, 8) = -1. / Ru, Y(9, 9) = 1. / Ru, Y(9, 11) = 1.;
	Y(10, 0) = C1, Y(10, 5) = -1., Y(10, 10) = 1. / R2;

	Y(11, 6) = -1.;
	Y(12, 9) = -1.;
}

// 0 - 2 производные
// 3 - 5 переменные состояния
// 6 - 10 узлы
// 11 - 12 токи на ЭДС

// Расчёт вектора невязки
void nevyzka(double dt, double cur_time) {
	N(0) = X(0) - (X(3) - UC1_prev) / dt;
	N(1) = X(1) - (X(4) - UCb_prev) / dt;
	N(2) = X(2) - (X(5) - IL_prev) / dt;

	N(3) = X(3) - X(10);
	N(4) = X(4) - (X(8) - X(9));
	N(5) = X(5) * L - (X(7) - X(10));
	
	N(6) = -X(11) + (X(6) - X(7)) / R1;
	N(7) = -(X(6) - X(7)) / R1 + X(5) + (X(7) - X(8)) / Rb;
	N(8) = -(X(7) - X(8)) / Rb + It + (X(8) - X(9)) / Ru + Cb * X(1);
	N(9) = -It - (X(8) - X(9)) / Ru - Cb * X(1) + X(11);
	N(10) = -X(5) + C1 * X(0) + X(10) / R2;

	N(11) = E1(cur_time) - X(6);
	N(12) = E2 - X(9);

	// Перед вектором невязок стоит минус
	N = -1 * N;
}

int main() {
	double dt = T_Start;			// Шаг по времени
	double dt_prev = dt;			// Предыдущий шаг по времени
	double eps_max = ACR;			// Максимальное отклонение
	double eps_min = 1e-10;			// Минимальное отклонение
	int counter = 0;				// Счетчик шагов по времени
	double cur_time = 0;			// Текущее время
	double eps_cur;
	double eps;
	double step_c = 0;

	while (dt <= TK) {
		// Счётчик итераций
		int iteration = 0;
		bool flag = true;
		while (flag) {
			Jacobian(dt);
			nevyzka(dt, cur_time);

			#ifdef _DEBUG
			std::cout << Y << std::endl << N;
			#endif
			
			dX = Y.fullPivLu().solve(N);
			#ifdef _DEBUG
			std::cout << '\n' << dX;
			#endif

			X += dX;
			iteration++;
			double maximum = X.maxCoeff();
			if (maximum <= ACR) {
				flag = false;
			}
			else if (iteration == 7 && flag) {
				iteration = 0;
				dt /= 2.;
				if (dt < SMN) {
					std::cerr << "Sorry, but i can not solve this" << std::endl;
					exit(-1);
				}
				X = X_backup;
			}
		}
		// TODO: реализовать вычисление уточнённое решения, сравнить его с вычисленным, 
		// возможно изменить шаг моделирования
		
	}
	// Для примера
	/*Jacobian(0.1);
	nevyzka(0.1, 0);
	std::cout << Y << std::endl << N;*/
}