import math
import matplotlib.pyplot as plt


#initial condition
mass = 5.125 #Oz
circumference = 9.125 #inches
x0  = 0 #ft
y0 = 2.000 #ft
z0 = 3 #ft
exit_speed  = 100 #mph
launch_angle = 30 #degree
direction  = 0 #degree
sign = 1
backspin= -763+120*launch_angle+21*direction*sign#rpm
sidespin = -sign*849-94*direction #rpm
wg = 0 #rpm
tau = 25 #sec
dt = 0.01
pi = 3.1415


Temp_F = 78 #F
elev_ft = 0 #ft
vwind = 0 #mph
phiwind  = 0 #degree
hwind = 0 #mph
relative_humidity = 50 #%
barometric_pressure = 29.92 #Hg

Temp_C = (5/9)*(Temp_F-32)
elev_m = elev_ft / 3.2808

vxw = vwind*1.467*math.sin(phiwind*math.pi/180) #ft/s
vyw = vwind*1.467*math.cos(phiwind*math.pi/180) #ft/s

beta = 0.0001217 #1/m
SVP = 4.5841*math.exp((18.687-Temp_C /234.5)*Temp_C/(257.14+Temp_C ))
barometric_pressure_mmHg = barometric_pressure*1000/39.37
rho_kg_m3 =  1.2929*(273/(Temp_C+273)*(barometric_pressure_mmHg*math.exp(-beta*elev_ft)-0.3783*relative_humidity*SVP/100)/760) #kg/m^3
rho_lb_ft3 = rho_kg_m3*0.06261 #lb/ft^3
# const
c0 = 0.07182*rho_lb_ft3*(5.125/mass)*(circumference/9.125)**2

v0 = 1.467*exit_speed
v0x = v0*math.cos(launch_angle*math.pi/180)*math.sin(direction*math.pi/180)
v0y = v0*math.cos(launch_angle*math.pi/180)*math.cos(direction*math.pi/180)
v0z = v0*math.sin(launch_angle*math.pi/180)
wx = (backspin*math.cos(direction*math.pi/180)-
      sidespin*math.sin(launch_angle*math.pi/180)*math.sin(direction*math.pi/180)+wg*v0x/v0)*math.pi/30
wy = (-backspin*math.sin(direction*math.pi/180)-
      sidespin*math.sin(launch_angle*math.pi/180)*math.cos(direction*math.pi/180)+wg*v0y/v0)*math.pi/30
wz = (sidespin*math.cos(launch_angle*math.pi/180)+wg*v0z/v0)*math.pi/30
omega = math.sqrt(wx*wx+wy*wy+wz*wz)
romega = (circumference/2/math.pi)*omega/12





def accelaration(t,vx,vy,vz,z):

    v = math.sqrt(vx*vx+vy*vy+vz*vz)
    if(z > hwind):
        vw = math.sqrt((vx-vxw)**2 + (vy-vyw)**2+vz**2)
        vyw_sim = vyw
    else:
        vw = v
        vyw_sim = 0
    S = (romega/vw)*math.exp(-t/(tau*146.7/v))
    Cd = 0.4105*(1+0.2017*S*S)
    Cl = 1/(2.32+0.4/S)
    vxw_sim = 0
    adragx = -c0*Cd*vw*(vx-vxw)
    adragy = -c0*Cd*vw*(vy-vyw_sim)
    adragz = -c0*Cd*vw*vz
    w = omega*math.exp(-t/tau)*30/math.pi
    aMagx = c0*(Cl/omega)*vw*(wy*vz-wz*(vy-vyw))
    aMagy = c0*(Cl/omega)*vw*(wz*(vx-vw)-wx*vz)
    aMagz = c0*(Cl/omega)*vw*(wx*(vy-vyw)-wy*(vx-vxw))
    ax = adragx+aMagx
    ay = adragy+aMagy
    az = adragz+aMagz-32.174


    return ax,ay,az

def RK4(x,y,z,vx,vy,vz,t):
    k1x,k1y,k1z = tuple([dt*i for i in accelaration(t,vx,vy,vz,z)]) #dt * f(x,t)
    k2x,k2y,k2z = tuple([dt*i for i in accelaration(t+dt/2,vx+k1x/2,vy+k1y/2,vz+k1z/2,z)])
    k3x,k3y,k3z = tuple([dt*i for i in accelaration(t+dt/2,vx+k2x/2,vy+k2y/2,vz+k2z/2,z)])
    k4x,k4y,k4z = tuple([dt*i for i in accelaration(t+dt,vx+k3x,vy+k3y,vz+k3z,z)])
    print('  ')
    print(k1x)
    print(k2x)

    print('  ')

    vx += (1/6)*(k1x+2*k2x+2*k3x+k4x)
    vy += (1/6)*(k1y+2*k2y+2*k3y+k4y)
    vz += (1/6)*(k1z+2*k2z+2*k3z+k4z)

    x += vx * dt
    y += vy * dt
    z += vz * dt

    return x,y,z,vx,vy,vz


