#define __USE_GNU
#define _GNU_SOURCE

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <ctype.h>
#include <time.h>
#include <sysexits.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_statistics.h>
#include <fitsio.h>



int main (int argc, char *argv[]) {
  fitsfile *A;
  fitsfile *B;
  fitsfile *O;

  int status = 0;

  int c = 0;
  int opt_index;

  int operation = 0;
  
  static struct option long_options[] = {
    {"help",     no_argument, 0, 'h'},
    {"add",      no_argument, 0, 'A'},
    {"subtract", no_argument, 0, 'S'},
    {"multiply", no_argument, 0, 'M'},
    {"divide",   no_argument, 0, 'D'},
    {0,0,0,0}
  };

  while ((c = getopt_long(argc,argv,"hASMD",
			  long_options,&opt_index)) != -1) {
    switch (c) {
    case 'h':
      fprintf(stderr,"immath3d [options] imageA imageB outimage\n");
      fprintf(stderr,"    -A, --add               (%d)\n",operation & 0x01);
      fprintf(stderr,"    -S, --subtract          (%d)\n",operation & 0x02);
      fprintf(stderr,"    -M, --multiply          (%d)\n",operation & 0x04);
      fprintf(stderr,"    -D, --divide            (%d)\n",operation & 0x08);
      exit(1);
    case 'A':
      operation = 0x01;
      break;
    case 'S':
      operation = 0x02;
      break;
    case 'M':
      operation = 0x04;
      break;
    case 'D':
      operation = 0x08;
      break;
    case '?':
      if (isprint(optopt)) {
	fprintf(stderr, "Unknown option `-%c'.\n",optopt);
      }
      else {
	fprintf(stderr, "Unknown option character `\\x%x'.\n",optopt);
      }
      break;
    default:
      fprintf(stderr, "Unknown option `-%c'.\n",optopt);
      break;
    }
  }
  
  fprintf(stderr,"%s %s (0x%x) %s\n",argv[optind],argv[optind + 1],operation,argv[optind + 2]);

#define FRE fits_report_error(stderr,status); if (status != 0) { fprintf(stderr, "  AT %s:%d\n",__FILE__,__LINE__); } ; status = 0;

  // Open everything up
  fits_open_file(&A,argv[optind],READONLY,&status);  FRE;
  fits_open_file(&B,argv[optind + 1], READONLY,&status);  FRE;
  fits_create_file(&O,argv[optind + 2], &status);  FRE;

  // Copy the structure of A to O
  fits_copy_file(A,O,1,1,1,&status);  FRE;

  // Iterate over the HDUs to perform this operation on all extensions
  int hdunum;
  fits_get_num_hdus(A,&hdunum,&status); FRE;
  int i;
  for (i = 1; i < hdunum; i++) {
    printf("%d\n",i);
    int j;
    // Get everyone on the same page.
    int type;
    fits_movabs_hdu(A,i,&type,&status); FRE;
    fits_movabs_hdu(B,i,&type,&status); FRE;
    fits_movabs_hdu(O,i,&type,&status); FRE;

    if (type != IMAGE_HDU) { continue; } // Skip tables, I guess.

    // Get dimension
    int dimA;
    int dimB;
    int dimO;
    fits_get_img_dim(A,&dimA,&status); FRE;
    fits_get_img_dim(B,&dimB,&status); FRE;
    fits_get_img_dim(O,&dimO,&status); FRE;
    if (dimA == 0) {
      continue;
    }
    
    // Get size
    long *fpixelA = malloc(dimA * sizeof(long));
    long *fpixelB = malloc(dimB * sizeof(long));
    long *fpixelO = malloc(dimO * sizeof(long));
    long *lpixelA = malloc(dimA * sizeof(long));
    long *lpixelB = malloc(dimB * sizeof(long));
    long *lpixelO = malloc(dimO * sizeof(long));
    fits_get_img_size(A,dimA,lpixelA,&status); FRE;
    fits_get_img_size(B,dimB,lpixelB,&status); FRE;
    fits_get_img_size(O,dimO,lpixelO,&status); FRE;

    long nA = lpixelA[0]; fpixelA[0] = 1;
    long nB = lpixelB[0]; fpixelB[0] = 1;
    long nO = lpixelO[0]; fpixelO[0] = 1;
    for (j = 1; j < dimA; j++) {       nA *= lpixelA[j]; fpixelA[j] = 1;}
    for (j = 1; j < dimB; j++) {       nB *= lpixelB[j]; fpixelB[j] = 1;}
    for (j = 1; j < dimO; j++) {       nO *= lpixelO[j]; fpixelO[j] = 1;}

    printf("  %d %d %d\n",dimA,dimB,dimO);
    
    // Get data
    double *dataA = malloc(sizeof(double) * nA);
    double *dataB = malloc(sizeof(double) * nB);
    double *dataO = malloc(sizeof(double) * nO);

    fits_read_pix(A,TDOUBLE,fpixelA,nA,
		  NULL,dataA,NULL,&status); FRE;
    fits_read_pix(B,TDOUBLE,fpixelB,nB,
		  NULL,dataB,NULL,&status); FRE;

    // Do operation
    for (j = 0; j < nA; j++) {
      dataO[j] = 0.0;
      switch (operation) {
      case 0x01:
	dataO[j] = dataA[j] + dataB[j];
	break;
      case 0x02:
	dataO[j] = dataA[j] - dataB[j];
	break;
      case 0x04:
	dataO[j] = dataA[j] * dataB[j];
	break;
      case 0x08:
	dataO[j] = dataA[j] / dataB[j];
	break;
      }
    }

    // Save this output
    fits_write_pix(O,TDOUBLE,fpixelO,nO,dataO,&status); FRE;

    // Cleanup
    free(fpixelA);
    free(fpixelB);
    free(fpixelO);
    free(lpixelA);
    free(lpixelB);
    free(lpixelO);

    free(dataA);
    free(dataB);
    free(dataO);
    
  }

  fits_close_file(A,&status); FRE;
  fits_close_file(B,&status); FRE;
  fits_close_file(O,&status); FRE;
  return(0);
}

