/*
** compute correlation matrix
**
** G.Lohmann, Feb 2011
*/

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_histogram.h>

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

extern void VNormalize(float *data,int nt,VBoolean stddev);
extern long GetAddr(VImage mapimage,int bi,int ri,int ci,int m,int k,int l);

int VNumNeigbours(size_t id,VImage map,VImage mapimage,int adjdef)
{
  int nslices = VImageNBands(mapimage);
  int nrows = VImageNRows(mapimage);
  int ncols = VImageNColumns(mapimage);

  int bi = VPixel(map,0,0,id,VShort);
  int ri = VPixel(map,0,1,id,VShort);
  int ci = VPixel(map,0,2,id,VShort);
  if (bi < 1 || bi >= nslices-2) return 0;
  if (ri < 1 || ri >= nrows-2) return 0;
  if (ci < 1 || ci >= ncols-2) return 0;

  int wn=1,rad2=0;
  if (adjdef == 3) {
    wn = 2;
    rad2 = 2*2;
  }

  int n=0;
  int k,l,m;
  for (m=-wn; m<=wn; m++) {
    for (k=-wn; k<=wn; k++) {
      for (l=-wn; l<=wn; l++) {
      	long jj = GetAddr(mapimage,bi,ri,ci,m,k,l);
      	if (jj < 0) continue;  /* outside of brain mask */

      	if (adjdef == 0) {     /* 6 adjacency */
      	  if (ABS(m)+ABS(k)+ABS(l) > 1) continue;
      	}
      	if (adjdef == 1) {     /* 18-adjacency */
      	  if (ABS(m) > 0 && ABS(k) > 0 && ABS(l) > 0) continue;
      	}
	if (adjdef == 3) {     /* sphere */
	  if (m*m + k*k + l*l > rad2) continue;
	}
      	n++;
      }
    }
  }
  return n;
}

/* convert to ranks */
void GetRank(float *data,gsl_vector *v,gsl_permutation *perm,gsl_permutation *rank)
{
  size_t i;
  size_t n = v->size;
  for (i=0; i<n; i++) gsl_vector_set(v,i,(double)data[i]);
  gsl_sort_vector_index (perm, v);
  gsl_permutation_inverse (rank, perm);
  for (i=0; i<n; i++) data[i] = (float)rank->data[i];
}

double Spearman(const float *data1,const float *data2,int n)
{
  int i;
  double nx = (double)n;
  double kx = nx*(nx*nx-1.0);
  double sxy=0.0;
  for (i=0; i<n; i++) {
    const double u = (double)data1[i];
    const double v = (double)data2[i];
    const double d = (u-v);
    sxy += d*d;
  }
  double rho = 1.0 - 6.0*sxy/kx;
  return rho;
}

double Correlation(const float *data1,const float *data2,int n)
{
  int i;
  double corr=0;
  for (i=0; i<n; i++) {
    const double u = (double)data1[i];
    const double v = (double)data2[i];
    corr += u*v;
  }
  corr /= (double)n;
  return corr;
}

float EdgeCorr(const float *data1,const float *data2,int n,int metric)
{
  float corr=0.0;
  if (metric == 0) corr = Correlation(data1,data2,n);
  if (metric == 1) corr = Spearman(data1,data2,n);
  if (corr > 0) return corr;
  else return 0.0;
}

void GetSNR(gsl_matrix_float **X1,gsl_matrix_float **X2,int *table,int n,gsl_matrix_float *SNR,int metric)
{
  long j,nvox=X1[0]->size1;
  long k,s,nt=X1[0]->size2;

  if (n < 3) VError(" n: %d",n);
  double nx = (double)n;
  double *sum1 = (double *)VCalloc(nt,sizeof(double));
  double *sum2 = (double *)VCalloc(nt,sizeof(double));

  gsl_vector *vec = NULL;
  gsl_permutation *perm = NULL;
  gsl_permutation *rank = NULL;
  if (metric == 1) {  /* only needed for spearman correlation */
    vec = gsl_vector_calloc(nt);
    perm = gsl_permutation_alloc(nt);
    rank = gsl_permutation_alloc(nt);
  }

  double ave=0,var=0,snr=0;

  for (j=0; j<nvox; j++) {
    memset(sum1,0,nt*sizeof(double));
    memset(sum2,0,nt*sizeof(double));
    const float *pp=NULL;
    for (s=0; s<n; s++) {
      if (table[s] == 0)
	pp = gsl_matrix_float_const_ptr(X1[s],j,0);
      else
	pp = gsl_matrix_float_const_ptr(X2[s],j,0);
      for (k=0; k<nt; k++) {
	const double x = (double)(*pp++);
	sum1[k] += x;
	sum2[k] += x*x;
      }
    }
    for (k=0; k<nt; k++) {
      ave = (sum1[k]/nx);
      var = (sum2[k] - nx*ave*ave) / (nx - 1.0);
      snr = 0;
      if (var > 0) snr = ave / sqrt(var);
      gsl_matrix_float_set(SNR,j,k,snr);
    }

    if (metric == 0) {  /* pearson correlation */
      float *qq = gsl_matrix_float_ptr(SNR,j,0);
      VNormalize(qq,nt,TRUE);
    }
    if (metric == 1) {  /* spearman ranks      */
      float *qq = gsl_matrix_float_ptr(SNR,j,0);
      GetRank(qq,vec,perm,rank);
    }
  }
  VFree(sum1);
  VFree(sum2);
}

