/* DirectCalc v1.8.1 Ver.:OK (c) 01/01 - Ch Iossif */
/*
   Name:         DIRectCALCulator
   Version:      1.0.1
   Author:       Ch Iossif <chiossif@yahoo.com>
   Date:         26/06/09 10:41
   Modified:     01/01/01 12:00
   Description:  Direct Georeference Calculations on SAR images
   Usage:        $DirectCalc
   Uses:         THESE additional inline parameters
   Compile line: $gcc -o DirectCalc DirectCalc.c -lm

   Copyright (C) January 2001 Ch Iossif <chiossif@yahoo.com>

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <stdio.h>
#include <math.h>

#define MAX_ITERATIONS 100
#define ACCURACY 1e-9

double sqr(double x) {
    return x*x;
}
double pow3_2(double x) {
    return sqrt(x*x*x);
}

int main(void) {
    double xk[3], xk_1[3], f[3], df[3][3], df_1[3][3], df_1_f[3];
    double px, py, pz, sx, sy, sz, vsx, vsy, vsz, a, b, h, lamda, r, ff;
    double zero=ACCURACY, s, s1, s2, c, opx, opy, opz, or;
    int pid;
    register i, j, iter;

    a=6378137;          /* THIS additional inline parameter */
    ff=1.0/298.2572236; /* THIS additional inline parameter */
    b=a-a*ff;
    c=299792458.0;      /* THIS additional inline parameter */
    lamda=0.0566660;    /* THIS additional inline parameter */


    while (scanf("%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",&pid,&sx,&sy,&sz,&vsx,&vsy,&vsz,&px,&py,&pz,&h)==11) {

        r=sqrt(sqr(sx-px)+sqr(sy-py)+sqr(sz-pz));

        opx=xk_1[0]=px;
        opy=xk_1[1]=py;
        opz=xk_1[2]=pz;

        iter=0;
        do {
            px=xk_1[0];
            py=xk_1[1];
            pz=xk_1[2];

            f[0]=sqr(sx-px)+sqr(sy-py)+sqr(sz-pz)-sqr(r);
            f[1]=2*(vsx*(px-sx)+vsy*(py-sy)+vsz*(pz-sz))/lamda/r;
            f[2]=sqr(px/(a+h))+sqr(py/(a+h))+sqr(pz/(b+h))-1;

            s1=sqr(sx-px)+sqr(sy-py)+sqr(sz-pz);
            s2=vsx*(px-sx)+vsy*(py-sy)+vsz*(pz-sz);
            df[0][0]=2*px-2*sx;
            df[0][1]=2*py-2*sy;
            df[0][2]=2*pz-2*sz;
            df[1][0]=2*vsx/lamda/sqrt(s1) - 2*s2*(px-sx)/lamda/pow3_2(s1);
            df[1][1]=2*vsy/lamda/sqrt(s1) - 2*s2*(py-sy)/lamda/pow3_2(s1);
            df[1][2]=2*vsz/lamda/sqrt(s1) - 2*s2*(pz-sz)/lamda/pow3_2(s1);
            df[2][0]=2*px/sqr(a+h);
            df[2][1]=2*py/sqr(a+h);
            df[2][2]=2*pz/sqr(b+h);

            s=	df[0][0]*df[1][1]*df[2][2]-
               df[0][0]*df[1][2]*df[2][1]-
               df[1][0]*df[0][1]*df[2][2]+
               df[1][0]*df[1][2]*df[2][1]+
               df[0][1]*df[2][0]*df[1][2]-
               df[0][2]*df[2][1]*df[1][1];

            df_1[0][0]=-(-df[1][1]*df[2][2]+df[1][2]*df[2][1])/s;
            df_1[0][1]=-( df[0][1]*df[2][2]-df[0][2]*df[2][1])/s;
            df_1[0][2]= ( df[0][1]*df[1][2]-df[0][2]*df[1][1])/s;
            df_1[1][0]= (-df[1][0]*df[2][2]+df[1][2]*df[2][0])/s;
            df_1[1][1]=-(-df[0][0]*df[2][2]+df[0][2]*df[2][0])/s;
            df_1[1][2]=-( df[0][0]*df[1][2]-df[0][2]*df[1][0])/s;
            df_1[2][0]=-(-df[1][0]*df[2][1]+df[1][1]*df[2][0])/s;
            df_1[2][1]= (-df[0][0]*df[2][1]+df[0][1]*df[2][0])/s;
            df_1[2][2]=-(-df[0][0]*df[1][1]+df[0][1]*df[1][0])/s;

            for (i=0;i<3;i++) {
                for (s=0,j=0;j<3;j++)
                    s+=df_1[i][j]*f[j];
                df_1_f[i]=s;
            }

            for (i=0;i<3;i++)
                xk[i]=xk_1[i]-df_1_f[i];
            for (s=0,i=0;i<3;i++)
                s+=fabs(xk[i]-xk_1[i]);
            for (i=0;i<3;i++)
                xk_1[i]=xk[i];
        } while (s>zero&&++iter<MAX_ITERATIONS);

        or=sqrt(sqr(xk[0]-opx)+sqr(xk[1]-opy)+sqr(xk[2]-opz));
        printf("%07d:",pid);
        for (i=0;i<3;i++)
            printf("%12.3lf ", xk[i]);
        printf("%4d %14.3lf\n", iter, or);
    }

    printf("\n\nDirectCalc by Christos Iossifides (c) 2001 NaTUReS Lab.\n");
    return 0;
}