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

/*====================================================================*/
/*           prediction                                               */
/*           Version 1.0 - July 2012                                  */
/*                                                                    */
/* This code starts with wind dat, ballon parameters and launch       */
/* location and simulates the fight of the balloon.  It can also take */
/* real flight data and start the simulation from the last measured   */
/* location.                                                          */
/*                                                                    */
/* Input is done through a file called "prediction.conf"              */
/*             which contains its own documentation                   */
/*                                                                    */
/* Output include                                                     */
/*    Burst and Landing location printed to screen                    */
/*    "flight.kml" the path of the ballon.                            */
/*             Red is real, Blue is predicted                         */
/*    "prediction.dat"  landing prediction file which can be read     */
/*             by the program "mapViewer"                             */
/*                                                                    */
/*                                                   Tim Smith        */
/*====================================================================*/



float wind[7][40];
int nWind;
char windFile[256];
char flightFile[256];
float latOrg, lngOrg, altOrg;

float feet2meters = 0.3048;
// real fight data
float FlData[6][100000]; // 0=time 1=lat 2=lng 3=alt 4=x 5=y (x,y=km from start)
int nFlData;
// projected flight data
float PrData[6][100000]; // 0=time 1=lat 2=lng 3=alt 4=x 5=y (x,y=km from start)
int nPrData;

float maxAlt;
float rise;
float fall;

int launchFlag, burstFlag;
float launchTime;
float burstTime, burstAlt;

 

void LoadWindData(char *file){
  FILE *fp = fopen(file,"r");
  char inline2[1000];
  int mod = 0;
  float press, el, temp, dewpt, dir, speed;
  while (fgets(inline2,1000,fp)>0){
    if (strncmp(inline2," PRESS HGT(MSL)",15)==0){
      //printf("match: %s\n", inline2);
      nWind = 0;
      mod = 1;}
    if (mod ==1){
      el = 0;
      sscanf(inline2,"%f%f%f%f%f%f", &press, &el, &temp, &dewpt, &dir, &speed);
      //printf("%d) el: %f   dir: %f \n", nWind, el, dir);
      if (el>0){
	wind[0][nWind] = press;
	wind[1][nWind] = el;
	wind[2][nWind] = temp;
	wind[3][nWind] = dewpt;
	wind[4][nWind] = dir;
	wind[5][nWind] = speed;
	wind[6][nWind] = press/(temp+273.2); // density
	nWind++;
      }
      else if (nWind>5) mod=2;
    }
  }
  fclose(fp);
}

float GetSpeed(float el){
  int i, j;
  float speed;
  j = -1;
  for (i=0;i<nWind-1;i++){
    if (el<wind[1][i+1] && el>wind[1][i]){ 
      j = i;
      speed = wind[5][j]; 
	//   speed = wind[5][j] + 
	//	((el-wind[1][j])/(wind[1][j+1]-wind[1][j]))
	//	*(wind[5][j+1]-wind[5][j]);
    }
  }
  if (el<wind[1][0]) speed = wind[5][0];
  if (el>wind[1][nWind-1]) speed = wind[5][nWind-1];
      
  return speed;
}

float GetDir(float el){
  int i, j;
  float dir, dir1, dir2;
  j = -1;
  for (i=0;i<nWind-1;i++){
    if (el<wind[1][i+1] && el>wind[1][i]){ 
      j = i;
      dir1 = wind[4][j];
      dir2 = wind[4][j+1];
      if (dir1>270 && dir2<90) dir2 = dir2+360;
      if (dir1<90 && dir2>270) dir2 = dir2-360;
      dir = dir1;
      //     dir = dir1 + 
      //	((el-wind[1][j])/(wind[1][j+1]-wind[1][j]))
      //	*(dir2-dir1);
      if (dir > 360) dir = dir-360;
      if (dir < 0) dir = dir+360;
    }
  }
  if (el<wind[1][0]) dir = wind[4][0];
  if (el>wind[1][nWind-1]) dir = wind[4][nWind-1];
  return dir;
}

