#include <stdio.h>
#include "Head.h"
#include "Quickdraw.h"
#include "events.h"
#include <math.h>
#include <Traps.h>
#include <stdlib.h>
#define consv 0
#define spin 0

static int min(int x, int y){return x<y?x:y;}
static float sqr(float x){return x*x;}
static void sm(float x){if(x>0.00001 || x<-0.00001) 
{printf("Big %13.7f\n",x); exit(1);}}
/*static void smt(tri t){smv(t.A[0]); smv(t.A[1]); smv(t.A[2]);}*/
/* sqz merely codes a lower trang. matrix into a shorter lt. */
static lt sqz(tri z) {lt x;
   sm(z.A[0].a[1]); sm(z.A[0].a[2]); sm(z.A[1].a[2]);
  x.a=z.A[0].a[0]; x.b=z.A[1].a[0]; x.c=z.A[1].a[1];
  x.d=z.A[2].a[0]; x.e=z.A[2].a[1]; x.f=z.A[2].a[2]; return x;}
/* Transpose a matrix. */
tri tp(tri x) {int i,j; tri y; for (i=0; i<3; i++) for (j=0; j<3; j++)
  y.A[i].a[j] = x.A[j].a[i]; return y;}
vec operator-(vec a, vec b) /* vector difference */
  {vec d; int i; for (i=0; i<3; i++) d.a[i]=a.a[i]-b.a[i]; return d;}
vec operator+(vec a, vec b) /* vector sum */
  {vec d; int i; for (i=0; i<3; i++) d.a[i]=a.a[i]+b.a[i]; return d;}
/* Invert a lower triangular matrix, yielding another lt matrix. */
static lt invd(lt x) {lt q; q.a = 1./x.a; q.c = 1./x.c;
  q.f = 1./x.f; q.b = -x.b*q.a*q.c; q.e = - x.e*q.c*q.f;
  q.d = -(q.a*x.d+q.b*x.e)*q.f; return q;}
/* Product of a lt with a vector */
vec operator*(lt d, vec e) {vec r; r.a[0] = d.a*e.a[0];
  r.a[1] = d.b*e.a[0]+d.c*e.a[1];
  r.a[2] = d.d*e.a[0]+d.e*e.a[1]+d.f*e.a[2]; return r;}
/* Inner product */
float operator*(vec e, vec f) {register double s=0; int i;
   for(i=0; i<3; i++) s += e.a[i]*f.a[i]; return s;}
/* Scalar product */
vec operator*(register float e, vec f)
  {vec v; int i; for (i=0; i<3; i++) v.a[i] = e*f.a[i]; return v;}
/* Inverse of length of a vector */
static float il(vec e) {return 1./sqrt(e*e);}
/* Cross product of two vectors */
vec operator%(vec x, vec y) {vec z;
  z.a[0] = x.a[1]*y.a[2] - x.a[2]*y.a[1];
  z.a[1] = x.a[2]*y.a[0] - x.a[0]*y.a[2];
  z.a[2] = x.a[0]*y.a[1] - x.a[1]*y.a[0]; return z;}
/* Gram Schmidt */
static tri gs(tri t) {tri v; v.A[0]=il(t.A[0])*t.A[0];
  {vec x = t.A[1] - t.A[1]*v.A[0]*v.A[0]; v.A[1] = il(x)*x;}
  {vec x = t.A[2] - t.A[2]*v.A[0]*v.A[0] - 
     t.A[2]*v.A[1]*v.A[1];
    v.A[2]=il(x)*x;} return v;}
/* Express a triad in terms of its own gs frame */
static tri rx(tri s) {tri sr; int i,j; tri st = gs(s);
  for (i=0; i<3; i++) for (j=0; j<3; j++)
     sr.A[i].a[j] = s.A[i]*st.A[j]; return sr;}
/* Compute a tetrahedron's "preferred" shape. */
static lt shape(tri s) {return invd(sqz(rx(s)));}
/* void pr(vec v) 
    {int i; for (i=0; i<3; i++) printf("%11.6f ", v.a[i]); printf("\n");}
static void pt(tri v)
   {int i; printf("\n"); for (i=0; i<3; i++) pr(v.A[i]);}*/
vec zvec = {0,0,0}; int damp = 0;
vert cvert(vec v, float m) /* Construct a vertex given parts. */
  {vert z; z.wh = v; z.mom = zvec; z.mass = m; return z;}
/* Construct a tri given three vectors. */
tri ctri(vec i, vec j, vec k) {tri t; t.A[0]=i; t.A[1]=j; t.A[2]=k; return t;}
vec cvec(float i, float j, float k) /* Construct a vector given components.*/
  {vec v; v.a[0]=i; v.a[1]=j; v.a[2]=k; return v;}
tri starting;/* This is not a tensor, it just looks like one! */
/* Compute vector displacement from one vertex to another. */
vec cdvec(int i, int j) {return u[j].wh - u[i].wh;}
/* Construct a zone given its prefered shape and vertex list. */
typedef struct {short q[4];} int4;
static simp csimp(lt sh, int4 n, short m) {simp s;
  int i; s.mat=m; s.sh=sh; for (i=0; i<4; i++) s.v[i] = n.q[i]; return s;}
