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

/**/
struct sph_harm {
    int len;
    int* ll;
    int* mm;
    float* clm;
    float* slm;
};

typedef struct sph_harm sph_harm;


/**/
struct mesh {
    int ntheta;
    int nphi;
    double *theta;
    double *phi;
};

typedef struct mesh mesh;


/**/
struct field {
    mesh *grid;
    float **data;
};

typedef struct field field;


void print_help()
{
    const char msg[] = ""
        "Project the spherical harmonic coefficients to a regular mesh\n"
        "\n"
        "Usage: project_geoid infile outfile n_longitude n_latitude\n"
        "\n"
        "infile: CitcomS geoid file\n"
        "outfile: file will contain 3 columns (longitude, latitude, geoid)\n"
        "n_longitude: # of grid points in longitude direction for the mesh\n"
        "n_latitude: # of grid points in latitudee direction for the mesh\n";

    fputs(msg, stderr);

    return;
}


void allocate_sph_harm(sph_harm *coeff, int len)
{
    coeff->ll = malloc(len * sizeof(int));
    coeff->mm = malloc(len * sizeof(int));
    coeff->clm = malloc(len * sizeof(float));
    coeff->slm = malloc(len * sizeof(float));
}


void get_sph_harm_coeff(char *filename, sph_harm* coeff)
{
    FILE *fp;
    char buffer[256];
    int junk0;
    float fjunk0, fjunk1, fjunk2, fjunk3;
    int ll_max, index;


    /* open CitcomS geoid file */
    fp = fopen(filename, "r");
    if(fp == NULL) {
        snprintf(buffer, 255, "Error: cannot open file: %s\n", filename);
        fputs(buffer, stderr);
    }


    /* read in the max sph harm degree */
    fgets(buffer, 255, fp);
    sscanf(buffer, "%d %d %f", &junk0, &ll_max, &fjunk0);
    /* fprintf(stderr, "%d %d %f\n", junk0, ll_max, fjunk0); */


    /* allocate memory */
    coeff->len = (ll_max + 1) * (ll_max + 2) / 2;
    allocate_sph_harm(coeff, coeff->len);


    /* read in the geoid coefficients */
    for(index=0; index<coeff->len; index++) {
        fgets(buffer, 255, fp);
        sscanf(buffer, "%d %d %f %f %f %f %f %f",
               &(coeff->ll[index]), &(coeff->mm[index]),
               &(coeff->clm[index]), &(coeff->slm[index]),
               &fjunk0, &fjunk1, &fjunk2, &fjunk3);
        /*
        fprintf(stderr, "%d %d %d %e %e\n", index,
                coeff->ll[index], coeff->mm[index],
                coeff->clm[index], coeff->slm[index]);
        */
    }

    return;
}


void get_test_coeff(sph_harm *coeff)
{
    coeff->len = 1;
    allocate_sph_harm(coeff, coeff->len);

    coeff->ll[0] = 3;
    coeff->mm[0] = 2;
    coeff->clm[0] = 1e-2;
    coeff->slm[0] = 0;

    return;
}


void get_mesh(mesh *grid, int ntheta, int nphi)
{
    int i;

    grid->ntheta = ntheta;
    grid->nphi = nphi;

    /* allocate memory */
    grid->theta = malloc(ntheta * sizeof(double));
    grid->phi = malloc(nphi * sizeof(double));


    /* create a regular mesh */
    for(i=0; i<grid->ntheta; i++)
        grid->theta[i] = i * (M_PI / (grid->ntheta-1));

    for(i=0; i<grid->nphi; i++)
        grid->phi[i] = i * (2.0 * M_PI / (grid->nphi-1));

    /*
    for(i=0; i<grid->ntheta; i++)
        fprintf(stderr, "%d, %f\n", i, grid->theta[i]);
    for(i=0; i<grid->nphi; i++)
        fprintf(stderr, "%d, %f\n", i, grid->phi[i]);
    */
    return;
}


void allocate_field(field* geoid)
{
    int i;
    mesh *grid = geoid->grid;

    /* allocate memory */
    geoid->data = malloc(grid->ntheta * sizeof(float));
    for(i=0; i<grid->ntheta; i++)
        geoid->data[i] = calloc(grid->nphi, sizeof(float));

    return;
}


/* Compute fully normalized spherical harmonics coefficients
 * Copied from CitcomS lib/General_matrix_functions.c */

