/* -----------------------------------------------------------------------------
 *
 * Copyright (c) 2019 Alexis Naveros.
 * Portions developed under contract to the SURVICE Engineering Company.
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
 * -----------------------------------------------------------------------------
 */

#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include <float.h>

#include "cpusimd.h"

#include "cc.h"

#include "polywithin.h"


////


/* Padding functions access 4 vertices beyond the end of the vertexlist array */
#define PW_PATHVLIST_PADDING (4)


////


#define PW_DEBUG (0)


int pwPointInContourD( double *vertexlist, size_t vertexcount, double pointx, double pointy )
{
  int edgeparity;
  int abovestateflag;
  double crsl, crsr;
  double *vcur, *vend, *vprev;

  if( vertexcount < 3 )
    return PW_RESULT_OUTSIDE;
  edgeparity = 0;

  vend = &vertexlist[ vertexcount * 2 ];

  /* Detect orientation above/below Y of last vertex, iterate until nonequal */
  for( vprev = vend - 2 ; vprev[1] == pointy ; vprev -= 2 )
  {
    if( vprev == vertexlist )
    {
#if PW_DEBUG
      printf( "  >> OUTSIDE\n" );
#endif
      return PW_RESULT_OUTSIDE;
    }
  }
  abovestateflag = ( pointy > vprev[1] );

  for( vcur = vertexlist ; vcur < vend ; vcur += 2 )
  {
    /* Iterate until we toggle above/below pointy, meaning we have a possible intersection */
    if( abovestateflag )
    {
      for( ; ( vcur < vend ) && ( vcur[1] < pointy ) ; vcur += 2 );
    }
    else
    {
      for( ; ( vcur < vend ) && ( vcur[1] > pointy ) ; vcur += 2 );
    }
    if( vcur >= vend )
      break;

    vprev = vcur - 2;
    if( vcur == vertexlist )
      vprev = vend - 2;

    if( vcur[1] == pointy )
    {
      /* Point either matches contour point, or between two points of horizontal edge */
      if( ( vcur[0] == pointx ) || ( ( vcur[1] == vprev[1] ) && ( ( pointx < vprev[0] ) != ( pointx < vcur[0] ) ) ) )
      {
#if PW_DEBUG
        printf( "  >> ON\n" );
#endif
        return PW_RESULT_ON;
      }
      /* If Y of point and contour matches, continue until we get out of that horizontal collinear special case */
      continue;
    }

    abovestateflag ^= 1;

    /* We are only interested in edges crossing on the left */
    if( ( pointx >= vcur[0] ) || ( pointx >= vprev[0] ) )
    {
      if( ( pointx > vcur[0] ) && ( pointx > vprev[0] ) )
        edgeparity ^= 1;
      else
      {
        /* Compare sides of cross product */
        crsl = ( vcur[0] - vprev[0] ) * ( pointy - vcur[1] );
        crsr = ( vcur[1] - vprev[1] ) * ( pointx - vcur[0] );
        if( crsl == crsr )
        {
#if PW_DEBUG
          printf( "  >> ON\n" );
#endif
          return PW_RESULT_ON;
        }
        edgeparity ^= ( ( crsl < crsr ) ^ abovestateflag );
      }
    }
  }
#if PW_DEBUG
  printf( "  >> %d -> %s\n", edgeparity, ( edgeparity ? "INSIDE" : "OUTSIDE" ) );
#endif
  return ( edgeparity ? PW_RESULT_INSIDE : PW_RESULT_OUTSIDE );
}


////


