/* Calculate periodic orbits */

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

/* Macros for doing vector arithmatic */
#define setv(v, x1, y1) {v.x = x1; v.y = y1;}
#define copyv(v, w) {v.x = w.x; v.y = w.y;}
#define dotv(v1, v2) (v1.x*v2.x + v1.y*v2.y)
#define hypotv(v) hypot(v.x, v.y)
#define smulv(v1, a, v2) {v1.x=a*v2.x; v1.y=a*v2.y;}
#define subv(v1, v2, v3) {v1.x=v2.x-v3.x; v1.y=v2.y-v3.y;}
#define addv(v1, v2, v3) {v1.x=v2.x+v3.x; v1.y=v2.y+v3.y;}
#define addvx(v1, v2, x1, y1) {v1.x=v2.x+x1; v1.y=v2.y+y1;}
#define addtov(v, x1, y1) {v.x=v.x+x1; v.y=v.y+y1;}
#define linear(v, a, va, b, vb) {v.x=a*va.x+b*vb.x; v.y=a*va.y+b*vb.y;}
#define polar(v, r, theta) {v.x=r*cos(theta); v.y=r*sin(theta);}
#define printv(v) printf("(%f, %f)", v.x, v.y)

#define SMALL 1.0e-15
#define ND 3
#define PI 3.14159265358979323846

typedef struct _Vector {
  double x;
  double y;
} Vector;

char *progname, *orbit_str;

main(argc, argv)		/* driver */
     int argc;
     char *argv[];
{
  int N, *orbit, old, i;
  void periodic();
  double a=1.0/6.0;

  progname = argv[0];

  while (argc>1 && argv[1][0]=='-') {
    switch (argv[1][1]) {
    case 'a':
      a = atof(argv[2]);
      break;
    case 'R':
      a = 1.0/atof(argv[2]);
      break;
    default:
      fprintf(stderr, "usage: %s [-a x.xx|-R x.xx] orbit_string\n", progname);
      fprintf(stderr, "  e.g. > orbit -R 6 123\n");
      exit(0);
      break;
    }
    argc-=2;
    argv+=2;
  }
  if (argc!=2) {
    fprintf(stderr, "usage: %s [-a x.xx|-R x.xx] orbit_string\n", progname);
    fprintf(stderr, "  e.g. > orbit -R 6 123\n");
    exit(0);
  }

  orbit_str = argv[1];

  /* Convert orbit_string to list of disks and check for illegal orbit */
  N = strlen(orbit_str);
  if (N<2) {
    fprintf(stderr, "error: %s is an illegal orbit\n", argv[1]);
    exit(0);
  }
  orbit = (int *) calloc(N, sizeof(int));
  old = orbit_str[N-1] - '1';
  for(i=0; i<N; i++) {
    orbit[i] = orbit_str[i] - '1';
    if(orbit[i]<0 || orbit[i]>2 || orbit[i]==old) {
      fprintf(stderr, "error: %s is an illegal orbit\n", orbit_str);
      exit(0);
    }
    old = orbit[i];
  }
  periodic(orbit, N, a);
}
      
/* calculate the periodic orbit */
void periodic(orbit, N, a)
     int *orbit;		/* list of disk bounces */
     int N;			/* length of orbit */
     double a;			/* size of disk */
{
  Vector tmp_v, tmp_v1, A, B, C, disk[3], start[3], *Pos;
  int i, j, old, next;
  double len, old_len, tmp, tau, r, J[4], lam;
  double tmp1, tmp2, tmp3, tmp4, old_Bx;
  double l=0.5/sqrt(3.0), ia, asq;

  /* Set things up */
  Pos = (Vector *) calloc(N+1, sizeof(Vector));
  ia = 1.0/a;
  asq = a*a;
  setv(disk[0], -0.5, -l);
  setv(disk[1], 0.5, -l);
  setv(disk[2], 0, 2.0*l);
  addvx(start[0], disk[0], a, 0);
  addvx(start[1], disk[1], -a, 0);
  addvx(start[2], disk[2], 0, -a);

  /* starting positions */
  for(i=0; i<N; i++)
    copyv(Pos[i], start[orbit[i]]);

  /* Find minimum orbit by path minimization */
  len =5.0*N;
  do {
    old_len = len;
    len = 0.0;
    old = N-1;
    copyv(Pos[N], Pos[0]);
    for(i=0; i<N; i++) {
      subv(A, Pos[old], disk[orbit[i]]);
      subv(B, Pos[i], disk[orbit[i]]);
      subv(C, Pos[i+1], disk[orbit[i]]);
      old = i;
      tmp1 = dotv(A,A) + asq;
      tmp2 = dotv(C,C) + asq;
      j = 0;
      do {
	j++;
	old_Bx = B.x;
	tmp3 = 1.0/sqrt(tmp1 - 2.0*dotv(A, B));
	tmp4 = 1.0/sqrt(tmp2 - 2.0*dotv(C, B));
	linear(B, tmp3, A, tmp4, C);
	tmp = a/hypotv(B);
	smulv(B, tmp, B);
      } while (fabs(B.x-old_Bx)>SMALL);
      addv(Pos[i], B, disk[orbit[i]]);
      subv(tmp_v, A, B);
      len += hypotv(tmp_v);
      subv(tmp_v, C, B);
      len += hypotv(tmp_v);
    }
  } while(fabs(old_len-len) > SMALL);

  printf("Orbit = %s,  a = %f\n", orbit_str, a);
  for(i=0; i<N; i++) {
    printf("Position[%d] = ", i);
    printv(Pos[i]);
    printf("\n");
  }

  /* Calculate Jacobian */
  len = 0.0;
  J[0] = J[3] = 1.0;
  J[1] = J[2] = 0.0;
  for(i=0; i<N; i++) {
    next = (i+1)%N;
    subv(tmp_v, Pos[next], Pos[i]);
    len += tau = hypotv(tmp_v);
    subv(tmp_v1, Pos[i], disk[orbit[i]]);
    r = 2.0*a*tau/dotv(tmp_v, tmp_v1);
    tau *= ia;
    J[1] += J[0]*r;
    J[3] += J[2]*r;
    J[0] += J[1]*tau;
    J[2] += J[3]*tau;
  }

  if (N%2)
    tmp = -J[0]-J[3];
  else
    tmp = J[0]+J[3];

  if (tmp>0.0)
    lam = 0.5*(tmp+sqrt(tmp*tmp-4.0));
  else
    lam = 0.5*(tmp-sqrt(tmp*tmp-4.0));

  /* Note that we have not remove the symmetry factor */
  printf("Time = %f   Stability = %f\n", ia*len, lam);
}