/* class Robot
 *
 * Doug DeCarlo
 */

import javax.media.opengl.GL;
import javax.vecmath.*;

public class Robot extends Shape
{
    // Robot configuration
    private DoubleParameter h1, theta, h2, phi, h3, psi;

    // Constructor for Robot
    public Robot()
    {
	super("Robot");
        
        h1    = addParameter(new DoubleParameter("h1",    0.25, 0.05, 2, 2));
        theta = addParameter(new DoubleParameter("theta", 0, 0.0,  360, 2));
        h2    = addParameter(new DoubleParameter("h2",    0.5, 0.05, 2, 2));
        phi   = addParameter(new DoubleParameter("phi",   0, -90, 90, 2));
        h3    = addParameter(new DoubleParameter("h3",    0.4, 0.05, 2,  2));
        psi   = addParameter(new DoubleParameter("psi",   70, 0.0,  180, 2));
    }

    // Draw labeled XYZ axes at the origin with length 1
    // (this method changes the current color to yellow)
    public void drawXYZAxis(GL gl)
    {
        float text_loc = 1.15f, text_sc = 0.1f;
        float arrow_in = 0.1f, arrow_up = 0.02f;

        float xpts[][] = {
            { -1, 1, 0 }, {  1, -1, 0 },
            {  1, 1, 0 }, { -1, -1, 0 }
        };
        float ypts[][] = {
            { -1,  1, 0 }, { 0, 0, 0 },
            {  1,  1, 0 }, { 0, 0, 0 },
            {  0, -1, 0 }, { 0, 0, 0 }
        };
        float zpts[][] = {
            { 0,  1, 1}, { 0,  1, -1 },
            { 0, -1, 1}, { 0,  1, -1 },
            { 0, -1, 1}, { 0, -1, -1 }
        };

        // Turn off lighting if it was on
//        boolean[] lightingWasOn = new boolean[1];
//        gl.glGetBooleanv(GL.GL_LIGHTING, lightingWasOn, 0);
//        if (lightingWasOn[0])
            gl.glDisable(GL.GL_LIGHTING);

        gl.glLineWidth(2.0f);

        gl.glBegin(GL.GL_LINES);

        // X-axis
        gl.glVertex3f(-1,0,0);
        gl.glVertex3f(1,0,0);

        gl.glVertex3f(1-arrow_in,  arrow_up, 0);
        gl.glVertex3f(1,0,0);
        gl.glVertex3f(1-arrow_in, -arrow_up, 0);
        gl.glVertex3f(1,0,0);
        for (int i = 0; i < xpts.length; i++) {
            gl.glVertex3f(xpts[i][0] * text_sc + text_loc,
                           xpts[i][1] * text_sc,
                           xpts[i][2] * text_sc);
        }

        // Y-axis 
        gl.glVertex3f(0,-1,0);
        gl.glVertex3f(0,1,0);
    
        gl.glVertex3f( arrow_up, -1+arrow_in, 0);
        gl.glVertex3f(0,-1,0);
        gl.glVertex3f(-arrow_up, -1+arrow_in, 0);
        gl.glVertex3f(0,-1,0);

        gl.glVertex3f( arrow_up, 1-arrow_in, 0);
        gl.glVertex3f(0,1,0);
        gl.glVertex3f(-arrow_up, 1-arrow_in, 0);
        gl.glVertex3f(0,1,0);

        for (int i = 0; i < ypts.length; i++) {
            gl.glVertex3f(ypts[i][0] * text_sc,
                           ypts[i][1] * text_sc + text_loc,
                           ypts[i][2] * text_sc);
        }

        // Z-axis
        gl.glVertex3f(0,0,-1);
        gl.glVertex3f(0,0,1);

        gl.glVertex3f(0,  arrow_up, -1+arrow_in);
        gl.glVertex3f(0,0,-1);
        gl.glVertex3f(0, -arrow_up, -1+arrow_in);
        gl.glVertex3f(0,0,-1);

        gl.glVertex3f(0,  arrow_up, 1-arrow_in);
        gl.glVertex3f(0,0,1);
        gl.glVertex3f(0, -arrow_up, 1-arrow_in);
        gl.glVertex3f(0,0,1);

        for (int i = 0; i < zpts.length; i++) {
            gl.glVertex3f(zpts[i][0] * text_sc,
                           zpts[i][1] * text_sc,
                           zpts[i][2] * text_sc + text_loc);
        }

        gl.glEnd();

        // Restore lighting if necessary
//        if (lightingWasOn[0])
          gl.glEnable(GL.GL_LIGHTING);
    }

