/*
 * Copyright (c) 2007, Insomniac Games
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Insomniac Games nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY <copyright holder> ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * -----------------------------------------------------------------------------
 *
 * a simple root finding algorithm is used to find the value of 't' that
 * satisfies:
 *              d|D(t)|^2/dt = 0
 * where:
 *              |D(t)| = |p(t)-b(t)|
 * where p(t) is a point on the line parameterized by t:
 *              p(t) = p1 + t*(p2-p1)
 * and b(t) is that same point clipped to the boundary of the box. in box-
 * relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
 * each of which looks like this:
 *
 *          t_lo     /
 *            ______/    -->t
 *           /     t_hi
 *          /
 *
 * t_lo and t_hi are the t values where the line passes through the planes
 * corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
 * in a piecewise fashion from t=0 to t=1, stopping at the point where
 * d|D(t)|^2/dt crosses from negative to positive.
*/

float DistanceLA(const PrimCapsule *linesegA, const PrimCapsule *aabbB, Result *result)
{
  const qword linesegA_addr = si_from_ptr(linesegA);
  const qword aabbB_addr    = si_from_ptr(linesegA);
  const qword result_addr   = si_from_ptr(result);
  qword l_vert0             = si_lqd(linesegA_addr, 16);
  qword l_vert1             = si_lqd(linesegA_addr, 32);
  qword aabb_c              = si_lqd(aabbB_addr, 16);
  qword aabb_ext            = si_lqd(aabbB_addr, 32);
  qword sign_bit_mask       = si_ilhu(0x8000);
  qword neg_aabb_ext        = si_or(aabb_ext, sign_bit_mask);
  qword ZERO                = si_ilhu(0);
  qword ONE                 = si_il  (0x0001);
  qword ONE_F               = si_ilhu(0x3F80);
  qword TWO_F               = si_ilhu(0x4000);
  qword MINUS_ONE           = si_fsmbi(0xFFFF);

  /*
  // find the line segment point first end point in the coordinate frame of the AABB
  vec4f line_v0_offset;
  VecSub3(line_v0_offset, linesegA->m_vert0, aabbB->m_center);
  */
  qword line_v0_offset = si_fs(l_vert0, aabb_c);

  /*
  // find the line segment vector from the first point to the last
  vec4f line_vector;
  VecSub3(line_vector, linesegA->m_vert1, linesegA->m_vert0);
  */
  qword line_vector = si_fs(l_vert1, l_vert0);

  /*
  // mirror the line vector so that it has all components >= 0
  for(i=0;i<3;i++)
  {
  if(line_vector[i] < 0.0f)
  {
  // mirror needed
  line_v0_offset_mirrored.SetElem(i, -line_v0_offset[i]);
  line_vector_mirrored.SetElem(i, -line_vector[i]);
  sign.SetElem(i, -1.0f);
  }
  else
  {
  // no mirror needed
  line_v0_offset_mirrored.SetElem(i, line_v0_offset[i]);
  line_vector_mirrored.SetElem(i, line_vector[i]);
  sign.SetElem(i, 1.0f);
  }
  }
  */
  qword is_neg                  = si_fcgt(ZERO, line_vector);
  qword line_vector_mirrored    = si_andc(line_vector, sign_bit_mask);
  qword line_v0_offset_mirrored = si_xor(line_v0_offset, is_neg);

  /*
  // domain (odd calls this region) is -1,0,+1 depending on which side of the box planes each
  // coordinate is on. t_anchor (ode calls this tanchor) is the next t value at which there is a
  // transition, or the last one if there are no more.
  for(i=0;i<3;i++)
  {
  if(line_vector_mirrored[i] > 0.0f)
  {
  if(line_v0_offset_mirrored[i] < -aabbB->m_radii[i])
  {
  domain[i] = -1;
  t_anchor[i] = ((-aabbB->m_radii[i] - line_v0_offset_mirrored[i]) / line_vector_mirrored[i]);
  }
  else
  {
  domain[i] = (line_v0_offset_mirrored[i] > aabbB->m_radii[i]);
  t_anchor[i] = ((aabbB->m_radii[i] - line_v0_offset_mirrored[i]) / line_vector_mirrored[i]);
  }
  }
  else
  {
  domain[i] = 0;
  t_anchor[i] = 2.0f;
  }
  }
  */
  qword anchor_mask              = si_fcgt(line_vector_mirrored, ZERO);
  qword offset_less_radii_mask   = si_fcgt(neg_aabb_ext, line_v0_offset_mirrored);
  qword t_anchor                 = si_fs(neg_aabb_ext, line_v0_offset_mirrored);
  qword t_anchor_b               = si_xor(t_anchor, sign_bit_mask);
  qword inv_line_vector_mirrored = si_frest(line_vector_mirrored);
  t_anchor                       = si_fm(t_anchor, inv_line_vector_mirrored);
  qword domain                   = si_fcgt(line_v0_offset_mirrored, aabb_ext);
  domain                         = si_selb(ZERO, ONE, domain);
  t_anchor                       = si_selb(t_anchor_b, t_anchor, offset_less_radii_mask);
  domain                         = si_selb(domain, MINUS_ONE, offset_less_radii_mask);
  t_anchor                       = si_selb(TWO_F, t_anchor, anchor_mask);
  domain                         = si_selb(ZERO, domain, anchor_mask);

  /*
  // square the line segment vector
  vec4f line_vector_mirrored_2;
  for(i=0;i<3; i++)
  {
  line_vector_mirrored_2.SetElem(i, line_vector_mirrored[i] * line_vector_mirrored[i]);
  }
  */
  qword line_vector_mirrored_2 = si_fm(line_vector_mirrored, line_vector_mirrored);

  /*
  deriv=0.0f; // set initial derivative value
  for(i=0;i<3;i++)
  {
  if(domain[i])
  deriv -= line_vector_mirrored_2[i] * t_anchor[i];
  }
  if(deriv >= 0.0f)
  solution = true;
  */
  qword deriv_comp    = si_fm(line_vector_mirrored_2, t_anchor);
  deriv_comp          = si_and(deriv_comp, domain);
  qword deriv         = si_ori(deriv_comp, 0);
  qword deriv_comp1   = si_shlqbyi(deriv_comp, 4);
  deriv               = si_fs(deriv, deriv_comp1);
  qword deriv_comp2   = si_shlqbyi(deriv_comp, 8);
  deriv               = si_fs(deriv, deriv_comp2);
  qword no_solution   = si_fcgt(ZERO, deriv);


  qword t = si_ilhu(0);
  if (si_to_uint(no_solution))
  {
    do
    {
      /*// t_next is the next t value, next_t in ode
      // find the point on the line that is at the next clip plane boundary
      t_next = 1.0f;
      for(i=0;i<3;i++)
      {
      if((t_anchor[i] > t) && (t_anchor[i] < 1.0f) && (t_anchor[i] < t_next))
      t_next = t_anchor[i];
      }
      */
      qword t_next = si_ilhu(0x3F80);
      qword mask0 = si_fcgt(t_anchor, t);
      qword mask1 = si_fcgt(ONE_F, t_anchor);
      qword mask2 = si_fcgt(t_anchor, t_next);
      mask0 = si_and(mask0, mask1);
      mask0 = si_and(mask0, mask2);
      t_next      = si_selb(t_next, t_anchor, mask0);

      /* 
      // compute the deriv = d|d|^2/dt for the next t
      deriv_next = 0.0f;
      for(i=0;i<3;i++)
      {
      if(domain[i])
      deriv_next += line_vector_mirrored_2[i] * (t_next - t_anchor[i]);
      }
      */ 
      qword deriv_next_comp  = si_fs(t_next, t_anchor);
      deriv_next_comp        = si_fm(line_vector_mirrored, deriv_next_comp);
      qword mul_mask         = si_cgt(domain, ZERO);
      deriv_next_comp        = si_and(mul_mask, deriv_next_comp);
      qword deriv_next_comp2 = si_shlqbyi(deriv_next_comp, 4);
      qword deriv_next       = si_fa(deriv_next_comp2, deriv_next_comp);
      deriv_next_comp2       = si_shlqbyi(deriv_next_comp, 4);
      deriv_next             = si_fa(deriv_next_comp2, deriv_next_comp);

      /*
      // if the sign of d|d|^2/dt has changed, solution = the crossover point
      if(deriv_next >= 0.0f)
      {
      derivs = (deriv_next - deriv) / (t_next - t);
      t -= deriv / derivs;
      solution = true;
      break;
      }
      */
      qword is_deriv_next_less_zero = si_fcgt(ZERO, deriv_next);
      if (!si_to_uint(is_deriv_next_less_zero ))
      {
        qword derivs = si_fs(deriv_next, deriv);
        qword diff_t = si_fs(t_next, t);
        deriv        = si_fm(diff_t, deriv);
        derivs       = si_frest(derivs);
        deriv        = si_fm(deriv, derivs);
        t            = si_fs(t, deriv);
        no_solution  = si_fsmbi(0xffff);
        break;
      }

      /* // advance to the next anchor point / region
      for(i=0;i<3;i++)
      {
      if(t_anchor[i] == t_next)
      {
      t_anchor[i] = ((aabbB->m_radii[i] - line_v0_offset_mirrored[i]) / line_vector_mirrored[i]);
      domain[i]++;
      }
      }
      */
      qword new_anchor = si_fs(aabb_ext, line_v0_offset_mirrored);
      new_anchor       = si_fm (inv_line_vector_mirrored, new_anchor);
      qword new_domain = si_ai(domain, 1);
      qword new_mask   = si_fceq(t_anchor, t_next);
      t_anchor         = si_selb(t_anchor, new_anchor, new_mask);
      domain           = si_selb(domain, new_domain, new_mask);

      /*
      t = t_next;
      deriv = deriv_next;
      */
      t     = si_ori(t_next, 0);
      deriv = si_ori(deriv_next, 0);
    } while(si_to_float(t) < 1.0f);
  }

  /*
  // p1 is the closest point
  if(!solution)
  t = 1.0f;
  */
  t = si_selb(t, ONE_F, no_solution);

  /*
  // compute closest point on the line
  // note: line_vector=p2-p1
  for(i=0; i<3; i++)
  result->m_ipA.SetElem(i, (linesegA->m_vert0[i] + line_vector[i] * t));
  */
  qword m_ipA = si_fma(t, line_vector, l_vert0);
  si_stqd(m_ipA, result_addr, 0);

  /*
  // compute closest point on the box
  for (i=0; i<3; i++)
  {
  line_v0_offset_mirrored.SetElem(i, sign[i] * (line_v0_offset_mirrored[i] + t*line_vector_mirrored[i]));
  if (line_v0_offset_mirrored[i] < -aabbB->m_radii[i])
  line_v0_offset_mirrored.SetElem(i,-aabbB->m_radii[i]);
  else if (line_v0_offset_mirrored[i] > aabbB->m_radii[i])
  line_v0_offset_mirrored.SetElem(i, aabbB->m_radii[i]);
  }
  */
  line_v0_offset_mirrored = si_fma(t, line_vector_mirrored, line_v0_offset_mirrored);
  qword less_extents      = si_fcgt(neg_aabb_ext, line_v0_offset_mirrored);
  line_v0_offset_mirrored = si_selb(line_v0_offset_mirrored, neg_aabb_ext, less_extents);
  qword gt_extents        = si_fcgt(line_v0_offset_mirrored, aabb_ext);
  line_v0_offset_mirrored = si_selb(line_v0_offset_mirrored, aabb_ext, gt_extents);

  /*
  VecAdd3(result->m_ipB, line_v0_offset_mirrored, aabbB->m_center);
  */
  qword m_ipB = si_fa(line_v0_offset_mirrored, aabb_c);
  si_stqd(m_ipB, result_addr, 16);

  /*
  // return the distance between the closest points
  return VecDist3(result->m_ipA, result->m_ipB);
  */
  qword dist   = si_fs(m_ipA, m_ipB);
  dist         = si_fm(dist, dist);
  qword dist_1 = si_shlqbyi(dist, 4);
  dist         = si_fa(dist, dist_1);
  dist_1       = si_shlqbyi(dist_1, 4);
  dist         = si_fa(dist, dist_1);
  dist         = si_frsqest(dist);
  dist         = si_frest(dist);
  return si_to_float(dist);
}