import numpy as np
import pybullet as p
import time
import pybullet_data
import os
import json


class StabilityCheckUseCase:
    def call(self, outPath: str, buildNumber: int, duration=10000):
        DURATION = duration
        try:
            assemblyUrdf = json.loads(
                (open(outPath + 'urdf-generation.json')).read()).get(buildNumber)
            print(assemblyUrdf)
        except Exception as e:
            print(e)            
            return TypeError('not found urfd file or not found build number')
        inc = 0
        urdfs = []
        for el in assemblyUrdf:
            inc += 1
            file_to_open = outPath + str(inc) + '.urdf'

            f = open(file_to_open, 'w', encoding='utf-8',
                     errors='ignore')
            f.write(el)
            urdfs.append(os.path.abspath(f.name))
            f.close()
        p.connect(p.GUI)

        p.setGravity(0, 0, -10)
        p.setAdditionalSearchPath(pybullet_data.getDataPath())

        bulletIds = []
        p.loadURDF("plane.urdf")
        for el in urdfs:
            bulletIds.append(p.loadURDF(el))
        resultCoords = []
        for i in range(DURATION):
            if (i + 200 == DURATION):
                inc = 0
                for el in bulletIds:
                    inc += 1
                    pos, rot = p.getBasePositionAndOrientation(el)
                    resultCoords.append({
                        'id': inc,
                        "quaternion": rot,
                        "position": pos
                    })
            p.stepSimulation()
            time.sleep(1./240.)

        file_to_open = outPath + buildNumber + "_" + 'stability_coords.json'

        f = open(file_to_open, 'w', encoding='utf-8',
                 errors='ignore')
        f.write(json.dumps(resultCoords))
        for el in urdfs:
            os.remove(el)
        f.close()
        p.disconnect()