/* InDiRes v1.0.3 Ver.:OK (c) 01/02 - Ch Iossif */
/*
   Name:         INDIRectRESampling
   Version:      1.0.3
   Author:       Ch Iossif <chiossif@yahoo.com>
   Date:         26/06/09 10:12
   Modified:     15/01/02 11:37
   Description:  Indirect orthorectification - resampling on SAR images
   Usage:        $InDiRes parameters.txt
   Uses:         en2flHGRS87, FLH2XYZHGRS87, HGRS872WGS84 all by ChIossif under GPLv3
   Compile line: $gcc -o InDiRes InDiRes.c -lm

   Copyright (C) January 2002 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 <stdlib.h>
#include <math.h>

#define ACCURACY 1e-5
#define MAX_ITERATIONS 10
#define sqr(x) ((x)*(x))

void warning(const char*);
void error(const char*);
void progress(float);
void calculateposition(double, double, double, double*, double*);
void en2flHGRS87(double, double, double*, double*);
void FLH2XYZHGRS87(double, double, double, double*, double*, double*);
void HGRS872WGS84(double, double, double, double*, double*, double*);

double ax3, ax2, ax1, ax0, ay3, ay2, ay1, ay0, az3, az2, az1, az0;
double t_start, t_end, delta_t, C, RF, PixelSpacingInRange, tekin;
double xp, yp, zp, dxp, dyp, dzp, azp, rap, inter_time, inter_dist;
double ulx, uly, lrx, lry, dtmulx, dtmuly, x, y, z, psize, dtmpsize;
int RecordLength, NumberOfBytesPerPixel, Header, PixelNumberAtRowStart;
int rows, cols, imrows, imcols, dtmrows, dtmcols;

int main(int argc,char **argv) {
    FILE *fp, *fpim, *fpdtm;
    double dr, dc, XX, YY, ZZ, XXX, YYY, ZZZ, f, l;
    unsigned short int *buffdtm;
    unsigned char  *buff, *bv;
    char imfile[256], file[256], dtmfile[256];
    int row, column, r, c;
    register i;

    if (argc!=2)
        error("Bad command line");
    if ((fp=fopen(argv[1],"r"))==NULL)
        error("Bad file open 1");

    if (fscanf(fp,"%s %d %d %lf %lf", imfile, &imrows, &imcols, &t_start, &t_end)!=5)
        error("Bad text read 1");
    if (fscanf(fp,"%d %d %d", &NumberOfBytesPerPixel, &RecordLength, &Header)!=3)
        error("Bad text read 2");
    if (fscanf(fp,"%s %d %d %lf %lf %lf", dtmfile, &dtmrows, &dtmcols, &dtmpsize, &dtmulx, &dtmuly)!=6)
        error("Bad text read 3");
    if (fscanf(fp,"%s", file)!=1)
        error("Bad text read 4");
    if (fscanf(fp,"%lf %lf %lf",&C, &RF, &PixelSpacingInRange)!=3)
        error("Bad text read 5");
    if (fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
               &xp, &yp, &zp, &dxp, &dyp, &dzp, &azp, &rap, &inter_time, &inter_dist)!=10)
        error("Bad text read 6");
    if (fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
               &ax3, &ax2, &ax1, &ax0, &ay3, &ay2, &ay1, &ay0, &az3, &az2, &az1, &az0)!=12)
        error("Bad text read 7");

    rows=dtmrows;
    cols=dtmcols;
    psize=dtmpsize;
    ulx=dtmulx;
    uly=dtmuly;
    lrx=dtmulx+psize*cols;
    lry=dtmuly-psize*rows;
    tekin=t_start;
    delta_t=(t_end-t_start)/imrows;
    PixelNumberAtRowStart=(RecordLength-imcols*NumberOfBytesPerPixel)/NumberOfBytesPerPixel;

    printf("Resampling reports:\n\nImageIn:\nfilename=<%s>\nrows=%d columns=%d\nAzimuth time of first pixel=%lf\nAzimuth time of last  pixel=%lf\n",
           imfile, imrows, imcols, t_start, t_end);
    printf("Number of bytes per pixel=%d\nRecord Length=%d\nImage header record length=%d\nPixelNumberAtRowStart=%d\n",
           NumberOfBytesPerPixel, RecordLength, Header,PixelNumberAtRowStart);
    printf("\nDTMIn:\nfilename=<%s>\nrows=%d columns=%d pixelsize=%lf\nUpperLeftX=%lf UpperLeftY=%lf\n",
           dtmfile, dtmrows, dtmcols, dtmpsize, dtmulx, dtmuly);
    printf("\nImageOut:\nfilename=<%s>\nrows=%d columns=%d pixelsize=%lf\n",
           file, rows, cols, psize);
    printf("UpperLeftX=%lf UpperLeftY=%lf\nLowerRightX=%lf LowerRightY=%lf\n",
           ulx, uly, lrx, lry);
    printf("\nSpeed of light=%lf\nRadar Frequency=%lf\nPixel spacing in range=%lf\nTime spacing in azimuth=%lf\n",
           C, RF, PixelSpacingInRange, delta_t);
    printf("\nPoint:\nX= %14.8le Y= %14.8le Z= %14.8le\ndX=%14.8le dY=%14.8le dZ=%14.8le\nAzimuth=%14.2lf Range=%14.2lf\nInterpolated Zero-Doppler Time=%14.6lf\nSatellite to Point Distance=%14.3lf\n",
           xp, yp, zp, dxp, dyp, dzp, azp, rap, inter_time, inter_dist);
    printf("\nSystem Coefficients:\nax3=%14.8le ax2=%14.8le ax1=%14.8le ax0=%14.8le\nay3=%14.8le ay2=%14.8le ay1=%14.8le ay0=%14.8le\naz3=%14.8le az2=%14.8le az1=%14.8le az0=%14.8le\n",
           ax3, ax2, ax1, ax0, ay3, ay2, ay1, ay0, az3, az2, az1, az0);

    if (fabs(t_start-inter_time)>10||fabs(t_end-inter_time)>10)/* Security check */
        error("Bad Data");
    if (fclose(fp))
        error("Bad file close 1");
    if ((fpim=fopen(imfile,"rb"))==NULL)
        error("Bad file open 2");
    if ((fpdtm=fopen(dtmfile,"rb"))==NULL)
        error("Bad file open 3");
    if ((fp=fopen(file,"wb"))==NULL)
        error("Bad file open for write");

    if ((bv=(unsigned char *)malloc(NumberOfBytesPerPixel))==NULL)
        error("Not enough memory 1");
    if ((buff=(unsigned char *)malloc(cols*NumberOfBytesPerPixel))==NULL)
        error("Not enough memory 2");
    if ((buffdtm=(unsigned short int *)malloc(cols*sizeof(unsigned short int)))==NULL)
        error("Not enough memory 3");

    for (row=0;row<rows;row++) {
        if (fread(buffdtm,sizeof(unsigned short int),cols,fpdtm)!=(size_t)cols)
            error("Bad read 1");
        for (column=0;column<cols;column++) {
            x=ulx+(double)column*psize;
            y=uly-(double)row*psize;
            z=(double)buffdtm[column];
            if (z<0.0||z>1925.0) { /* Security check */
                for (i=0;i<NumberOfBytesPerPixel;i++)
                    bv[i]=(unsigned char)0;
                warning("Height out of limits");
            } else {
                en2flHGRS87(x, y, &f, &l);
                FLH2XYZHGRS87(f, l, z, &XX, &YY, &ZZ);
                HGRS872WGS84(XX, YY, ZZ, &XXX, &YYY, &ZZZ);
                calculateposition(XXX, YYY, ZZZ, &dr, &dc);
                r=(int)dr;
                c=(int)dc;
                if (r<0 || r>=imrows || c<0 || c>=imcols)
                    bv[0]=bv[1]=bv[2]=bv[3]=(unsigned char)255;
                else {
                    if (fseek(fpim,(long)r*RecordLength+(long)c*NumberOfBytesPerPixel+(long)Header,SEEK_SET))
                        error("Bad seek");
                    if (fread(bv,NumberOfBytesPerPixel,1,fpim)!=(size_t)1)
                        error("Bad read 2");
                }
            }
            for (i=0;i<NumberOfBytesPerPixel;i++)
                buff[column*NumberOfBytesPerPixel+i]=bv[i];
        }
        if (fwrite(buff,NumberOfBytesPerPixel,cols,fp)!=(size_t)cols)
            error("Bad write");

        progress((float)row/(rows-1));
    }

    if (fclose(fpim))
        error("Bad file close 2");
    if (fclose(fpdtm))
        error("Bad file close 3");
    if (fclose(fp))
        error("Bad file close 4");

    printf("\n\nInDiRes by Christos Iossifides (c) 2002 NaTUReS Lab.\n");
    getchar();
    return 0;
}

