#include <stdio.h>
#include <stdint.h>
#include <fcntl.h>

#include "servo.h"

#define NUM_LEGS 6

const float x_offsets[NUM_LEGS]={ 0.20, 0.00,-0.20,-0.20, 0.00, 0.20};
const float y_offsets[NUM_LEGS]={ 0.10, 0.15, 0.10,-0.10,-0.15,-0.10};

float phi1[6];
float phi2[6];
float phi3[6];


void
draw_hexapod() {
   const float c =0.05f;
   const float f =0.20f;
   const float t =0.325f;
   const float p1=0.025f;

   const float thickness=0.005f;

   int i,fd;

   fd=open("/tmp/servodata",O_RDONLY);
   if(fd==-1) {
//      perror("/tmp/servodata");
   } else {
      read(fd,servo_pwm,sizeof(servo_pwm));
      close(fd);
      for(i=0;i<6;i++) { 
         phi1[i]=servo_pwm[i*3];
         phi1[i]-=servo_offsets[i*3]; 
         phi2[i]=servo_pwm[i*3+1];
         phi2[i]-=servo_offsets[i*3+1]; 
         phi3[i]=servo_pwm[i*3+2];
         phi3[i]-=servo_offsets[i*3+2]; 
         phi1[i]*=0.1/8.5;
         phi2[i]*=-0.1/8.5;
         phi3[i]*=-0.1/8.5;
         if(i>=3) {
            phi2[i]=-phi2[i];
            phi3[i]=-phi3[i];
         }
            
            
       }
   }
   glRotatef(90,0,0,1);
   glRotatef(60,0,1,0);

   glColor3f(0.8,0.8,0.8);
   glBegin(GL_TRIANGLES);
      for(i=0;i<NUM_LEGS;i++) {
         glVertex3f(x_offsets[i],y_offsets[i],0);
      }
   glEnd();

   glBegin(GL_QUADS);
      glVertex3f(x_offsets[0],y_offsets[0],0);
      glVertex3f(x_offsets[2],y_offsets[2],0);
      glVertex3f(x_offsets[3],y_offsets[3],0);
      glVertex3f(x_offsets[5],y_offsets[5],0);
   glEnd();
   
   for(i=0;i<NUM_LEGS;i++) {
      glPushMatrix();

      // move into leg base position
      glTranslatef(x_offsets[i],y_offsets[i],0);
      // rotate 180 degrees if on other side
      if(i>=(NUM_LEGS/2)) {
         glRotatef(-180,0,0,1);
      }

      glRotatef(phi1[i],0,0,1);
      glColor3f(1,1,0);
      glBegin(GL_QUADS);
         glVertex3f(0,0,0);
         glVertex3f(0,c,0);
         glVertex3f(thickness,c,0);
         glVertex3f(thickness,0,0);
      glEnd();
      
      glTranslatef(0,c,0);
      glRotatef(90,0,1,0);
      glRotatef(phi2[i],0,0,1);
      glColor3f(0,0,1);
      glBegin(GL_QUADS);
         glVertex3f(0,0,0);
         glVertex3f(0,f,0);
         glVertex3f(thickness,f,0);
         glVertex3f(thickness,0,0);

         glVertex3f(0,f,0);
         glVertex3f(0,0,0);
         glVertex3f(0,0,thickness);
         glVertex3f(0,f,thickness);

         glVertex3f(0,f,0);
         glVertex3f(thickness,f,0);
         glVertex3f(0,f,thickness);
         glVertex3f(thickness,f,thickness);

         glVertex3f(thickness,f,0);
         glVertex3f(thickness,0,0);
         glVertex3f(thickness,0,thickness);
         glVertex3f(thickness,f,thickness);


         glVertex3f(0,0,thickness);
         glVertex3f(0,f,thickness);
         glVertex3f(thickness,f,thickness);
         glVertex3f(thickness,0,thickness);
      glEnd();

      glTranslatef(0,f,0);      
      
      glRotatef(-90-phi3[i],0,0,1);
      glColor3f(1,0,0);
      glBegin(GL_QUADS);
         glVertex3f(  p1, 0.0f, thickness/2);
         glVertex3f(0.0f,  -p1, thickness/2);
         glVertex3f( -p1, 0.0f, thickness/2);
         glVertex3f(0.0f,    t, thickness/2);

         glVertex3f(  p1, 0.0f, thickness/2);
         glVertex3f(  p1, 0.0f, -thickness/2);
         glVertex3f(0.0f,  -p1, thickness/2);
         glVertex3f(0.0f,  -p1, -thickness/2);
      
         glVertex3f(0.0f,  -p1, thickness/2);
         glVertex3f(0.0f,  -p1, -thickness/2);
         glVertex3f( -p1, 0.0f, thickness/2);
         glVertex3f( -p1, 0.0f, -thickness/2);

         glVertex3f( -p1, 0.0f, thickness/2);
         glVertex3f( -p1, 0.0f, -thickness/2);
         glVertex3f(0.0f,    t, thickness/2);
         glVertex3f(0.0f,    t, -thickness/2);

         glVertex3f(0.0f,    t, thickness/2);
         glVertex3f(  p1, 0.0f, -thickness/2);
         glVertex3f(  p1, 0.0f, thickness/2);
         glVertex3f(0.0f,    t, -thickness/2);

      
         glVertex3f(  p1, 0.0f, -thickness/2);
         glVertex3f(0.0f,  -p1, -thickness/2);
         glVertex3f( -p1, 0.0f, -thickness/2);
         glVertex3f(0.0f,    t, -thickness/2);
      glEnd();
      glPopMatrix();
   }
         
}