float ZMatrix(gsl_histogram *histogram,gsl_matrix_float *SNR1,gsl_matrix_float *SNR2,int n1,int n2,
	      VImage roi,VImage map,VImage mapimage,int adjdef,float elength,float quantile,int step,int metric)
{
  size_t i;
  size_t nvox=SNR1->size1;
  int nt=SNR1->size2;
  size_t progress=0;
  int rad2 = (int)(elength*elength);
  double tiny=1.0e-8;
  gsl_set_error_handler_off ();

  fprintf(stderr," Computing matrix...\n");

  gsl_histogram_reset(histogram);
  size_t nbins = gsl_histogram_bins (histogram);
  double hmax = gsl_histogram_max (histogram);
  double hmin = gsl_histogram_min (histogram);
  double zmin = 99999.0;
  double zmax = -99999.0;
  int minadj = 1;

#pragma omp parallel for shared(progress,histogram) schedule(dynamic) firstprivate(SNR1,SNR2)
  for (i=0; i<nvox; i+=step) {
    if (i%1000 == 0) fprintf(stderr," %ld000\r",(long)progress++);
    int bi = (int)VPixel(map,0,0,i,VShort);
    int ri = (int)VPixel(map,0,1,i,VShort);
    int ci = (int)VPixel(map,0,2,i,VShort);
    int nadjx = VNumNeigbours(i,map,mapimage,adjdef);
    if (nadjx < minadj) continue;

    int roiflagi = 0;
    if (roi != NULL) {
      if (VGetPixel(roi,bi,ri,ci) > 0.5) roiflagi = 1;
    }

    gsl_histogram *tmphist = gsl_histogram_alloc (nbins);
    gsl_histogram_set_ranges_uniform (tmphist,hmin,hmax);
    gsl_histogram_reset(tmphist);

    const float *datax1 = gsl_matrix_float_const_ptr(SNR1,i,0);
    const float *datax2 = gsl_matrix_float_const_ptr(SNR2,i,0);

    size_t j=0;
    for (j=0; j<i; j+=step) {
      int bj = (int)VPixel(map,0,0,j,VShort);
      int rj = (int)VPixel(map,0,1,j,VShort);
      int cj = (int)VPixel(map,0,2,j,VShort);
      int d = SQR(bi-bj) + SQR(ri-rj) + SQR(ci-cj);
      if (d < rad2) continue;
      int nadjy = VNumNeigbours(j,map,mapimage,adjdef);
      if (nadjy < minadj) continue;

      int roiflagj = 0;
      if (roi != NULL) {
	if (VGetPixel(roi,bj,rj,cj) > 0.5) roiflagj = 1;
	if (roiflagi + roiflagj != 1) continue;
      }
      const float *datay1 = gsl_matrix_float_const_ptr(SNR1,j,0);
      const float *datay2 = gsl_matrix_float_const_ptr(SNR2,j,0);

      /* edge z-value */
      double z1 = EdgeCorr(datax1,datay1,nt,metric);
      double z2 = EdgeCorr(datax2,datay2,nt,metric);
      double z = (z1-z2);
      if (z < zmin) zmin = z;
      if (z > zmax) zmax = z;

      if (z < hmin) z = hmin;
      if (z > hmax-tiny) z = hmax-tiny;
      gsl_histogram_increment (tmphist,z);
    }

#pragma omp critical
    {
      gsl_histogram_add (histogram,tmphist);
    }
    gsl_histogram_free (tmphist);
  }

  /* get quantile cutoff */
  gsl_histogram_pdf *pdf = gsl_histogram_pdf_alloc(nbins);
  gsl_histogram_pdf_init(pdf,histogram);
  double lower=0,upper=0;

  size_t i0=0;
  for (i=nbins-1; i>=0; i--) {
    gsl_histogram_get_range (histogram,i,&lower,&upper);
    if (pdf->sum[i] < quantile) {
      if (gsl_histogram_get_range (histogram,i,&lower,&upper) == GSL_EDOM) VError(" err hist");
      i0 = i;
      break;
    }
  }
  double sum=0;
  for (i=nbins-1; i>i0; i--) {
    sum += gsl_histogram_get(histogram,i);
  }
  return (float)upper;
}


/* randist/pascal.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * 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, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The Pascal distribution is a negative binomial with valued integer n

   prob(k) =  (n - 1 + k)!/(n!(k - 1)!) *  p^n (1-p)^k for k = 0, 1, ..., n

   */

