/*
 * Copyright (C) 2019-2021 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#include <stop_controlled_intersection_plugin.h>
#include <carma_wm/WMTestLibForGuidance.h>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <carma_wm/CARMAWorldModel.h>
#include <cav_msgs/LaneFollowingManeuver.h>
#include <cav_msgs/VehicleState.h>
#include <cav_srvs/PlanManeuversRequest.h>
#include <cav_srvs/PlanManeuversResponse.h>

namespace stop_controlled_intersection_transit_plugin
{
    TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_one)
    {
      //Test Stop controlled Intersection tactical plugin generation
      ros::Time::setNow(ros::Time(0.0));
      StopControlledIntersectionTacticalPluginConfig config;
      std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
      StopControlledIntersectionTacticalPlugin plugin(wm, config, [&](auto msg) {});
      
      auto map = carma_wm::test::buildGuidanceTestMap(3.7, 50);

      wm->setMap(map);
      carma_wm::test::setSpeedLimit(40_mph, wm);
      /**
       * Total route length should be 100m
       *
       *        |1203|1213|1223|
       *        | _  _  _  _  _|
       *        |1202| Ob |1222|
       *        | _  _  _  _  _|
       *        |1201|1211|1221|    num   = lanelet id hardcoded for easier testing
       *        | _  _  _  _  _|    |     = lane lines
       *        |1200|1210|1220|    - - - = Lanelet boundary
       *        |              |    O     = Default Obstacle
       *        ****************
       *           START_LINE
      */

     carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm);
    
    //Create a request and maneuver that meets case 1 criteria
    //In order to be case 1 - estimated_stop_time > scheduled_stop_time and speed_before_stop < speed_limit
    //speed_before_stop
     cav_srvs::PlanTrajectoryRequest req;
     req.vehicle_state.x_pos_global = 1.5;
     req.vehicle_state.y_pos_global = 5.0;
     req.vehicle_state.orientation = 0;
     req.vehicle_state.longitudinal_vel = 11.176;

     cav_msgs::Maneuver maneuver;
     maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
     maneuver.lane_following_maneuver.start_dist = req.vehicle_state.y_pos_global;
     maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
     maneuver.lane_following_maneuver.start_speed = 11.176;  //25 mph in mps

     maneuver.lane_following_maneuver.end_dist = 100.0;
     maneuver.lane_following_maneuver.end_speed = 0.0;

    //Enter meta data
    maneuver.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1);    //Case number
    maneuver.lane_following_maneuver.parameters.string_valued_meta_data.push_back("Carma/stop_controlled_intersection");
    //Float meta data list - a_acc, a_dec, t_acc, t_dec, speed_before_decel
    
    //Calculate speed before decel - The speed the vehicle should accelerate before slowing down
    //Assuming the a_dec to be 0.5m/s^2, the vehicle should be able to stop at that deceleration- over total_dist/2
    //So accelerate half way and decelerate the rest
    double a_dec = -1.5;
    double assumed_dec_dist = maneuver.lane_following_maneuver.end_dist/2;
    double speed_before_dec = sqrt(2*std::abs(a_dec)*assumed_dec_dist);
    
    double a_acc = (pow(speed_before_dec,2) - pow(maneuver.lane_following_maneuver.start_speed,2))/(2*(maneuver.lane_following_maneuver.end_dist - assumed_dec_dist));
    double t_acc = (speed_before_dec - maneuver.lane_following_maneuver.start_speed)/a_acc;
    double t_dec = (speed_before_dec)/std::abs(a_dec);

    maneuver.lane_following_maneuver.end_time = maneuver.lane_following_maneuver.start_time + ros::Duration(t_acc + t_dec);

    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_acc);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_dec);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(t_acc);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(t_dec);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(speed_before_dec);

    req.maneuver_plan.maneuvers.push_back(maneuver);
    req.maneuver_index_to_plan = 0;

    cav_srvs::PlanTrajectoryResponse resp;

    plugin.plan_trajectory_cb(req, resp);

    EXPECT_EQ(0, resp.related_maneuvers.back());

    //Test maneuvers_to_points
    std::vector<cav_msgs::Maneuver> maneuver_plan;
    maneuver_plan.push_back(maneuver);
    std::vector<PointSpeedPair> points_and_target_speeds = plugin.maneuvers_to_points(maneuver_plan, wm, req.vehicle_state);
    EXPECT_EQ(points_and_target_speeds[0].speed, req.vehicle_state.longitudinal_vel);

    //Test create_case_one_speed_profile
      //get geometry points from maneuvers_to_points
    std::vector<lanelet::BasicPoint2d> geometry_profile;
    std::vector<double> speeds;
    basic_autonomy::waypoint_generation:: split_point_speed_pairs(points_and_target_speeds, &geometry_profile, &speeds);
    std::vector<PointSpeedPair> case_one_profile = plugin.create_case_one_speed_profile(wm, maneuver, geometry_profile,  req.vehicle_state.longitudinal_vel, req.vehicle_state);
    //Ensure acceleration and deceleration is happening
    double dist_to_cover = maneuver.lane_following_maneuver.end_dist - maneuver.lane_following_maneuver.start_dist;
    double total_dist_covered = 0;
  
    double prev_speed = case_one_profile[0].speed;
    lanelet::BasicPoint2d prev_point = case_one_profile[0].point;
    double current_speed;
    lanelet::BasicPoint2d current_point;
    
    for(int i = 0;i < case_one_profile.size();i++){
      current_point = case_one_profile[i].point;
      current_speed = case_one_profile[i].speed;
      double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
      total_dist_covered += delta_d;   
      if(total_dist_covered < 50.1){ //According to test conditions acceleration till approx 50 meters 
        EXPECT_TRUE(current_speed >= prev_speed);
      }
      else{
        EXPECT_TRUE(current_speed <= prev_speed);
      }

      prev_speed = current_speed;
      prev_point = current_point;
    }


    //Test compose_trajectory_from_centerline
    req.header.stamp = ros::Time(0.0);
    std::vector<cav_msgs::TrajectoryPlanPoint> trajectory = plugin.compose_trajectory_from_centerline(points_and_target_speeds, req.vehicle_state, req.header.stamp);
    EXPECT_TRUE(trajectory.size() > 2);
  }

  TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_two){
    //Test Stop controlled Intersection tactical plugin generation
    ros::Time::setNow(ros::Time(0.0));
    StopControlledIntersectionTacticalPluginConfig config;
    std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
    StopControlledIntersectionTacticalPlugin plugin(wm, config, [&](auto msg) {});
    
    auto map = carma_wm::test::buildGuidanceTestMap(3.7, 50);

    wm->setMap(map);
    carma_wm::test::setSpeedLimit(30_mph, wm);
    /**
     * Total route length should be 100m
     *
     *        |1203|1213|1223|
     *        | _  _  _  _  _|
     *        |1202| Ob |1222|
     *        | _  _  _  _  _|
     *        |1201|1211|1221|    num   = lanelet id hardcoded for easier testing
     *        | _  _  _  _  _|    |     = lane lines
     *        |1200|1210|1220|    - - - = Lanelet boundary
     *        |              |    O     = Default Obstacle
     *        ****************
     *           START_LINE
    */

    carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm);   

    //Create a request and maneuver that meets case 2 criteria
    //speed_before_stop
     cav_srvs::PlanTrajectoryRequest req;
     req.vehicle_state.x_pos_global = 1.5;
     req.vehicle_state.y_pos_global = 5.0;
     req.vehicle_state.orientation = 0;
     req.vehicle_state.longitudinal_vel = 11.176;

     cav_msgs::Maneuver maneuver;
     maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
     maneuver.lane_following_maneuver.start_dist = req.vehicle_state.y_pos_global;
     maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
     maneuver.lane_following_maneuver.start_speed = 11.176;  //25 mph in mps

     maneuver.lane_following_maneuver.end_dist = 100.0;
     maneuver.lane_following_maneuver.end_speed = 0.0;

    //Enter meta data
    maneuver.lane_following_maneuver.parameters.int_valued_meta_data.push_back(2);    //Case number
    maneuver.lane_following_maneuver.parameters.string_valued_meta_data.push_back("Carma/stop_controlled_intersection");

    //Float meta data list - a_acc, a_dec, t_acc, t_dec, t_cruise, speed_before_decel
    
    //Calculate speed before decel - The speed the vehicle should accelerate and then cruise before slowing down
    //Assuming the a_dec to be 2m/s^2, the vehicle should be able to stop at that deceleration starting from speed limit

    double a_dec = -2.0;
    double speed_before_decel = 13.4112; //30_mph in mps (Speed Limit)
    double dist_decel = pow(speed_before_decel, 2)/(2*std::abs(a_dec));
    //Fix Cruising distance
    double dist_cruising = 20.0;
    double  dist_acc = (maneuver.lane_following_maneuver.end_dist - maneuver.lane_following_maneuver.start_dist) - dist_decel - dist_cruising;
    double a_acc = (pow(speed_before_decel,2) - pow(maneuver.lane_following_maneuver.start_speed, 2))/(2*dist_acc);
    double t_acc = (speed_before_decel - maneuver.lane_following_maneuver.start_speed)/a_acc;
    double t_dec = speed_before_decel/std::abs(a_dec);
    double t_cruising = dist_cruising/speed_before_decel;

    maneuver.lane_following_maneuver.end_time = maneuver.lane_following_maneuver.start_time + ros::Duration(t_acc + t_cruising + t_dec);

    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_acc);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_dec);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(t_acc);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(t_dec);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(t_cruising);
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(speed_before_decel);

    req.maneuver_plan.maneuvers.push_back(maneuver);
    req.maneuver_index_to_plan = 0;

    cav_srvs::PlanTrajectoryResponse resp;

    plugin.plan_trajectory_cb(req, resp);
    EXPECT_EQ(0, resp.related_maneuvers.back());

    //Test create_case_two_speed_profile
    std::vector<lanelet::BasicPoint2d> route_geometry_points = wm->sampleRoutePoints(
            std::min(maneuver.lane_following_maneuver.start_dist + 1.0, maneuver.lane_following_maneuver.end_dist), 
            maneuver.lane_following_maneuver.end_dist, 1.0);
    
    std::vector<PointSpeedPair> case_two_profile = plugin.create_case_two_speed_profile(wm, maneuver, route_geometry_points, req.vehicle_state.longitudinal_vel);

    double prev_speed = case_two_profile[0].speed;
    for(int i = 0;i< case_two_profile.size();i++){
      double current_speed = case_two_profile[i].speed;
      if(i <= 31){
        EXPECT_TRUE(current_speed >= prev_speed);
      }
      else if(i > 31 && i < 52){
        EXPECT_TRUE(current_speed == prev_speed);
      }
      else{
        EXPECT_TRUE(current_speed < prev_speed);
      }
      
      prev_speed = current_speed;
    }

  }

  TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_three){
    //Test Stop controlled Intersection tactical plugin generation
    ros::Time::setNow(ros::Time(0.0));
    StopControlledIntersectionTacticalPluginConfig config;
    std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
    StopControlledIntersectionTacticalPlugin plugin(wm, config, [&](auto msg) {});
    
    auto map = carma_wm::test::buildGuidanceTestMap(3.7, 50);

    wm->setMap(map);
    carma_wm::test::setSpeedLimit(30_mph, wm);
    /**
     * Total route length should be 100m
     *
     *        |1203|1213|1223|
     *        | _  _  _  _  _|
     *        |1202| Ob |1222|
     *        | _  _  _  _  _|
     *        |1201|1211|1221|    num   = lanelet id hardcoded for easier testing
     *        | _  _  _  _  _|    |     = lane lines
     *        |1200|1210|1220|    - - - = Lanelet boundary
     *        |              |    O     = Default Obstacle
     *        ****************
     *           START_LINE
    */

    carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm);   

    //Create a request and maneuver that meets case 3 criteria
    //In order to be case 2 - estimated_stop_time > scheduled_stop_time and speed_before_decel =  speed_limit
    //speed_before_stop
     cav_srvs::PlanTrajectoryRequest req;
     req.vehicle_state.x_pos_global = 1.5;
     req.vehicle_state.y_pos_global = 5.0;
     req.vehicle_state.orientation = 0;
     req.vehicle_state.longitudinal_vel = 11.176;

     cav_msgs::Maneuver maneuver;
     maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
     maneuver.lane_following_maneuver.start_dist = req.vehicle_state.y_pos_global;
     maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
     maneuver.lane_following_maneuver.start_speed = 11.176;  //25 mph in mps

     maneuver.lane_following_maneuver.end_dist = 100.0;
     maneuver.lane_following_maneuver.end_speed = 0.0;

    //Enter meta data
    maneuver.lane_following_maneuver.parameters.int_valued_meta_data.push_back(3);    //Case number
    maneuver.lane_following_maneuver.parameters.string_valued_meta_data.push_back("Carma/stop_controlled_intersection");

    //Float meta data list - a_dec
    double a_dec = -pow(maneuver.lane_following_maneuver.start_speed, 2)/(2*(maneuver.lane_following_maneuver.end_dist - maneuver.lane_following_maneuver.start_dist));
    maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_dec);

    //Test create_case_three_speed_profile
    std::vector<lanelet::BasicPoint2d> route_geometry_points = wm->sampleRoutePoints(
            std::min(maneuver.lane_following_maneuver.start_dist , maneuver.lane_following_maneuver.end_dist), 
            maneuver.lane_following_maneuver.end_dist, 1.0);
    
    std::vector<PointSpeedPair> case_three_profile = plugin.create_case_three_speed_profile(wm, maneuver, route_geometry_points, req.vehicle_state.longitudinal_vel);
    double prev_speed = case_three_profile.front().speed;
    for(int i = 1;i <case_three_profile.size();i++){
      double current_speed = case_three_profile[i].speed;
      EXPECT_TRUE(current_speed <= prev_speed);
      
      prev_speed = current_speed;
    }
    EXPECT_TRUE(case_three_profile.back().speed < 0.5); //Last speed is less than 0.5mps
  }

}

// Run all the tests
int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::Time::init();
  ROSCONSOLE_AUTOINIT;
  if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { // Change to Debug to enable debug logs
    ros::console::notifyLoggerLevelsChanged();
  }
  auto res = RUN_ALL_TESTS();
  
  return res;
}