print(v0x)
print(v0y)
print(v0z)
print('bruh')
x_path_RK4 = []
y_path_RK4 = []
z_path_RK4 = []

x = 0
y = y0
z = z0

vx = v0x
vy = v0y
vz = v0z
for t in range(1000):
    x_path_RK4.append(x)
    y_path_RK4.append(y)
    z_path_RK4.append(z)

    t = dt*t

    x,y,z,vx,vy,vz = RK4(x,y,z,vx,vy,vz,t)

    if(z <0):

        print('Hang time = %1.2f s Distance = %1.1f feet' % (t, y))


        break


        My Translation Effort Below:


#include <iostream>
#include <cmath>
#include <iomanip>
#include <vector>
#include <math.h>

using namespace std;

///Use structs possibly to not have to declare all these variables globally

vector<double> acceleration(double, double, double, double, double);

double distance_final(double, double, double, double, double, double, double);

    ///These are variables that change with the user input. User will be able to change exit speed, launch angle, etc.
    ///Because it would take so long to go through every single input from the user, some were put in the "advanced section", that the user can elect to edit or skip over, leaving the results blank.


//initial condition
double mass = 5.125; //Oz
double circumference = 9.125; //inches
double x0  = 0; //ft
double y0 = 2.000; //ft
double z0 = 3; //ft
double exit_speed  = 100; //mph
double launch_angle = 30; //degree
double direction  = 0; //degree
double sign = 1;
double backspin = -763 + 120*launch_angle + 21*direction * sign;//rpm
double sidespin = (-1*sign)*849-94*direction; //rpm
double wg = 0;//rpm
double tau = 25; //sec
double dt = 0.01;
double pi = 3.1415;
double e = 2.7182818;


double Temp_F = 78; //F
double elev_ft = 0; //ft
double vwind = 0; //mph
double phiwind  = 0; //degree
double hwind = 0; //mph
double relative_humidity = 50; //%
double barometric_pressure = 29.92; //Hg

double Temp_C = (5/9)*(Temp_F-32);
double elev_m = elev_ft / 3.2808;

double vxw = vwind*1.467*sin(phiwind*pi/180); //ft/s  wind speed in x direction
double vyw = vwind*1.467*cos(phiwind*pi/180); //ft/s  wind speed in y direction

double beta = 0.0001217; //1/m
double SVP = 4.5841*(pow(e,((18.687-Temp_C /234.5)*Temp_C/(257.14+Temp_C )))); //barometric calculations incorporated into distance and involving temperature
double barometric_pressure_mmHg = barometric_pressure*1000/39.37;
double rho_kg_m3 =  1.2929*(273/(Temp_C+273)*(barometric_pressure_mmHg*(pow(e,(-beta*elev_ft)))-0.3783*relative_humidity*SVP/100)/760); //kg/m^3
double rho_lb_ft3 = rho_kg_m3*0.06261; //lb/ft^3
// constant value, non editable variables
double c0 = 0.07182*rho_lb_ft3*(5.125/mass)*(pow(2, (circumference/9.125)));

double v0 = 1.467*exit_speed;
double v0x = v0*cos(launch_angle*pi/180)*sin(direction*pi/180);
double v0y = v0*cos(launch_angle*pi/180)*cos(direction*pi/180);
double v0z = v0*sin(launch_angle*pi/180);
double wx = (backspin*cos(direction*pi/180)-
             sidespin*sin(launch_angle*pi/180)*sin(direction*pi/180)+wg*v0x/v0)*pi/30;
double wy = (-backspin*sin(direction*pi/180)-
             sidespin*sin(launch_angle*pi/180)*cos(direction*pi/180)+wg*v0y/v0)*pi/30;
double wz = (sidespin*cos(launch_angle*pi/180)+wg*v0z/v0)*pi/30;
double omega = sqrt(wx*wx+wy*wy+wz*wz);
double romega = (circumference/2/pi)*omega/12;



    ///All the constants/variables declared above...

