#include "ifocad.h"
#include "qbeam.h"

#define RINDEX 1.44963
#define LAMBDA 1.064e-3		/* mm */
#define BEAM_RADIUS 3.

#define PD_RADIUS 10.
#define PD_DEPTH 30.
#define PD_RCHIP 10.

#define BS_R1 0.49
#define BS_T1 0.49
#define BS_R2 0.01
#define BS_T2 0.98
#define BS_AC1 "rd"
#define BS_AC2 "rd"

#define M_RC 5


#define QBEAM(bx) qbeam bx[1]
#define BEAM(bx) beam bx[1]
#define BEAMSPLITTER(bsx) beamsplitter bsx[1]
#define PHOTODIODE(pdx) photodiode pdx[1]
#define SURF(sx)  surf sx[1]
#define PBS(sx)  pbscube sx[1]

#define OCBEAMLABEL(bx) ocbeamlabel(bx, #bx, incfp)

#define MC_LEN 10.05e3
#define MC_HI 300.
#define MC_CUR -37.8e3

#define PD2DIST 100.
#define PDADIST 100.
#define PDBDIST 100.
#define PDCDIST 2000.
/*#define PDCDIST 0.0001*/

#define LASER_X 600.
#define LASER_Y -400.
#define SM1_X 300.

#define EPS 1e-9

#define EPS_NEWTON 1e-8
#define NEWT_TOL 1e-13

#define N_BS 8
#define N_PD 4
#define N_SIG 22



void makepd (beam * b, double dist, char *label, photodiode * pd);
void makedummybs (beamsplitter * bs, int type, char *name);
void init_nominal (beamsplitter bs[], photodiode pd[], beam b[]);
void pd_signals (beamsplitter bs[], photodiode pd[], beam b[], double *res);
void dcmatrix (void);
void prepare_newton (void);
void newtonfunc (double *x, double *y, beamsplitter * bs);
void pd_signals_lock (beamsplitter bs[], double *res);
void dcmatrix_lock (void);
int main (void);

FILE *incfp;
FILE *ocfp;

void
makepd (beam * b, double dist, char *label, photodiode * pd)
/* create photodiode pd at distance dist along beam b */
{
  addsmul3 (b->p0, dist, b->dir, pd->s.zen);
  pd->s.rs = PD_RADIUS;		/* radius */
  pd->s.k = 0.;
  cp3 (b->dir, pd->s.nv);
  rev3 (pd->s.nv);
  pd->d = PD_DEPTH;		/* depth */
  pd->rchip = PD_RCHIP;		/* active radius */
  strncpy (pd->name, label, ACLEN - 1);
  pd->labx = pd->laby = 0;
}

void
makedummybs (beamsplitter * bs, int type, char *name)
{
  switch (type)
    {
    case M_RC:			/* Modecleaner mirror */
      bs->s.k = 0;		/* curvature */
      bs->s.rs = 50.;		/* substrate radius */
      bs->d = 50.;		/* max thickness */
      bs->n = RINDEX;		/* refr. index */
      bs->wedge = 0;		/* wedge angle */
      strcpy (bs->ac1, "srt");	/* action string front */
      strcpy (bs->ac2, "t");	/* action string back */
      bs->t1 = 0.5;		/* power transmission front */
      bs->r1 = 0.5;		/* power refl. front */
      bs->t2 = 1.;		/* power transmission back */
      bs->r2 = 0.;		/* power refl. back */
      strcpy (bs->name, name);	/* name to be plotted */
      bs->labx = 0;		/* x-offset of name-label in mm */
      bs->laby = 0;		/* y-offset of name-label in mm */
      bs->k2 = 0;		/* curvature of second surface */
      break;
    default:
      fprintf (stderr, "makedummy:illegal type %d\n", type);
      exit (1);
    }

}

static beamsplitter bs_nom[N_BS], bs_test[N_BS];
static photodiode pd_nom[N_PD];
static beam b0_nom[1];


static char bsnames[N_BS][10] = {
  "SM1", "SM2", "MA", "MB", "MC", "M00"
};

