import os
from helper.fs import FS

from src.model.sdf_join import SdfJoin
import typing
import uuid


def from_str(x):
    assert isinstance(x, str)
    return x


def from_none(x):
    assert x is None
    return x


def from_union(fs, x):
    for f in fs:
        try:
            return f(x)
        except:
            pass
    assert False


def to_class(c, x):
    assert isinstance(x, c)
    return x.to_dict()


class SdfGeometryModel:
    def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction):
        self.name = name
        self.ixx = ixx
        self.ixy = ixy
        self.ixz = ixz
        self.iyy = iyy
        self.izz = izz
        self.massSDF = massSDF
        self.posX = posX
        self.posY = posY
        self.posZ = posZ
        self.eulerX = eulerX
        self.eulerY = eulerY
        self.eulerZ = eulerZ
        self.iyz = iyz
        self.stl = stl
        self.link = link
        self.friction = friction

    @staticmethod
    def from_dict(obj):
        assert isinstance(obj, dict)
        name = from_union([from_str, from_none], obj.get("name"))
        ixx = from_union([from_str, from_none], obj.get("ixx"))
        ixy = from_union([from_str, from_none], obj.get("ixy"))
        ixz = from_union([from_str, from_none], obj.get("ixz"))
        iyy = from_union([from_str, from_none], obj.get("iyy"))
        izz = from_union([from_str, from_none], obj.get("izz"))
        massSDF = from_union([from_str, from_none], obj.get("massSDF"))
        posX = from_union([from_str, from_none], obj.get("posX"))
        posY = from_union([from_str, from_none], obj.get("posY"))
        posZ = from_union([from_str, from_none], obj.get("posZ"))
        eulerX = from_union([from_str, from_none], obj.get("eulerX"))
        eulerY = from_union([from_str, from_none], obj.get("eulerY"))
        eulerZ = from_union([from_str, from_none], obj.get("eulerZ"))
        iyz = from_union([from_str, from_none], obj.get("iyz"))
        stl = from_union([from_str, from_none], obj.get("stl"))
        link = from_union([from_str, from_none], obj.get('link'))
        friction = from_union([from_str, from_none], obj.get("friction"))

        return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction)

    def to_dict(self):
        result = {}
        if self.name is not None:
            result["name"] = from_union([from_str, from_none], self.name)
        if self.ixx is not None:
            result["ixx"] = from_union([from_str, from_none], self.ixx)
        if self.ixy is not None:
            result["ixy"] = from_union([from_str, from_none], self.ixy)
        if self.ixz is not None:
            result["ixz"] = from_union([from_str, from_none], self.ixz)
        if self.iyy is not None:
            result["iyy"] = from_union([from_str, from_none], self.iyy)
        if self.izz is not None:
            result["izz"] = from_union([from_str, from_none], self.izz)
        if self.massSDF is not None:
            result["massSDF"] = from_union([from_str, from_none], self.massSDF)
        if self.posX is not None:
            result["posX"] = from_union([from_str, from_none], self.posX)
        if self.posY is not None:
            result["posY"] = from_union([from_str, from_none], self.posY)
        if self.posZ is not None:
            result["posZ"] = from_union([from_str, from_none], self.posZ)
        if self.eulerX is not None:
            result["eulerX"] = from_union([from_str, from_none], self.eulerX)
        if self.eulerY is not None:
            result["eulerY"] = from_union([from_str, from_none], self.eulerY)
        if self.eulerZ is not None:
            result["eulerZ"] = from_union([from_str, from_none], self.eulerZ)
        if self.iyz is not None:
            result["iyz"] = from_union([from_str, from_none], self.iyz)
        if self.stl is not None:
            result["stl"] = from_union([from_str, from_none], self.stl)
        if self.link is not None:
            result['link'] = from_union([from_str, from_none], self.link)
        if self.friction is not None:
            result["friction"] = from_union([from_str, from_none], self.eulerZ)
        return result

    def toJSON(self) -> str:
        return str(self.to_dict()).replace('\'', '"')

    def toSDF(self):
        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
                           + '/../../mocks/sdf/model.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)

    def toSdfLink(self):

        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
                           + '/../../mocks/sdf/link.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
    def includeLink(self, pose = False):
        if(pose == False):
             return FS.readFile(os.path.dirname(os.path.realpath(__file__))
                           + '/../../mocks/sdf/include.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name)
        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
                           + '/../../mocks/sdf/include_pose.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz)
        
    def generateSDFatJoinFixed(self, sdfModels: list['SdfGeometryModel']):
        sdf = '\n<model name="assembly">\n'
        sdf+=  '    <link name="base_link">\n'
        sdf += "        <pose>0 0 0 0 0 0</pose>\n"
        sdf+= "     </link>\n"
        
        link = sdf + self.includeLink(pose=True)
        if sdfModels.__len__() == 0:
            return link
        endTagLinkInc = link.__len__()
        beginSDF = link[0: endTagLinkInc]


        sdfJoin = beginSDF + '\n'
       
        for el in sdfModels:
            if el.name != self.name:
                sdfJoin += el.includeLink(pose=True) + '\n'
                
        endSDF = link[endTagLinkInc:link.__len__()]

        for el in sdfModels:
            if el.name != self.name:
                sdfJoin += SdfJoin(name=str(uuid.uuid4()),
                                   parent=self.name, child=el.name,modelAt=el).toSDF() + '\n'

        sdfJoin += endSDF
        sdfJoin += '</model>'
        return sdfJoin