#include <stdio.h>
#include "path.h"
#include <stddef.h> /* for size_t */
#include "mt19937ar.h"
#include <math.h> /* for sin(), cos(), etc. */
#include "vector.h"
#include <errno.h> /* for errno */
#include <string.h> /* for strerror() */
#include "random.h"

typedef struct sim {
    double *pos;
    path *p;
} sim;

static double getKineticEnergy(double x, double T0)
{
    return 1.0;
}

sim *simulate_path(double *pos0, double *dir0, double range, double theta0, size_t N, size_t n)
{
    size_t i;
    double *s, *theta1, *theta2, *x, *y, *z, *z1, *z2, tmp1[3], tmp2[3], k[3], normal[3];
    double dx, dy, dz, theta, phi;

    sim *simvalue = malloc(sizeof(sim));

    s = calloc(N,sizeof(double));
    theta1 = calloc(N,sizeof(double));
    theta2 = calloc(N,sizeof(double));
    x = calloc(N,sizeof(double));
    y = calloc(N,sizeof(double));
    z = calloc(N,sizeof(double));
    simvalue->pos = calloc(N*3,sizeof(double));
    z1 = calloc(n,sizeof(double));
    z2 = calloc(n,sizeof(double));

    init_genrand(0);

    simvalue->pos[0] = pos0[0];
    simvalue->pos[1] = pos0[1];
    simvalue->pos[2] = pos0[2];

    for (i = 1; i < N; i++) {
        s[i] = range*i/(N-1);

        theta1[i] = theta1[i-1] + randn()*theta0*sqrt(s[i]-s[i-1]);
        theta2[i] = theta2[i-1] + randn()*theta0*sqrt(s[i]-s[i-1]);

        theta = sqrt(theta1[i]*theta1[i] + theta2[i]*theta2[i]);
        phi = atan2(theta2[i],theta1[i]);

        dx = (s[i]-s[i-1])*sin(theta)*cos(phi);
        dy = (s[i]-s[i-1])*sin(theta)*sin(phi);
        dz = (s[i]-s[i-1])*cos(theta);

        x[i] = x[i-1] + dx;
        y[i] = y[i-1] + dy;
        z[i] = z[i-1] + dz;

        tmp1[0] = x[i];
        tmp1[1] = y[i];
        tmp1[2] = z[i];

        k[0] = 0.0;
        k[1] = 0.0;
        k[2] = 1.0;

        CROSS(normal,k,dir0);

        normalize(normal);

        phi = acos(DOT(k,dir0));

        rotate(tmp2,tmp1,normal,phi);

        simvalue->pos[i*3] = pos0[0] + tmp2[0];
        simvalue->pos[i*3+1] = pos0[1] + tmp2[1];
        simvalue->pos[i*3+2] = pos0[2] + tmp2[2];
    }

    for (i = 0; i < n; i++) {
        z1[i] = path_get_coefficient(i+1,s,theta1,theta0,N);
        z2[i] = path_get_coefficient(i+1,s,theta2,theta0,N);
    }

    simvalue->p = path_init(pos0,dir0,1.0,range,theta0,getKineticEnergy,z1,z2,n,1.0);

    free(s);
    free(theta1);
    free(theta2);
    free(x);
    free(y);
    free(z);
    free(z1);
    free(z2);

    return simvalue;
}

void usage(void)
{
    fprintf(stderr,"Usage: ./test-path [options]\n");
    fprintf(stderr,"  -N      number of points along track\n");
    fprintf(stderr,"  -n      number of terms in KL expansion\n");
    fprintf(stderr,"  -t      standard deviation of angular distribution\n");
    fprintf(stderr,"  --range range of track\n");
    fprintf(stderr,"  -h      display this help message\n");
    exit(1);
}

int main(int argc, char **argv)
{
    size_t i, n, N;
    double pos0[3], dir0[3], theta0, range, pos2[3], dir[3], T, t;

    n = 2;
    N = 1000;
    theta0 = 0.1;
    range = 100.0;

    for (i = 1; i < argc; i++) {
        if (!strncmp(argv[i], "--", 2)) {
            if (!strcmp(argv[i]+2,"range")) {
                range = strtod(argv[++i],NULL);
                continue;
            }
        } else if (argv[i][0] == '-') {
            switch (argv[i][1]) {
            case 'N':
                N = atoi(argv[++i]);
                break;
            case 'n':
                n = atoi(argv[++i]);
                break;
            case 't':
                theta0 = strtod(argv[++i],NULL);
                break;
            case 'h':
                usage();
            default:
                fprintf(stderr, "unrecognized option '%s'\n", argv[i]);
                exit(1);
            }
        }
    }

    pos0[0] = 0.0;
    pos0[1] = 0.0;
    pos0[2] = 0.0;

    dir0[0] = 1.0;
    dir0[1] = 0.0;
    dir0[2] = 0.0;

    sim *simvalue = simulate_path(pos0,dir0,range,theta0,N,n);

    FILE *pipe = popen("graph -T X --bitmap-size 2000x2000 -X X -Y Y", "w");

    if (!pipe) {
        fprintf(stderr, "error running graph command: %s\n", strerror(errno));
        exit(1);
    }

    for (i = 0; i < N; i++) {
        fprintf(pipe,"%.10g %.10g\n", simvalue->pos[i*3], simvalue->pos[i*3+1]);
    }
    fprintf(pipe,"\n\n");

    for (i = 0; i < N; i++) {
        path_eval(simvalue->p,i*range/(N-1),pos2,dir,&T,&t,&theta0);
        fprintf(pipe,"%.10g %.10g\n", pos2[0], pos2[1]);
    }
    fprintf(pipe,"\n\n");

    if (pclose(pipe)) {
        fprintf(stderr, "error closing graph command: %s\n", strerror(errno));
        exit(1);
    }

    return 0;
}