static char signames[N_SIG][16] = {
  "PD2 DC h",
  "PDA DC h",
  "PDB DC h",
  "PDC DWS I h",
  "PDC DWS Q h",
  "SM1 h",
  "SM2 h",
  "PDC DC h t",
  "PDC DC h r",
  "PDC ang h t",
  "PDC ang h r",
  "PD2 DC v",
  "PDA DC v",
  "PDB DC v",
  "PDC DWS I v",
  "PDC DWS Q v",
  "SM1 v",
  "SM2 v",
  "PDC DC v t",
  "PDC DC v r",
  "PDC ang v t",
  "PDC ang v r",
};

static char sigunits[N_SIG][16] = {
  "[mm]",
  "[mm]",
  "[mm]",
  "[mm]",
  "[rad]",
  "[rad]",
  "[rad]",
  "[mm]",
  "[mm]",
  "[rad]",
  "[rad]",
  "[mm]",
  "[mm]",
  "[mm]",
  "[mm]",
  "[rad]",
  "[rad]",
  "[rad]",
  "[mm]",
  "[mm]",
  "[rad]",
  "[rad]",
};


void
init_nominal (beamsplitter bs[], photodiode pd[], beam b[])
{
  int i;

  BEAM (ba);
  BEAM (bb);
  BEAM (bc);
  BEAM (bcback);

  BEAM (b00);
  BEAM (b0);
  BEAM (b1);
  BEAM (b2t);
  BEAM (b2r);
  BEAM (bcr);
  BEAM (bat);
  BEAM (bbr);
  BEAM (bbt);
  BEAM (bct);



  BEAMSPLITTER (ma);
  BEAMSPLITTER (mb);
  BEAMSPLITTER (mc);

  BEAMSPLITTER (sm1);
  BEAMSPLITTER (sm2);
  BEAMSPLITTER (m00);



  PHOTODIODE (pd2);
  PHOTODIODE (pda);
  PHOTODIODE (pdb);
  PHOTODIODE (pdc);

  QBEAM (qb);
  QBEAM (qbc);
  QBEAM (qbca);
  QBEAM (qbcab);
  QBEAM (qct);
  QBEAM (q2r);
  QBEAM (q1r);
  QBEAM (q0);


  surf *slist[3];

  double plz[3] = { LASER_X, LASER_Y, 0 };
  double psa[3] = { SM1_X, LASER_Y, 0 };
  double pa[3] = { 0, -0.5 * MC_HI, 0 };
  double pc[3] = { 0, 0.5 * MC_HI, 0 };
  double pb[3] = { -MC_LEN, 0, 0 };


  double vab[3], vac[3], vcb[3], vca[3];

  double a45[3] = { 1, 1, 0 };
  double nz[3] = { 0, 0, 1 };

/* cavity mirrors */

  makedummybs (ma, M_RC, "MA");
  makedummybs (mb, M_RC, "MB");
  mb->s.k = 1. / MC_CUR;

  makedummybs (mc, M_RC, "MC");

  cp3 (pa, ma->s.zen);
  cp3 (pb, mb->s.zen);
  cp3 (pc, mc->s.zen);

  setnva (0, &(mb->s));

  sub3 (pb, pc, vcb);
  sub3 (pa, pc, vca);
  sub3 (pb, pa, vab);
  sub3 (pc, pa, vac);

  between (vca, vcb, mc->s.nv);
  between (vab, vac, ma->s.nv);

  bsprint (ma, "MA");
  bsprint (mb, "MB");
  bsprint (mc, "MC");


/* nominal Eigenmode (clockwise = reverse direction) */
  slist[0] = &(ma->s);
  slist[1] = &(mb->s);
  slist[2] = &(mc->s);
  eigenmode (slist, 3, ba);

  splitf (mb, ba, bb, NULL, NULL, NULL, 0);
  splitf (mc, bb, bc, bcback, NULL, NULL, 0);	/* bcback   leaks from cavity towards source */

/* set mirror SM1 position and orientation */

  makedummybs (sm1, M_RC, "SM1");
  makedummybs (sm2, M_RC, "SM2");
  makedummybs (m00, M_RC, "M00");

//  p2b (plz, psa, b0);         /* beam from laser to SM1 */

/* additional input mirror M00 for testing */
  {
    double p00a[3] = { 400, -550, 0 };
    double p00b[3] = { 400, -500, 0 };
    double p00[3] = { 400, -400, 0 };
    double a00[3] = { -1, -1, 0 };
    p2b (p00a, p00b, b00);
    nvecs (a00);
    cp3 (p00, m00->s.zen);
    cp3 (a00, m00->s.nv);
    bsprint (m00, "M00");
    splitf (m00, b00, b0, NULL, NULL, NULL, 0);
  }

  cp3 (psa, sm1->s.zen);
  nvecs (a45);			/* normalize to get +45 degree direction */
  cp3 (a45, sm1->s.nv);
  bsprint (sm1, "SM1");

  splitf (sm1, b0, b1, NULL, NULL, NULL, 0);	/* b1 is reflected beam */

/* set mirror SM2 position and orientation */

  adjust_surf_2beams (b1, bcback, &(sm2->s));
  bsprint (sm2, "SM2");
/* nominal mirror positions are now fixed. */

/* now set photodiodes */

  splitf (sm2, b1, b2r, b2t, NULL, NULL, 0);
  makepd (b2t, PD2DIST, "PD2", pd2);
  makepd_nv (pd2, nz);
  pdprint (pd2, "PD2");

  splitb (mc, b2r, bcr, bct, NULL, NULL);
  makepd (bcr, 1594.9, "PDC", pdc);
  makepd_nv (pdc, nz);
  pdprint (pdc, "PDC");

  splitf (mb, bct, bbr, bbt, NULL, NULL, 0);
  makepd (bbt, PDBDIST, "PDB", pdb);
  makepd_nv (pdb, nz);
  pdprint (pdb, "PDB");

  splitf (ma, bbr, NULL, bat, NULL, NULL, 0);
  makepd (bat, PDADIST, "PDA", pda);
  makepd_nv (pda, nz);
  pdprint (pda, "PDA");





/* copy beamsplitters */
  memcpy (&(bs[0]), sm1, sizeof (beamsplitter));
  memcpy (&(bs[1]), sm2, sizeof (beamsplitter));
  memcpy (&(bs[2]), ma, sizeof (beamsplitter));
  memcpy (&(bs[3]), mb, sizeof (beamsplitter));
  memcpy (&(bs[4]), mc, sizeof (beamsplitter));
  memcpy (&(bs[5]), m00, sizeof (beamsplitter));


/* copy photodiodes */
  memcpy (&(pd[0]), pd2, sizeof (photodiode));
  memcpy (&(pd[1]), pda, sizeof (photodiode));
  memcpy (&(pd[2]), pdb, sizeof (photodiode));
  memcpy (&(pd[3]), pdc, sizeof (photodiode));

/* copy initial beam */
  memcpy (&(b[0]), b00, sizeof (beam));

/* Find Eigenmode beam parameters and trace them back to input */

  slist[0] = &(mb->s);
  slist[1] = &(mc->s);
  slist[2] = &(ma->s);
  qeigenmode (slist, 3, qb, LAMBDA);
  qbprint (qb, "b->c");
  qsplitf (mc, qb, qbc, NULL, NULL, NULL, 0);
  qbprint (qbc, "c refl");
  qsplitf (ma, qbc, qbca, NULL, NULL, NULL, 0);
  qbprint (qbca, "a refl");
  qsplitf (mb, qbca, qbcab, NULL, NULL, NULL, 0);
  qbprint (qbcab, "b refl");

//  exit (1);

  qsplitf (mc, qb, NULL, qct, NULL, NULL, 0);
  qbprint (qct, "MC trans");
  qsplitf (sm2, qct, q2r, NULL, NULL, NULL, 0);
  qbprint (q2r, "M2 refl");
  qsplitf (sm1, q2r, q1r, NULL, NULL, NULL, 0);
  qbprint (q1r, "M1 refl");
  qprop (&(m00->s), q1r, q0);
  qbprint (q0, "M00");
  qbrev (q0);
  qbprint (q0, "Initial beam");

//  ocbeam (b00, BEAM_RADIUS, ocfp);
  ocqbeam (q0, ocfp);

/* write OPTOCAD file */
  for (i = 0; i < N_BS; i++)
    ocbs (bs + i, ocfp);
  for (i = 0; i < N_PD; i++)
    ocpd (pd + i, ocfp);

  OCBEAMLABEL (b00);
  OCBEAMLABEL (b0);
  OCBEAMLABEL (b1);
  OCBEAMLABEL (b2r);
  OCBEAMLABEL (b2t);
  OCBEAMLABEL (bcr);
}

