import sys
import os.path
import math
from numpy import *

import Sofa
import SofaTest
from SofaTest.Macro import *

import Anatomy.Tools

# data test dir
dataDir = os.path.join(os.path.dirname(os.path.abspath(__file__)),"../data")

points_1 = [ [1., 0., 0.], [0., 1., 0.], [-1., 0., 0.], [0., 0., -1.] ]
result_1 = [ [0., 0., 0.], 1.]

mesh_01_vtu = {101:[1.,0.,0.], 102:[0.,2.,0.]}
mesh_02_vtu = {1011:[1.5,0.,0.], 1012:[0.,2.5,0.], 1013:[0.,0.,3.5]}

def test_fitSphere(points, result):
    [c, r] = Anatomy.Tools.fitSphere(points)
    [c1, r1] = result_1
    return EXPECT_VEC_EQ(c1,c,"center") and EXPECT_FLOAT_EQ(r1, r, "radius")

def run() :
    ok = True

    ok &= EXPECT_TRUE(test_fitSphere(points_1, result_1),"test_fitSphere")

    ok &= EXPECT_EQ(mesh_01_vtu, Anatomy.Tools.readVtuIdVertices(os.path.join(dataDir, "mesh_01.vtu")), "readVtuIdVertices")

    mesh_vtu={}
    mesh_vtu.update(mesh_01_vtu)
    mesh_vtu.update(mesh_02_vtu)
    ok &= EXPECT_EQ(mesh_vtu, Anatomy.Tools.readVtmIdVertices(os.path.join(dataDir, "model.vtm")), "readVtmIdVertices")

    return ok