/* vertexlist must have valid memory storage beyond 'vertexcount' by 4*2*sizeof(int64_t) bytes */
int pwPointInContour64_Padding( int64_t *vertexlist, size_t vertexcount, int64_t pointx, int64_t pointy )
{
  int edgeparity;
  int abovestateflag;
  double crsl, crsr;
  int64_t *vcur, *vend, *vprev;
#if CPU_SSE4_2_SUPPORT
  uint32_t cmpmask;
  __m128i vload0, vload1, vcmp0, vcmp1, vpointy;
#endif

  if( vertexcount < 3 )
    return PW_RESULT_OUTSIDE;
  edgeparity = 0;

  /* We insert guard values beyond 'vertexcount', to avoid bound checking in loops */
  /* 4*2*sizeof(int64_t) is a minimum of extra storage beyond 'vertexcount' for all path vertexlists */
  /* SSE 4.2 path needs 4*2*sizeof(int64_t), scalar path needs 1*2*sizeof(int64_t) */
  vend = &vertexlist[ vertexcount * 2 ];
  vend[0] = pointx;
  vend[1] = pointy;

  /* Detect orientation above/below Y of last vertex, iterate until nonequal */
  for( vprev = vend - 2 ; vprev[1] == pointy ; vprev -= 2 )
  {
    if( vprev == vertexlist )
      return PW_RESULT_OUTSIDE;
  }
  abovestateflag = ( pointy > vprev[1] );

#if CPU_SSE4_2_SUPPORT
  vpointy = _mm_set1_epi64x( pointy );
#endif

  for( vcur = vertexlist ; vcur < vend ; vcur += 2 )
  {
#if CPU_SSE4_2_SUPPORT
    /* Iterate until we toggle above/below pointy, meaning we have a possible intersection */
    if( abovestateflag )
    {
      for( ; ; vcur += 8 )
      {
        vload0 = _mm_unpackhi_epi64( _mm_loadu_si128( (void *)&vcur[0] ), _mm_loadu_si128( (void *)&vcur[2] ) );
        vload1 = _mm_unpackhi_epi64( _mm_loadu_si128( (void *)&vcur[4] ), _mm_loadu_si128( (void *)&vcur[6] ) );
        vcmp0 = _mm_cmpgt_epi64( vpointy, vload0 );
        vcmp1 = _mm_cmpgt_epi64( vpointy, vload1 );
        if( !_mm_test_all_ones( _mm_and_si128( vcmp0, vcmp1 ) ) )
          break;
      }
    }
    else
    {
      for( ; ; vcur += 8 )
      {
        vload0 = _mm_unpackhi_epi64( _mm_loadu_si128( (void *)&vcur[0] ), _mm_loadu_si128( (void *)&vcur[2] ) );
        vload1 = _mm_unpackhi_epi64( _mm_loadu_si128( (void *)&vcur[4] ), _mm_loadu_si128( (void *)&vcur[6] ) );
        vcmp0 = _mm_cmpgt_epi64( vload0, vpointy );
        vcmp1 = _mm_cmpgt_epi64( vload1, vpointy );
        if( !_mm_test_all_ones( _mm_and_si128( vcmp0, vcmp1 ) ) )
          break;
      }
    }
    cmpmask = _mm_movemask_ps( _mm_shuffle_ps( _mm_castsi128_ps( vcmp0 ), _mm_castsi128_ps( vcmp1 ), 0x88 ) );
    vcur += ( ( 0x484c484 >> ( cmpmask + cmpmask ) ) & 3 ) << 1;
#else
    /* Iterate until we toggle above/below pointy, meaning we have a possible intersection */
    if( abovestateflag )
    {
      for( ; vcur[1] < pointy ; vcur += 2 );
    }
    else
    {
      for( ; vcur[1] > pointy ; vcur += 2 );
    }
#endif
    if( vcur >= vend )
      break;

    vprev = vcur - 2;
    if( vcur == vertexlist )
      vprev = vend - 2;

    if( vcur[1] == pointy )
    {
      /* Point either matches contour point, or between two points of horizontal edge */
      if( ( vcur[0] == pointx ) || ( ( vcur[1] == vprev[1] ) && ( ( pointx < vprev[0] ) != ( pointx < vcur[0] ) ) ) )
        return PW_RESULT_ON;
      /* If Y of point and contour matches, continue until we get out of that horizontal collinear special case */
      continue;
    }

    abovestateflag ^= 1;
    /* We are only interested in edges crossing on the left */
    if( ( pointx >= vcur[0] ) || ( pointx >= vprev[0] ) )
    {
      if( ( pointx > vcur[0] ) && ( pointx > vprev[0] ) )
        edgeparity ^= 1;
      else
      {
        /* Compare sides of cross product ~ we have only one flop rounding, correctness can not flip around */
        crsl = (double)( vcur[0] - vprev[0] ) * (double)( pointy - vcur[1] );
        crsr = (double)( vcur[1] - vprev[1] ) * (double)( pointx - vcur[0] );
        if( crsl == crsr )
          return PW_RESULT_ON;
        edgeparity ^= ( ( crsl < crsr ) ^ abovestateflag );
      }
    }
  }
  return ( edgeparity ? PW_RESULT_INSIDE : PW_RESULT_OUTSIDE );
}