void
pd_signals (beamsplitter bs[], photodiode pd[], beam b[], double *res)
{
  BEAM (ba);
  BEAM (bb);
  BEAM (bc);

  BEAM (b00);
  BEAM (b0);
  BEAM (b1);
  BEAM (b2t);
  BEAM (b2r);
  BEAM (bcr);
  BEAM (bat);
  BEAM (bbt);
  BEAM (bct);



  surf *slist[3];

  double res2[6];
  double resa[6];
  double resb[6];
  double resct[6];
  double rescr[6];
  double sph[3], sph0[3];

/* initial beam */
  memcpy (b00, &(b[0]), sizeof (beam));
  splitf (&(bs[5]), b00, b0, NULL, NULL, NULL, 0);	/* M00 */

/* trace incoming beam */
  splitf (&(bs[0]), b0, b1, NULL, NULL, NULL, 0);	/* SM1 */
  splitf (&(bs[1]), b1, b2r, b2t, NULL, NULL, 0);	/* SM2 */
  splitb (&(bs[4]), b2r, bcr, NULL, NULL, NULL);	/* MC */

/* find Eigenmode (counterclockwise = real direction) */
  slist[0] = &(bs[4].s);	/* MC */
  slist[1] = &(bs[3].s);	/* MB */
  slist[2] = &(bs[2].s);	/* MA */
  eigenmode (slist, 3, bc);	/* eigenmode beam from MC towards MB */

/* trace Eigenmode */
  splitf (&(bs[3]), bc, bb, bbt, NULL, NULL, 0);	/* MB */
  splitf (&(bs[2]), bb, ba, bat, NULL, NULL, 0);	/* MA */
  splitf (&(bs[4]), ba, NULL, bct, NULL, NULL, 0);	/* MC */

/* compute signals on photodiodes */


  pdisect (&(pd[0]), b2t, res2);	/* PD2, beam b2t */
  pdisect (&(pd[1]), bat, resa);	/* PDA, beam bat */
  pdisect (&(pd[2]), bbt, resb);	/* PDB, beam bbt */
  pdisect (&(pd[3]), bct, resct);	/* PDC, beam bct (cavity leak) */
  pdisect (&(pd[3]), bcr, rescr);	/* PDC, beam bcr (reflected incoming) */

  if (res == NULL)
    return;
  res[0] = res2[0];		/* PD2 DC horiz. */
  res[1] = resa[0];		/* PDA DC horiz. */
  res[2] = resb[0];		/* PDB DC horiz. */
  res[3] = resct[0] - rescr[0];	/* PDC DWS 0� horiz. (spot pos diff) */
  res[4] = resct[2] - rescr[2];	/* PDC DWS 90� horiz. (angle diff) */
  res[7] = resct[0];		/* PDC spot pos horiz. cavity leak */
  res[8] = rescr[0];		/* PDC spot pos horiz. reflected */
  res[9] = resct[2];		/* PDC spot angle horiz. cavity leak */
  res[10] = rescr[2];		/* PDC spot angle horiz. reflected */


  res[11] = res2[1];		/* PD2 DC vert. */
  res[12] = resa[1];		/* PDA DC vert. */
  res[13] = resb[1];		/* PDB DC vert. */
  res[14] = resct[1] - rescr[1];	/* PDC DWS 0� vert. (spot pos diff) */
  res[15] = resct[3] - rescr[3];	/* PDC DWS 90� vert. (angle diff) */
  res[18] = resct[1];		/* PDC spot pos vert. cavity leak */
  res[19] = rescr[1];		/* PDC spot pos vert. reflected */
  res[20] = resct[3];		/* PDC spot angle vert. cavity leak */
  res[21] = rescr[3];		/* PDC spot angle vert. reflected */
/* extra signals (SM1,2 feedback) */

  r2s (bs_nom[0].s.nv, sph0);	/* nominal SM1  */
  r2s (bs[0].s.nv, sph);	/* locked SM1 */
  res[5] = sph[2] - sph0[2];	/* hor */
  res[16] = sph[1] - sph0[1];	/* vert */

  r2s (bs_nom[1].s.nv, sph0);	/* nominal SM2  */
  r2s (bs[1].s.nv, sph);	/* locked SM2 */
  res[6] = sph[2] - sph0[2];	/* hor */
  res[17] = sph[1] - sph0[1];	/* vert */



}