/* Add a zone to world given its four verticies. */
int xx=0;
static void cxsimp(int i, int j, int k, int l)
  {int4 q; vec a, b, c; float mass;
  q.q[0]=i; q.q[1]=j; q.q[2]=k; q.q[3]=l;
  a=cdvec(i,j); b=cdvec(i,k); c=cdvec(i,l);
  mass = a%b*c/24.; 
  if(mass<=0) printf("mass=%11.5f %u %u %u %u\n", mass, i,j,k,l);
  u[i].mass+=mass; u[j].mass+=mass; u[k].mass+=mass; u[l].mass+=mass;
  s[xx++]= csimp(shape(ctri(a,b,c)), q, material);}
/* Express a triangular prism as three tetrahedra. */
  void tent( int i, int j, int k, int l, int m, int n)
  {cxsimp(i,j,k,l); cxsimp(j,k,l,m); cxsimp(k,l,m,n);}
int cycle=0; float dt = .01;

static tri conserve(){vec p=zvec, mom=zvec, am=zvec; int i;
   for (i=0; i<n_vert; i++)
   {float m = u[i].mass; vec mm = u[i].mom, w = u[i].wh;
   p=p + m*w; mom=mom + 100.*mm; am=am+w%mm;}
   return ctri(p, mom, am);}
GrafPort p; 
vec H, V; 
int oh=0,ov=0; 
int ScrDepth; 
float vel;
float dang = 0.1, sn, cs; short fre = 0;
static void draw_num(long j)
{if(j<0){DrawChar('-'); draw_num(-j);}
   else {if(j>9)draw_num(j/10); DrawChar('0'+j%10);}}
static void dn(float a){Move(4,0); draw_num((long) (10000.*a));}
static void draw_vec(int i, int j, vec c){MoveTo(i,j);
  dn(c.a[0]); dn(c.a[1]); dn(c.a[2]);}
static void rot(float s) {vec h=cs*H + s*V;
  V=cs*V - s*H; H=h;}
static void rotx(float s) {H=cs*H + s*il(V)*(H%V);}
static void roty(float s) {V=cs*V + s*il(H)*(H%V);}
static void dl(vec a, vec b)
#if 1
   {MoveTo((int)(a*H)+oh, ScrDepth-(int)(a*V)+ov);
    LineTo((int)(b*H)+oh, ScrDepth-(int)(b*V)+ov);}
#else
   {float qq = a*H+oh; 
    int qw =  qq;
    MoveTo(qw,              ScrDepth-(int)(a*V)+ov);
    LineTo((int)(b*H)+oh, ScrDepth-(int)(b*V)+ov);}
#endif
int show_vel=0, run=0, change=1, n_segs=0;
void rec_seg(int i, int j) {segs[n_segs].a = i; segs[n_segs++].b = j;}
static void draw() {{FillRect(&p.portRect, &p.bkPat);
  MoveTo(10,10); draw_num(cycle); {int i;
  for (i=0; i<n_segs; i++) dl(u[segs[i].a].wh, u[segs[i].b].wh);}
  if(show_vel){int i; for (i=0; i<n_vert; i++)
  dl(u[i].wh,u[i].wh + vel*u[i].mom);}
  if(consv){tri a=conserve(); int i; for (i=0; i<3; i++)
  {draw_vec(8, 20+22*i, starting.A[i]);
   draw_vec(8, 30+22*i,        a.A[i]);}}}}
static void sincos() {sn=sin(dang); cs=cos(dang);}

void main() {init();
  sn=sin(dang); cs=cos(dang);
  if(spin){/* Spin */ vec centroid, axis = cvec(-.01,.01,.03); int i;
  {int i; float mass=0; vec cn=zvec; for (i=0; i<n_vert; i++)
   {mass += u[i].mass; cn = cn + u[i].mass*u[i].wh;}
   centroid = 1/mass*cn;}   
    for (i=0; i<n_vert; i++) u[i].mom =
     u[i].mass*(u[i].wh - centroid)%axis;}
   starting=conserve();
   {{InitGraf(&qd.thePort); OpenPort(&p);
    H=zvec; V=zvec; ScrDepth=p.portRect.bottom;
    H.a[0]=min(ScrDepth,p.portRect.right); V.a[1] = H.a[0];}
   /*while(1)*/{ int ves=0;
  {EventRecord ev;
    while(1){
    while(GetOSEvent(/*keyDownMask*/ 8, &ev))
      {change=1; switch(ev.message&charCodeMask){
       case 's': H=0.9*H; V=0.9*V; break;
       case 'S': H=1.1*H; V=1.1*V; break;
       case  28: oh-=25;    break;  case  29: oh+=25;     break;
       case  30: ov-=25;    break;  case  31: ov+=25;     break;
       case 'T': rot(sn);  break;  case 't': rot(-sn);  break;
       case 'R': rotx(-sn); break; case 'r': rotx(sn);  break; 
       case 'U': roty(sn); break;  case 'u': roty(-sn); break;
       case 'D': damp = 1; break; case 'd': damp = 0; break;
       case 'F': fre = 1; {vec z = {0, 0, 0}; int i; for(i=0; i<n_vert; ++i) u[i].mom = z;}
            break;  case 'f': fre = 0;   break;
       case 'G': run=1; break;     case 'g': run=0;     break;
       case 'M': dang *= 2.0; sincos(); break;
       case 'm': dang *= 0.5; sincos(); break;
       case 'v': show_vel=0; break;
       case 'V': show_vel=1; {int i; float l=0;
       for (i=0; i<n_vert; i++) l += u[i].mom*u[i].mom/sqr(u[i].mass);
       vel = sqrt(l);
       } 
       break;
       case 'q': goto end_all;
       default: break;
       }}
       if(change){draw(); 
       change=0;}
       if(run){physics(); if(!fre) force(dt); change=1;}}}}} end_all:;}