int pwPointInContour64( int64_t *vertexlist, size_t vertexcount, int64_t pointx, int64_t pointy )
{
  int edgeparity;
  int abovestateflag;
  double crsl, crsr;
  int64_t *vcur, *vend, *vprev;

  if( vertexcount < 3 )
    return PW_RESULT_OUTSIDE;
  edgeparity = 0;

  vend = &vertexlist[ vertexcount * 2 ];

  /* Detect orientation above/below Y of last vertex, iterate until nonequal */
  for( vprev = vend - 2 ; vprev[1] == pointy ; vprev -= 2 )
  {
    if( vprev == vertexlist )
      return PW_RESULT_OUTSIDE;
  }
  abovestateflag = ( pointy > vprev[1] );

  for( vcur = vertexlist ; vcur < vend ; vcur += 2 )
  {
    /* Iterate until we toggle above/below pointy, meaning we have a possible intersection */
    if( abovestateflag )
    {
      for( ; ( vcur < vend ) && ( vcur[1] < pointy ) ; vcur += 2 );
    }
    else
    {
      for( ; ( vcur < vend ) && ( vcur[1] > pointy ) ; vcur += 2 );
    }
    if( vcur >= vend )
      break;

    vprev = vcur - 2;
    if( vcur == vertexlist )
      vprev = vend - 2;

    if( vcur[1] == pointy )
    {
      /* Point either matches contour point, or between two points of horizontal edge */
      if( ( vcur[0] == pointx ) || ( ( vcur[1] == vprev[1] ) && ( ( pointx < vprev[0] ) != ( pointx < vcur[0] ) ) ) )
        return PW_RESULT_ON;
      /* If Y of point and contour matches, continue until we get out of that horizontal collinear special case */
      continue;
    }

    abovestateflag ^= 1;
    /* We are only interested in edges crossing on the left */
    if( ( pointx >= vcur[0] ) || ( pointx >= vprev[0] ) )
    {
      if( ( pointx > vcur[0] ) && ( pointx > vprev[0] ) )
        edgeparity ^= 1;
      else
      {
        /* Compare sides of cross product ~ we have only one flop rounding, correctness can not flip around */
        crsl = (double)( vcur[0] - vprev[0] ) * (double)( pointy - vcur[1] );
        crsr = (double)( vcur[1] - vprev[1] ) * (double)( pointx - vcur[0] );
        if( crsl == crsr )
          return PW_RESULT_ON;
        edgeparity ^= ( ( crsl < crsr ) ^ abovestateflag );
      }
    }
  }
  return ( edgeparity ? PW_RESULT_INSIDE : PW_RESULT_OUTSIDE );
}


////


int pwPointInContour32( int32_t *vertexlist, size_t vertexcount, int32_t pointx, int32_t pointy )
{
  int edgeparity;
  int abovestateflag;
  double crsl, crsr;
  int32_t *vcur, *vend, *vprev;

  if( vertexcount < 3 )
    return PW_RESULT_OUTSIDE;
  edgeparity = 0;

  vend = &vertexlist[ vertexcount * 2 ];

  /* Detect orientation above/below Y of last vertex, iterate until nonequal */
  for( vprev = vend - 2 ; vprev[1] == pointy ; vprev -= 2 )
  {
    if( vprev == vertexlist )
      return PW_RESULT_OUTSIDE;
  }
  abovestateflag = ( pointy > vprev[1] );

  for( vcur = vertexlist ; vcur < vend ; vcur += 2 )
  {
    /* Iterate until we toggle above/below pointy, meaning we have a possible intersection */
    if( abovestateflag )
    {
      for( ; ( vcur < vend ) && ( vcur[1] < pointy ) ; vcur += 2 );
    }
    else
    {
      for( ; ( vcur < vend ) && ( vcur[1] > pointy ) ; vcur += 2 );
    }
    if( vcur >= vend )
      break;

    vprev = vcur - 2;
    if( vcur == vertexlist )
      vprev = vend - 2;

    if( vcur[1] == pointy )
    {
      /* Point either matches contour point, or between two points of horizontal edge */
      if( ( vcur[0] == pointx ) || ( ( vcur[1] == vprev[1] ) && ( ( pointx < vprev[0] ) != ( pointx < vcur[0] ) ) ) )
        return PW_RESULT_ON;
      /* If Y of point and contour matches, continue until we get out of that horizontal collinear special case */
      continue;
    }

    abovestateflag ^= 1;
    /* We are only interested in edges crossing on the left */
    if( ( pointx >= vcur[0] ) || ( pointx >= vprev[0] ) )
    {
      if( ( pointx > vcur[0] ) && ( pointx > vprev[0] ) )
        edgeparity ^= 1;
      else
      {
        /* Compare sides of cross product ~ we have only one flop rounding, correctness can not flip around */
        crsl = ( (double)vcur[0] - (double)vprev[0] ) * ( (double)pointy - (double)vcur[1] );
        crsr = ( (double)vcur[1] - (double)vprev[1] ) * ( (double)pointx - (double)vcur[0] );
        if( crsl == crsr )
          return PW_RESULT_ON;
        edgeparity ^= ( ( crsl < crsr ) ^ abovestateflag );
      }
    }
  }
  return ( edgeparity ? PW_RESULT_INSIDE : PW_RESULT_OUTSIDE );
}