double modified_plgndr_a(int l, int m, double t)
{
    int i,ll;
    double x,fact1,fact2,fact,pll,pmm,pmmp1,somx2,plgndr;
    const double three=3.0;
    const double two=2.0;
    const double one=1.0;

    x = cos(t);
    pmm=one;
    if(m>0) {
        somx2=sqrt((one-x)*(one+x));
        fact1= three;
        fact2= two;
        for (i=1;i<=m;i++)   {
            fact=sqrt(fact1/fact2);
            pmm = -pmm*fact*somx2;
            fact1+=  two;
            fact2+=  two;
        }
    }

    if (l==m)
        plgndr = pmm;
    else  {
        pmmp1 = x*sqrt(two*m+three)*pmm;
        if(l==m+1)
            plgndr = pmmp1;
        else   {
            for (ll=m+2;ll<=l;ll++)  {
                fact1= sqrt((4.0*ll*ll-one)*(double)(ll-m)/(double)(ll+m));
                fact2= sqrt((2.0*ll+one)*(ll-m)*(ll+m-one)*(ll-m-one)
                            /(double)((two*ll-three)*(ll+m)));
                pll = ( x*fact1*pmmp1-fact2*pmm)/(ll-m);
                pmm = pmmp1;
                pmmp1 = pll;
            }
            plgndr = pll;
        }
    }

    plgndr /= sqrt(4.0*M_PI);

    if (m!=0) plgndr *= sqrt(two);

    return plgndr;
}


void project_sph_harm_to_mesh(sph_harm *coeff, field *geoid)
{
    int ll, mm;
    int i, j;
    int index;
    float val;
    float *cosm, *sinm;
    mesh *grid = geoid->grid;
    /* double modified_plgndr_a(int, int, double); */

    allocate_field(geoid);

    cosm = malloc(grid->nphi * sizeof(float));
    sinm = malloc(grid->nphi * sizeof(float));

    /* projecting */
    for(index=0; index<coeff->len; index++) {
        ll = coeff->ll[index];
        mm = coeff->mm[index];

        for(j=0; j<grid->nphi; j++) {
            cosm[j] = cos(mm * grid->phi[j]);
            sinm[j] = sin(mm * grid->phi[j]);
        }

        for(i=0; i<grid->ntheta; i++) {
            /* val = Plm(theta) */
            val = modified_plgndr_a(ll, mm, grid->theta[i]);

            for(j=0; j<grid->nphi; j++) {
                /* data = val*(Clm*cos(m*phi) + Slm*sin(m*phi)) */
                geoid->data[i][j] += val *
                    (coeff->clm[index] * cosm[j] +
                     coeff->slm[index] * sinm[j]);
            }
        }
    }

    free(cosm);
    free(sinm);

    return;
}


void write_projected_field(char *filename, field *geoid)
{
    FILE *fp;
    char buffer[256];
    int i, j;
    float r2d = 180 / M_PI;

    /* open output file */
    fp = fopen(filename, "w");
    if(fp == NULL) {
        snprintf(buffer, 255, "Error: cannot open file: %s\n", filename);
        fputs(buffer, stderr);
    }


    /* write the field */
    /* the order is (longitude, latitude, geoid) */
    for(i=0; i<geoid->grid->ntheta; i++)
        for(j=0; j<geoid->grid->nphi; j++) {
            fprintf(fp, "%.3f %.3f %e\n",
                    geoid->grid->phi[j] * r2d,
                    90 - geoid->grid->theta[i] * r2d,
                    geoid->data[i][j]);
        }

    fclose(fp);
    return;
}


int main(int argc, char **argv)
{
    sph_harm coeff;
    mesh grid;
    field geoid_field;
    int ntheta, nphi;

    /* check the input */
    if(argc == 1) {
        print_help();
        return 1;
    }

    if(argc != 5) {
        fputs("Not enought input parameters provided!\n"
              "Run command with no argument for usage.\n", stderr);
        return 1;
    }

    /* we will use (theta, phi) in radian internally and will write the
     * result as (longitude, latitude) in degree later */
    ntheta = strtol(argv[3], NULL, 10);
    nphi = strtol(argv[4], NULL, 10);


    /* create a uniform mesh of (theta, phi) with (ntheta * nphi) points */
    /* theta in [0, pi] and phi in [0, 2*pi] */
    get_mesh(&grid, ntheta, nphi);

    /* attach the mesh to a (currently empty) field */
    geoid_field.grid = &grid;

    /* read the spherical harmonic coefficients from CitcomS geoid file */
    get_sph_harm_coeff(argv[1], &coeff);
    /* if debug, using this coeff */
    /* get_test_coeff(&coeff); */

    /* project the sph harm coefficients to the mesh */
    project_sph_harm_to_mesh(&coeff, &geoid_field);

    /* write the projected field as (longitude, latitude, field) */
    write_projected_field(argv[2], &geoid_field);

    return 0;
}