import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;

public class Main {
// Beispiel Kommandozeilen Eingabe   /home/dan/Downloads/test.world zyl 0 0.7 0.2 0.25 box 0 0.0 0.2 0.2 0.2
    public static int id = 1;

    public static ArrayList<String> obj = new ArrayList<>();

    public static void boxSlec(String posx, String posy, String len, String wid, String high){
        String box = "     <model name='unit_box_"+ id + "'>\n" +
                "      <pose frame=''>" + posx + " " + posy + " 0 0 0 0</pose>\n" +
                "      <link name='link'>\n" +
                "        <inertial>\n" +
                "          <mass>1</mass>\n" +
                "          <inertia>\n" +
                "            <ixx>0.166667</ixx>\n" +
                "            <ixy>0</ixy>\n" +
                "            <ixz>0</ixz>\n" +
                "            <iyy>0.166667</iyy>\n" +
                "            <iyz>0</iyz>\n" +
                "            <izz>0.166667</izz>\n" +
                "          </inertia>\n" +
                "        </inertial>\n" +
                "        <collision name='collision'>\n" +
                "          <geometry>\n" +
                "            <box>\n" +
                "              <size>" + len + " " + wid + " " + high + "</size>\n" +
                "            </box>\n" +
                "          </geometry>\n" +
                "          <max_contacts>10</max_contacts>\n" +
                "          <surface>\n" +
                "            <contact>\n" +
                "              <ode/>\n" +
                "            </contact>\n" +
                "            <bounce/>\n" +
                "            <friction>\n" +
                "              <torsional>\n" +
                "                <ode/>\n" +
                "              </torsional>\n" +
                "              <ode/>\n" +
                "            </friction>\n" +
                "          </surface>\n" +
                "        </collision>\n" +
                "        <visual name='visual'>\n" +
                "          <geometry>\n" +
                "            <box>\n" +
                "              <size>" + len + " " + wid + " " + high + "</size>\n" +
                "            </box>\n" +
                "          </geometry>\n" +
                "          <material>\n" +
                "            <script>\n" +
                "              <name>Gazebo/Grey</name>\n" +
                "              <uri>file://media/materials/scripts/gazebo.material</uri>\n" +
                "            </script>\n" +
                "          </material>\n" +
                "        </visual>\n" +
                "        <self_collide>0</self_collide>\n" +
                "        <enable_wind>0</enable_wind>\n" +
                "        <kinematic>0</kinematic>\n" +
                "      </link>\n" +
                "    </model>";
        obj.add(box);
        id = id +1;
    }
    public static void zylSelec(String posx, String posy, String rad, String len){
        String zyl = "    <model name=\"obstacle_" + id + "\">\n" +
                "      <pose>" + posx + " " + posy + " 0 0 0 0</pose>\n" +
                "      <link name=\"link\">\n" +
                "        <collision name=\"collision\">\n" +
                "          <geometry>\n" +
                "            <cylinder>\n" +
                "              <radius>"+ rad + "</radius>\n" +
                "              <length>"+ len +"</length>\n" +
                "            </cylinder>\n" +
                "          </geometry>\n" +
                "        </collision>\n" +
                "\n" +
                "        <visual name=\"visual\">\n" +
                "          <geometry>\n" +
                "            <cylinder>\n" +
                "              <radius>" + rad +"</radius>\n" +
                "              <length>" + len + "</length>\n" +
                "            </cylinder>\n" +
                "          </geometry>\n" +
                "        </visual>\n" +
                "      </link>\n" +
                "    </model>" + "\n";
        obj.add(zyl);
        id = id +1;
    }
    public static void main(String[] args) {
        obj.add("<sdf version='1.4'>\n" +
                "  <world name='default'>\n" +
                "   \n" +
                "   <!-- A global light source -->\n" +
                "    <include>\n" +
                "      <uri>model://sun</uri>\n" +
                "    </include>\n" +
                "\n" +
                "    <!-- A ground plane -->\n" +
                "    <include>\n" +
                "      <uri>model://ground_plane</uri>\n" +
                "    </include>\n" +
                "\n" +
                "    <physics type=\"ode\">\n" +
                "      <real_time_update_rate>1000.0</real_time_update_rate>\n" +
                "      <max_step_size>0.001</max_step_size>\n" +
                "      <real_time_factor>1</real_time_factor>\n" +
                "      <ode>\n" +
                "        <solver>\n" +
                "          <type>quick</type>\n" +
                "          <iters>150</iters>\n" +
                "          <precon_iters>0</precon_iters>\n" +
                "          <sor>1.400000</sor>\n" +
                "          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>\n" +
                "        </solver>\n" +
                "        <constraints>\n" +
                "          <cfm>0.00001</cfm>\n" +
                "          <erp>0.2</erp>\n" +
                "          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>\n" +
                "          <contact_surface_layer>0.01000</contact_surface_layer>\n" +
                "        </constraints>\n" +
                "      </ode>\n" +
                "    </physics>\n" +
                "      \n" +
                "    <scene>\n" +
                "      <ambient>0.4 0.4 0.4 1</ambient>\n" +
                "      <background>0.7 0.7 0.7 1</background>\n" +
                "      <shadows>true</shadows>\n" +
                "    </scene>\n" +
                "\n" +
                "    <gui fullscreen='0'>\n" +
                "      <camera name='user_camera'>\n" +
                "        <pose>0.0 0.0 17.0 0 1.5708 0</pose>\n" +
                "        <view_controller>orbit</view_controller>\n" +
                "      </camera>\n" +
                "    </gui>" + "\n");

        File file = new File(args[0]);
        try {
            if (file.createNewFile())
            {
                System.out.println("File is created!");
            } else {
                System.out.println("File overwrite!");
            }
        } catch (IOException e) {
            e.printStackTrace();
        }
        for(int i = 1; i < args.length-1; ) {
            if ((args[i].compareTo("zyl") == 0)) {
                zylSelec(args[i+1], args[i+2], args[i+3], args[i+4]);
                i=i+5;
            }
            if ((args[i].compareTo("box") == 0)) {
                boxSlec(args[i+1], args[i+2], args[i+3], args[i+4], args[i+5]);
                i =i+6;
            }
        }
        obj.add("  </world>\n" +
                "</sdf>");
        FileWriter writer = null;
        try {
            writer = new FileWriter(file);

            for (int i = 0;i<obj.size();i++){
                writer.write(obj.get(i));
                writer.write("\n");
            }

            writer.close();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

}