void warning(const char *s) {
    fprintf(stderr,"\nResampling reports: Warning: <%s>.\n",s);
    printf("\n\aResampling reports: Warning: <%s>.\n",s);
}

void error(const char *s) {
    fprintf(stderr,"\nResampling reports: Error: <%s>.\n",s);
    printf("\n\aResampling reports: Error: <%s>.\n",s);
    exit(1);
}

void progress(float f) {
    static int k, pk;

    if (pk==0&&k==0&&pk==k) {
        printf("Resampling: Just Started...****************************************************\r");
        pk=-1;
    } else {
        k=(int)(f*79)+1;
        if (pk==k)
            return;
        else if (k<80)
            putchar('.');
        else
            printf("\rResampling: Just Finished...\n");
        pk=k;
    }
}

void calculateposition(double xp, double yp, double zp, double *az, double *ra) {
    double c, f0, R, pR, t, pt, fd, pfd, a, fa, b;
    double x, y, z, t0, tt, dt, dx, dy, dz;
    int count;

    c=C;
    f0=RF;
    t0=tekin-1;
    tt=t_end;
    dt=delta_t;
    count=0;
    fd=-1.0;
    t=t0;
    do {
        pfd=fd;
        pt=t;
        pR=R;

        t+=dt;
        x=ax3*t*t*t+ax2*t*t+ax1*t+ax0;
        y=ay3*t*t*t+ay2*t*t+ay1*t+ay0;
        z=az3*t*t*t+az2*t*t+az1*t+az0;
        dx=3.0L*ax3*t*t+2.0L*ax2*t+ax1;
        dy=3.0L*ay3*t*t+2.0L*ay2*t+ay1;
        dz=3.0L*az3*t*t+2.0L*az2*t+az1;

        R=sqrt(sqr(x-xp)+sqr(y-yp)+sqr(z-zp));
        fd=2.0L*f0/c/R*(dx*(x-xp)+dy*(y-yp)+dz*(z-zp));
    } while (pfd*fd>0);

    tekin=pt;
    count=0;
    a=pt;
    fa=pfd;
    b=t;
    do {
        t=(a+b)/2;
        x=ax3*t*t*t+ax2*t*t+ax1*t+ax0;
        y=ay3*t*t*t+ay2*t*t+ay1*t+ay0;
        z=az3*t*t*t+az2*t*t+az1*t+az0;
        dx=3.0L*ax3*t*t+2.0L*ax2*t+ax1;
        dy=3.0L*ay3*t*t+2.0L*ay2*t+ay1;
        dz=3.0L*az3*t*t+2.0L*az2*t+az1;

        R=sqrt(sqr(x-xp)+sqr(y-yp)+sqr(z-zp));
        fd=2.0L*f0/c/R*(dx*(x-xp)+dy*(y-yp)+dz*(z-zp));
        if (fd*fa>0) {
            fa=fd;
            a=t;
        } else
            b=t;
    } while (fabs(fd)>ACCURACY && count++<MAX_ITERATIONS);

    *az=(t-inter_time)/dt-azp;
    *ra=(R-inter_dist)/PixelSpacingInRange+rap+PixelNumberAtRowStart;

    return;
}