unsigned int
gsl_ran_pascal (const gsl_rng * r, double p, unsigned int n)
{
  /* This is a separate interface for the pascal distribution so that
     it can be optimized differently from the negative binomial in
     future.

     e.g. if n < 10 it might be faster to generate the Pascal
     distributions as the sum of geometric variates directly.  */

  unsigned int k = gsl_ran_negative_binomial (r, p, (double) n);
  return k;
}

double
gsl_ran_pascal_pdf (const unsigned int k, const double p, unsigned int n)
{
  double P = gsl_ran_negative_binomial_pdf (k, p, (double) n);
  return P;
}


#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <dc1394/dc1394.h>
#include <math.h>
#include <fitsio.h>
#include <gsl/gsl_statistics.h>
#include <xpa.h>
#include <time.h>
#include <sys/time.h>
#include <string.h>
#include <inttypes.h>

#define NXPA 10

typedef struct _Box {
    double  x;
    double  y;
    double  fwhm;
    double  cenx;
    double  ceny;
    double  counts;
    double  background;
    double  noise;
    double  sigmaxythresh;
    double  snr;
    double  sigmaxy;
    double  sigmafwhmthresh;
    double  sigmafwhm;
    double  strehl;
    double  max;
    int     r;
} Box;

typedef struct _Back {
    double x;
    double y;
    double background;
    double sigma;
    int r;
    int width;
} Back;

Box box[3];
Back back;
long nelements, naxes[2], fpixel;
int boxsize = 17;

/* MASSDIMM values */
/*  20101228:  determined DIMM pixel scale to be 1.78"/pixel using HR1866
    and HR8895 as astrometric double stars
*/
double pixel_scale = 1.78;
double d = 0.080;
double r = 0.170;

double lambda = 0.6e-6;

double stardist(int i, int j) {
    return( sqrt( (box[i].cenx-box[j].cenx)*(box[i].cenx-box[j].cenx) +
		  (box[i].ceny-box[j].ceny)*(box[i].ceny-box[j].ceny) ) );
}

/* this routine uses a. tokovinin's modified equation given in
   2002, PASP, 114, 1156
 */
double seeing(double var) {
    double b, K, seeing;

    b = r/d;

    /* pixel scale in "/pixel, convert to rad */
    var = var * pow(pixel_scale/206265.0, 2);

    K = 0.364 * (1.0 - 0.532 * pow(b, -1/3) - 0.024 * pow(b, -7/3));

    seeing = 206265.0 * 0.98 * pow(d/lambda, 0.2) * pow(var/K, 0.6);

    return seeing;
}

/* this routine uses the classic DIMM equation */
double old_seeing(double var) {
    double r0;

    /* pixel scale in "/pixel, convert to rad */
    var = var * pow(pixel_scale/206265.0, 2);

    r0 = pow(2.0 * (lambda * lambda) * (
		 ( 0.1790 * pow(d, -1.0/3.0) -
		   0.0968 * pow(r, -1.0/3.0) ) / var
		 ), 0.6);

    // return 206265.0*0.98*lambda/r0;
    return r0;
}

/* measure the background in an annulus around the spot pattern */
int background(unsigned char *image, int imwidth, int imheight) {
    int i, j, backpix;
    int low_y, up_y, low_x, up_x;
    double dist, sum, sumsq;

    backpix = 0;
    sum = 0.0;
    sumsq = 0.0;

    low_y = back.y - back.r - back.width;
    up_y  = back.y + back.r + back.width;
    low_x = back.x - back.r - back.width;
    up_x  = back.x + back.r + back.width;
    if (low_y < 0) {
	low_y = 0;
    }
    if (up_y >= imheight) {
	up_y = imheight;
    }
    if (low_x < 0) {
	low_x = 0;
    }
    if (up_x >= imwidth) {
	up_x = imwidth;
    }

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
	    if (dist >= back.r && dist <= back.r+back.width) {
		sum += image[i*naxes[0]+j];
		backpix++;
	    }
	}
    }

    back.background = sum/backpix;
    //back.background = 0.0;

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
	    if (dist >= back.r && dist <= back.r+back.width) {
		sumsq += (image[i*naxes[0]+j]-back.background)*
		    (image[i*naxes[0]+j]-back.background);
	    }
	}
    }

    back.sigma = sqrt(sumsq/backpix);
    //back.sigma = 1.0;

    return 1;
}

