//+
// NAME:
//	A_mod_L12_all
// PURPOSE:
// CATEGORY:
// CALLING SEQUENCE:
// INPUTS:
// OUTPUTS:
// CALLS:
// SEE ALSO:
// INCLUDE:
// SIDE EFFECTS:
// RESTRICTIONS:
// PROCEDURE:
//	This file contains all of the C++ Spatial Index functions to
//	be called from Fortran in order to index and make a sky map
//	from TMO/SMEI data frames.
//	All of the Spatial Index functions have been taken from the
//	free software developed by Peter Z. Kunzst of The Johns Hopkins University
//	at http://www.sdss.org
// MODIFICATION HISTORY:
//	???-2002, Aaron Smith (UCSD/CASS)
//		Original version
//	DEC-2002, Paul Hick (UCSD/CASS; pphick@ucsd.edu)
//		Fixed bug in subroutine indexframe (fdusk was not declared as pointer)
//	DEC-2002, Andy Buffington (UCSD/CASS; abuffington@ucsd.edu)
//		Changed bunch of scalar variables from single to double precision.
//-

#include <VarVecDef.h>
#include "VarStr.h"
#include "SpatialIndex.h"
#include "SpatialGeneral.h"
#include "SpatialVector.h"
#include "SpatialConvex.h"

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

#define NORTH 1
#define SOUTH 0

//the extern "C" eliminates all function name mangling, allowing for calls
//to be made from Fortran
extern "C" void averagenight_(short * level, float node[], uint64 nodeid[], short hits[],
		float64 * cra,float64 * cdec, int imagecnt[1600][3200]);
extern "C" void indexframe_(short * level, float node[], uint64 nodeid[], short hits[],
		float data[600][1280],int * fnight, int * fn, float64 *ddusk, float64 euler[3], float64 affine[6]);
extern "C" int airplane_(int night, int f, int j);
extern "C" void writedata_(char * nameoutFile, int imagecnt[1600][3200]);
extern "C" void readfile_(char * nameFile, float data[600][1280]);
extern "C" void pedestal_(float frame[600][1280]);
extern "C" long getindex_(const short level, short idx[]);
extern "C" void getindices_(const short level, const uint64 id, short index[]);
extern "C" void rotate_(float64 alfa, float64 beta, float64 gamma, float64 *ra, float64 *dec);