void en2flHGRS87(double e, double n, double *latit, double *longt) {
    double Pi, Deg2Rad, SemiAxis, Flat, Eccen1, Eccen2, K0;
    double lon0, VerRad, MerRad, h, t, lat, Mo, P, Dlat;
    double M0, M2, M4, M6, M8, M, MerArc;
    double Lat11, Lat12, Lat13, Lat14, Lat, Lon11, Lon12, Lon13, Lon;

    Pi = 3.14159265358979323846;
    Deg2Rad = Pi/180.0;
    SemiAxis = 6378137;                             /* GRS80 */
    Flat =  (double) 1.0 / 298.257222101;           /* GRS80 */
    Eccen1 = (double) 2.0 * Flat - Flat * Flat;
    Eccen2 =  Eccen1 / (1.0 - Eccen1);
    K0 = (double) 0.9996;

    e -= (double)500000;
    lon0 = (double) 24.0*Deg2Rad;
    Mo = (double) 1 + 0.75 * Eccen1 + ((double)45 / 64) * Eccen1 * Eccen1 + ((double)175 / 256) * Eccen1 * Eccen1 * Eccen1 + ((double)11025 / 16384) * Eccen1 * Eccen1 * Eccen1 * Eccen1;
    Mo *= ((double)1 - Eccen1) * SemiAxis;
    lat = n / (Mo * K0);

    do {
        M0 = (double) 1 + 0.75 * Eccen1 + ((double)45 / 64) * Eccen1 * Eccen1 + ((double)175 / 256) * Eccen1 * Eccen1 * Eccen1 + ((double)11025 / 16384) * Eccen1 * Eccen1 * Eccen1 * Eccen1;
        M2 = ((double)3 / 8) * Eccen1 + ((double)15 / 32) * Eccen1 * Eccen1 + ((double)525 / 1024) * Eccen1 * Eccen1 * Eccen1 + ((double)2205 / 4096) * Eccen1 * Eccen1 * Eccen1 * Eccen1;
        M4 = ((double)15 / 256) * Eccen1 * Eccen1 + ((double)105 / 1024) * Eccen1 * Eccen1 * Eccen1 + ((double)2205 / 8820) * Eccen1 * Eccen1 * Eccen1 * Eccen1;
        M6 = ((double)35 / 3072) * Eccen1 * Eccen1 * Eccen1 + ((double)315 / 12288) * Eccen1 * Eccen1 * Eccen1 * Eccen1;
        M8 = ((double)315 / 130784) * Eccen1 * Eccen1 * Eccen1 * Eccen1;
        M = M0 * lat - M2 * sin(2*lat) + M4 * sin(4*lat) - M6 * sin(6*lat) + M8 * sin(8*lat);
        MerArc = M * ((double)1 - Eccen1) * SemiAxis;
        Dlat = (n / K0 - MerArc) / Mo;
        lat+=Dlat;
    } while (fabs(Dlat)>1.0e-15);

    VerRad = (double) SemiAxis / sqrt(1.0 - Eccen1 * sin(lat) *sin(lat));
    MerRad = (double) SemiAxis * (1.0 - Eccen1) / sqrt( ( 1 - Eccen1 * sin(lat) * sin(lat) )*( 1 - Eccen1 * sin(lat) * sin(lat) )*( 1 - Eccen1 * sin(lat) * sin(lat) ) );
    h = (double)Eccen2 * cos(lat) * cos(lat);
    P = e / (K0 * VerRad);
    t = (double)tan(lat) * tan(lat);

    Lat11 = ((double)1385 + 3633 * t + 4095 * t * t) / 40320;
    Lat12 = ((double)61 + 90 * t + 46 * h + 45 * t * t - 252 * t * h - 3 * h * h - 66 * t * h * h - 90 * t * t * h) / 720;
    Lat13 = ((double)5 + 3 * t + h - 4 * h * h - 9 * h * t) / 24;
    Lat14 = VerRad / MerRad;
    Lat = (((((Lat11)) * P * P - (Lat12)) * P * P + (Lat13)) * P * P - 0.5) * P * P * (Lat14) * tan(lat) + lat;

    Lon11 = (double)-61 - 662 * t - 1320 * t * t;
    Lon12 = (double)5 + 6 * h + 28 * t - 3 * h * h + 8 * t * h + 24 * t * t + 4 * t * h * h;
    Lon13 = (double)1 + 2 * t + h;
    Lon = (((((Lon11) / 5040) * P * P + (Lon12) / 120) * P * P - (Lon13) / 6) * P * P + 1) * P * (1 / cos(lat)) + lon0;

    *latit = Lat / Deg2Rad;
    *longt = Lon / Deg2Rad;
    return;
}

