/* a program to calculate the range and altitude of a 3" sighter rocket
written by Paul Wilson
School of EESE, Queensland University of Technology, Gardens Point, Brisbane
email pa.wilson@qut.edu.au

This program can be freely used on the following conditions:
Any work of any kind, including but not limited to:
    derived software, projects, reports, papers, books, computer publication,
    which uses part or all, or any results derived from this program must
    acknowledge the author, Paul Wilson.
*/
/*  Set the parameters of the rocket by altering the #define's and recompiling
    the program - crude, I know, but it works.
    "It may not be great to look at, but its got it where it counts, kid"
                       Han Solo, describing the Millenium Falcon
*/

#include <stdio.h>
#include <math.h>

#define interval1 0.05       /* sampling interval of burn phase (sec) */
#define interval2 0.05       /* sampling interval of ballistic phase (sec) */
#define QE 70                       /* quadrant elevation of launcher */
#define cross_sect_area 0.005352    /* cross sectional area of nosecone */
#define payload 5.0             /* mass of payload Kgm  */
#define tube_mass 7.55          /* mass of tube in Kgm */
#define nosecone 1            /* 0 for flat plug, 1 for aerodynamic
                              nose (2:1 aspect ratio, conical) */

float thrust_map[36];   /* one sample for every 0.05 sec over 1.8 sec  */
float drag_map[45];
float mp_map[36];
float sonic_velocity[18];
float atm_density[18];
float Z;                /* altitude */
float Mach_no,Cd;

/***************** drag profiles ********************/
float Cd_for_shaped_plug(float Mach_no)
{
    float temp;

    if (Mach_no > 0 && Mach_no <= 0.2)
        {  temp = 0.13;  }
    if (Mach_no > 0.2 && Mach_no <= 0.4)
        {  temp = 0.32;  }
    if (Mach_no > 0.4 && Mach_no <= 0.5)
        {  temp = 0.37;   }
    if (Mach_no > 0.5 && Mach_no <= 0.9)
	{  temp = 0.415;  }			/* 0.415 */
    if (Mach_no > 0.9 && Mach_no <= 1.05)
        {  temp = 0.487;  }
    if (Mach_no > 1.05 && Mach_no <= 1.2)
        {  temp = 0.658;  }
    if (Mach_no > 1.2 && Mach_no <= 1.5)
        {  temp = 0.755;  }
    if (Mach_no > 1.5)
        {  temp = 0.653;  }
  return(temp);
}


/*************************************************************************/
float Cd_for_flat_plug(float Mach_no)
{
    float temp;

    if (Mach_no > 0 && Mach_no <= 0.2)
        {  temp = 0.35;  }
    if (Mach_no > 0.2 && Mach_no <= 0.4)
        {  temp = 0.85;  }
    if (Mach_no > 0.4 && Mach_no <= 0.5)
        {  temp = 1.006;   }
    if (Mach_no > 0.5 && Mach_no <= 0.9)
        {  temp = 1.162;  }
    if (Mach_no > 0.9 && Mach_no <= 1.05)
        {  temp = 1.335;  }
    if (Mach_no > 1.05 && Mach_no <= 1.2)
        {  temp = 1.633;  }
    if (Mach_no > 1.2 && Mach_no <= 1.5)
        {  temp = 1.581;  }
    if (Mach_no > 1.5)
        {  temp = 1.697;  }
  return(temp);
}
/*************************************************************************/
float Cd_for_flat_plugX(float Mach_no)
{
    float temp;

    if(Mach_no <= 0.2)   /* 0 to 0.7 */
        {
          temp = Mach_no*7.0/2.0;
        }
    if((Mach_no>0.2)&&(Mach_no<=0.4))  /* 0.7 to 1.0 */
        {
            temp = ((Mach_no-0.2)*1.5)+0.7;
        }
    if((Mach_no>0.4)&&(Mach_no<=0.7))  /* 1.0 to 1.162 */
        {
          temp = 1.0+((Mach_no-0.4)*0.162/0.3);
        }
    if((Mach_no>0.7)&&(Mach_no<=0.975))  /* 1.162 to 1.335 */
        {
	  temp = 1.162+((Mach_no-0.7)*0.173/0.275);
        }
    if((Mach_no>0.975)&&(Mach_no<=1.125))    /* 1.335 to 1.7 */
        {
          temp = 1.335+((Mach_no-0.975)*0.365/0.15);
        }
    if((Mach_no > 1.125)&&(Mach_no <= 1.275))    /* 1.7 to 1.581 */
    {
          temp = 1.581 + ((1.275-Mach_no)*0.119/0.05);
    }
    if((Mach_no > 1.275)&&(Mach_no <= 1.6))    /* 1.581 to 1.697 */
    {
          temp = 1.581 + ((Mach_no-1.275)*0.116/0.325);
    }
    if(Mach_no > 1.6) { temp = 1.697; }
    return(temp);
}