float GetFallFactor(float el){
  int i, j;
  float ff;
  j = -1;
  for (i=0;i<nWind-1;i++){
    if (el<wind[1][i+1] && el>wind[1][i]) j = i;
  }
  if (el<wind[1][0]) j = 0;
  if (el>wind[1][nWind-1]) j = nWind-1;
  //printf("ff %f %d) %f %f \n", el, j, wind[0][0], wind[0][j]);
  if (wind[0][j] != 0.0)
    ff = sqrt(wind[6][0]/wind[6][j]);
  else
    ff = 1;
  return ff;
}


void Projection(float lng0, float lat0, float alt0, int phase){
  float pi = 4.0*atan(1.0);
  float feet2meters = 0.3048;

  float x=0;
  float y=0;
  float dir, vel, dx, dy;
  float alt;
  int i=0;
  int j=0;
	 
  float xmax, xmin,ymax, ymin;
  xmax=0; xmin=0; ymax=0; ymin=0;
  float latOrg = lat0;
  float lngOrg = lng0;
  float time;

  float km2lat = 90.0/10000.0;
  float km2lng = km2lat/cos(latOrg*pi/180);

  float lat, lng;
  float el;
  i = 0;
  alt = alt0; 
 
  if (phase <3){
    for (alt=alt0;alt<maxAlt;alt=alt+rise){
      dir = GetDir(alt)*pi/180.;
      vel = GetSpeed(alt);
      dx = vel*sin(dir);
      dy = vel*cos(dir);
      x -= dx/1000;
      y -= dy/1000;
      lng = lngOrg + x*km2lng;
      lat = latOrg + y*km2lat;
      time = i;
      PrData[0][i] = time;
      PrData[1][i] = lat;
      PrData[2][i] = lng;
      PrData[3][i] = alt;
      nPrData = i;
      i++;
    }
  }

  if (phase == 3) maxAlt = alt0;

  for (alt=maxAlt;alt>0;alt=alt-fall*GetFallFactor(alt)){
    //for (alt=maxAlt;alt>0;alt=alt-fall*sqrt(1./sa->GetDensity_km(alt/1000.))){
    dir = GetDir(alt)*pi/180.;
    vel = GetSpeed(alt);
    /*
    j = alt/stepWind;
    if (j<nMeasWind){
      dir = measWind[1][j]*3.1415/180.;
      vel = measWind[0][j];
      }*/
    dx = vel*sin(dir);
    dy = vel*cos(dir);
    x -= dx/1000;
    y -= dy/1000;
    lng = lngOrg + x*km2lng;
    lat = latOrg + y*km2lat;
    time = i;
    PrData[0][i] = time;
    PrData[1][i] = lat;
    PrData[2][i] = lng;
    PrData[3][i] = alt;
    nPrData = i;
    i++;
  }

  launchFlag = -99;
  burstFlag = -99;
  for (i=0;i<nPrData;i++){
    if (PrData[3][i]-100>PrData[3][0] && launchFlag == -99) launchFlag = i; 
    if (PrData[3][i]>burstAlt){
      burstAlt = PrData[3][i];
      burstFlag = i;
    }
  }
  if (launchFlag>1) launchTime = PrData[0][launchFlag];
  if (burstFlag>1) burstTime = PrData[0][burstFlag];

}

void PrPrint(){
  int sec;
  printf("Prediction - nPrData: %d \n", nPrData);
  sec = PrData[0][launchFlag];
  printf("   launch:  [%6d]     %5d     %7.3f   %7.3f    %8.2f \n", launchFlag,
	   sec, PrData[2][launchFlag],
	   PrData[1][launchFlag], PrData[3][launchFlag]);
  sec = PrData[0][burstFlag];
  printf("   burst:   [%6d]     %5d     %7.3f   %7.3f    %8.2f \n", burstFlag,
	 sec, PrData[2][burstFlag], 
	 PrData[1][burstFlag], PrData[3][burstFlag]);
  sec = PrData[0][nPrData-1];
  printf("   landing: [%6d]     %5d     %7.3f   %7.3f    %8.2f \n", nPrData-1,
	 sec, PrData[2][nPrData-1], 
	 PrData[1][nPrData-1], PrData[3][nPrData-1]);
}

