#!/bin/bash
#########################################################################
#   Author: zhangshuai                                                  #
#   Date:   2019.03.21                                                  #
#   Description:    Automated generation of launch and world            #
#   Usage:  ./Distributed_Gazebo configuration_file                     #
#########################################################################

configuration_file=$1

world="WORLD:"
node="GAZEBO_NODE:"
ip="GAZEBO_IP:"
port="SOCKET_PORT:"
hector="HECTOR_NAME:"

world_name=0
idex=(0 0)
id0=0
id1=1
node_0_hector_count=0
node_1_hector_count=0
gazebo_idex_count=0 
# gazebo_count
# gazebo_id
# local_hector_
gazebo_param_0=(0 0)
gazebo_param_1=(0 0)
hector_node_0=(0)
hector_node_1=(0)

echo "begin"

while read line
do
    if [[ $line =~ $world ]]
    then
        temp=${line#*\"}
        temp=${temp%\"*}
        world_name=$temp
        echo ${temp}
    fi
    
    if [[ $line =~ $node ]]
    then
        temp=${line#*\"}
        temp=${temp%\"*}
        idex[gazebo_idex_count]=$temp
        
        
        echo ${temp}
        
        echo ${idex[gazebo_idex_count]}
        echo "gazebo_idex_count:$gazebo_idex_count"
        
        let gazebo_idex_count++
    fi
    
    if [[ $line =~ $ip ]]
    then
        temp=${line#*\"}
        temp=${temp%\"*}
        
        if [ "${idex[gazebo_idex_count-1]}" == "$id0" ]
        then
            gazebo_param_0[0]=$temp
            # echo "gazebo_param_0[0]:${gazebo_param_0[0]}"
        fi
        if [ "${idex[gazebo_idex_count-1]}" == "$id1" ]
        then
            gazebo_param_1[0]=$temp
            # echo "gazebo_param_1[0]:${gazebo_param_1[0]}"
        fi
        
        echo ${temp}
    fi
    
    if [[ $line =~ $port ]]
    then
        temp=${line#*\"}
        temp=${temp%\"*}
        
        if  [ "${idex[gazebo_idex_count-1]}" == "$id0" ]
        then
            gazebo_param_0[1]=$temp
        fi
        if  [ "${idex[gazebo_idex_count-1]}" == "$id1" ]
        then
            gazebo_param_1[1]=$temp
        fi
        
        echo ${temp}
    fi
    
    if [[ $line =~ $hector ]]
    then
        temp=${line#*\"}
        temp=${temp%\"*}
        
        if [ "${idex[gazebo_idex_count-1]}" == "$id0" ]
        then
            hector_node_0[node_0_hector_count]=$temp
            let node_0_hector_count++
        fi
        if [ "${idex[gazebo_idex_count-1]}" == "$id1" ]
        then
            hector_node_1[node_1_hector_count]=$temp
            let node_1_hector_count++
        fi
        
        echo ${temp}
    fi
    
done  < $configuration_file


echo "end"
# echo ${gazebo_param_0[0]}
# echo ${gazebo_param_0[1]}
# echo ${gazebo_param_1[0]}
# echo ${gazebo_param_1[1]}
# echo ${hector_node_0[0]}
# echo ${hector_node_0[1]}
# echo ${hector_node_1[0]}
# echo ${hector_node_1[1]}
echo "node0:${node_0_hector_count}"
echo "node1:${node_1_hector_count}"

file="var/log/messages"
echo "${file#*/}"
echo "${file##*/}"


# 开始写文件
launch_file=(0 0)
world_file=(0 0)
# node_0的launch文件
launch_file[0]="src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_node_0_${node_0_hector_count}_${node_1_hector_count}.launch"
if [ -f ${launch_file[0]} ]
then
    `rm -rf ${launch_file[0]}`
fi
`touch ${launch_file[0]}`

# node_1的launch文件
launch_file[1]="src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_node_1_${node_0_hector_count}_${node_1_hector_count}.launch"
if [ -f ${launch_file[1]} ]
then
    `rm -rf ${launch_file[1]}`
fi
`touch ${launch_file[1]}`

# node_0的world文件
world_file[0]="src/hector_quadrotor/hector_quadrotor_gazebo/worlds/${world_name}_node_0.world"
if [ -f ${world_file[0]} ]
then
    `rm -rf ${world_file[0]}`
fi
`touch ${world_file[0]}`

# node_1的world文件
world_file[1]="src/hector_quadrotor/hector_quadrotor_gazebo/worlds/${world_name}_node_1.world"
if [ -f ${world_file[1]} ]
then
    `rm -rf ${world_file[1]}`
fi
`touch ${world_file[1]}`

# 写world文件
for((i=0;i<$gazebo_idex_count;i++))
do
    echo "<?xml version=\"1.0\" ?>" >> ${world_file[i]}
    echo "<sdf version=\"1.4\">" >> ${world_file[i]}
    echo "  <world name=\"default\">" >> ${world_file[i]}
    echo "" >> ${world_file[i]}
    echo "    <!-- A global light source -->" >> ${world_file[i]}
    echo "    <include>" >> ${world_file[i]}
    echo "      <uri>model://sun</uri>" >> ${world_file[i]}
    echo "    </include>" >> ${world_file[i]}
    echo "" >> ${world_file[i]}
    echo "    <include>" >> ${world_file[i]}
    echo "      <uri>model://${world_name}</uri>" >> ${world_file[i]}
    echo "      <pose>0 0 0  0 0 0</pose>" >> ${world_file[i]}
    echo "    </include>" >> ${world_file[i]}
    echo "" >> ${world_file[i]}
    echo "    <scene>" >> ${world_file[i]}
    echo "      <ambient>0.68 0.68 0.68 1.0</ambient>" >> ${world_file[i]}
    echo "      <sky>" >> ${world_file[i]}
    echo "        <sunrise/>" >> ${world_file[i]}
    echo "        <clouds>" >> ${world_file[i]}
    echo "          <speed>0</speed>" >> ${world_file[i]}
    echo "        </clouds>" >> ${world_file[i]}
    echo "      </sky>" >> ${world_file[i]}
    echo "    </scene>" >> ${world_file[i]}
    echo "" >> ${world_file[i]}
    
    tmp_int=$i
    if [ "$tmp_int" == "$id0" ]
    then
        echo "    <distributed type=$i ip=\"${gazebo_param_1[0]}\" port=${gazebo_param_1[1]} simula_entity_num=${node_0_hector_count} shadow_entity_num=${node_1_hector_count} flag=1 />" >> ${world_file[i]}
    fi
    if [ "$tmp_int" == "$id1" ]
    then
        echo "    <distributed type=$i ip=\"${gazebo_param_0[0]}\" port=${gazebo_param_0[1]} simula_entity_num=${node_1_hector_count} shadow_entity_num=${node_0_hector_count} flag=1 />" >> ${world_file[i]}
    fi
    
    echo "" >> ${world_file[i]}
    echo "    <physics type="ode" name="ode">" >> ${world_file[i]}
    echo "      <real_time_update_rate>0</real_time_update_rate>" >> ${world_file[i]}
    echo "       <max_step_size>0.001</max_step_size>" >> ${world_file[i]}
    echo "      <real_time_factor>0</real_time_factor>" >> ${world_file[i]}
    echo "    </physics>" >> ${world_file[i]}
    echo "" >> ${world_file[i]}
    echo "  </world>" >> ${world_file[i]}
    echo "</sdf>" >> ${world_file[i]}
done

# 写launch文件
for((i=0;i<$gazebo_idex_count;i++))
do
    
    echo "<!-- This is a launch file that runs the bare minimum requirements to get -->" >> ${launch_file[i]}
    echo "<!-- gazebo running for a fixed-wing aircraft -->" >> ${launch_file[i]}
    echo "" >> ${launch_file[i]}
    echo "<launch>" >> ${launch_file[i]}
    echo "" >> ${launch_file[i]}
    echo "  <include file=\"\$(find gazebo_ros)/launch/empty_world.launch\">" >> ${launch_file[i]}
    echo "    <arg name=\"paused\" value=\"true\"/>" >> ${launch_file[i]}
    echo "    <arg name=\"gui\" value=\"true\"/>" >> ${launch_file[i]}
    echo "    <arg name=\"verbose\" value=\"false\"/>" >> ${launch_file[i]}
    echo "    <arg name=\"debug\" value=\"false\"/>" >> ${launch_file[i]}
    echo "    <arg name=\"world_name\" value=\"\$(find hector_quadrotor_gazebo)/worlds/${world_file[i]##*/}\"/>" >> ${launch_file[i]}
    echo "  </include>" >> ${launch_file[i]}
    echo "" >> ${launch_file[i]}
    
    x=0
    y=0
    t_x=0
    t_y=0
    # tmp0=$node_0_hector_count+$node_1_hector_count
    # tmp1=$node_1_hector_count
    
    tmp_int=$i
    
    if [ "$tmp_int" == "$id0" ]
    then
        while [ $x -lt $(($node_0_hector_count + $node_1_hector_count)) ]
        do
            
            for((j=0;j<$node_0_hector_count;j++))
            do
                if [ $y -eq 10 ]
                then
                    let t_y=0
                    let t_x=t_x+5
                fi
                
                echo "  <group ns=\"${hector_node_0[j]}\">" >> ${launch_file[i]}
                echo "    <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch\">" >> ${launch_file[i]}
                echo "      <arg name=\"name\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro\"/>" >> ${launch_file[i]}
                echo "      <arg name=\"tf_prefix\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
                echo "      <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
                echo "      <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
                echo "    </include>" >> ${launch_file[i]}
                echo "  </group>" >> ${launch_file[i]}
                echo "" >> ${launch_file[i]}
                
                let t_y=t_y+5
                let y++;
                let x++;
            done
            
            for((j=0;j<$node_1_hector_count;j++))
            do
                if [ $y -eq 10 ]
                then
                    let t_y=0
                    let t_x=t_x+5
                fi
                
                echo "  <group ns=\"${hector_node_1[j]}\">" >> ${launch_file[i]}
                echo "    <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor_shadow.launch\">" >> ${launch_file[i]}
                echo "      <arg name=\"name\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_shadow.gazebo.xacro\"/>" >> ${launch_file[i]}
                echo "      <arg name=\"tf_prefix\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
                echo "      <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
                echo "      <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
                echo "    </include>" >> ${launch_file[i]}
                echo "  </group>" >> ${launch_file[i]}
                echo "" >> ${launch_file[i]}
                
                let t_y=t_y+5
                let y++;
                let x++;
            done
        done
    fi
    if [ "$tmp_int" == "$id1" ]
    then
        while [ $x -lt 4 ]
        do
            
            for((j=0;j<$node_0_hector_count;j++))
            do
                if [ $y -eq 10 ]
                then
                    let t_y=0
                    let t_x=t_x+5
                fi
                
                echo "  <group ns=\"${hector_node_0[j]}\">" >> ${launch_file[i]}
                echo "    <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor_shadow.launch\">" >> ${launch_file[i]}
                echo "      <arg name=\"name\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_shadow.gazebo.xacro\"/>" >> ${launch_file[i]}
                echo "      <arg name=\"tf_prefix\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
                echo "      <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
                echo "      <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
                echo "    </include>" >> ${launch_file[i]}
                echo "  </group>" >> ${launch_file[i]}
                echo "" >> ${launch_file[i]}
                
                let t_y=t_y+5
                let y++;
                let x++;
            done
            
            for((j=0;j<$node_1_hector_count;j++))
            do
                if [ $y -eq 10 ]
                then
                    let t_y=0
                    let t_x=t_x+5
                fi
                
                echo "  <group ns=\"${hector_node_1[j]}\">" >> ${launch_file[i]}
                echo "    <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch\">" >> ${launch_file[i]}
                echo "      <arg name=\"name\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro\"/>" >> ${launch_file[i]}
                echo "      <arg name=\"tf_prefix\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
                echo "      <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
                echo "      <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
                echo "      <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
                echo "    </include>" >> ${launch_file[i]}
                echo "  </group>" >> ${launch_file[i]}
                echo "" >> ${launch_file[i]}
                
                let t_y=t_y+5
                let y++;
                let x++;
            done
        done
    fi
    
    echo "  <node name=\"simple_takeoff\" pkg=\"hector\" type=\"hector_simple_takeoff\"/>" >> ${launch_file[i]}
    echo "" >> ${launch_file[i]}
    echo "</launch>" >> ${launch_file[i]}
done