void FLH2XYZHGRS87(double f, double l, double h, double *x, double *y, double *z) {
    double Pi, Deg2Rad, a, F, e, sf, cf, sl, cl, n;

    Pi = 3.14159265358979323846;
    Deg2Rad = Pi/180.0;
    a = (double) 6378137;						/* GRS80 */
    F = (double) 1.0 / 298.257222101;           /* GRS80 */
    e = (double) 2.0 * F - F * F;

    f *= Deg2Rad;
    l *= Deg2Rad;

    sf=sin(f);
    cf=cos(f);
    sl=sin(l);
    cl=cos(l);

    n = a / sqrt(1.0 - e * sf * sf);
    *x = (n+h) * cf * cl;
    *y = (n+h) * cf * sl;
    *z = (n * (1.0 - e) + h) * sf;
    return;
}

void HGRS872WGS84(double xh, double yh, double zh, double *xw, double *yw, double *zw) {
    *xw = xh - 199.652;
    *yw = yh + 74.759;
    *zw = zh + 246.057;
    return;
}

/* parameters.txt file example follows:

Dat_01.001
26499
4900
75042.619
75058.393
4
19612
19612

dem.bil
2262
1804
25
280287.5
4263837.5

outputfileimage

299792458.0
5300000000.0
7.904

4652355.197
1850762.336
3938826.869
0.0
0.0
0.0
-19676
3480
75054.33800903
858747.84841288

 6.302442111323E-04
-1.450465050581E+02
 1.111826138101E+07
-2.838617046323E+11

 7.193354916155E-04
-1.626518662842E+02
 1.225595766451E+07
-3.077478044178E+11

-9.943737224445E-04
 2.215240551222E+02
-1.644249789209E+07
 4.066199880725E+11
*/