/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * 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.
 *
*/

//! [complete]
#include <atomic>
#include <chrono>
#include <csignal>
#include <iostream>
#include <string>
#include <thread>
#include <fstream>
#include <iostream>
#include <string>

#include <gz/msgs.hh>
#include <gz/transport.hh>

#include <gz_gep_tools/gz_gep_tools.hh>
#include <gz_gep_tools/perception_action_loop.hh>


using namespace gz_transport_hw_tools;
//////////////////////////////////////////////////
int main(int argc, char **argv)
{
  // Dealing with the arguments.
  int choice = 0;

  if (argc!=2) {
    std::cerr << argv[0] << " h|t" << std::endl
              << " h : for the H1 robot (default)" << std::endl
              << " t : for the TALOS robot" << std::endl;
    return -1;
  }
  else if (std::string(argv[1])==std::string("t"))
    choice = 1;

  // Create a transport node and advertise a topic.
  gz::transport::Node node;
  RobotCtrlJointInfos talos_ctrl_joint_infos {
    { "arm_left_1_joint", ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.25847 ,  0.0)},
    { "arm_left_2_joint", ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.173046,  0.0)},
    { "arm_left_3_joint", ControlJointValue(   5000.0, 0.0 , 1.0,  9.0, -0.0002  , 0.0)},
    { "arm_left_4_joint", ControlJointValue(   5000.0, 0.0 , 1.0,  9.0, -0.525366, 0.0)},
    { "arm_left_5_joint", ControlJointValue(    500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
    { "arm_left_6_joint", ControlJointValue(    500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
    { "arm_left_7_joint", ControlJointValue(    100.0, 1.0 , 0.0,  3.0,  0.0,      0.0)},
    { "arm_right_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.25847 , 0.0)},
    { "arm_right_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.173046, 0.0)},
    { "arm_right_3_joint", ControlJointValue(  5000.0, 0.0 , 1.0,  9.0,  0.0002  , 0.0)},
    { "arm_right_4_joint", ControlJointValue(  5000.0, 0.0 , 1.0,  9.0, -0.525366, 0.0)},
    { "arm_right_5_joint", ControlJointValue(   500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
    { "arm_right_6_joint", ControlJointValue(   500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
    { "arm_right_7_joint", ControlJointValue(   500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
    { "gripper_left_joint",  ControlJointValue(1000.0,10.0 , 1.0, 10.0,  0.0,      0.0)},
    { "gripper_right_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0,  0.0,      0.0)},
    { "head_1_joint", ControlJointValue(        300.0, 0.1,  1.0,  5.0,  0.0,      0.0)},
    { "head_2_joint", ControlJointValue(        300.0, 0.1,  1.0,  1.5,  0.0,      0.0)},
    { "leg_left_1_joint", ControlJointValue(   5000.0,20.0,  5.0,   7.0, 0.0,      0.0)},
    { "leg_left_2_joint", ControlJointValue(   5000.0,20.0,  5.0,  14.0, 0.0,      0.0)},
    { "leg_left_3_joint", ControlJointValue(   5000.0,20.0,  5.0,  14.0,-0.448041, 0.0)},
    { "leg_left_4_joint", ControlJointValue(   5000.0,20.0,  5.0,  25.0, 0.896082, 0.0)},
    { "leg_left_5_joint", ControlJointValue(   5000.0,20.0,  5.0,  14.0,-0.448041, 0.0)},
    { "leg_left_6_joint", ControlJointValue(   5000.0,20.0,  5.0,   9.0, 0.0,      0.0)},
    { "leg_right_1_joint", ControlJointValue(  5000.0,20.0,  5.0,   7.0, 0.0,      0.0)},
    { "leg_right_2_joint", ControlJointValue(  5000.0,20.0,  5.0,  14.0, 0.0,      0.0)},
    { "leg_right_3_joint", ControlJointValue(  5000.0,20.0,  5.0,  14.0,-0.448041, 0.0)},
    { "leg_right_4_joint", ControlJointValue(  5000.0,20.0,  5.0,  25.0, 0.896082, 0.0)},
    { "leg_right_5_joint", ControlJointValue(  5000.0,20.0,  5.0,   9.0,-0.448041, 0.0)},
    { "leg_right_6_joint", ControlJointValue(  5000.0,20.0,  5.0,   7.0, 0.0,      0.0)},
    { "torso_1_joint",     ControlJointValue( 10000.0,10.0,  1.0,  10.0, 0.0,      0.0)},
    { "torso_2_joint",     ControlJointValue( 10000.0,10.0,  1.0,  10.0, 0.0,      0.0)}
  };

  gz_transport_hw_tools::RobotCtrlJointInfos h1_2_ctrl_joint_infos {
    { "left_shoulder_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_shoulder_roll_joint",  ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_shoulder_yaw_joint",   ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_elbow_joint",          ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_wrist_roll_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_wrist_pitch_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_wrist_yaw_joint",      ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_shoulder_pitch_joint",ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_shoulder_roll_joint", ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_shoulder_yaw_joint",  ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_elbow_joint",         ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_wrist_roll_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_wrist_pitch_joint",   ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_wrist_yaw_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_hip_yaw_joint",        ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_hip_pitch_joint",      ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_hip_roll_joint",       ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_knee_joint",           ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_ankle_pitch_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "left_ankle_roll_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_hip_yaw_joint",       ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_hip_pitch_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_hip_roll_joint",      ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_knee_joint",          ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_ankle_pitch_joint",   ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "right_ankle_roll_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
    { "torso_joint",               ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
  };


  std::string talos_prefix_model_root("/model/Pyrene/");
  std::string talos_prefix_world("/world/empty_talos_gz");

  std::string h1_v2_prefix_model_root("/model/H1/");
  std::string h1_v2_prefix_world("/world/empty_h1_2_gz");


  std::string & a_prefix_model_root = ( choice ==0 ) ?
      h1_v2_prefix_model_root : talos_prefix_model_root;

  std::string & a_prefix_world = (choice == 0) ?
      h1_v2_prefix_world: talos_prefix_world;

  gz_transport_hw_tools::RobotCtrlJointInfos &
    a_robot_ctrl_joint_infos = (choice==0) ?
      h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ;

  bool debug_level = false;


  gz_transport_hw_tools::PerceptionActionLoop a_pal(a_robot_ctrl_joint_infos,
                             a_prefix_model_root,
                             a_prefix_world,
                             debug_level);


  a_pal.InitGz();

  a_pal.MainLoop();

  return 0;
}
//! [complete]