// Das Doppelpendel - Erstellung des Poincare-Schnitts bei fester Gesamtenergie
/* Beispiel eines schwingenden Systems aus dem Bereich der Mechanik
 * welches bei gewissen Parameterkonstellationen deterministisch chaotische Bewegungen zeigt
 * Berechnung der Loesung eines Systems von Differentialgleichungen
 * mittels Runge-Kutta Verfahren 4. Ordnung
 * Verfahren zur Berechnung des Poincare-Schnitts ist in einer Klasse ausgelagert
 * Konstruktor: Poincare(Endzeit t_end, Anzahl der Punkte N, Laenge Pendel1, Laenge Pendel2, Gesamtenergie E0, Gittergroesse grid_size)
 * Parallelisierung mittels OpenMP
 * Ausgabe zum Plotten mittels Python Skript plot_poincare.py
 */
#include <iostream>
#include <cmath>
#include <vector>
#include <omp.h>
using namespace std;

// Klasse zum Berechnen des Doppelpendel-Poincare-Schnitts bei fester Gesamtenergie
class Poincare {
    // Private Instanzvariablen (Daten-Member) der Klasse
    double t_end = 3000;                                                   // Obergrenze des t-Intervalls [0,t_end]
    unsigned N = 500000;                                                   // Anzahl der Punkte in die das t-Intervall aufgeteilt wird
    double l1 = 1.0, l2 = 1.0;                                             // Längen der beiden Pendel
    double E0 = 10.0;                                                      // Gesamtenergie
    unsigned grid_size = 5;                                                // Gittergroesse zur Erzeugung der Anfangsbedingungen (maximal grid_size*grid_size Werte)

     // Definition der Bewegungsgleichung des Doppelpendel als private Member-Funktion der Klasse
    void dgls(double t, const vector<double>& u, double l1, double l2, vector<double>& du_dt) {
        double g = 9.81;// Erdbeschleunigung
        double m1 = 1.0;// Masse Pendel 1
        double m2 = 1.0;// Masse Pendel 2

        du_dt[0] = u[2];
        du_dt[1] = u[3];

        double delta = u[0] - u[1];
        double cos_d = cos(delta);
        double sin_d = sin(delta);
        double sin2_d = sin(2.0 * u[0] - 2.0 * u[1]);
        double den1 = l1 * (-m1 + m2 * cos_d * cos_d - m2);
        double den2 = 2.0 * l2 * (m1 - m2 * cos_d * cos_d + m2);

        // eigentliche Bewegungsgleichung des Doppelpendels
        du_dt[2] = (g*m1*sin(u[0]) + g*m2*sin(u[0])/2.0 + g*m2*sin(u[0] - 2.0*u[1])/2.0 +
                    l1*m2*u[2]*u[2]*sin2_d/2.0 + l2*m2*u[3]*u[3]*sin_d) / den1;
        du_dt[3] = (-g*m1*sin(u[1]) + g*m1*sin(2.0*u[0] - u[1]) - g*m2*sin(u[1]) +
                    g*m2*sin(2.0*u[0] - u[1]) + 2.0*l1*m1*u[2]*u[2]*sin_d +
                    2.0*l1*m2*u[2]*u[2]*sin_d + l2*m2*u[3]*u[3]*sin2_d) / den2;
    }

    // private Member-Funktion: Winkel in den Bereich [-pi, pi] verschieben
    double wrap_angle(double angle) {
        double wrapped = fmod(angle + M_PI, 2.0 * M_PI);
        if (wrapped < 0) wrapped += 2.0 * M_PI;
        return wrapped - M_PI;
    }

