/*
 * @(#) smt2_2.c - Program for targets unification and trajectory
 * information joining (Task 2_2 at the Special Military Training course).
 * (c) 1997 Ivan Maidanski <ivmai@chat.ru> http://ivmai.chat.ru
 * Freeware program source. All rights reserved.
 **
 * Language: ANSI C
 * Tested with: Borland C++ v3.1
 * Last modified: 1997-05-06 11:05:00 GMT+04:00
 */

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

float extrapolate(float dt, float x, float dx)
{
 return (x+dx*dt);
}

void coordrotate(float *x, float *y, float cos_a, float sin_a)
{
 float t=*x;
 *x=t*cos_a-(*y)*sin_a;
 *y=t*sin_a+(*y)*cos_a;
}

void sigmarotate(float *x, float *y, float cos_a, float sin_a)
{
 float t=*x;
 cos_a*=cos_a;
 sin_a*=sin_a;
 *x=t*cos_a+(*y)*sin_a;
 *y=t*sin_a+(*y)*cos_a;
}

void coordrecalc(float dt, float sx, float sy, float cos_a, float sin_a,
                 float *x, float *y, float *z, float *dx, float *dy, float *dz)
{
 coordrotate(x,y,cos_a,sin_a);
 coordrotate(dx,dy,cos_a,sin_a);
 *x=extrapolate(dt,*x+sx,*dx);
 *y=extrapolate(dt,*y+sy,*dy);
 *z=extrapolate(dt,*z,*dz);
}

float getdelta(float v_a, float v_b, float sigma_a, float sigma_b)
{
 v_b-=v_a;
 return (v_b*v_b/(sigma_a*sigma_a+sigma_b*sigma_b)/9.0);
}

float averagecoord(float v_a, float v_b, float sigma_a, float sigma_b)
{
 return (v_a+(v_b-v_a)*sigma_a/(sigma_a+sigma_b));
}

int main()
{
 int i,j,mi;
 float tc;
 float s,ds,sum,minsum;
 float sx[2],sy[2],cos_a[2],sin_a[2],t[2];
 float sigma_x[2],sigma_y[2],sigma_z[2];
 float sigma_dx[2],sigma_dy[2],sigma_dz[2];
 float x[2][3],y[2][3],z[2][3],dx[2][3],dy[2][3],dz[2][3];
 static int v[6][3]= {{0,1,2},{1,0,2},{0,2,1},{2,1,0},{2,0,1},{1,2,0}};
 printf("t(Center)= ");
 scanf("%f",&tc);
 for (i=0;i<2;i++)
 {
  printf("\nStation %d\n",i+1);
  printf("x= ");
  scanf("%f",&sx[i]);
  printf("y= ");
  scanf("%f",&sy[i]);
  printf("angle= ");
  scanf("%f",&cos_a[i]);
  printf("t= ");
  scanf("%f",&t[i]);
  printf("sigma_x= ");
  scanf("%f",&sigma_x[i]);
  printf("sigma_x'= ");
  scanf("%f",&sigma_dx[i]);
  printf("sigma_y= ");
  scanf("%f",&sigma_y[i]);
  printf("sigma_y'= ");
  scanf("%f",&sigma_dy[i]);
  printf("sigma_z= ");
  scanf("%f",&sigma_z[i]);
  printf("sigma_z'= ");
  scanf("%f",&sigma_dz[i]);
  for (j=0;j<3;j++)
  {
   printf("\nTarget %d for station %d\n",j+1,i+1);
   printf("x= ");
   scanf("%f",&x[i][j]);
   printf("x'= ");
   scanf("%f",&dx[i][j]);
   printf("y= ");
   scanf("%f",&y[i][j]);
   printf("y'= ");
   scanf("%f",&dy[i][j]);
   printf("z= ");
   scanf("%f",&z[i][j]);
   printf("z'= ");
   scanf("%f",&dz[i][j]);
  }
 }
 printf("\n");
 for (i=0;i<2;i++)
 {
  cos_a[i]*=M_PI/180.0;
  sin_a[i]=sin(cos_a[i]);
  cos_a[i]=cos(cos_a[i]);
  sigmarotate(&sigma_x[i],&sigma_y[i],cos_a[i],sin_a[i]);
  sigmarotate(&sigma_dx[i],&sigma_dy[i],cos_a[i],sin_a[i]);
  for (j=0;j<3;j++)
   coordrecalc(tc-t[i],sx[i],sy[i],cos_a[i],sin_a[i],
               &x[i][j],&y[i][j],&z[i][j],
               &dx[i][j],&dy[i][j],&dz[i][j]);
 }
 minsum=-1.0;
 for (i=0;i<6;i++)
 {
  sum=0.0;
  for (j=0;j<3;j++)
  {
   if ((s=getdelta(x[0][j],x[1][v[i][j]],sigma_x[0],sigma_x[1])+
          getdelta(y[0][j],y[1][v[i][j]],sigma_y[0],sigma_y[1])+
          getdelta(z[0][j],z[1][v[i][j]],sigma_z[0],sigma_z[1]))>1.0 ||
       (ds=getdelta(dx[0][j],dx[1][v[i][j]],sigma_dx[0],sigma_dx[1])+
           getdelta(dy[0][j],dy[1][v[i][j]],sigma_dy[0],sigma_dy[1])+
           getdelta(dz[0][j],dz[1][v[i][j]],sigma_dz[0],sigma_dz[1]))>1.0)

   {
    sum=-1.0;
    break;
   }
   sum+=s+ds;
  }
  if (sum>=0.0 && (minsum<0.0 || minsum>sum))
  {
   mi=i;
   minsum=sum;
  }
 }
 if (minsum>=0.0)
 {
  printf("Variant: %d\n",mi+1);
  for (j=0;j<3;j++)
   printf("\nTarget %d real coordinates\n"
          " x= %f\n y= %f\n z= %f\n x'= %f\n y'= %f\n z'= %f\n",j+1,
          averagecoord(x[0][j],x[1][v[mi][j]],sigma_x[0],sigma_x[1]),
          averagecoord(y[0][j],y[1][v[mi][j]],sigma_y[0],sigma_y[1]),
          averagecoord(z[0][j],z[1][v[mi][j]],sigma_z[0],sigma_z[1]),
          averagecoord(dx[0][j],dx[1][v[mi][j]],sigma_dx[0],sigma_dx[1]),
          averagecoord(dy[0][j],dy[1][v[mi][j]],sigma_dy[0],sigma_dy[1]),
          averagecoord(dz[0][j],dz[1][v[mi][j]],sigma_dz[0],sigma_dz[1]));
 }
  else printf("Cannot find all matching targets!\n");
 printf("\n");
 return 0;
}