/*
 * Copyright (c) 2012, Willow Garage, Inc.
 * 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 Willow Garage, Inc. 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
 */

#include <string>
#include <ros/package.h>
#include <gtest/gtest.h>
#include <navfn/navfn.h>
#include <navfn/read_pgm_costmap.h>

// Load a willow garage costmap and return a NavFn instance using it.
// If the costmap file fails to load, returns NULL.
navfn::NavFn* make_willow_nav()
{
  int sx,sy;
  std::string path = ros::package::getPath( ROS_PACKAGE_NAME ) + "/test/willow_costmap.pgm";

  COSTTYPE *cmap = readPGM( path.c_str(), &sx, &sy, true );
  if( cmap == NULL )
  {
    return NULL;
  }
  navfn::NavFn* nav = new navfn::NavFn(sx,sy);

  nav->priInc = 2*COST_NEUTRAL;	// thin wavefront

  memcpy( nav->costarr, cmap, sx*sy );

  return nav;
}

void print_neighborhood_of_last_path_entry( navfn::NavFn* nav )  
{
  printf("last path entries:\n");
  for( int i = nav->npath - 4; i < nav->npath; i++ )
  {
    printf("%.3f, %.3f\n", nav->pathx[ i ], nav->pathy[ i ]);
  }
  printf("potential field neighborhood of last entry:\n");
  int xf = nav->pathx[ nav->npath-1 ];
  int yf = nav->pathy[ nav->npath-1 ];

  printf( "   " );
  for( int x = xf - 2; x <= xf + 2; x++ )
  {
    printf( " %6d", x );
  }
  printf( "\n" );

  for( int y = yf - 2; y <= yf + 2; y++ )
  {
    printf( "%5d:", y );
    for( int x = xf - 2; x <= xf + 2; x++ )
    {
      printf( " %5.1f", nav->potarr[ y * nav->nx + x ] );
    }
    printf( "\n" );
  }

  printf("gradient neighborhood of last entry:\n");
  printf( "     " );
  for( int x = xf - 2; x <= xf + 2; x++ )
  {
    printf( " %6d", x );
  }
  printf( "\n" );

  for( int y = yf - 2; y <= yf + 2; y++ )
  {
    printf( "%5d x:", y );
    for( int x = xf - 2; x <= xf + 2; x++ )
    {
      printf( " %5.1f", nav->gradx[ y * nav->nx + x ] );
    }
    printf( "\n" );

    printf( "      y:" );
    for( int x = xf - 2; x <= xf + 2; x++ )
    {
      printf( " %5.1f", nav->grady[ y * nav->nx + x ] );
    }
    printf( "\n" );
  }
}

TEST(PathCalc, oscillate_in_pinch_point)
{
  navfn::NavFn* nav = make_willow_nav();
  ASSERT_TRUE( nav != NULL );

  int goal[2];
  int start[2];

  start[0] = 428;
  start[1] = 746;
  
  goal[0] = 350;
  goal[1] = 450;

  nav->setGoal( goal );
  nav->setStart( start );

  bool plan_success = nav->calcNavFnDijkstra( true );
  EXPECT_TRUE( plan_success );
  if( !plan_success )
  {
    print_neighborhood_of_last_path_entry( nav );
  }
}

TEST(PathCalc, easy_nav_should_always_work)
{
  navfn::NavFn* nav = make_willow_nav();
  ASSERT_TRUE( nav != NULL );

  int goal[2];
  int start[2];

  start[0] = 350;
  start[1] = 400;

  goal[0] = 350;
  goal[1] = 450;

  nav->setGoal( goal );
  nav->setStart( start );

  EXPECT_TRUE( nav->calcNavFnDijkstra( true ));
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}