    // private Hilfsfunktion zur Generierung der Anfangsbedingungen
    vector<double> calc_d_theta2(double theta1, double theta2, double d_theta1, double E0, double l1, double l2) {
        double g = 9.81; double m1 = 1.0; double m2 = 1.0;
        double under_sqrt = E0 - 0.5*pow(d_theta1, 2)*pow(l1, 2)*m1 + 0.5*pow(d_theta1, 2)*pow(l1, 2)*m2*pow(cos(theta1 - theta2), 2) - 0.5*pow(d_theta1, 2)*pow(l1, 2)*m2 + g*l1*m1*cos(theta1) - g*l1*m1 + g*l1*m2*cos(theta1) - g*l1*m2 + g*l2*m2*cos(theta2) - g*l2*m2;
        if (under_sqrt >= 0) {
            return {- d_theta1*l1*cos(theta1 - theta2)/l2 + sqrt(2.0)*sqrt(under_sqrt)/(l2*sqrt(m2)) , - d_theta1*l1*cos(theta1 - theta2)/l2 - sqrt(2.0)*sqrt(under_sqrt)/(l2*sqrt(m2)) };
        }
        return {0.0, 0.0};
    }

public:
    // Konstruktor mit sechs Argumenten (Initialisierung der Parameter)
    Poincare(double t_end_, unsigned N_, double l1_, double l2_, double E0_, unsigned grid_size_) : t_end(t_end_),N(N_),l1(l1_),l2(l2_),E0(E0_),grid_size(grid_size_) {}

    // öffentliche Struktur zum Speichern eines Poincare-Punktes
    struct PoincarePoint {
        double t_cross;     // Zeitpunkt des Schnitts
        double theta1;      // Winkel theta1
        double theta2;      // Winkel theta2 (hier immer 0)
        double d_theta1;    // Winkelgeschwindigkeit dtheta1
        double d_theta2;    // Winkelgeschwindigkeit dtheta2
    };

    // öffentliche Member-Funktion: Generierung der Anfangsbedingungen
    vector<vector<double>> Erzeug_Anfangsb() {
        vector<vector<double>> Anfangsb;
        double d_theta2_lim = calc_d_theta2(0.0,0.0,0.0,E0,l1,l2)[0];
        double theta1_lim = M_PI;
        for (int i = 0; i < 300; ++i) {
            double theta1 = i * M_PI/300.0;
            if (calc_d_theta2(theta1,0.0,0.0,E0,l1,l2)[0] != 0.0) { theta1_lim = theta1; }
        }
        for (unsigned i = 0; i < grid_size; ++i) {
            double theta1 = - theta1_lim + i * theta1_lim/grid_size*2.0;
            for (unsigned j = 0; j < grid_size; ++j) {
                double d_theta1 = - d_theta2_lim + j * d_theta2_lim/grid_size*2.0;
                vector<double> d_theta2 = calc_d_theta2(theta1,0.0,d_theta1,E0,l1,l2);
                if (d_theta2[0] != 0.0) {
                    Anfangsb.push_back({theta1, 0.0, d_theta1, d_theta2[0]});
                    Anfangsb.push_back({theta1, 0.0, d_theta1, d_theta2[1]});
                }
            }
        }
        return Anfangsb;
    }

