#include <cstdlib>
#include "api3ds.h"
#include "turtle.h"

TURTLE::~TURTLE() {}

TURTLE::TURTLE()
{
  U.x= 0.0;U.y=-1.0;U.z= 0.0;
  V.x=-1.0;V.y= 0.0;V.z= 0.0;
  N.x= 0.0;N.y= 0.0;N.z= -1.0;

  POS.x=0.0;
  POS.y=0.0;
  POS.z=0.0;
}

void TURTLE::SetPos(float x,float y,float z)
{
  POS.x=x;
  POS.y=y;
  POS.z=z;
}

void TURTLE::SetVectors(Vector3f x,Vector3f y,Vector3f z)
{
  U=x;V=y;N=z;
}

void TURTLE::SaveVectors(void)
{
  X=U;Y=V;Z=N;
}

void TURTLE::RestoreVectors(void)
{
  U=X;V=Y;N=Z;
}


void TURTLE::Move(float s)
{
  POS.x+=s*U.x;
  POS.y+=s*U.y;
  POS.z+=s*U.z;
}

void TURTLE::Fly(float s)
{
  POS.x+=s*V.x;
  POS.y+=s*V.y;
  POS.z+=s*V.z;
}

void TURTLE::Strafe(float s)
{
  POS.x+=s*N.x;
  POS.y+=s*N.y;
  POS.z+=s*N.z;
}



void TURTLE::RotateVector(Vector3f& U,float Nroll,float Vroll,float Uroll)
{
            float sp = sin(Nroll);
            float cp = cos(Nroll);

            float sf = sin(Vroll);
            float cf = cos(Vroll);

            float sg = sin(Uroll);
            float cg = cos(Uroll);

            float x,y,z;

            x=U.x;
            y=U.y;
            z=U.z;
            U.x=x;
            U.y=cp*y+sp*z;
            U.z=-sp*y+cp*z;

            x=U.x;
            y=U.y;
            z=U.z;
            U.x=cf*x-sf*z;
            U.y=y;
            U.z=sf*x+cf*z;

            x=U.x;
            y=U.y;
            z=U.z;
            U.x=cg*x+sg*y;
            U.y=-sg*x+cg*y;
            U.z=z;
}

void TURTLE::ChangeBase(Vector3f& U,Vector3f& I,Vector3f& J,Vector3f& K)
{
            float x=U.x;
            float y=U.y;
            float z=U.z;

            U.x= x*I.x+ y*J.x+ z*K.x;
            U.y= x*I.y+ y*J.y+ z*K.y;
            U.z= x*I.z+ y*J.z+ z*K.z;
}

void TURTLE::Rotate(float a, float b, float c)
{

  Vector3f I=U;
  Vector3f J=V;
  Vector3f K=N;

  U.x=1;U.y=0;U.z=0;
  V.x=0;V.y=1;V.z=0;
  N.x=0;N.y=0;N.z=1;


  RotateVector(U,a,b,c);
  RotateVector(V,a,b,c);
  RotateVector(N,a,b,c);

  ChangeBase(U,I,J,K);
  ChangeBase(V,I,J,K);
  ChangeBase(N,I,J,K);
}