/*
 * Turtle.C - definitions for 3D turtle.
 *
 * Copyright (C) 1992, Christoph Streit (streit@iam.unibe.ch)
 *                     University of Berne, Switzerland
 * All rights reserved.
 *
 * This software may be freely copied, modified, and redistributed
 * provided that this copyright notice is preserved on all copies.
 *
 * You may not distribute this software, in whole or in part, as part of
 * any commercial product without the express consent of the authors.
 *
 * There is no warranty or other guarantee of fitness of this software
 * for any purpose.  It is provided solely "as is".
 *
 */

#include <stdlib.h>
#include <values.h>
#include "Turtle.h"
#include "TransMatrix.h"
#include "Error.h"
#include "Ray.h"

//___________________________________________________________ Tropism

Tropism::Tropism(real x, real y, real z, real w)
: F(x, y, z), weight(w), apply(0)
{
  if (F.zero()) 
    F = Vector(0,0,-1);
  F.normalize();
}

//___________________________________________________________ Turtle

/* init of turtle:
 *
 *                   *
 *		    / \ H
 *		     |
 *                   |
 *          L        |
 *        / _________|
 *        \         /
 *                 / U
 *               \ /
 *                *
 */                                          

Turtle::Turtle()
: P(0,0,0), lastP(0,0,0), H(0,0,1), L(0,-1,0), U(1,0,0),
  tropism(0,0,-1,0.5),
  hullActivated(0), hull(NULL), reflectanceFactor(1), stopOnHit(0)
{
 width = 1;
 lastWidth = -1; // no move has been previously done
  
 b.expand(P);
}

Turtle::Turtle(const Turtle& t)
:b(t.b), 
 P(t.P), lastP(t.lastP), 
 H(t.H), L(t.L), U(t.U),
 width(t.width), lastWidth(t.lastWidth),
 tropism(t.tropism),
 color(t.color), lastColor(t.lastColor),
 texture(t.texture), lastTexture(t.lastTexture),
 hullActivated(t.hullActivated),
 stopOnHit(t.stopOnHit),
 hull(t.hull),
 reflectanceFactor(t.reflectanceFactor),
 closestObject(t.closestObject),
 distanceToClosestObject(t.distanceToClosestObject)
{}

Turtle::~Turtle()
{}

/*
 * forward moves the turtle in the current direction according to
 * the formula:
 *
 *     P' = P + step*H
 */

int Turtle::forward(real step)
{
  lastWidth = width;
  lastP = P;
  lastColor = color;
  lastTexture = texture;

  if (hullActivated)
    return forwardWithRespectToHull(step);
  else {
    P += step*H;
    applyTropism();
    b.expand(P);
    return 0;
  }
}

/*
 * Moves the turtle in the current direction with respect to the
 * activated hull. If the turtle hits the hull it changes direction.
 * This is repeated until step == 0.
 */

int Turtle::forwardWithRespectToHull(real step)
{
  hull->intersect(Ray(P, H), distanceToClosestObject, closestObject);

  while (step > 0) {
    if (distanceToClosestObject > step) {
      P += H*step;
      b.expand(P);
      applyTropism();
      distanceToClosestObject -= step;
      break;
    }
    else {
      P += H*distanceToClosestObject;
      b.expand(P);

      if (stopOnHit)
	return 1;

      bounce();
      applyTropism();
      step -= distanceToClosestObject;
      hull->intersect(Ray(P, H), distanceToClosestObject, closestObject);
    }
  }

  return 0;
}

/*
 * Reflect turtle at the current closest object -> changes H, U
 */

void Turtle::bounce()
{
  /*
   * Reflect turtle at position P. The result is a new heading (H)
   * and a changed up vector (U). The left vector (L) does not change
   * (a reflected ray stays in the same plane).
   */

  Vector normal = closestObject->getNormal(P);

  H = H-2.*((normal^H)*normal)*reflectanceFactor; 
  H.normalize();
  U = H*L;
}

/*
 *  pitch rotates H, L, U around L according to the formula
 *
 *                              [  cos(a)   0   -sin(a) ]
 *                              [                       ]
 *    [H' L' U'] =  [ H L U ] * [     0     1       0   ]
 *                              [                       ]
 *                              [  sin(a)   0   cos(a)  ]
 */

void Turtle::pitch(real a)
{
  Vector tmp(H);
  real sin_a, cos_a;

  SinCos(a, sin_a, cos_a);
  
  H = H*cos_a + U*sin_a;  
  H.normalize();
  U = U*cos_a - tmp*sin_a;
}

/*
 * turn rotates H, L, U around U according to the formula
 *
 *                              [  cos(a)   sin(a)  0 ]
 *                              [                     ]
 *    [H' L' U'] =  [ H L U ] * [ - sin(a)  cos(a)  0 ]
 *                              [                     ]
 *                              [     0        0    1 ]
 */  
  
void Turtle::turn(real a)
{
  Vector tmp(H);
  real sin_a, cos_a;

  SinCos(a, sin_a, cos_a);
  
  H = H*cos_a - L*sin_a;   
  H.normalize();
  L = tmp*sin_a + L*cos_a;
}

/*
 *  roll rotates H, L, U around H according to the formula
 *
 *                              [  1     0       0     ]
 *                              [                      ]
 *    [H' L' U'] =  [ H L U ] * [  0   cos(a)  sin(a)  ]
 *                              [                      ]
 *                              [  0  -sin(a)  cos(a)  ]
 */

void Turtle::roll(real a)
{
  Vector tmp(L);
  real sin_a, cos_a;

  SinCos(a, sin_a, cos_a);
  
  L = L*cos_a - U*sin_a;
  U = tmp*sin_a + U*cos_a;
}

/*
 *   reverse spins the turtle around 180 degrees
 */

void Turtle::reverse()
{
  H = -H;
  L = -L;
}

void Turtle::rotate_vertical()
{
  const real tolerance = 1e-4;
  static Vector gravity(0,0,-1);
  Vector tmp = gravity * H;

  if (tmp.length() < tolerance)
    return;

  L = tmp.normalized();
  U = H*L;
}
  
void Turtle::setWeight(real w)    
{ 
  tropism.weight = w; 
  if (equal(tropism.weight, 0) || tropism.F.zero())
    tropism.apply = 0;
  else
    tropism.apply = 1;
}

void Turtle::setTropism(real x, real y, real z) 
{
  tropism.F = Vector(x, y, z);
  
  if (!tropism.F.zero()) 
    tropism.F.normalize();

  if (equal(tropism.weight, 0) || tropism.F.zero())
    tropism.apply = 0;
  else
    tropism.apply = 1;
} 

ostream& operator<<(ostream& os, const Turtle& t)
{
  os << "P: " << t.P << "\n"
     << "H: " << t.H << "\n"
     << "U: " << t.U << "\n"
     << "L: " << t.L << "\n";
  return os;
}

/*
 * Apply the given tropism vector to the turtle.
 */

void Turtle::applyTropism()
{
  static TransMatrix rot;

  /*
   * Should tropism be applied.
   */
  if (!tropism.apply)
    return;

  Vector rotvec(H*tropism.F);
  real alpha = acos(H^tropism.F)*tropism.weight*rotvec.length();

  /*
   * Limit the rotation.
   */
  if (alpha >  M_PI/6) alpha =  M_PI/6;
  if (alpha < -M_PI/6) alpha = -M_PI/6;

  if (!equal(alpha,0) && !rotvec.zero()) {
    rot.setRotate(rotvec, alpha);
    H = H*rot; H.normalize();
    L = L*rot; L.normalize();
    U = H*L;   
  }
}