'''
parameter2osim.py
Generate OpenSim models file given the parameters listed in seed.mot
Seed could be optimized by removing suboptimal results

input:
Base OpenSim model
Filtered seed.mot with selected results
Output OpenSim model name

output:
Edited opensim models in names with prefix of selected population number in seed.mot

usage:
python parameter2osim.py LFTibia.osim data/seed.mot LFTibia.osim


'''



import os,sys
import opensim as osim

def vec_to_list(vec):
    """Converts an OpenSim Vec3/6 to a Python list. from farms_opensim
    """
    return [vec.get(i) for i in range(vec.size())]


def para2osim(oldosim,parafile,newosim,start_col=3):
    f = open(parafile,'r')
    lines = f.readlines()
    f.close()
    i = 0
    for line in lines:
        i += 1
        model = osim.Model(oldosim)
        model.initSystem()
        working_state = model.getWorkingState()
        var = []
        temp_split = line.split()
        for tmp in temp_split:
            var.append(float(tmp))
        var = var[start_col:]  
        i = 0
        job = temp_split[start_col-1]
        for muscle in model.getMuscles():  
            mname = muscle.getName()
            mp = var[i*6:(i+1)*6] 
            i += 1
            muscle.set_max_isometric_force(mp[0])
            muscle.setMaxContractionVelocity(mp[1])
            Lmtu1 = muscle.getOptimalFiberLength() 
            Lmtu2 = muscle.getTendonSlackLength()
            Lmtu = Lmtu1+Lmtu2
            lopt = Lmtu * mp[2]
            lslack = Lmtu - lopt
            #print('lopt=' ,lopt,Lmtu1,Lmtu2,Lmtu,mp[2])
            muscle.setOptimalFiberLength(lopt)
            muscle.setTendonSlackLength(lslack)
            # set ap insertion
            path_point = None
            idx = 0
            geometry = muscle.getGeometryPath()
            for idx, point in enumerate(geometry.getPathPointSet()): 
                path_point = osim.PathPoint.safeDownCast(point)
            initial_guess = vec_to_list(path_point.get_location()) 
            new_insertion = osim.Vec3(mp[3]+initial_guess[0], mp[4]+initial_guess[1], mp[5]+initial_guess[2])
            pathpoint_name = path_point.getName()
            pathpoint_name = pathpoint_name+'{}'.format(job)
            geometry.appendNewPathPoint(pathpoint_name,geometry.getPathPointSet().get(idx).getParentFrame(),new_insertion)
            geometry.deletePathPoint(working_state,idx)
    
        model.initSystem()
        model.printToXML('{}'.format(job)+newosim)


if __name__ == '__main__':
    print('python parameter2osim.py oldosim parafile newosim')
    print('parafile 1 line n+25 cols')
    oldosim = sys.argv[1]
    parafile = sys.argv[2]
    newosim = sys.argv[3]
    para2osim(oldosim,parafile,newosim,3)  #n=2  seed.mot starts from 3rd column，2nd column is job number
    