    // Draw a cube with side-length 2, centered at the origin
    protected void cube(GL gl)
    {
        /* Cube vertices */
        double[] v0 = {-1.0, -1.0, -1.0 };
        double[] v1 = {-1.0, -1.0,  1.0 };
        double[] v2 = {-1.0,  1.0,  1.0 };
        double[] v3 = {-1.0,  1.0, -1.0 };
        double[] v4 = { 1.0, -1.0, -1.0 };
        double[] v5 = { 1.0, -1.0,  1.0 };
        double[] v6 = { 1.0,  1.0,  1.0 };
        double[] v7 = { 1.0,  1.0, -1.0 };
	
        /* Cube normals */
        double[] nxp = {  1.0f,  0.0f,  0.0f };
        double[] nxn = { -1.0f,  0.0f,  0.0f };
        double[] nyp = {  0.0f,  1.0f,  0.0f };
        double[] nyn = {  0.0f, -1.0f,  0.0f };
        double[] nzp = {  0.0f,  0.0f,  1.0f };
        double[] nzn = {  0.0f,  0.0f, -1.0f };

        gl.glPushMatrix();
        gl.glTranslated(0, 0.5, 0);
        gl.glScalef(0.5f, 0.5f, 0.5f);

        gl.glBegin(GL.GL_QUADS);
        gl.glNormal3dv(nxn,0);
        gl.glVertex3dv(v0,0);
        gl.glVertex3dv(v1,0);
        gl.glVertex3dv(v2,0);
        gl.glVertex3dv(v3,0);

        gl.glNormal3dv(nxp,0);
        gl.glVertex3dv(v7,0);
        gl.glVertex3dv(v6,0);
        gl.glVertex3dv(v5,0);
        gl.glVertex3dv(v4,0);

        gl.glNormal3dv(nyn,0);
        gl.glVertex3dv(v4,0);
        gl.glVertex3dv(v5,0);
        gl.glVertex3dv(v1,0);
        gl.glVertex3dv(v0,0);

        gl.glNormal3dv(nyp,0);
        gl.glVertex3dv(v3,0);
        gl.glVertex3dv(v2,0);
        gl.glVertex3dv(v6,0);
        gl.glVertex3dv(v7,0);

        gl.glNormal3dv(nzn,0);
        gl.glVertex3dv(v0,0);
        gl.glVertex3dv(v3,0);
        gl.glVertex3dv(v7,0);
        gl.glVertex3dv(v4,0);

        gl.glNormal3dv(nzp,0);
        gl.glVertex3dv(v1,0);
        gl.glVertex3dv(v5,0);
        gl.glVertex3dv(v6,0);
        gl.glVertex3dv(v2,0);

        gl.glEnd();

        gl.glPopMatrix();
    }

    void cyl(GL gl)
    {
        gl.glPushMatrix();
        gl.glTranslatef(0, 1, 0);
        gl.glRotatef(90, 1, 0, 0);
        Objs.cylinder(gl);
        gl.glPopMatrix();
    }

    void base(GL gl)
    {
        gl.glColor3f(1.0f, 0.5f, 0.5f);
        gl.glPushMatrix();        
        gl.glScalef(1.0f, (float)h1.value, 1.0f);
        cube(gl);
        gl.glPopMatrix();
    }

    void arm(GL gl)
    {
        gl.glColor3f(0.5f, 1.0f, 0.5f);
        gl.glPushMatrix();        
        gl.glScalef(0.1f, (float)h2.value, 0.1f);
        cyl(gl);
        gl.glPopMatrix();
    }

    void forearm(GL gl)
    {
        gl.glColor3f(0.5f, 0.5f, 1.0f);
        gl.glPushMatrix();        
        gl.glScalef(0.1f, (float)h3.value, 0.1f);
        cyl(gl);
        gl.glPopMatrix();
    }

    public void drawIt(GL gl)
    {
        robot(gl, (float)h1.value, (float)h2.value, (float)theta.value, 
              (float)phi.value, (float)psi.value);
    }

    // ------------------------

    // Draw a simple robot as a hierarchical structure
    void robot(GL gl, float H1, float H2, float Theta, float Phi, float Psi)
    {
        gl.glPushMatrix();

        drawXYZAxis(gl);

        gl.glRotatef(Theta, 0, 1, 0);
        base(gl);

        gl.glTranslatef(0, H1, 0);
        gl.glRotatef(Phi, 0, 0, 1);
        arm(gl);

        gl.glTranslatef(0, H2, 0);
        gl.glRotatef(Psi, 0, 0, 1);
        forearm(gl);

        gl.glPopMatrix();
    }
}