/* measure centroid using center-of-mass algorithm */
int centroid(unsigned char *image, int imwidth, int imheight, int num) {
    int i, j;
    double  max    = 0.0;
    double  sum    = 0.0;
    double  sumx   = 0.0;
    double  sumxx  = 0.0;
    double  sumy   = 0.0;
    double  sumyy  = 0.0;
    double  val = 0.0;
    double  gain = 0.9;
    double  rmom;
    double  dist, dx;
    double  nsigma = 3.0;
    int low_y, up_y, low_x, up_x;
    int sourcepix = 0;

    dx = pixel_scale/206265.0;

    low_y = box[num].y - box[num].r;
    up_y  = box[num].y + box[num].r;
    low_x = box[num].x - box[num].r;
    up_x  = box[num].x + box[num].r;
    if (low_y < 0) {
	low_y = 0;
    }
    if (up_y >= imheight) {
	up_y = imheight;
    }
    if (low_x < 0) {
	low_x = 0;
    }
    if (up_x >= imwidth) {
	up_x = imwidth;
    }

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(box[num].x-j, 2) + pow(box[num].y-i, 2));
	    if (dist <= box[num].r) {
		val = image[i*naxes[0]+j] - back.background;
		if (val >= nsigma*back.sigma) {
		    if (val > max) {
			max = val;
		    }
		    sum   += val;
		    sumx  += val*j;
		    sumxx += val*j*j;
		    sumy  += val*i;
		    sumyy += val*i*i;
		    sourcepix++;
		}
	    }
	}
    }

    if ( sum <= 0.0 ) {
	box[num].sigmaxy = -1.0;
	box[num].sigmafwhm = -1.0;
	box[num].snr = 0.0;
	box[num].fwhm = -1.0;
	box[num].counts = 1.0;
	box[num].strehl = 0.0;
    } else {
	box[num].max = max;
	box[num].strehl = (max/sum)*(4.0/3.14159)*pow( lambda/(d*dx), 2);
	rmom = ( sumxx - sumx * sumx / sum + sumyy - sumy * sumy / sum ) / sum;

	if ( rmom <= 0 ) {
	    box[num].fwhm = -1.0;
    } else {
	    box[num].fwhm = sqrt(rmom)  * 2.354 / sqrt(2.0);
    }

    box[num].counts = sum;
    box[num].cenx   = sumx / sum;
    box[num].ceny   = sumy / sum;
    box[num].noise  = back.sigma*sourcepix;
    box[num].x += gain*(box[num].cenx - box[num].x);
    box[num].y += gain*(box[num].ceny - box[num].y);
    box[num].snr = sum/sqrt(box[num].noise*box[num].noise + sum);
    box[num].sigmaxy = 1.0 / box[num].snr / sqrt(6.0);
    box[num].sigmafwhm = back.sigma * pow(sourcepix,1.5) / 10.
	/ box[num].fwhm / box[num].counts
	* 2.354 * 2.354 / 2.0;
  }

  return 1;

}

int grab_frame(dc1394camera_t *c, unsigned char *buf, int nbytes) {
    dc1394video_frame_t *frame=NULL;
    dc1394error_t err;

    err = dc1394_capture_dequeue(c, DC1394_CAPTURE_POLICY_WAIT, &frame);
    if (err != DC1394_SUCCESS) {
	dc1394_log_error("Unable to capture.");
	dc1394_capture_stop(c);
	dc1394_camera_free(c);
	exit(1);
    }
    memcpy(buf, frame->image, nbytes);
    dc1394_capture_enqueue(c, frame);
    return 1;
}

int add_gaussian(unsigned char *buffer, float cenx, float ceny, float a, float sigma) {
    float gauss, rsq;
    int i, j, low_x, up_x, low_y, up_y, size;
    double xoff, yoff;

    xoff = 2.0*drand48() - 1.0;
    yoff = 2.0*drand48() - 1.0;

    cenx += 0.5*xoff;
    ceny += 0.5*yoff;

    size = 30;
    low_x = (int)(cenx-size);
    up_x = (int)(cenx+size);
    low_y = (int)(ceny-size);
    up_y = (int)(ceny+size);

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    rsq = (cenx - j)*(cenx - j) + (ceny - i)*(ceny - i);
	    gauss = a*expf(-1.0*rsq/(sigma*sigma));
	    if (gauss > 255)
		gauss = 255;
	    buffer[i*naxes[0]+j] += (unsigned char)gauss;
	}
    }

    return 1;
}