void 
dcmatrix (void) 
{
  
int ibs, isig, dir;
  
double spher[3], res0[N_SIG], res[N_SIG];
  


printf ("\n%4s   ", " ");
  
for (isig = 0; isig < N_SIG; isig++)
    
printf ("%12s ", signames[isig]);
  
printf ("\n");
  
printf ("%4s   ", " ");
  
for (isig = 0; isig < N_SIG; isig++)
    
printf ("%12s ", sigunits[isig]);
  
printf ("\n");
  

pd_signals (bs_nom, pd_nom, b0_nom, res0);
  

for (dir = 0; dir < 2; dir++)
    
    {
      
for (ibs = 0; ibs < N_BS; ibs++)
	
	{
	  
	    /* copy nominal configuration */ 
	    memcpy (bs_test, bs_nom, N_BS * sizeof (beamsplitter));
	  
r2s (bs_test[ibs].s.nv, spher);
	  
spher[2 - dir] += EPS;	/* dir: 0=horizontal, 1=vertical */
	  
s2r (spher, bs_test[ibs].s.nv);
	  
pd_signals (bs_test, pd_nom, b0_nom, res);
	  
printf ("%4s %s ", bsnames[ibs], dir ? "v" : "h");
	  
for (isig = 0; isig < N_SIG; isig++)
	    
printf ("%12.5f ", (res[isig] - res0[isig]) / EPS);
	  
printf ("\n");
	

}
    
}

}



