#include <dm_view_tools.h>
#include <jsrandom.h>
#include <assert.h>

void dm_view_tools_draw_tetrahedron(void){
  
  r3_t v[4] = {
    (r3_t) {{+1,+1,+1}},
    (r3_t) {{+1,-1,-1}},
    (r3_t) {{-1,+1,-1}},
    (r3_t) {{-1,-1,+1}}
  } ;
  glBegin(GL_LINES);
  int i,j;
  for(i = 0; i < 3; i++){
    for(j = i+1; j < 4; j++){
      glVertex3f(v[i].c[0],v[i].c[1],v[i].c[2]);
      glVertex3f(v[j].c[0],v[j].c[1],v[j].c[2]);
    }
  }
  
  glEnd();
  
  
}


void dm_view_tools_draw_sequence(dm_seq_t *z, frgb_t* color, double radius, r3_t perturbation)
{
     /* Set surface finish: */
    glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
    glEnable(GL_COLOR_MATERIAL);
    glColor3f(color->c[0],color->c[1],color->c[2]);
    r3_t qi; /*previous point*/
    int i;
    GLUquadricObj* quad =  gluNewQuadric();

    for(i = 0; i < dm_seq_num_datums(z); i++){
      dm_datum_t di = dm_seq_get_datum(z,i);
      r3_t pi;
      int j;
      for(j = 0; j < 3; j++){
	pi.c[j] = dm_sample_decode(di.c[j])*z->sfac.f[j];
	pi.c[j] +=  perturbation.c[j]; 
      }
      dm_view_tools_draw_ball(&pi,radius,quad);
      if( i > 0 ){
	dm_view_tools_draw_stick(&qi,&pi,0.30*radius,quad);
      }
      qi = pi;
    }
    gluDeleteQuadric(quad);
    glDisable(GL_COLOR_MATERIAL);
  }

void dm_view_tools_draw_ball(r3_t* p,double radius,GLUquadricObj* quad){
   glPushMatrix();
   glTranslatef(p->c[0], p->c[1], p->c[2]);
   gluSphere( quad, radius, 6,6);
   glPopMatrix();
}

void dm_view_tools_draw_stick(r3_t* p, r3_t* q, double radius, GLUquadricObj* quad){
  glPushMatrix();
  glTranslatef(p->c[0], p->c[1], p->c[2]);
  r3_t z = (r3_t){{0,0,1}};
  r3_t d;
  r3_sub(q,p,&d);
  (void) r3_dir(&d,&d);
  double angle = (180.0/M_PI) * acos(r3_dot(&z,&d)) ;
  if(angle > 1.0e-3){
    r3_t t;
    r3_cross(&z,&d,&t);
    glRotatef(angle,t.c[0],t.c[1],t.c[2]);
  }
  gluCylinder( quad,radius, radius, r3_dist(p,q), 6,1 );
  glPopMatrix();
}

void dm_view_tools_draw_pairing(
  msm_rung_vec_t* gv,
  dm_seq_t* x,dm_seq_t* y,
  frgb_t* color_x,frgb_t* color_y, frgb_t* color_p,
  double radius,
  bool_t perturbation
  )
{
  
  assert(x->nsub == 1);
  assert(y->nsub == 1);
  
  r3_t zero_pt = (r3_t){{ 0,0,0}};
  r3_t disp_pt = (perturbation ? (r3_t){{ radius,radius,radius}} : zero_pt);
  
  auto void draw_pairing_stick(int ix, int iy, GLUquadricObj* qd);
  auto void draw_pairing_stick(int ix, int iy, GLUquadricObj* qd){
    dm_datum_t dx = dm_seq_get_datum(x,ix);
    dm_datum_t dy = dm_seq_get_datum(y,iy);
    r3_t px,py;
    int j;
    for(j = 0; j < 3; j++){
      px.c[j] = dm_sample_decode(dx.c[j])*x->sfac.f[j];
      py.c[j] = dm_sample_decode(dy.c[j])*y->sfac.f[j] + disp_pt.c[j];
    }
    dm_view_tools_draw_stick(&px,&py,radius*0.4, qd);
    
  }
  
  /*Draw the sequences*/
  
  dm_view_tools_draw_sequence(x,color_x,radius,zero_pt);
  dm_view_tools_draw_sequence(y,color_y,radius,disp_pt);
  
  glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
  glEnable(GL_COLOR_MATERIAL);
  glColor3f(color_p->c[0],color_p->c[1],color_p->c[2]);
  GLUquadricObj* quad  = gluNewQuadric();
  /*Now we draw the sticks between sequences*/
  msm_rung_t gini = gv->e[0];
  int i;
  for(i = 0; i < gv->ne; i++){
    msm_rung_t h = gv->e[i];
    int hxk = h.c[0] - gini.c[0];
    int hyk = h.c[1] - gini.c[1];
    draw_pairing_stick(hxk,hyk,quad);
  }
  gluDeleteQuadric(quad);
  glDisable(GL_COLOR_MATERIAL);
}