int main(int argc, char *argv[]) {
    dc1394_t * dc;
    dc1394camera_t * camera;
    dc1394camera_list_t * list;
    dc1394error_t err;
    dc1394video_mode_t mode;
    unsigned int max_height, max_width, winleft, wintop;
    uint64_t total_bytes = 0;
    unsigned char *buffer, *buffer2, *average;
    char *names[NXPA];
    char *messages[NXPA];
    char filename[256], xpastr[256];
    char *froot;
    fitsfile *fptr;
    int i, j, f, fstatus, status, nimages, nboxes, test, xsize, ysize;
    int nbad = 0, nbad_l = 0;
    FILE *init, *out, *cenfile;
    float xx = 0.0, yy = 0.0, xsum = 0.0, ysum = 0.0;
    float elapsed_time, fps, avemax;
    double *dist, *sig, *dist_l, *sig_l, *weight, *weight_l;
    double mean, var, var_l, avesig, airmass, exptime;
    double r0, seeing_short, seeing_long, seeing_ave;
    struct timeval start_time, end_time;
    struct tm ut;
    time_t start_sec, end_sec;
    suseconds_t start_usec, end_usec;

    srand48((unsigned)time(NULL));

    XPA xpa;
    xpa = XPAOpen(NULL);

    stderr = freopen("measure_seeing.log", "w", stderr);

    if (argc <= 3) {
	printf("Usage: measure_seeing <n> <airmass> <exptime>\n");
	exit(-1);
    }

    nimages = atoi(argv[1]);
    airmass = atof(argv[2]);
    exptime = atof(argv[3]);
    fstatus = 0;
    status = 0;
    xsize = 320;
    ysize = 240;
    naxes[0] = xsize;
    naxes[1] = ysize;
    fpixel = 1;

    fps = 1.0/exptime;
    if (fps > 330.0) {
	fps = 330.0;
    }

    nelements = naxes[0]*naxes[1];

    mode = DC1394_VIDEO_MODE_FORMAT7_1;
    dc = dc1394_new();
    if (!dc)
	return -1;
    err = dc1394_camera_enumerate(dc, &list);
    DC1394_ERR_RTN(err, "Failed to enumerate cameras.");

    if (list->num == 0) {
	dc1394_log_error("No cameras found.");
	return -1;
    }

    camera = dc1394_camera_new(dc, list->ids[0].guid);
    if (!camera) {
	dc1394_log_error("Failed to initialize camera with guid %"PRIx64".",
			 list->ids[0].guid);
	return -1;
    }
    dc1394_camera_free_list(list);

    printf("Using camera with GUID %"PRIx64"\n", camera->guid);

    // need to use legacy firewire400 mode for now.  800 not quite reliable.
    dc1394_video_set_iso_speed(camera, DC1394_ISO_SPEED_400);

    // configure camera for format7
    err = dc1394_video_set_mode(camera, mode);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't choose video mode.");
    err = dc1394_format7_get_max_image_size(camera, mode, &max_width, &max_height);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot get max image size.");

    winleft = 0;
    wintop = 0;

    err = dc1394_format7_set_roi(camera,
				 mode,
				 DC1394_COLOR_CODING_MONO8,
				 DC1394_USE_MAX_AVAIL,
				 winleft, wintop, // left, top
				 xsize, ysize);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't set ROI.");

    // set the frame rate to absolute value in frames/sec
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_FRAME_RATE,
				  DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free (camera), "cannot set framerate to manual");
    err = dc1394_feature_set_absolute_control(camera,
					      DC1394_FEATURE_FRAME_RATE, DC1394_TRUE);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free (camera),
		       "cannot set framerate to absolute mode");
    err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_FRAME_RATE, fps);
    DC1394_ERR_CLN_RTN(err,dc1394_camera_free (camera),"cannot set framerate");
    printf("I: framerate is %f fps\n", fps);

    // set the shutter speed to absolute value in seconds
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set shutter to manual");
    err = dc1394_feature_set_absolute_control(camera, DC1394_FEATURE_SHUTTER, DC1394_TRUE);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set shutter to absolute mode");
    err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_SHUTTER, exptime);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set shutter");
    printf("I: exptime is %f s\n", exptime);

    // set gain manually.  use relative value here in range 48 to 730.
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set gain to manual");
    err = dc1394_feature_set_value(camera, DC1394_FEATURE_GAIN, 400);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set gain");

    // set brightness manually.  use relative value in range 0 to 1023.
    err = dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, DC1394_FEATURE_MODE_MANUAL);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set brightness to manual");
    err = dc1394_feature_set_value(camera, DC1394_FEATURE_BRIGHTNESS, 100);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "cannot set brightness");

    err = dc1394_format7_get_total_bytes(camera, DC1394_VIDEO_MODE_FORMAT7_1, &total_bytes);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't get total bytes.");

    err = dc1394_capture_setup(camera, 16, DC1394_CAPTURE_FLAGS_DEFAULT);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Error capturing.");

    // start the camera up
    err = dc1394_video_set_transmission(camera, DC1394_ON);
    if (err != DC1394_SUCCESS) {
	dc1394_log_error("Unable to start camera iso transmission.");
	dc1394_capture_stop(camera);
	dc1394_camera_free(camera);
	return -1;
    }

    printf("Camera successfully initialized.\n");

    out = fopen("seeing.dat", "a");
    cenfile = fopen("centroids.dat", "w");
    init = fopen("init_cen_all", "r");

    i = 0;
    while (fscanf(init, "%f %f\n", &xx, &yy) != EOF) {
	box[i].x = xx;
	box[i].cenx = xx;
	box[i].y = yy;
	box[i].ceny = yy;
	box[i].r = boxsize/2.0;
	i++;
    }
    nboxes = i;
    fclose(init);

    back.r = 60;
    back.width = 10;

    /* allocate the buffers */
    if (!(dist = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate dist array.\n");
	exit(-1);
    }
    if (!(sig = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig array.\n");
	exit(-1);
    }
    if (!(weight = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig array.\n");
	exit(-1);
    }
    if (!(weight_l = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig array.\n");
	exit(-1);
    }
    if (!(dist_l = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate dist_l array.\n");
	exit(-1);
    }
    if (!(sig_l = calloc(nimages, sizeof(double)))) {
	printf("Couldn't allocate sig_l array.\n");
	exit(-1);
    }
    if (!(buffer = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate Image Buffer\n");
	exit(-1);
    }
    if (!(buffer2 = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate 2nd Image Buffer\n");
	exit(-1);
    }
    if (!(average = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate Average Image Buffer\n");
	exit(-1);
    }

    froot = "seeing.fits";
    avemax = 0.0;

    /* get initial frame */
    grab_frame(camera, buffer2, nelements*sizeof(char));
    grab_frame(camera, buffer2, nelements*sizeof(char));
    grab_frame(camera, buffer2, nelements*sizeof(char));
    grab_frame(camera, buffer2, nelements*sizeof(char));
    // add_gaussian(buffer2, 195.0, 130.0, 140.0, 1.5);
    // add_gaussian(buffer2, 140.0, 115.0, 140.0, 1.5);

    gettimeofday(&start_time, NULL);

    for (f=0; f<nimages; f++) {
	grab_frame(camera, buffer, nelements*sizeof(char));
	// add_gaussian(buffer, 195.0, 130.0, 140.0, 1.5);
	// add_gaussian(buffer, 140.0, 115.0, 140.0, 1.5);

	// find center of star images and calculate background
	xsum = 0.0;
	ysum = 0.0;
	for (i=0; i<nboxes; i++) {
	    xsum += box[i].cenx;
	    ysum += box[i].ceny;
	}
	back.x = xsum/nboxes;
	back.y = ysum/nboxes;
	background(buffer, naxes[0], naxes[1]);

	for (i=0; i<nboxes; i++) {
	    box[i].r = boxsize/2.0;
	    centroid(buffer, naxes[0], naxes[1], i);
	}

	/* fix both boxes to the same Y */
	ysum = (box[0].y + box[1].y)/2.0;
	box[0].y = ysum;
	box[1].y = ysum;

	if (box[0].snr < box[1].snr) {
	    weight[f] = (box[0].snr/box[0].counts)*(box[0].snr/box[0].counts);
	} else {
	    weight[f] = (box[1].snr/box[1].counts)*(box[1].snr/box[1].counts);
	}

	if (weight[f] == 0.0) {
	    nbad++;
	}

	dist[f] = stardist(0, 1);
	if (dist[f] > 55.0 || dist[f] < 15.0) {
	    weight[f] = 0.0;
	}

	if (box[0].fwhm > 0.0 && box[1].fwhm > 0.0) {
	    for (i=0; i<nboxes; i++) {
		fprintf(cenfile,
			"%6.2f %6.2f %5.2f %9.4f %8.4f %7.4f %7.4f %8.4f %5.3f\t ",
			box[i].cenx,
			box[i].ceny,
			box[i].fwhm,
			box[i].counts,
			back.background,
			box[i].noise,
			box[i].sigmaxy,
			box[i].snr,
			box[i].strehl
		    );
	    }
	    sig[f] = sqrt(box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy);
	    fprintf(cenfile, "%6.2f %6.2f %.1e\n", dist[f], sig[f], exptime);
	    if (box[0].max > box[1].max) {
		avemax += box[0].max;
	    } else {
		avemax += box[1].max;
	    }
	}

	/* now average two exposures */
	for (j=0; j<nelements; j++) {
	    test = (int)buffer[j] + (int)buffer2[j];
	    if (test <= 254) {
		average[j] = buffer[j]+buffer2[j];
	    } else {
		average[j] = 254;
	    }
	}
	memcpy(buffer2, buffer, nelements*sizeof(char));

	xsum = 0.0;
	ysum = 0.0;
	for (i=0; i<nboxes; i++) {
	    xsum += box[i].cenx;
	    ysum += box[i].ceny;
	}
	back.x = xsum/nboxes;
	back.y = ysum/nboxes;
	background(average, naxes[0], naxes[1]);

	for (i=0; i<nboxes; i++) {
	    box[i].r = boxsize/2.0;
	    //centroid(average, i);
	    //box[i].r = boxsize/3.0;
	    //centroid(average, i);
	    //box[i].r = boxsize/4.0;
	    centroid(average, naxes[0], naxes[1], i);
	}

	if (box[0].snr < box[1].snr) {
	    weight_l[f] = (box[0].snr/box[0].counts)*(box[0].snr/box[0].counts);
	} else {
	    weight_l[f] = (box[1].snr/box[1].counts)*(box[1].snr/box[1].counts);
	}

	if (weight_l[f] == 0.0) {
	    nbad_l++;
	}

	dist_l[f] = stardist(0, 1);
	sig_l[f] = sqrt(box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy);

	if (f % 160 == 0) {
	    sprintf(filename, "!%s", froot);
	    fits_create_file(&fptr, "!seeing.fits", &fstatus);
	    fits_create_img(fptr, BYTE_IMG, 2, naxes, &fstatus);
	    fits_write_img(fptr, TBYTE, fpixel, nelements, buffer, &fstatus);
	    fits_close_file(fptr, &fstatus);
	    fits_report_error(stdout, fstatus);

	    status = XPASet(xpa, "timDIMM", "array [xdim=320,ydim=240,bitpix=8]", "ack=false",
			    (char *)buffer, nelements, names, messages, NXPA);
	    sprintf(xpastr, "image; box %f %f %d %d 0.0",
		    box[0].x, box[0].y, boxsize, boxsize);
	    status = XPASet(xpa, "timDIMM", "regions", "ack=false",
			    xpastr, strlen(xpastr), names, messages, NXPA);
	    sprintf(xpastr, "image; box %f %f %d %d 0.0",
		    box[1].x, box[1].y, boxsize, boxsize);
	    status = XPASet(xpa, "timDIMM", "regions", "ack=false",
			    xpastr, strlen(xpastr), names, messages, NXPA);
	}

    }

    gettimeofday(&end_time, NULL);
    printf("End capture.\n");

    /*-----------------------------------------------------------------------
     *  stop data transmission
     *-----------------------------------------------------------------------*/
    err = dc1394_video_set_transmission(camera, DC1394_OFF);
    DC1394_ERR_RTN(err,"couldn't stop the camera?");

    start_sec = start_time.tv_sec;
    start_usec = start_time.tv_usec;
    end_sec = end_time.tv_sec;
    end_usec = end_time.tv_usec;

    elapsed_time = (float)((end_sec + 1.0e-6*end_usec) - (start_sec + 1.0e-6*start_usec));
    fps = nimages/elapsed_time;
    printf("Elapsed time = %g seconds.\n", elapsed_time);
    printf("Framerate = %g fps.\n", fps);

    avemax /= nimages;
    printf("Avemax is %.3f\n", avemax);

    /* analyze short exposure */
    printf("\t SHORT EXPOSURE\n");
    mean = gsl_stats_wmean(weight, 1, dist, 1, nimages);
    avesig = gsl_stats_wmean(weight, 1, sig, 1, nimages);
    printf("mean = %f, avesig = %f\n", mean, avesig);

    printf("\n");

    var = gsl_stats_wvariance_m(weight, 1, dist, 1, nimages, mean);
    var = var - avesig*avesig;
    if (var < 0.0) {
	var = 0.0;
    }
    seeing_short = seeing(var)/pow(airmass,0.6);
    r0 = old_seeing(var);
    printf("sigma = %f, seeing = %f\n", sqrt(var), seeing_short);

    /* analyze long exposure */
    printf("\t LONG EXPOSURE\n");
    mean = gsl_stats_wmean(weight_l, 1, dist_l, 1, nimages);
    avesig = gsl_stats_wmean(weight_l, 1, sig_l, 1, nimages);
    printf("mean_l = %f, avesig_l = %f\n", mean, avesig);

    printf("\n");

    var_l = gsl_stats_wvariance_m(weight_l, 1, dist_l, 1, nimages, mean);
    var_l = var_l - avesig*avesig;
    if (var_l < 0.0) {
	var_l = 0.0;
    }
    seeing_long = seeing(var_l)/pow(airmass,0.6);
    printf("sigma_l = %f, seeing_l = %f\n", sqrt(var_l), seeing_long);

    printf("Bad samples:  %d for short, %d for long.\n", nbad, nbad_l);

    /* reduce exposure when too bright */
    if (avemax > 100.0) {
      if (exptime > 1.0e-3) {
	exptime -= 1.0e-3;
      } else {
	exptime /= 2.0;
      }
      printf("\033[0;33mStar too bright, reducing exposure time to %.1e seconds.\033[0;39m\n", exptime);
    }

    /* increase if too faint, but max out at 8 ms */
    if (avemax < 25.0) {
      if (exptime >= 1.0e-3) {
	exptime += 1.0e-3;
      } else {
	exptime *= 2.0;
      }
      if (exptime > 8.0e-3) {
	exptime = 8.0e-3;
	printf("\033[0;33mStar too faint, but exposure time max'ed out at %.1e seconds.\033[0;39m\n",
	       exptime);
      } else {
	printf("\033[0;33mStar too faint, exposure time increased to %.1e seconds.\033[0;39m\n",
	       exptime);
      }
    }

    init = fopen("exptime", "w");
    fprintf(init, "%.2e\n", exptime);
    fclose(init);

    if (nbad < 30) {
	seeing_ave = pow(seeing_short, 1.75)*pow(seeing_long,-0.75);
	printf("\033[0;32mAirmass corrected seeing = %4.2f\"\033[0;39m\n\n", seeing_short);
	printf("\033[0;32mFried Parameter, R0 = %.2f cm\033[0;39m\n\n", 100*r0);

	gmtime_r(&end_sec, &ut);
	fprintf(out, "%d-%02d-%02d %02d:%02d:%02d %f %f %f %f %f %f\n",
		ut.tm_year+1900,
		ut.tm_mon+1,
		ut.tm_mday,
		ut.tm_hour,
		ut.tm_min,
		ut.tm_sec,
		var,
		var_l,
		seeing_short,
		seeing_long,
		seeing_ave,
		airmass);

	init = fopen("init_cen_all", "w");
	for (i=0; i<nboxes; i++) {
	    fprintf(init, "%f %f\n", box[i].cenx, box[i].ceny);
	}
	fclose(init);
	init = fopen("seeing.out", "w");
	fprintf(init, "%.2f\n", seeing_short);
	fclose(init);
	init = fopen("r0.out", "w");
	fprintf(init, "%.1f\n", 100*r0);
	fclose(init);
    } else {
	printf("\n\n\033[0;31mABORTING measurement!  Lost at least one box.\033[0;39m\n\n");
    }

    fclose(cenfile);
    fclose(out);

    return (status);
}


#include "../include/paralleltt.h"

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <cblas.h>
#include <lapacke.h>
#include <unistd.h>
#include <mpi.h>

#define NUM_THREAD 1
#define min(a,b) ((a)>(b)?(b):(a))
#define VERBOSE ((int) 1)

#ifndef HEAD
#define HEAD ((int) 0)
#endif

#define BUF ((int) 2)

tensor_train* TT_init(const int d, const int* restrict n)
{
    tensor_train* sim = (tensor_train*) malloc(sizeof(tensor_train));
    sim->d = d;
    sim->n = (int*) malloc(sizeof(int)*d);
    sim->r = (int*) malloc(sizeof(int)*(d+1));
    sim->trains = (double**) malloc(sizeof(double*)*d);

    sim->r[0] = 1;
    for (int j = 0; j < d; ++j) {
        sim->n[j] = n[j];

        if (j < d-1) {
            sim->r[j+1] = n[j];
        }
    }
    sim->r[d] = 1;

    for (int j = 0; j < d; ++j) {
        sim->trains[j] = (double*) malloc(sizeof(double)*(sim->r[j])*(sim->n[j])*(sim->r[j+1]));
    }

    return sim;
}

tensor_train* TT_init_rank(const int d, const int* restrict n, const int* restrict r)
{
    tensor_train* sim = (tensor_train*) malloc(sizeof(tensor_train));
    sim->d = d;
    sim->n = (int*) malloc(sizeof(int)*d);
    sim->r = (int*) malloc(sizeof(int)*(d+1));
    sim->trains = (double**) malloc(sizeof(double*)*d);

    for (int j = 0; j < d; ++j) {
        sim->n[j] = n[j];
    }
    for (int j = 0; j < d+1; ++j) {
        sim->r[j] = r[j];
    }

    for (int j = 0; j < d; ++j) {
        sim->trains[j] = (double*) malloc(sizeof(double)*(sim->r[j])*(sim->n[j])*(sim->r[j+1]));
    }

    return sim;
}

tensor_train* TT_copy(tensor_train* X)
{
    int d = X->d;
    int* n = X->n;
    int* r = X->r;
    tensor_train* Y = TT_init_rank(d, n, r);
    for (int j = 0; j < X->d; ++j) {
        int N = r[j] * n[j] * r[j+1];
        memcpy(Y->trains[j], X->trains[j], N * sizeof(double));
    }

    return Y;
}

void TT_free(tensor_train* sim)
{
    for (int j = 0; j < sim->d; ++j) {
        free(sim->trains[j]); sim->trains[j] = NULL;
    }
    free(sim->trains); sim->trains = NULL;
    free(sim->r);      sim->r = NULL;
    free(sim->n);      sim->n = NULL;
    free(sim);
}

void TT_print(tensor_train* tt)
{

    int d = tt->d;
    int* n = tt->n;
    int* r = tt->r;
    double** trains = tt->trains;
    printf("Printing the tensor train with d = %d\n",d);

    for (int ii = 0; ii < d; ++ii){
        printf("\nTensor %d/d (r[%d] = %d, n[%d]=%d, r[%d]=%d):\n",ii,ii,r[ii],ii,n[ii],ii+1,r[ii+1]);
        matrix_tt* A = matrix_tt_wrap(r[ii], n[ii] * r[ii+1], trains[ii]);
        matrix_tt_print(A, 1);
        free(A); A = NULL;
    }
}

void tt_broadcast(MPI_Comm comm, tensor_train* tt){
    for (int ii = 0; ii < tt->d; ++ii){
        matrix_tt* A = matrix_tt_wrap((tt->r[ii]) * (tt->n[ii]) * (tt->r[ii+1]), 1, tt->trains[ii]);
        matrix_tt_broadcast(comm, A);
        free(A);
    }
}

double get_compression(const tensor_train* tt)
{
    long N = 1;
    for (int ii = 0; ii < tt->d; ++ii)
    {
        N = N * tt->n[ii];
    }

    long Ntt = 0;
    for (int ii = 0; ii < tt->d; ++ii)
    {
        Ntt = Ntt + ((tt->r[ii])*(tt->n[ii])*(tt->r[ii+1]));
    }

    double compression = (double) Ntt/N;
    return compression;
}