static double tv[4][3];
static double res0[N_SIG];

void
prepare_newton (void)
{
  double nz[3] = { 0, 0, 1 };
  cross (bs_nom[0].s.nv, nz, tv[0]);
  cross (bs_nom[0].s.nv, tv[0], tv[1]);
  cross (bs_nom[1].s.nv, nz, tv[2]);
  cross (bs_nom[1].s.nv, tv[2], tv[3]);
  pd_signals (bs_nom, pd_nom, b0_nom, res0);
}

void
newtonfunc (double *x, double *y, beamsplitter * bs)
{
  double res[N_SIG];

/* copy nominal SM1,SM2; leave MA,MB,MC untouched */
  memcpy (bs, bs_nom, 2 * sizeof (beamsplitter));

/* inputs x[0]...x[3]: tilts of SM1, SM2 w.r.t. nominal */
  bs[0].s.nv[0] = bs_nom[0].s.nv[0] + x[0] * tv[0][0] + x[1] * tv[1][0];
  bs[0].s.nv[1] = bs_nom[0].s.nv[1] + x[0] * tv[0][1] + x[1] * tv[1][1];
  bs[0].s.nv[2] = bs_nom[0].s.nv[2] + x[0] * tv[0][2] + x[1] * tv[1][2];

  bs[1].s.nv[0] = bs_nom[1].s.nv[0] + x[2] * tv[2][0] + x[3] * tv[3][0];
  bs[1].s.nv[1] = bs_nom[1].s.nv[1] + x[2] * tv[2][1] + x[3] * tv[3][1];
  bs[1].s.nv[2] = bs_nom[1].s.nv[2] + x[2] * tv[2][2] + x[3] * tv[3][2];

  nvecs (bs[0].s.nv);
  nvecs (bs[1].s.nv);

  pd_signals (bs, pd_nom, b0_nom, res);

/* outputs: DWS signals on PD2 */
  y[0] = (res[3] - res0[3]) / 1000.;	/* spot pos diff */
  y[1] = (res[14] - res0[14]) / 1000.;
  y[2] = res[4] - res0[4];	/* angle diff */
  y[3] = res[15] - res0[15];

}


