#include <stdio.h>
#include <assert.h>

/*
 * 32768*sin((pi/2)*((i+0.5)/256)), 0<=i<=255
 */
static unsigned short int sinTableData[] = {
  0x0065, 0x012e, 0x01f7, 0x02c0, 0x0389, 0x0452, 0x051b, 0x05e3, 
  0x06ac, 0x0775, 0x083e, 0x0906, 0x09cf, 0x0a97, 0x0b60, 0x0c28, 
  0x0cf0, 0x0db8, 0x0e80, 0x0f47, 0x100f, 0x10d6, 0x119e, 0x1265, 
  0x132b, 0x13f2, 0x14b9, 0x157f, 0x1645, 0x170b, 0x17d1, 0x1896, 
  0x195b, 0x1a20, 0x1ae5, 0x1ba9, 0x1c6e, 0x1d31, 0x1df5, 0x1eb8, 
  0x1f7b, 0x203e, 0x2101, 0x21c3, 0x2284, 0x2346, 0x2407, 0x24c8, 
  0x2588, 0x2648, 0x2708, 0x27c7, 0x2886, 0x2945, 0x2a03, 0x2ac1, 
  0x2b7e, 0x2c3b, 0x2cf7, 0x2db3, 0x2e6f, 0x2f2a, 0x2fe5, 0x309f, 
  0x3159, 0x3212, 0x32cb, 0x3383, 0x343b, 0x34f2, 0x35a9, 0x365f, 
  0x3715, 0x37ca, 0x387f, 0x3933, 0x39e7, 0x3a9a, 0x3b4c, 0x3bfe, 
  0x3caf, 0x3d60, 0x3e10, 0x3ec0, 0x3f6f, 0x401d, 0x40cb, 0x4178, 
  0x4224, 0x42d0, 0x437b, 0x4426, 0x44d0, 0x4579, 0x4621, 0x46c9, 
  0x4770, 0x4817, 0x48bd, 0x4962, 0x4a06, 0x4aaa, 0x4b4d, 0x4bef, 
  0x4c91, 0x4d31, 0x4dd1, 0x4e71, 0x4f0f, 0x4fad, 0x504a, 0x50e6, 
  0x5181, 0x521c, 0x52b6, 0x534f, 0x53e7, 0x547f, 0x5515, 0x55ab, 
  0x5640, 0x56d4, 0x5767, 0x57fa, 0x588c, 0x591c, 0x59ac, 0x5a3b, 
  0x5ac9, 0x5b57, 0x5be3, 0x5c6f, 0x5cf9, 0x5d83, 0x5e0c, 0x5e94, 
  0x5f1b, 0x5fa1, 0x6026, 0x60aa, 0x612e, 0x61b0, 0x6232, 0x62b2, 
  0x6332, 0x63b0, 0x642e, 0x64ab, 0x6526, 0x65a1, 0x661b, 0x6693, 
  0x670b, 0x6782, 0x67f8, 0x686d, 0x68e0, 0x6953, 0x69c5, 0x6a36, 
  0x6aa5, 0x6b14, 0x6b82, 0x6bee, 0x6c5a, 0x6cc4, 0x6d2e, 0x6d96, 
  0x6dfe, 0x6e64, 0x6ec9, 0x6f2d, 0x6f90, 0x6ff2, 0x7053, 0x70b3, 
  0x7112, 0x7170, 0x71cc, 0x7228, 0x7282, 0x72dc, 0x7334, 0x738b, 
  0x73e1, 0x7436, 0x7489, 0x74dc, 0x752d, 0x757e, 0x75cd, 0x761b, 
  0x7668, 0x76b4, 0x76fe, 0x7748, 0x7790, 0x77d8, 0x781e, 0x7863, 
  0x78a6, 0x78e9, 0x792a, 0x796a, 0x79aa, 0x79e7, 0x7a24, 0x7a60, 
  0x7a9a, 0x7ad3, 0x7b0b, 0x7b42, 0x7b78, 0x7bac, 0x7bdf, 0x7c11, 
  0x7c42, 0x7c72, 0x7ca0, 0x7cce, 0x7cfa, 0x7d25, 0x7d4e, 0x7d77, 
  0x7d9e, 0x7dc4, 0x7de9, 0x7e0c, 0x7e2f, 0x7e50, 0x7e70, 0x7e8e, 
  0x7eac, 0x7ec8, 0x7ee3, 0x7efd, 0x7f16, 0x7f2d, 0x7f43, 0x7f58, 
  0x7f6c, 0x7f7e, 0x7f90, 0x7fa0, 0x7fae, 0x7fbc, 0x7fc8, 0x7fd3, 
  0x7fdd, 0x7fe6, 0x7fed, 0x7ff4, 0x7ff8, 0x7ffc, 0x7fff, 0x8000,
};

static int sinTableBins = 4*sizeof(sinTableData)/sizeof(sinTableData[0]);
static int *sinTable = 0;

/*
 * Given iphi = 1024*phi/2pi, return 32768*(cos(phi),sin(phi))
 */
void
fixedCosSin(int iphi, int *icos, int *isin)
{
  int nb4 = sinTableBins/4;
  if (!sinTable) {
    int i = 0;
    sinTable = (int *) malloc(sinTableBins*sizeof(sinTable[0]));
    assert(sinTable!=0);
    for (i = 0; i<nb4; i++) {
      sinTable[i] = sinTableData[i];
      sinTable[nb4+i] = sinTableData[nb4-1-i];
      sinTable[2*nb4+i] = -sinTable[i];
      sinTable[3*nb4+i] = -sinTableData[nb4-1-i];
    }
  }
  *icos = sinTable[(iphi+nb4) & (sinTableBins-1)];
  *isin = sinTable[iphi & (sinTableBins-1)];
}

/*
 * Compute beam correction table with fixed-point math
 *
 * inputs:
 *   nphi     : number of phi bins (should equal 1024 for now)
 *   nz       : number of z bins
 *   ix,iy    : arrays of nz integer beam x,y values, in 1 um units
 *
 * output:
 *   beamcorr : array of nz*nphi beam offsets, in 10 um units
 */
int
calcBeamCorr(int *beamcorr, int nphi, int nz, int *ix, int *iy)
{
  int iz = 0, jphi = 0;
  assert(nphi==sinTableBins); /* could be relaxed later */
  for (iz = 0; iz<nz; iz++) {
    for (jphi = 0; jphi<nphi; jphi++) {
      int c = 0, s = 0, davg = 0;
      int k = nphi*iz + jphi;
      fixedCosSin(jphi, &c, &s);
      davg = (iy[iz]*c-ix[iz]*s)/32768;
      beamcorr[k] = (davg+5)/10 & 0x3ff;
    }
  }
  return 0;
}

int
main(int argc, char **argv)
{
  int i = 0, c = 0, s = 0;
  int ix[] = { -351, +99 };
  int iy[] = { -4763, +2177 };
  int beamcorr[2*1024];
  for (i = 0; i<1024; i++) {
    fixedCosSin(i, &c, &s);
    printf("%d %d %d\n", i, c, s);
  }
  return 0;
  calcBeamCorr(beamcorr, 1024, 2, ix, iy);
  for (i = 0; i<2*1024; i++)
    printf("%d %d %d %d\n", 
	   i/1024, i%1024, 
	   beamcorr[i],
	   beamcorr[i] - (beamcorr[i] & 0x200 ? 0x400 : 0));
  return 0;
}