//////////////////////////////////////////////////////////////////////////
//
//	test stuff for TMO_Transit
//
/////////////////////////////////////////////////////////////////////////

#include "SpatialIndex.h"
#include "SpatialEdge.h"
#include "SpatialGeneral.h"
#include "SpatialVector.h"
#include "SpatialConstraint.h"
#include "SpatialConvex.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

#define NORTH 1
#define SOUTH 0
#define INDEX_SIZE 11

static int adata4[600][1280], triangle[10000], imagecnt[1600][800];
static short adata2[600][1280];
static float image[1600][800];
static float node[2][4][4][4][4][4][4][4][4][4][4];
static IDTYPE nodeid[2][4][4][4][4][4][4][4][4][4][4];
static int hits[2][4][4][4][4][4][4][4][4][4][4];

//////////////////////////////////////////////////////////////////////
//  calculate the center of a given spatial triangle
//////////////////////////////////////////////////////////////////////
void WspatialTriangleCenter( SpatialIndex *si, 
				IDTYPE *nodename, 
				float64 *ra, float64 *dec )
{
  SpatialVector *v1, *v2, *v3;
  IDTYPE bitname;
  v1 = spatialVectorNew();
  v2 = spatialVectorNew();
  v3 = spatialVectorNew();
  bitname = spatialIndexLeafNumberById(si, *nodename);
 
  //get vertices of triangle
  spatialIndexNodeVertexLeaf( si, bitname, v1, v2, v3 );

  //average over ra/dec of vertices to find center ra/dec
  *ra = ( v1->ra_ + v2->ra_ + v3->ra_ ) / 3;
  *dec = ( v1->dec_ + v2->dec_ + v3->dec_ ) / 3;
  
  if ( (fabs(*ra - v1->ra_)) > 10 )
  {
    if (v1->ra_>180)  v1->ra_ = v1->ra_ - 360;      
    if (v2->ra_>180)  v2->ra_ = v2->ra_ - 360;      
    if (v3->ra_>180)  v3->ra_ = v3->ra_ - 360;
  
  *ra = ( v1->ra_ + v2->ra_ + v3->ra_ ) / 3;
  // 0<ra<360
  if(*ra<0)  *ra = *ra + 360;
  }
  
  spatialVectorDelete(v1);
  spatialVectorDelete(v2);
  spatialVectorDelete(v3);
  
  return;
}
//////////////////////////////////////////////////////////////////////
//	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 IDTYPE id, short index[])
{
  uint32 size=0, i;
  bool ok=true;
  
  for(i=0; i<INDEX_SIZE; 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;
}


//////////////////////////////////////////////////////////////////
//	Main test Program
//////////////////////////////////////////////////////////////////
int main(int argc,char ** argv)
{
  char * nameoutFile = "Enif_T_pict.grd";
  char nameFile[] = "/home/soft/smei/user/asmith/TMO_Data/TMO420000.nic";
  FILE  *inFile, *outFile;
  short head[3];
  size_t itemsread;
  float64 ped=0;
  int pedcnt=0, icount=0;
  int xarg, yarg, one=1, flen;
  int i, j, k, l, z, f, ps=768000;
  int a,b,c,d,e,m,n,o,p,q,r,s,t,u;
  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, cra, cdec;
  float64 degperminute=0.2506844731,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], *pra, *pdec;
  float64 rup=0, rdown=360,dup=0,ddown=90;
  float noderesponse=0, response=0;
  SpatialIndex *si;
  SpatialVector *v[4], *tempv;
  SpatialConvex *pixelconvex;
  SpatialMarkup *markup;
  SpatialIDVec *partialvec, *fullvec;
  IDTYPE *partialnode, *fullnode, *imagenode; 
  short idx[INDEX_SIZE]; 
  time_t tt1, tt2, ft[600], at=0, bt=0, ct=0, t1, t2;
  
  printf("Starting...\n");
  
  
  
  // adjustment for the night 42xxx
  ra0=279.425 + 0.97; //**ad hoc**

  for(i=0; i<1600; i++)
  {
    for(j=0; j<400; j++)
    {
      image[i][j] = 0;
      imagecnt[i][j] = 0;
    }
  }
  for(i=0; i<60; i++) 
  {
    ft[i]=0;
  }
  
  //initialize for indexing
  si = spatialIndexNew( 12 , 2 );
  markup = spatialMarkupNew(si);    
  partialvec = (SpatialIDVec*) malloc( sizeof(SpatialIDVec));
  fullvec = (SpatialIDVec*) malloc( sizeof(SpatialIDVec));
  spatialIDVecInit(partialvec);
  spatialIDVecInit(fullvec);
 
for(a=0; a<2; a++)
{
 for(b=0; b<4; b++)
 {
  for(c=0; c<4; c++)
  {
   for(d=0; d<4; d++)
   {
    for(e=0; e<4; e++)
    {
     for(m=0; m<4; m++)
     {
      for(n=0; n<4; n++)
      {
       for(o=0; o<4; o++)
       {
        for(p=0; p<4; p++)
        {
         for(q=0; q<4; q++)
         {
          for(r=0; r<4; r++)
          {
             
node[a][b][c][d][e][m][n][o][p][q][r] = 0;
hits[a][b][c][d][e][m][n][o][p][q][r] = 0;
 
  	  }
  	 }
  	}
       }
      }
     }
    }
   }
  }
 }
}
 
 
  printf("Looping...\n");
  
for (f=180; f<190; f++)
{    
  tt1 = clock();
  
  sprintf(&nameFile[41],"%04d.nic",f);
  //printf("inFile Name: %s\n",nameFile);
    
  for(i=0; i<600; i++)
  {
    for(j=0; j<1280; j++)
    {
      adata4[i][j] = 0;
      adata2[i][j] = 0;
    }
  }
 
  // Read in both file
  inFile = fopen("TMO420180.nic","rb");

  // Test if open
  if (inFile == NULL) {
    printf("Error opening file\n");
    exit(1);
  }

  itemsread = fread(&head, sizeof(short),3,inFile);  
  itemsread = fread(&adata2, sizeof(short),ps,inFile);
  fclose(inFile);

  for(i=0; i<600; i++)
  {
    for(j=0; j<1280; j++)
    {
      adata4[i][j] = adata2[i][j];
      if(adata4[i][j]<0)
      {
        adata4[i][j] = adata4[i][j] + 65536;
      }
    }
  }

  //pedestal subtraction from both files
  for(i=2; i<600; i++)
  {
    for(j=1271; j<1280; j++)
    {
      ped = ped + adata4[i][j];
      pedcnt++;
    }
    for(j=4; j<13; j++)
    {
      ped = ped + adata4[i][j];
      pedcnt++;
    }
  }    
  ped = ped/pedcnt;     
  for(i=0; i<600; i++)
  {
    for(j=0; j<1280; j++)
    {
      adata4[i][j] = adata4[i][j] - ped;
    }
  }
  ped=0;
  pedcnt=0;    
  
  //loop for each pixel
  for (i=0; i<350; i++)
  {
    for (j=640; j<1264; j++)// 22<j<1264
    {                            
     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( dr < 27.0339 )
     {
      if( fabs((j-x0)/(i-y0)) < 0.57735 )// less than tan 30
      { 
       response = adata4[i][j]*(rccd/r0);//correct for the 1/r pixel size in sky area        
                                                    
       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++)
       {         
         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 = ( -bq - sqrt( (bq*bq)-(4*aq*(cq-rq))) ) / ( 2*aq );
         thetay = rq / 18.0226; //note: +18.0226 to flip thetay/cy sign relative to Andy's program
                                                            
         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); 
         
         /*
         if(*pra>rup) rup=*pra;
         if(*pra<rdown) rdown=*pra;
         if(*pdec>dup) dup=*pdec;
         if(*pdec<ddown) ddown=*pdec;
         */         
                 
         v[k] = spatialVectorNewRaDec(*pra, *pdec);
      }//end vertex loop

      
     t1=clock();      
     pixelconvex = spatialConvexNewRectangle(v[0], v[1], v[2], v[3]);
     
     //free vector memory
     spatialVectorDelete(v[0]);
     spatialVectorDelete(v[1]);
     spatialVectorDelete(v[2]);
     spatialVectorDelete(v[3]);
     t2=clock();
     at=at+t2-t1;
     t1=t2;
          
     //intersect with index
     spatialConvexIntersect( pixelconvex, si, markup, partialvec, fullvec );
     t2=clock();
     bt=bt+t2-t1;          
     t1=t2;
     
     //test center of partial nodes to see if they are within pixelconvex
     partialnode = partialvec->vector_;     
     for (k=0; k<partialvec->length_; k++, partialnode++)
     {        
        WspatialTriangleCenter( si, partialnode, pra, pdec );        
        tempv = spatialVectorNewRaDec(*pra, *pdec);       
        triangle[k] = spatialConvexTestVertex(pixelconvex,tempv);
        spatialVectorDelete(tempv);
        // triangle[k] = 1 or 0 for in or out of the pixel
        if (triangle[k] = 1) spatialIDVecAppend(fullvec, *partialnode);                    
     }
     spatialConvexDelete(pixelconvex);
     spatialIDVecClear(partialvec, true);
     t2=clock();
     ct=ct+t2-t1;
     
     //give each triangle an equal portion of the response
     fullnode = fullvec->vector_; 
     flen = fullvec->length_; 
     noderesponse = response;
     
     for(k=0; k<flen; k++, fullnode++)
     {
       getIndices(*fullnode, idx);
       
       if(idx[0]>1) icount++;
       for(l=0; l<14; l++)
       {
         if(idx[l]<0) icount++;
         if(idx[l]>3) icount++;
       }
       
       
       node[idx[0]][idx[1]][idx[2]][idx[3]][idx[4]][idx[5]][idx[6]][idx[7]][idx[8]][idx[9]][idx[10]] =  node[idx[0]][idx[1]][idx[2]][idx[3]][idx[4]][idx[5]][idx[6]][idx[7]][idx[8]][idx[9]][idx[10]] + noderesponse;
       hits[idx[0]][idx[1]][idx[2]][idx[3]][idx[4]][idx[5]][idx[6]][idx[7]][idx[8]][idx[9]][idx[10]]++;
       nodeid[idx[0]][idx[1]][idx[2]][idx[3]][idx[4]][idx[5]][idx[6]][idx[7]][idx[8]][idx[9]][idx[10]] = *fullnode;       
     }
       
     spatialIDVecClear(fullvec, true);
      
     }//end pixel's if #1
    }//end pixel's if #2
   }//end pixel loop(j)   
   //printf("%d: loop Index(i): %d\n",f,i);   
  }//end pixel loop(i)
  
  z = f - 314;//z=frame number starting at 1
  tt2 = clock();
  ft[z] = tt2 - tt1;
  printf("%d Time: %f     Icount: %d\n",f,(double)ft[z]/(double)CLOCKS_PER_SEC,icount); 
  //printf("Creation, Intersection, P to F: %f %f %f\n",(double)at/(double)CLOCKS_PER_SEC,(double)bt/(double)CLOCKS_PER_SEC,(double)ct/(double)CLOCKS_PER_SEC);  
  icount=0;
  at=0;
  bt=0;
  ct=0;
}//end f frame loop
  
  
  printf("Averaging and processing...\n");
  
  //free IDvec & markup memory
  free(partialvec);
  free(fullvec);
  free(markup);
  
  //approximate center of FOV
  cra=325;
  cdec=35;
  