KML(){
  int i;
  FILE *fpKML = fopen("flight.kml","w");
  fprintf(fpKML,"<kml> \n <Document> <name>fl2.kml</name> \n");

 fprintf(fpKML," <Style id=\"transRedPoly0\"> \n");
  fprintf(fpKML,"   <LineStyle> <color>ff0000ff</color> <width>1.5</width>  </LineStyle> \n  </Style> \n");

  fprintf(fpKML," <Style id=\"transGreenPoly0\"> \n");
  fprintf(fpKML,"   <LineStyle> <color>ff00ff00</color> <width>1.5</width>  </LineStyle> \n  </Style> \n");

  fprintf(fpKML," <Style id=\"transBluePoly0\"> \n");
  fprintf(fpKML,"   <LineStyle> <color>ffff0000</color> <width>1.5</width>  </LineStyle> \n  </Style> \n");

  
  fprintf(fpKML,"<Placemark>\n <name>fl22</name> \n <LookAt> \n <longitude>-71</longitude> \n <latitude>44.2</latitude>\n <altitude>0</altitude>\n <heading>45</heading>\n <tilt>30</tilt>\n <range>100000</range> \n </LookAt>\n");

  // flight data
  fprintf(fpKML,"<styleUrl>#transRedPoly0</styleUrl>\n<LineString>\n<tessellate>1</tessellate>\n<altitudeMode>absolute</altitudeMode>\n<coordinates> \n");
  for (i=0;i<nFlData;i+=20) 
    fprintf(fpKML,"%f,%f,%f \n", 
	   FlData[2][i],FlData[1][i],FlData[3][i]);
  fprintf(fpKML,"</coordinates> \n </LineString> \n </Placemark>\n");

  // prediction data
  fprintf(fpKML,"<Placemark>\n <name>fl23</name>  \n <LookAt> \n <longitude>-71</longitude> \n <latitude>44.2</latitude>\n <altitude>0</altitude>\n <heading>45</heading>\n <tilt>30</tilt>\n <range>100000</range> \n </LookAt>\n");
  fprintf(fpKML,"<styleUrl>#transBluePoly0</styleUrl>\n<LineString>\n<tessellate>1</tessellate>\n<altitudeMode>absolute</altitudeMode>\n<coordinates> \n");
  for (i=0;i<nPrData;i+=20) 
    fprintf(fpKML,"%f,%f,%f \n", 
	   PrData[2][i],PrData[1][i],PrData[3][i]);
  fprintf(fpKML,"</coordinates> \n </LineString>\n");

  fprintf(fpKML,"</Placemark>\n </Document>\n</kml>\n");
  fclose(fpKML);
}

void PredictionFile(){
  FILE *fpPre = fopen("prediction.dat","w");
  fprintf(fpPre,"%f   %f    %f \n", PrData[2][nPrData-1], 
	 PrData[1][nPrData-1], PrData[3][nPrData-1]);
  fclose(fpPre);
}