    // öffentliche Member-Funktion: Berechnung eines Poincare-Schnitts bei gegebener Anfangsbedingung
    vector<PoincarePoint> compute_poincare(vector<double> alpha) {
        double h = t_end / N;

        // Lokale Vektoren für das Runge-Kutta Verfahren 4. Ordnung
        vector<double> k1(4), k2(4), k3(4), k4(4);
        vector<double> y(alpha);
        vector<double> y_next(4);
        vector<double> y_tmp(4);

        vector<PoincarePoint> pschnitt; // Zum Speichern der Punkte des Poincare-Schnitts

        double t = 0;
        // Runge-Kutta Verfahren 4. Ordnung
        for (unsigned int i = 0; i < N; ++i) {
            double t_next = t + h;

            dgls(t, y, l1, l2, k1);
            for (int j = 0; j < 4; ++j) k1[j] *= h;

            for (int j = 0; j < 4; ++j) y_tmp[j] = y[j] + k1[j] * 0.5;
            dgls(t + h * 0.5, y_tmp, l1, l2, k2);
            for (int j = 0; j < 4; ++j) k2[j] *= h;

            for (int j = 0; j < 4; ++j) y_tmp[j] = y[j] + k2[j] * 0.5;
            dgls(t + h * 0.5, y_tmp, l1, l2, k3);
            for (int j = 0; j < 4; ++j) k3[j] *= h;

            for (int j = 0; j < 4; ++j) y_tmp[j] = y[j] + k3[j];
            dgls(t_next, y_tmp, l1, l2, k4);
            for (int j = 0; j < 4; ++j) k4[j] *= h;

            for (int j = 0; j < 4; ++j) {
                y_next[j] = y[j] + (k1[j] + 2.0 * k2[j] + 2.0 * k3[j] + k4[j]) / 6.0;
            }

            // Poincaré-Schnittprüfung
            double theta2_w = wrap_angle(y[1]);
            double theta2_wd = wrap_angle(y_next[1]);

            if (theta2_w * theta2_wd < 0 &&
                l2 * y[3] + l1 * y[2] * cos(y[0]) > 0 &&
                abs(theta2_w) < 1.0) {

                double t_cross = t - theta2_w * (h) / (theta2_wd - theta2_w);
                double theta1_cross = y[0] + (y_next[0] - y[0]) * (t_cross - t) / h;
                double d_theta1_cross = y[2] + (y_next[2] - y[2]) * (t_cross - t) / h;
                double d_theta2_cross = y[3] + (y_next[3] - y[3]) * (t_cross - t) / h;

                pschnitt.push_back({t_cross, wrap_angle(theta1_cross), 0.0, d_theta1_cross, d_theta2_cross});
            }

            y = y_next;
            t = t_next;
        }
        return pschnitt;
    }                        // Ende der Member-Funktion: Berechnung eines Poincare-Schnitts
};                           // Ende der Klasse

// Hauptfunktion
int main(){
    int startTime = time(NULL);    // Starten der Zeitmessung
    double E0 = 15.0;              // Deklaration und Initialisierung der Gesamtenergie
    double l1 = 1.0, l2 = 1.0;     // Parameterwerte des Doppelpendels
    double t_end = 1000;           // Obergrenze des t-Intervalls [0,t_end]
    unsigned  Anz_Punkte = 500000; // Anzahl der Punkte in die das t-Intervall aufgeteilt wird
    unsigned grid_size = 15;        // Gittergroesse zur Erzeugung der Anfangsbedingungen (maximal grid_size*grid_size Werte)

    Poincare DPendel {t_end, Anz_Punkte, l1, l2, E0, grid_size};            // Aufruf des Konstruktors der Klasse Poincare
    vector<vector<double>> Anfangsb = DPendel.Erzeug_Anfangsb();            // Vektor-Matrix zum speichern der unterschiedlichen Anfangsbedingungen
    vector<vector<Poincare::PoincarePoint>> pschnitt_wert(Anfangsb.size()); // Vektor-Matrix zum speichern der Loesungen der Punkte des Poincare-Schnitts

    // for-Schleife ueber unterschiedliche Anfangsbedingungen (mit OpenMP-Parallelisierung)
    #pragma omp parallel for
    for (size_t n = 0; n < Anfangsb.size(); ++n) {
        pschnitt_wert[n] = DPendel.compute_poincare(Anfangsb[n]); // Fuellen der Vektor-Matrix mit dem Poincare-Schnitt der n-ten Anfangsbedingung
    }

    // Ausgabe in die Datei
    FILE *ausgabe = fopen("Doppelpendel_pschnitt.dat", "w+");
    fprintf(ausgabe,"# 0: Anfbed i \n# 1: t-Wert \n# 2: theta_1 \n# 3: theta_2 \n# 4: d_theta_1 \n# 5: d_theta_2 \n");
    for(size_t n=0; n < Anfangsb.size(); ++n){
        for(const auto& p : pschnitt_wert[n]){
            fprintf(ausgabe,"%3ld %19.15f %19.15f %19.15f %19.15f %19.15f \n", n, p.t_cross, p.theta1, p.theta2, p.d_theta1, p.d_theta2);
        }
    }
    cout << "Das Programm benoetigte: " << time(NULL) - startTime << " Sekunden." << endl;
    return 0;
}