#script that copies evolved robot sdfs into a folder structure loadable by gazebo
#uses exp_maxes.csv from summarize.R, which has the following columns:
#"","robot_id","run","gen","vel","dvel","fitness","t_eval","exp","births","t_birth","parent1","parent2","nparts","x","y","z","extremity_count","joint_count","motor_count","inputs","outputs","hidden","conn"

#should be run in directory which contains output/ from the experiments and exp_maxes.csv

import os
import sys
import csv
from shutil import copyfile
from string import Template

config_string=Template("""<?xml version="1.0"?>
<model>
  <name>${robot_name}</name>
  <version>1.0</version>
  <sdf version='1.5'>model.sdf</sdf>
  <author>
   <name>Malin Aandahl</name>
   <email>mwaandah@ifi.uio.no</email>
  </author>
  <description>
    Autogenerated model file from evolved robot
  </description>
</model>
""")

def read_exp_maxes(models_path, file):
    with open((sub_dir + "/" + file), "rb") as f:
        reader = csv.reader(f)
        reader.next()

        for row in reader:
            print"ROW", row[0],row[1]
            print("WHOLE ROW", row)

            model_name=(row[0].split('.'))[0]+"-fit-"+"{:.0f}".format(float(row[6])*100.0)+"-id-"+row[1]
            print "MODELNAME", model_name

            #make a folder for each robot
            model_path=os.path.join(models_path,model_name)
            print model_path
            if not os.path.exists(model_path):
                os.makedirs(model_path)

            #write config file
            #conf_str=config_string.safe_substitute({'robot_name' : row[0]+"_"+row[1]+"_fit_"+row[6]})
            conf_str=config_string.safe_substitute({'robot_name' : model_name})
            #print conf_str
            config_file_name=os.path.join(model_path,"model.config")
            if not os.path.isfile(config_file_name):
                config_file= open(config_file_name,"w")
                config_file.write(conf_str)
                config_file.close()

            #copy model file
            model_src=os.path.join("offline-evolve/output",(row[0].split('.'))[0],"robot_"+row[1]+".sdf")
            print model_src
            model_dst=os.path.join(model_path,"model.sdf")
            print model_dst
            copyfile(model_src,model_dst)

sub_dir = sys.argv[1]
sub_dirs = os.listdir(sub_dir)
paths = []
for file in sub_dirs:
    models_path="models/" + file[10:-4]
    if not os.path.exists(models_path):
        os.makedirs(models_path)

    paths.append("$REVOLVE_PATH/tol-revolve/scripts/"+models_path+":")
    #sys.stdout.write(())
    read_exp_maxes(models_path, file)

#paths.sort()
#print(''.join(paths))