// loop to put all triangles with averaged response into data array
for(a=0; a<2; a++)
{
 for(b=0; b<4; b++)
 {
  for(c=0; c<4; c++)
  {
   for(d=0; d<4; d++)
   {
    for(e=0; e<4; e++)
    {
     for(m=0; m<4; m++)
     {
      for(n=0; n<4; n++)
      {
       for(o=0; o<4; o++)
       {
        for(p=0; p<4; p++)
        {
         for(q=0; q<4; q++)
         {
          for(r=0; r<4; r++)
          {
             
if(node[a][b][c][d][e][m][n][o][p][q][r]>0)
{
  imagenode = &(nodeid[a][b][c][d][e][m][n][o][p][q][r]);
  //average noderesponse by the number of hits the node got
  noderesponse = node[a][b][c][d][e][m][n][o][p][q][r]/hits[a][b][c][d][e][m][n][o][p][q][r];

  WspatialTriangleCenter( si, imagenode, pra, pdec );
  /*
  if(*pra>rup) rup=*pra;
  if(*pra<rdown) rdown=*pra;
  if(*pdec>dup) dup=*pdec;
  if(*pdec<ddown) ddown=*pdec;
  */
  
  *pra = (*pra - cra)*cos(*pdec*degtorad) + cra;

  xarg =  ( (*pra) - cra )*20 +399;//x-coordinate
  yarg =  ( (*pdec) - cdec )*20 + 799;//y-coordinate        
  //make sure xarg/yarg are within array bounds
  if (xarg<0)
  {
    xarg=0;
  }else if (xarg>=800)
  {
    xarg=799;
  }
  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   
  	    
  	     }
  	 }
  	}
       }
      }
     }
    }
   }
  }
 }
}
  
  for(i=0; i<1600; i++)
  {
    for(j=0; j<800; j++)
    {      
      if(imagecnt[i][j]>0) image[i][j]=image[i][j]/imagecnt[i][j];
    }
  }

  printf("Writing 'Enif_T_pict.grd'\n");
  
  //loop for writing the data array to a file
  outFile = fopen(nameoutFile,"w");
      
  //write header
  fprintf(outFile,"DSAA\n800 1600\n315 355\n-5 75\n0 10000\n");

  //write array
  for (i=0; i<1600;)
  {
      for (j=0; j<800;)
      {       
       fprintf(outFile,"%8.6f ",image[i][j]);                  
       j++;
      }
    i++;
  }
  fclose(outFile);

  //printf("%f < RA < %f\n",rdown,rup);
  //printf("%f < DEC < %f\n",ddown,dup);
  printf("Frame Times:\n");
  for(z=1; z<52; z++)
  {
    printf("%f, ",(double)ft[z]/(double)CLOCKS_PER_SEC);
  }
  
  //printf("Finished!!!\n");

  return 0;
}