void ReadFlightData(char *flightData)
{
  /*  ...........  Read flight .............  */

  int i, j;
  FILE *fp = fopen(flightData,"r");
  char inline2[1000];
  float pi = 4.*atan(1.0);
  int time; 
  float lat,lng, time0;
  char tag[10], a1[2], a2[2];
  int stat, nSat;
  float hor, alt;
  int deg;
  int hr, mi, sc;
  float min;
  float x, y;
  nFlData=0;
  while (fgets(inline2,1000,fp)>0){
    for (i=0;i<100;i++){
      if (strncmp(inline2+i,"$GPGGA",6)==0){
	//printf("inline: %s \n", inline2);
	sscanf(inline2+i,"%6s,%f,%f,%1s,%f,%1s,%d,%d,%f,%f",
	       tag, &time0, &lat, a1, &lng, a2, &stat, &nSat, &hor, &alt);
	time = time0;
	hr = time/10000;
	mi = time/100-hr*100;
	sc = time-hr*10000-mi*100;
	// printf("%d   %2d:%2d:%2d  %f     %f  aa%s    %f\n", 
	//     time, hr, mi, sc, lng, lat,a1, alt);
	time = hr*3600+mi*60+sc;
	
	deg = lat/100;
	min = lat - deg*100;
	lat = deg + min/60.0;
	
	deg = lng/100;
	min = lng - deg*100;
	lng = -1.0*(deg+min/60.0);
	
	//printf("%d %f %f %f \n", time, lat, lng, alt);
	if (lat>10 && lng<-50) {
	  FlData[0][nFlData] = time;
	  FlData[1][nFlData] = lat;
	  FlData[2][nFlData] = lng;
	  FlData[3][nFlData] = alt;
	  x = (lng-FlData[2][0])*1e7*cos(lat*pi/180.)/90.0;
	  y = (lat-FlData[1][0])*1e7/90.0;
	  FlData[4][nFlData] = x;
	  FlData[5][nFlData] = y;
	  if (nFlData==0){ lngOrg = lng; latOrg = lat; altOrg = alt;}
	  nFlData++;
	}
      }
    }
  }
  fclose(fp);
}



main(){
  int i, phase;

  printf("\nPrediction for balloon flights\n=================================\n\n");
  strcpy(windFile,"");
  strcpy(flightFile,"");
  maxAlt = -999; rise = -999; fall = -999;
  lngOrg = -999; latOrg = -999; altOrg = -999;

  // read prediction.conf file
  char inline2[1000];
  FILE *fp = fopen("prediction.conf","r");
  while (fgets(inline2,1000,fp)>0){
    if (strncmp(inline2,"WIND",4)==0) sscanf(inline2+4,"%s",windFile);
    if (strncmp(inline2,"FLIGHT",6)==0) sscanf(inline2+6,"%s",flightFile);
    if (strncmp(inline2,"MAX",3)==0) sscanf(inline2+3,"%f", &maxAlt);
    if (strncmp(inline2,"RISE",4)==0) sscanf(inline2+4,"%f", &rise);
    if (strncmp(inline2,"FALL",4)==0) sscanf(inline2+4,"%f", &fall);
    if (strncmp(inline2,"LAUNCH",6)==0) 
      sscanf(inline2+6,"%f %f %f ", &lngOrg, &latOrg, &altOrg);
   }

  if (strlen(windFile)<1){
    strcpy(windFile,"wind.dat");
    printf("WindData: Default: %s \n", windFile);
  }
  else
      printf("WindData: %s \n", windFile);

  if (strlen(flightFile)<1){
    printf("FlightData: no flight file  \n");
  }
  else
      printf("FlightData: %s \n", flightFile);

  if (maxAlt == -999) { maxAlt = 90000*feet2meters; printf("MaxAlt: default: %f m\n", maxAlt);}
  else  printf("MaxAlt: %f m\n", maxAlt);

  if (rise == -999) { rise = 1300*feet2meters/60.; printf("rise: default: %f m/s\n", rise);}
  else  printf("Rise: %f m/s \n", rise);

  if (fall == -999) { fall = 900*feet2meters/60.; printf("fall: default: %f m/s\n", fall);}
  else  printf("Fall: %f m/s \n", fall);

  phase = 1; // balloon on the rise
  LoadWindData(windFile);
  if (strlen(flightFile)>0){
    printf("Reading real flight data\n");
    ReadFlightData(flightFile);
    printf("    nFlData: %d \n", nFlData);
    latOrg = FlData[1][nFlData-1];
    lngOrg = FlData[2][nFlData-1];
    altOrg = FlData[3][nFlData-1];
    if (FlData[3][nFlData-1] < FlData[3][nFlData-2] ) phase = 3;
    if (phase == 1) printf("    ballon on the rise\n");
    else printf("   balloon falling\n");
  }

  Projection(lngOrg, latOrg, altOrg,  phase);
  PrPrint();
  KML();
  PredictionFile();
}