int main()
{
    int userNum, input;
    cout << "Welcome to my distance calculator! Would you like to go through all advanced settings(1) or the simplified settings(2)?" << endl;
    cin >> userNum;
    if (userNum == 1) ///Advanced settings
    {
        cout << "Enter exit speed in mph: ";
        cin >> input;
        exit_speed = input;
        cout << "Enter launch angle in degrees: ";
        cin >> input;
        launch_angle = input;
        cout << "Enter horizontal degrees, where zero degrees is the first base line and 90 is the third base line: ";
        cin >> input;
        direction = input;
        cout << "Enter temperature in Fahrenheit: ";
        cin >> input;
        Temp_F = input;
        cout << "Enter elevation above sea level: ";
        cin >> input;
        elev_ft = input;
        cout << "Enter wind speed: ";
        cin >> input;
        vwind = input;
        cout << "Enter wind direction in degrees: ";
        cin >> input;
        phiwind = input;
        cout << "Enter humidity: ";
        cin >> input;
        relative_humidity = input;
        cout << "Enter wind velocity coming in: ";
        cin >> input;
        hwind = input;

    }
    else ///Simplified settings
    {
        cout << "Enter exit speed in mph: ";
        cin >> input;
        exit_speed = input;
        cout << "Enter launch angle in degrees: ";
        cin >> input;
        launch_angle = input;
        cout << "Enter horizontal degrees, where zero degrees is the first base line and 90 is the third base line: ";
        cin >> input;
        direction = input;
    }

    exit_speed = 100; //mph
    launch_angle = 30; //degree
    direction  = 0; //degree
    sign = 1;
    Temp_F = 78; //F
    elev_ft = 0; //ft
    vwind = 0; //mph
    phiwind  = 0; //degree
    hwind = 0; //mph
    relative_humidity = 50; //%


    vector<int> x_path_RK4; ///Making the path in the x, y, and z coordinates.
    vector<int> y_path_RK4;
    vector<int> z_path_RK4;

    double x = 0;
    double y = y0;
    double z = z0;

    double vx = v0x;
    double vy = v0y;
    double vz = v0z;


    for (int t = 1; t < 1000; t++)
    {

        x_path_RK4.push_back(x);
        y_path_RK4.push_back(y);
        z_path_RK4.push_back(z);



        distance_final(x,y,z,vx,vy,vz,t);
        cout << " & " << z << " & " << endl;
        if(z <= 0)
        {
            t = dt * t;
            cout << "Hang time: " << t << endl;
            cout << "Distance Traveled: " << y << endl;
            break;
        }
    }

    ///Sfml code down here...
    ///Use vector math using horizontal angle and magnitude(distance) of ball vector find the coordinate points in the x and y direction


}

vector<double> acceleration(double t, double vx, double vy, double vz, double z)
{
    double v = sqrt(vx*vx+vy*vy+vz*vz);
    double vw, vyw_sim;
    if(z > hwind)
    {
        vw = sqrt((pow(2, (vx-vxw))) + (pow(2, (vy-vyw))) + (pow(2, vz)));
        vyw_sim = vyw;
    }
    else
    {
        vw = v;
        vyw_sim = 0;
    }

    double S = (romega/vw)*(pow(2, (-t/(tau*146.7/v))));
    double Cd = 0.4105*(1+0.2017*S*S);
    double Cl = 1/(2.32+0.4/S);
    double vxw_sim = 0;
    double adragx = -c0*Cd*vw*(vx-vxw);
    double adragy = -c0*Cd*vw*(vy-vyw_sim);
    double adragz = -c0*Cd*vw*vz;
    double w = omega*(pow(e, (-t/tau)))*30/pi;
    double aMagx = c0*(Cl/omega)*vw*(wy*vz-wz*(vy-vyw));
    double aMagy = c0*(Cl/omega)*vw*(wz*(vx-vw)-wx*vz);
    double aMagz = c0*(Cl/omega)*vw*(wx*(vy-vyw)-wy*(vx-vxw));
    double ax = adragx+aMagx;
    double ay = adragy+aMagy;
    double az = adragz+aMagz-32.174;


    vector<double> myArray;
    myArray.push_back(ax);
    myArray.push_back(ay);
    myArray.push_back(az);

    return myArray;

}


double distance_final(double x, double y, double z, double vx, double vy, double vz, double t)
{
    ///Final distances are calculated here in this function using all previous variable
    vector<double> result = acceleration(t,vx,vy,vz,z); ///Result of the acceleration function, gets three values from the acceleration function and puts them into this vector
    double a1x = result[0];
    double a1y = result[1];
    double a1z = result[2];
    double k1x = dt*a1x;
    double k1y = dt*a1y;
    double k1z = dt*a1z;


    vector<double> result2 = acceleration(t+dt/2,vx+k1x/2,vy+k1y/2,vz+k1z/2,z);
    double a2x = result2[0];
    double a2y = result2[1];
    double a2z = result2[2];
    double k2x = dt*a2x;
    double k2y = dt*a2y;
    double k2z = dt*a2z;

    vector<double> result3 = acceleration(t+dt/2,vx+k2x/2,vy+k2y/2,vz+k2z/2,z);
    double a3x = result3[0];
    double a3y = result3[1];
    double a3z = result3[2];
    double k3x = dt*a3x;
    double k3y = dt*a3y;
    double k3z = dt*a3z;

    vector<double> result4 = acceleration(t+dt,vx+k3x,vy+k3y,vz+k3z,z);
    double a4x = result4[0];
    double a4y = result4[1];
    double a4z = result4[2];
    double k4x = dt*a4x;
    double k4y = dt*a4y;
    double k4z = dt*a4z;




    vx += (1/6)*(k1x+2*k2x+2*k3x+k4x);
    vy += (1/6)*(k1y+2*k2y+2*k3y+k4y);
    vz += (1/6)*(k1z+2*k2z+2*k3z+k4z);

    x += vx * dt;
    y += vy * dt;///total distance calculated
    z += vz * dt;

    return x,y,z,vx,vy,vz;

}
````