float drag(float V)
{
    float result,CSA,rho,v_sound;
    int i;

    CSA = cross_sect_area;      /* get cross sectional area of rocket */
    i = (int)(Z/500.0);         /* calculate rho */
    rho = atm_density[i];
    v_sound = sonic_velocity[i];    /* calculate mach number */
    Mach_no = V/v_sound;
    if(nosecone==0) { Cd = Cd_for_flat_plugX(Mach_no);  }
    if(nosecone==1) { Cd = Cd_for_shaped_plug(Mach_no); }
    result = 0.5*rho*V*V*CSA*Cd;
    return(result);
}


void main()
{
    int i,j,k,airborne;
    float force,mass,acceleration,velocity;
    float dry_mass,prop_mass;
    float V_horiz,V_vert,x,z;
    float current_drag,delta_V,flight_duration;
    float angle,temp,degrees;
    float max_altitude,apogee_time,apogee_vel;
    FILE *report;
/******************* predictor variables *******/
    float v_hat;  /* predicted velocity */

    prop_mass = 5.43;
    dry_mass = tube_mass + payload;
    mass = prop_mass + dry_mass;        /*  16.05;   12.77; */
    velocity = 0.0;
    x = 0; z = 0;
    flight_duration = 0.0;
    angle = QE/57.29578;
    max_altitude = 0;

    thrust_map[0] = 2250;     /* 0 - 0.05 sec */
    thrust_map[1] = 1800;
    thrust_map[2] = 1690;
    thrust_map[3] = 1610;
    thrust_map[4] = 1530;
    thrust_map[5] = 1490;
    thrust_map[6] = 1470;
    thrust_map[7] = 1475;
    thrust_map[8] = 1465;
    thrust_map[9] = 1440;     /* 0.45 - 0.5 sec */
    thrust_map[10] = 1405;
    thrust_map[11] = 1380;
    thrust_map[12] = 1350;
    thrust_map[13] = 1325;
    thrust_map[14] = 1285;
    thrust_map[15] = 1245;
    thrust_map[16] = 1220;
    thrust_map[17] = 1190;
    thrust_map[18] = 1160;
    thrust_map[19] = 1130;
    thrust_map[20] = 1100;
    thrust_map[21] = 1080;
    thrust_map[22] = 1065;
    thrust_map[23] = 1040;
    thrust_map[24] = 1015;
    thrust_map[25] = 990;
    thrust_map[26] = 980;
    thrust_map[27] = 955;
    thrust_map[28] = 940;
    thrust_map[29] = 895;
    thrust_map[30] = 875;
    thrust_map[31] = 800;
    thrust_map[32] = 720;
    thrust_map[33] = 570;
    thrust_map[34] = 340;
    thrust_map[35] = 80;

  mp_map[0] = 5.43;
  mp_map[1] = 5.16;
  mp_map[2] = 4.93;
  mp_map[3] = 4.72;
  mp_map[4] = 4.51;
  mp_map[5] = 4.31;
  mp_map[6] = 4.12;
  mp_map[7] = 3.92;
  mp_map[8] = 3.73;
  mp_map[9] = 3.54;
  mp_map[10] = 3.35;
  mp_map[11] = 3.17;
  mp_map[12] = 2.99;
  mp_map[13] = 2.81;
  mp_map[14] = 2.64;
  mp_map[15] = 2.47;
  mp_map[16] = 2.31;
  mp_map[17] = 2.15;
  mp_map[18] = 2.00;
  mp_map[19] = 1.85;
  mp_map[20] = 1.70;
  mp_map[21] = 1.55;
  mp_map[22] = 1.41;
  mp_map[23] = 1.27;
  mp_map[24] = 1.14;
  mp_map[25] = 1.01;
  mp_map[26] = 0.88;
  mp_map[27] = 0.75;
  mp_map[28] = 0.63;
  mp_map[29] = 0.50;
  mp_map[30] = 0.39;
  mp_map[31] = 0.28;
  mp_map[32] = 0.18;
  mp_map[33] = 0.093;
  mp_map[34] = 0.030;
  mp_map[35] = 0.010;

    sonic_velocity[0] = 340.3;      /* 0 - 500 m */
    sonic_velocity[1] = 338.4;
    sonic_velocity[2] = 336.4;
    sonic_velocity[3] = 334.5;
    sonic_velocity[4] = 332.5;
    sonic_velocity[5] = 330.6;
    sonic_velocity[6] = 328.6;
    sonic_velocity[7] = 326.6;
    sonic_velocity[8] = 324.6;
    sonic_velocity[9] = 322.6;
    sonic_velocity[10] = 320.5;
    sonic_velocity[11] = 318.5;
    sonic_velocity[12] = 316.4;
    sonic_velocity[13] = 314.4;
    sonic_velocity[14] = 312.3;
    sonic_velocity[15] = 310.2;
    sonic_velocity[16] = 308.1;
    sonic_velocity[17] = 305.9;

atm_density[0] = 1.225;     /* 0 - 500 m */
atm_density[1] = 1.1672;    /* 500 - 1000 m etc */
atm_density[2] = 1.1083;
atm_density[3] = 1.0580;
atm_density[4] = 1.0065;
atm_density[5] = 0.9568;
atm_density[6] = 0.9091;
atm_density[7] = 0.8633;
atm_density[8] = 0.8192;
atm_density[9] = 0.7768;
atm_density[10] = 0.7361;
atm_density[11] = 0.6971;
atm_density[12] = 0.6597;
atm_density[13] = 0.6239;
atm_density[14] = 0.5895;
atm_density[15] = 0.5566;
atm_density[16] = 0.5252;
atm_density[17] = 0.4951;

	report = fopen("burn.rpt","w");
	fprintf(report,"time	velocity    altitude    Mach num    Cd     drag\n\n");
	k = 0;
    for(i=0;i<36;i++)
    {
	prop_mass = mp_map[i];
        mass = dry_mass + prop_mass;
/** predictor - rectangular integrator **/
	current_drag = drag(velocity);
        force = (thrust_map[i]*4.41892) - current_drag;
        acceleration = force/mass;
	delta_V = acceleration*interval1;
	v_hat = velocity + (delta_V/2.0);
/** corrector - trapezoidal integrator, 5 iterations **/
    for (j=0;j<5;j++)
    {
        current_drag = drag(v_hat);
        force = (thrust_map[i]*4.41892) - current_drag;
        acceleration = force/mass;
        delta_V = acceleration*interval1;
        v_hat = velocity +(delta_V/2.0);
    }
	velocity += delta_V;

	flight_duration += interval1;
	V_horiz = velocity*(cos(angle));
	V_vert = velocity*(sin(angle));

	V_vert -= (9.81*interval1);
	x += (V_horiz*interval1);
	z += (V_vert*interval1);
	Z = z;
	velocity = sqrt((V_vert*V_vert)+(V_horiz*V_horiz));
	temp = V_vert/velocity;
	if(temp>0.9999) temp = 0.9999;
	if(temp<-0.9999) temp = 0.0-0.9999;
	angle = asin(temp);

	if(k==0)
	{ fprintf(report,"%f   %f   %f   %f   %f
%f\n",flight_duration,velocity,z,Mach_no,Cd,current_drag); }
    k += 1;  if(k==10) { k = 0; }
    }
    printf("\nPARAMETERS ON MOTOR CUT-OUT");
    printf("\nvelocity      %f  m/sec",velocity);
    printf("\nmass          %f  Kgm",mass);
    printf("\naltitude      %f  metres",z);
    printf("\ndown-range    %f  metres",x);
    printf("\ntime          %f  seconds",flight_duration);
    printf("\n\n");

/********************** end of burn phase ****************/
    airborne = 1;
    mass = dry_mass;
    while(airborne)
    {
	velocity = sqrt((V_vert*V_vert)+(V_horiz*V_horiz));
	temp = V_vert/velocity;
	if(temp>0.9999) temp = 0.9999;
	if(temp<-0.9999) temp = 0.0-0.9999;
	angle = asin(temp);
/** predictor **/
	current_drag = drag(velocity);
        force = 0.0 - current_drag;
        acceleration = force/mass;
	delta_V = acceleration*interval2;
	v_hat = velocity + (delta_V/2.0);
/** corrector **/
    for(j=0;j<5;j++)
    {
        current_drag = drag(v_hat);
        force = 0.0 - current_drag;
	acceleration = force/mass;
        delta_V = acceleration*interval2;
	v_hat = velocity + (delta_V/2.0);
	V_vert = v_hat * (sin(angle));
	V_horiz = v_hat * (cos(angle));
	V_vert -= (9.81*interval2*0.5);
	v_hat = sqrt((V_vert*V_vert)+(V_horiz*V_horiz));
    }
	velocity += delta_V;

        V_vert = velocity * (sin(angle));
        V_horiz = velocity * (cos(angle));

	V_vert -= (9.81*interval2);
	z += V_vert*interval2;
	x += V_horiz*interval2;
	Z = z;
	flight_duration += interval2;
	if(z<0.0) airborne = 0;
	degrees = angle*57.3;
	if(z>max_altitude)
	{
		max_altitude = z;
		apogee_time = flight_duration;
		apogee_vel = velocity;
	}
	if(k==0)
	{ fprintf(report,"%f   %f   %f   %f   %f
%f\n",flight_duration,velocity,z,Mach_no,Cd,current_drag); }
	k += 1;  if(k==10) { k = 0; }
    }
    printf("\nPARAMETERS AT APOGEE");
    printf("\nvelocity      %f  m/sec",apogee_vel);
    printf("\naltitude      %f  metres",max_altitude);
    printf("\ntime          %f  seconds",apogee_time);
    printf("\n\n");

    printf("\nPARAMETERS ON IMPACT");
    printf("\nvelocity      %f  m/sec",velocity);
    printf("\nmass          %f  Kgm",mass);
    printf("\ndown-range    %f  metres",x);
    printf("\ntime          %f  seconds",flight_duration);
    printf("\n\nprogram written by Paul Wilson, QUT\n");
    fclose(report);
}