//////////////////////////////////////////////////////////////////////
//	Euler rotation function
//////////////////////////////////////////////////////////////////////
void rotate_(float64 alfa, float64 beta, float64 gamma, float64 *ra, float64 *dec)
{
  float64 sinb, cosb, sinl, cosl, sinp, cosp, cost;
  float64 degtorad=0.0174533, radtodeg=57.29578;
  float64 phi, rlat;

  phi = *ra;
  rlat = *dec;

  //convert from degrees to radians
  alfa = alfa * degtorad;
  beta = beta * degtorad;
  gamma = gamma * degtorad;
  phi = phi * degtorad;
  rlat = rlat * degtorad;

  phi = phi - alfa;  // rotation 1
  sinb = sin(beta);  // rotation 2...
  cosb = cos(beta);
  sinl = sin(rlat);
  cosl = cos(rlat);
  sinp = sin(phi);
  cosp = cos(phi);
  cost = ( sinl*cosb ) + ( cosl*sinb*cosp );
  
  if (fabs(cost)!=1)
  {
    sinp = cosl*sinp;
    cosp = ( cosl*cosb*cosp ) - ( sinl*sinb );
  }else if (sinb == 0.0)
  {
    cosp = cosb*cosp;
  }
  
  phi = atan2(sinp,cosp);
  phi = phi - gamma;  // rotation 3
  
  //convert back to degrees
  alfa = alfa * radtodeg;
  beta = beta * radtodeg;
  gamma = gamma * radtodeg;
  phi = phi * radtodeg;
  rlat = rlat * radtodeg;
  
  phi = fmod(fmod(phi,360.0) + 360.0, 360.0);  // 0<phi<360
  rlat = 90.0 - ( acos(cost) * radtodeg );

  *ra = phi;
  *dec = rlat;

  return;
}
//////////////////////////////////////////////////////////////////
//	Get the indices(0-3) of an IDTYPE nodename
//	the first indice(N or S) = 1 or 2
//////////////////////////////////////////////////////////////////
void getindices_(const short level, const uint64 id, short index[])
{
  uint32 size=0, i;
  bool ok=true;
  
  for(i=0; i<level+2; i++)
  {
    index[i] = 0;
  }
  
  //working with the same level i should be pre-determined
  for(i=0; i<IDSIZE; i+=2)
  {
    if( (id<<i) & IDHIGHBIT) break;
    if( (id<<i) & IDHIGHBIT2)
    {
      ok=false;//invalid id
      break;
    }
  }
  
  if(!ok || id==0)
  {
    return;//invalid id->exit
  }

  size=(IDSIZE-i) >> 1;

  for(i=0; i<size-1; i++)
  {
    index[size-i-1] = ( (id >> i*2) & 3);
  }

  if( (id>>(size*2-2)) & 1)
  {
    index[0] = NORTH;
  }else
  {
    index[0] = SOUTH;
  }

  return;
}
//////////////////////////////////////////////////////////////////
//	convert index_size(level+2) indices to one indice
//////////////////////////////////////////////////////////////////
long getindex_(const short level, short idx[])
{
  int i,e,p;
  long indice=0, max=0;

  max = 2*pow(4,level+1);

  for(i=0,e=level+1; i<level+2; i++,e--)
  {
    p=pow(4,e);
    if(e==0) p=1;
    indice += idx[i]*p;
  }
  if(indice>max || indice<0)
  {
    printf("Bad Indice: %ld\n",indice);
    printf("Indices: ");
    for(i=0; i<level+2; i++)
    {
      printf("%d, ",idx[i]);
    }
    exit(1);
  }

  return indice;
}
//////////////////////////////////////////////////////////////////
//	pedestal subtraction
//////////////////////////////////////////////////////////////////
void pedestal_(float frame[600][1280])
{
  float64 ped=0;
  int i, j, pedcnt=0;

  //pedestal subtraction from both files
  for(i=2; i<600; i++)
  {
    for(j=1271; j<1280; j++)
    {
      ped = ped + frame[i][j];
      pedcnt++;
    }
    for(j=4; j<13; j++)
    {
      ped = ped + frame[i][j];
      pedcnt++;
    }
  }
  ped = ped/pedcnt;
  for(i=0; i<600; i++)
  {
    for(j=0; j<1280; j++)
    {
      frame[i][j] = frame[i][j] - ped;
    }
  }
  
  return;
}
//////////////////////////////////////////////////////////////////
//	read in file
//////////////////////////////////////////////////////////////////
void readfile_(char * nameFile, float data[600][1280])
{
  FILE *inFile;
  size_t itemsread;
  int ps=768000, i, j;
  short head[3];
  unsigned short udata[600][1280];

  // Read in file
  inFile = fopen(nameFile,"rb");

  // Test if file access
  if (inFile == NULL)
  {
    cout << "Error opening file" << endl;
    exit(1);
  }

  itemsread = fread(&head, sizeof(short),3,inFile);
  itemsread = fread(&udata, sizeof(short),ps,inFile);
  fclose(inFile);
  
  for(i=0; i<600; i++)
  {
    for(j=0; j<1280; j++)
    {
      data[i][j]=udata[i][j];
    }
  }

  return;
}
//////////////////////////////////////////////////////////////////
//	write data array to a file
//////////////////////////////////////////////////////////////////
void writedata_(char * nameoutFile, int imagecnt[1600][3200])
{
  int i, j, linecount;
  FILE *outFile;

  //loop for writing the data array to a file
  outFile = fopen(nameoutFile,"w");

  //write header
  fprintf(outFile,"DSAA\n3198 1598\n-75 85\n0 80\n0 20000\n");

  //write array
  for (i=1; i<1599; i++)
  {
    linecount=0;
    for (j=1; j<3199; j++)
    {
      fprintf(outFile,"%05d ",imagecnt[i][j]);
      linecount++;
      if(linecount==20)
      {
        fprintf(outFile,"\n");
	linecount=0;
      }
    }
    fprintf(outFile,"\n");
  }
  fclose(outFile);

  return;
}
//////////////////////////////////////////////////////////////////
//	test pixel for airplanes, return 1 if there is one
//	No airplanes were found for sky reversed
//////////////////////////////////////////////////////////////////
int airplane_(int night, int f, int j)
{
  int airplane=0;

  if(night==42)
  {
     //stamp out columns in selected frames containing airplane transits
     if(f == 32 && fabs(j-150) < 30)airplane=1;      //(begin night 42)
     if(f == 36 && fabs(j-92) < 18)airplane=1;
     if(f == 45 && fabs(j-120) < 20)airplane=1;
     if(f == 55 && fabs(j-500) < 40)airplane=1;
     if(f == 67 && fabs(j-770) < 40)airplane=1;
     if(f == 87 && fabs(j-230) < 20)airplane=1;
     if(f == 89 && fabs(j-243) < 23)airplane=1;
     if(f == 91 && fabs(j- 84) < 16)airplane=1;
     if(f == 225 && fabs(j-330) < 40)airplane=1;
     if(f == 235 && j > 1239)airplane=1;
     if(f == 341 && fabs(j-150) < 20)airplane=1;
     if(f < 2 && fabs(j-459) < 2)airplane=1;         //(end night 42)
  }
  if(night==41)
  {
     if(f == 22 && fabs(j-160) < 20)airplane=1;      //(begin night 41)
     if(f == 37 && fabs(j-45) < 15)airplane=1;
     if(f == 42 && fabs(j-180) < 20)airplane=1;
     if(f == 48 && fabs(j-730) < 100)airplane=1;
     if(f == 82 && fabs(j-172) < 19)airplane=1;
     if(f == 105 && fabs(j-55) < 15)airplane=1;
     if(f == 303 && fabs(j-860) < 20)airplane=1;
     if(f == 340 && fabs(j-80) < 20)airplane=1;
     if(f < 4 && fabs(j-459) < 2)airplane=1;         //(end night 41)
  }
  if(night==40)
  {
     if(f == 41 && j > 1189)airplane=1;             //(begin night 40)
     if(f == 62 && fabs(j-1005) < 45)airplane=1;
     if(f == 46 && fabs(j-95) < 15)airplane=1;
     if(f == 119 && fabs(j-250) < 21)airplane=1;
     if(f == 153 && fabs(j-166) < 19)airplane=1;
     if(f == 381 && j < 55)airplane=1;
     if(f == 416 && fabs(j-45) < 15)airplane=1;
     if(f == 515 && fabs(j-750) < 10)airplane=1;
     if(f == 12 && fabs(j-165) < 15)airplane=1;
     if(f < 15 && fabs(j-459) < 2)airplane=1;          //(end night 40)
  }

  return airplane;
}
//////////////////////////////////////////////////////////////////
//	module to index a frame
//////////////////////////////////////////////////////////////////
void indexframe_(short * ilevel, float node[], uint64 nodeid[], short hits[],
		float data[600][1280], int * fnight, int * fn, 
		float64 * ddusk, float64 euler[3], float64 affine[6])
{
  short level=*ilevel;
  float64 dusk=*ddusk;
  size_t flen;
  long index;
  int airplane=0, night, ncount=0, hcount=0;
  int i, j, k, a, b, p, n, f=*fn;
  float64 aq, bq, cq, rq, dr, thetax, thetaxsq, thetay, cx, cy, cz, crsq, cr, phi, rlat;
//  float64 gamma=0.0, beta, alfa=0.50, camlat=34.9471,
  float64 cra, cdec, alfa, beta, gamma;
//  float64 degperminute=0.2506844731,
  float64 degtorad=0.0174533, radtodeg=57.29578;
  float64 x0=630.6, y0=1291.38, r0=1194.5, rccd; // ra0=279.425;
  float64 x[4], y[4], tempx, *pra, *pdec;
  float64 rup=0, rdown=360,dup=0,ddown=90;
  float response=0, arg, arg1;
  SpatialIndex si(level,2);
  SpatialVector *v[4], *tempv;
  SpatialConvex *pixelconvex;
  ValVec<uint64> *partialvec, fullvec, *tempfullvec;
  uint64 *partialnode, *fullnode, imagenode;
  short idx[level+2], triangle;
  time_t frametime, ti, tf;

  ti=clock();
  alfa=euler[0];
  beta=euler[1];
  gamma=euler[2];
  night= *fnight;

  //initializ dynamic arrays
  partialvec = new ValVec<uint64>();
  tempfullvec = new ValVec<uint64>();

//// loop for each pixel ////
  for (i=0; i<350; i++)
  {
    for (j=22; j<1264; j++) // 22<j<1264
    {
     //test pixel for airplanes
     airplane=airplane_(night,f,j);

     //this rccd/dr are only preliminary quantities for determining
     //if the pixel is in the FOV
     rccd = sqrt((j-x0)*(j-x0) + (i-y0)*(i-y0));
     dr = fabs(rccd - r0);

     //less than 1.5 deg x 18.0226 pix/deg
     if( airplane == 0 && dr < 27.0339 )
     {
      if( fabs((j-x0)/(i-y0)) < 0.57735 )// less than tan 30
      {

       arg = rccd/fabs(i-y0);          //x secant zenith angle
       arg1 = (rccd-r0-27)/54;
       arg=arg*(1 - 0.2*arg1);         //ad hoc E-W dawn/dusk gradient
       response = (data[i][j]-dusk*arg)*(rccd/r0);
       //correct for the 1/r pixel size in sky area, remove dawn/dusk


       x[0] = j - 0.5;
       y[0] = i - 0.5;
       x[1] = j - 0.5;
       y[1] = i + 0.5;
       x[2] = j + 0.5;
       y[2] = i + 0.5;
       x[3] = j + 0.5;
       y[3] = i - 0.5;

       //loop for each vertex of a pixel
       for (k=0; k<4; k++)
       {
         //affine transformation to std. ccd coordinates
	 tempx = affine[0] + (1+affine[1])*x[k] + affine[2]*y[k];
	 y[k] = affine[3] + affine[4]*x[k] + (1+affine[5])*y[k];
	 x[k] = tempx;
         rccd = sqrt((x[k] - x0)*(x[k] - x0) + (y[k] - y0)*(y[k] - y0));
         thetax = radtodeg*asin( (x[k] - x0) / rccd );
         thetax = thetax / 0.999744;//atmosphere adjustment

         //quadratic coefficiants for the formula: rq=aqTY^2+bqTY+cq
         //from July 11, 2001 memo(ignoring 3rd and 4th order terms)
         /*
         thetaxsq = thetax*thetax;
         aq = 0.1164 - ( pow(10,-5)*thetaxsq );
         bq = ( 3.6*pow(10,-5)*thetaxsq ) - 18.0266;
         cq = ( 0.000936*thetaxsq ) - ( 1.044*pow(10,-7)*thetaxsq*thetaxsq );
         */
         rq = (rccd - r0)/1.013;//y-plate scale adjustment
         thetay = rq / 18.0226; //note: +18.0226 to flip thetay/cy sign relative to Andy's program
	 //thetay = ( -bq - sqrt( (bq*bq)-(4*aq*(cq-rq))) ) / ( 2*aq );

         cx = -sin(thetax*degtorad);
         cy = sin(thetay*degtorad);
         crsq = (cx*cx)+(cy*cy);
         if(crsq>0)
         {
           phi = atan2(cy,cx);
         }else
         {
           phi = 0;
         }
         cz = sqrt(1-(crsq));
         rlat = radtodeg*asin(cz);
         phi = radtodeg*phi;

//         beta = camlat-90;
//         gamma = (-f*degperminute)-ra0; //(frame#+1)*degperminute
         pra = &phi;
         pdec = &rlat;

         rotate_(alfa, beta, gamma, pra, pdec);

         v[k] = new SpatialVector(*pra, *pdec);
      }//end vertex loop

     pixelconvex = new SpatialConvex(v[0], v[1], v[2], v[3]);

     //free vector memory
     delete v[0];
     delete v[1];
     delete v[2];
     delete v[3];

     //intersect with index
     pixelconvex->intersect(&si, partialvec, tempfullvec);
     fullvec = *tempfullvec;

     tempfullvec->keep(tempfullvec->length());

     //test center of partial nodes to see if they are within pixelconvex
     partialnode = partialvec->vector_;

     for (k=0; k<(partialvec->length()); k++, partialnode++)
     {
        si.CASStriangleCenter(*partialnode, pra, pdec );
        tempv = new SpatialVector(*pra, *pdec);

	triangle = pixelconvex->CASStestVertex(*tempv);
        delete tempv;

        // triangle = 1 or 0 for in or out of the pixel
        if (triangle == 1)  fullvec.append(*partialnode);
     }

     delete pixelconvex;
     partialvec->keep(partialvec->length());

     //give each triangle an equal portion of the response
     fullnode = fullvec.vector_; 
     flen = fullvec.length();

     for(k=0; k<flen; k++, fullnode++)
     {
        getindices_(level,*fullnode, idx);
	index = getindex_(level,idx);

	node[index] += response;
 	hits[index]++;
  	nodeid[index] = *fullnode;
     }

     fullvec.keep(fullvec.length());

     }//end pixel's if #1
    }//end pixel's if #2
   }//end pixel loop(j)
  }//end pixel loop(i)
  
  tf=clock();
  frametime=tf-ti;
  printf("%d  Time: %f\n",f,(double)frametime/(double)CLOCKS_PER_SEC);

  return;
}
//////////////////////////////////////////////////////////////////
//	module to average through all the triangles
//////////////////////////////////////////////////////////////////
void averagenight_(short * ilevel, float node[], uint64 nodeid[], short hits[],
		float64 * pcra,float64 * pcdec, int imagecnt[1600][3200])
{
  short level=*ilevel;
  int xarg, yarg;
  int i, j,k, ncount=0, hcount=0;
  float64 *pra, *pdec, dumra, dumdec;
  float64 cra, cdec;
  float noderesponse=0;
  uint64 imagenode;
  SpatialIndex si(level,2);
  float image[1600][3200]={0.0};
  long max;
  
  max=2*pow(4,level+1);

  //initialize cra, cdec and pointer variables to dummy addresses
  cra=*pcra;
  cdec=*pcdec;
  dumra=cra;
  dumdec=cdec;
  pra=&dumra;
  pdec=&dumdec;

// loop to put all triangles with averaged response into data array
for(i=0; i<max; i++)
{
 if(node[i]>0)
 {
  imagenode = nodeid[i];
  //average noderesponse by the number of hits the node got
  noderesponse = node[i] / hits[i];

  si.CASStriangleCenter(imagenode, pra, pdec );

  if((*pra-cra)>180)  *pra=*pra-360;//fix discontinuity over 0=360
  if((*pra-cra)<-180)  *pra=*pra+360;

  //*pra = (*pra - cra)*cos(*pdec*degtorad) + cra;

  xarg =  -( (*pra) - cra )*20 +1599;//x-coordinate, now flipped for true sky frame
  yarg =  ( (*pdec) - cdec )*20 + 799;//y-coordinate
  //make sure xarg/yarg are within array bounds
  if (xarg<0)
  {
    xarg=0;
  }else if (xarg>=3200)
  {
    xarg=3199;
  }
  if (yarg<0)
  {
    yarg=0;
  }else if (yarg>=1600)
  {
    yarg=1599;
  }

  //image[y][x] because C fills columns before rows
  image[yarg][xarg] = image[yarg][xarg] + noderesponse;
  imagecnt[yarg][xarg] = imagecnt[yarg][xarg] + 1;

 }//end if routine for all indices with response
}

  //average image and put into into imagecnt to print out integers;
  //dont worry about losing decimal precision
  for(i=0; i<1600; i++)
  {
    for(j=0; j<3200; j++)
    {
      if(imagecnt[i][j]>0)  image[i][j]=image[i][j]/imagecnt[i][j];
      imagecnt[i][j]=image[i][j];
    }
  }

  return;
}