rGraph(Orig,[[Orig,Z]|R],R2):-!,
	reorderGraph([[Orig,Z]|R],R2).
rGraph(Orig,R,R3):-
	member([Orig,X],R),!,
	delete(R,[Orig,X],R2),
	reorderGraph([[Orig,X]|R2],R3).
rGraph(Orig,R,R3):-
	member([X,Orig],R),
	delete(R,[X,Orig],R2),
	reorderGraph([[Orig,X]|R2],R3).


reorderGraph([],[]).
reorderGraph([[X,Y],[Y,Z]|R],[[X,Y]|R1]):-
	reorderGraph([[Y,Z]|R],R1).
reorderGraph([[X,Y],[Z,W]|R],[[X,Y]|R2]):-
	Y\=Z,
	reorderGraph2(Y,[[Z,W]|R],R2).

reorderGraph2(_,[],[]).
reorderGraph2(Y,R1,[[Y,Z]|R2]):-
	member([Y,Z],R1),!,
	delete(R1,[Y,Z],R11),
	reorderGraph2(Z,R11,R2).
reorderGraph2(Y,R1,[[Y,Z]|R2]):-
	member([Z,Y],R1),
	delete(R1,[Z,Y],R11),
	reorderGraph2(Z,R11,R2).

getCoordenadas(C, L):-
    city(C, L1, L2),
    L=(L1,L2).

segmentation([_], []).
segmentation([H|T], L):-
    segmentation(T, LA),
    T=[A|_],
    append([[H, A]], LA, L).

modificarCusto([P1, Q1], [P2, Q2], T, R, CI, CF):-
    append([[P2, P1], [Q2, Q1]], T, R),
    dist_cities(P1, Q1, C1),
    dist_cities(P2, Q2, C2),
    dist_cities(P2, P1, C3),
    dist_cities(Q2, Q1, C4),
    CF is CI - (C1 + C2) + (C3 + C4).

elimIntersection([H|T], CI, R, CF):-
    elimIntersection_(H, T, R, CI, CF).

elimIntersection_(S, [], [S], C, C).
elimIntersection_([P1,Q1], [[P2,Q2]|T], R, CI, CF):-
    getPosition(P1,L1),
    getPosition(Q1,L2),
    getPosition(P2,L3),
    getPosition(Q2,L4),
    P1\==Q1,
    P1\==Q2,
    P2\==Q1,
    P2\==Q2,
    doIntersect(L1, L2, L3, L4),
    modificarCusto([P1,Q1], [P2,Q2], T, R, CI, CF).

elimIntersection(S, [H|T], R, CI, CF):-
    findIntersection(S, T, R1, CI, CF),
    append([H], R1, R).

tsp3(Orig,Cam,Custo):-
    tsp2_(Orig,(_,0,[Orig]),Cam1,Custo1),
    segmentation(Cam1, S),
    elimIntersection(S, Custo1, R, Custo),
    rGraph(Orig, R, Cam),
    !.