void
pd_signals_lock (beamsplitter bs[], double *res)
{
  double x[4], f[4], xsave, h;
  double xtest[4], ftest[4], a[4 * 4];
  int i, j, niter = 0;

  memcpy (bs, bs_nom, 2 * sizeof (beamsplitter));

/* start at zero */
  x[0] = x[1] = x[2] = x[3] = 0;

/* Newton method */
  for (;;)
    {
      newtonfunc (x, f, bs);	/* function value */
      /*
         printf
         ("  Newton %12.8f %12.8f %12.8f %12.8f -> %12.8f %12.8f %12.8f %12.8f (%g)\n",
         x[0], x[1], x[2], x[3], f[0], f[1], f[2], f[3], l2norm (f, 4));
       */
//      printf ("%g ", l2norm (f, 4));
      if (l2norm (f, 4) < NEWT_TOL)
	break;			/* done; bs contains best placement */
      for (i = 0; i < 4; i++)	/* build matrix of partial derivatives */
	{
	  memcpy (xtest, x, 4 * sizeof (double));
	  xsave = xtest[i];
	  xtest[i] += EPS_NEWTON;
	  h = xtest[i] - xsave;
	  newtonfunc (xtest, ftest, bs);
	  for (j = 0; j < 4; j++)
	    a[i + j * 4] = (ftest[j] - f[j]) / h;
	}
      gaussj (a, 4, f);		/* invert matrix, solve for corrections */
      for (i = 0; i < 4; i++)	/* update */
	x[i] -= f[i];
      ++niter;
      if (niter > 50)
	{
	  fprintf (stderr, "pd_signals_lock does not converge\n");
	  exit (1);
	}
    }

/* 'bs' now contains the locked positions of M1,M2 */

/* get standard photodiode signals */
  pd_signals (bs, pd_nom, b0_nom, res);


}



void 
dcmatrix_lock (void) 
{
  
int ibs, isig, dir;
  
double spher[3], res0[N_SIG], res[N_SIG];
  

printf ("\n%4s   ", " ");
  
for (isig = 0; isig < N_SIG; isig++)
    
printf ("%12s ", signames[isig]);
  
printf ("\n");
  
printf ("%4s   ", " ");
  
for (isig = 0; isig < N_SIG; isig++)
    
printf ("%12s ", sigunits[isig]);
  
printf ("\n");
  

memcpy (bs_test, bs_nom, N_BS * sizeof (beamsplitter));
  
pd_signals_lock (bs_test, res0);
  

for (dir = 0; dir < 2; dir++)
    
    {
      
for (ibs = 2; ibs < N_BS; ibs++)	/* skip SM1, SM2; start with MA */
	
	{
	  
	    /* copy nominal configuration */ 
	    memcpy (bs_test, bs_nom, N_BS * sizeof (beamsplitter));
	  
r2s (bs_test[ibs].s.nv, spher);
	  
spher[2 - dir] += EPS;	/* dir: 0=horizontal, 1=vertical */
	  
s2r (spher, bs_test[ibs].s.nv);
	  
pd_signals_lock (bs_test, res);
	  
printf ("%4s %s ", bsnames[ibs], dir ? "v" : "h");
	  
for (isig = 0; isig < N_SIG; isig++)
	    
printf ("%12.5f ", (res[isig] - res0[isig]) / EPS);
	  
printf ("\n");
	
}
    
}

}


int
main (void)
{
  assert ((ocfp = fopen ("pt1.oc", "w")) != NULL);
  assert ((incfp = fopen ("pt1.inc", "w")) != NULL);

  init_nominal (bs_nom, pd_nom, b0_nom);
  dcmatrix ();

  prepare_newton ();

  dcmatrix_lock ();

  fclose (ocfp);
  fclose (incfp);

  return 0;
}