#include <fstream>
#include <math.h>
#include <string>
#include <vector>
#define _USE_MATH_DEFINES

double degree_to_rad(double phi) {
    return phi * M_PI / 180;
}

class args {
public:
    double dt = 1e-5;
    double yp0 = 0;
    double tau;
    double g = 9.81;
    args(double l) {
        tau = sqrt(2 * l / (3 * g));
    }
};

//function for solving an one dimensional differential equation
std::vector<std::vector<double>> differentialSolve(double f(double, args), double t0, double tmax, double y0, args Args) {
    int stepNumber = (int)(tmax - t0) / Args.dt;
    double h = Args.dt;
    std::vector<std::vector<double>> points; //vector of three elementric vectors. (t,phi,dphi/dt)
    points.push_back({t0, y0, Args.yp0});
    if (t0 < tmax) {
        for (int i = 0; i < stepNumber; i++) {
            double newT = points[i][0] + h;
            double newDPhi = points[i][2] + h * f(points[i][1], Args);
            double newPhi = points[i][1] + h * newDPhi;
            //for greater angles it doesn't make sense
            if (newPhi >= M_PI / 2) {
                break;
            }
            points.push_back({newT, newPhi, newDPhi});
        }
    }
    return points;
}

//function for calculating the fall time for a given phi0
double fallTime(double f(double, args), double t0, double tMax, double phi0, args Args) {
    std::vector<std::vector<double>> angles = differentialSolve(f, t0, tMax, phi0, Args);
    return angles[angles.size() - 1][0] - t0;
}

//the second derivitative of phi based on phi
double phiPP(double phi, args Args) {
    return sin(phi) / pow(Args.tau, 2);
}

//function for writing a string to a csv file
int writeTextToCsv(std::string text, std::string filename) {
    std::ofstream fileStream;
    fileStream.open(filename + ".csv");
    fileStream << text;
    fileStream.close();
    return 0;
}

int main() {
    int t0 = 0;
    int tMax = 100;
    args Args(1.45);
    std::vector<std::vector<double>> angles = differentialSolve(phiPP, t0, tMax, 0.25, Args);
    std::string outputString = "t\tphi\n";
    for (int i = 0; i < angles.size(); i++) {
        outputString += std::to_string(angles[i][0]) + "\t";
        outputString += std::to_string(angles[i][1]) + "\n";
    }
    writeTextToCsv(outputString, "Winkel_in_Abhaengigkeit_der_Zeit_bei_phi0_0_25");

    std::vector<std::vector<double>> fallTimePhi0;
    for (double phi0 = 0.001; phi0 < M_PI / 2; phi0 += 0.001) {
        fallTimePhi0.push_back({phi0, fallTime(phiPP, t0, tMax, phi0, Args)});
    }
    std::string outputStringPhi0 = "phi0\tt_fall\n";
    for (int i = 0; i < fallTimePhi0.size(); i++) {
        outputStringPhi0 += std::to_string(fallTimePhi0[i][0]) + "\t";
        outputStringPhi0 += std::to_string(fallTimePhi0[i][1]) + "\n";
    }
    writeTextToCsv(outputStringPhi0, "Fallzeit_in_Abhaengigkeit_des_Startwinkels");

    std::vector<std::vector<double>> fallTimeDeltaT;
    for (Args.dt = 1; Args.dt > 1e-6; Args.dt *= 0.5) {
        fallTimeDeltaT.push_back({Args.dt, fallTime(phiPP, t0, tMax, 0.25, Args)});
    }
    std::string outputStringDeltaT = "delta_t\tt_fall\n";
    for (int i = 0; i < fallTimeDeltaT.size(); i++) {
        outputStringDeltaT += std::to_string(fallTimeDeltaT[i][0]) + "\t";
        outputStringDeltaT += std::to_string(fallTimeDeltaT[i][1]) + "\n";
    }
    writeTextToCsv(outputStringDeltaT, "Fallzeit_in_Abhaengigkeit_der_Laenge_der_Zeitschritte");

    //now the part for the comparison of the numerical solution and the measured data

    args ArgsB1(1.81);                                                                                               //the length of the broom
    double phi01[] = {degree_to_rad(5), degree_to_rad(10), degree_to_rad(20), degree_to_rad(35), degree_to_rad(45)}; //the initial angles
    std::vector<std::vector<double>> fallTimePhi01;
    for (int i = 0; i < sizeof(phi01) / sizeof(phi01[0]); i++) {
        fallTimePhi01.push_back({phi01[i], fallTime(phiPP, t0, tMax, phi01[i], ArgsB1)});
    }
    std::string outputStringPhi01 = "phi0\tt_fall\n";
    for (int i = 0; i < fallTimePhi01.size(); i++) {
        outputStringPhi01 += std::to_string(fallTimePhi01[i][0]) + "\t";
        outputStringPhi01 += std::to_string(fallTimePhi01[i][1]) + "\n";
    }
    writeTextToCsv(outputStringPhi01, "Fallzeit_in_Abhaengigkeit_des_Startwinkels_zu_Besen1_81");

    args ArgsB2(1.358);                                                                                                                                     //the length of the broom
    double phi02[] = {degree_to_rad(10), degree_to_rad(20), degree_to_rad(25), degree_to_rad(35), degree_to_rad(45), degree_to_rad(55), degree_to_rad(65)}; //the initial angles
    std::vector<std::vector<double>> fallTimePhi02;
    for (int i = 0; i < sizeof(phi02) / sizeof(phi02[0]); i++) {
        fallTimePhi02.push_back({phi02[i], fallTime(phiPP, t0, tMax, phi02[i], ArgsB2)});
    }
    std::string outputStringPhi02 = "phi0\tt_fall\n";
    for (int i = 0; i < fallTimePhi02.size(); i++) {
        outputStringPhi02 += std::to_string(fallTimePhi02[i][0]) + "\t";
        outputStringPhi02 += std::to_string(fallTimePhi02[i][1]) + "\n";
    }
    writeTextToCsv(outputStringPhi02, "Fallzeit_in_Abhaengigkeit_des_Startwinkels_zu_Besen1_358");

    return 0;
}