(* ::Package:: *)

(************************************************************************)
(* This file was generated automatically by the Mathematica front end.  *)
(* It contains Initialization cells from a Notebook file, which         *)
(* typically will have the same name as this file except ending in      *)
(* ".nb" instead of ".m".                                               *)
(*                                                                      *)
(* This file is intended to be loaded into the Mathematica kernel using *)
(* the package loading commands Get or Needs.  Doing so is equivalent   *)
(* to using the Evaluate Initialization Cells menu command in the front *)
(* end.                                                                 *)
(*                                                                      *)
(* DO NOT EDIT THIS FILE.  This entire file is regenerated              *)
(* automatically each time the parent Notebook file is saved in the     *)
(* Mathematica front end.  Any changes you make to this file will be    *)
(* overwritten.                                                         *)
(************************************************************************)



BeginPackage["Mechanisms`",{"Developer`"}];


$MechanismsVersion::usage = "$MechanismsVersion returns the version number of the package.";
$MechanismsVersionText::usage = "$MechanismsVersionText returns the version number of the package as a string.";

$MechanismsVersion=0.94;
$MechanismsVersionText="Mechanisms version 0.94";

(* 
	We use MeshRegion in this package to work so make sure we are using at least version 10 of Mathematica
*)
Mechanism::usage = "Keyword used to report some errors.";

Mechanism::version="Mathematica version may be too low.";
If[ $VersionNumber<10, Message[Mechanism::version] ];


(*mechanism cells*)
RigidBar::usage="RigidBar[ {v1, v2} ] is a rigid bar. Use DefaultCellData[ RigidBar ] to see its associated data.";
Spring::usage="Spring[ {v1, v2} ] is a Spring allowing you to set an arbitrary force. Use DefaultCellData[ Spring ] to see its associated data.";
Face::usage="Face[ {v1,v2,...} ] is a Face. Use DefaultCellData[ Face ] to see its associated data.";
ElasticTriangle::usage="ElasticTriangle[ {v1,v2,...} ] is an elastic Face. Use DefaultCellData[ Face ] to see its associated data.";
TorsionalFold::usage="TorsionalFold[ {v1,v2} ] is a TorsionalFold with torsional Spring controlling its angle. Use DefaultCellData[ TorsionalFold ] to see its associated data.";
FreeJoint::usage="FreeJoint[v1] is a FreeJoint.";
AngleJoint::usage="AngleJoint[v1] is a torsional FreeJoint. Use DefaultCellData[ AngleJoint ] to see its associated data.";
PinnedJoint::usage="PinnedJoint[v1] is a pinned FreeJoint. Use DefaultCellData[ PinnedJoint ] to see its associated data.";
AnyJoint::usage="AnyJoint is a head that represents either a PinnedJoint or a FreeJoint.";


VertexPosition::usage=
"VertexPosition[v, c] represents the position of vertex v along a particular component c.
v is either an integer or a list of integers.
c is one of \"x\", \"y\", \"z\", a list of these components, or All[d], which represents all components in a vector of dimension d.";
SetAttributes[VertexPosition,{NHoldAll,Constant}]

VertexDisplacement::usage=
"VertexDisplacement[v, c] represents the displacement of vertex v along a particular component c.
v is either an integer or a list of integers.
c is one of \"x\", \"y\", \"z\", a list of these components, or All[d], which represents all components in a vector of dimension d.";
SetAttributes[VertexDisplacement,{NHoldAll,Constant}]


DefaultCellData::usage="DefaultCellData[ component ] returns the default data associated with a component.";


MechanismFaces::usage="MechanismFaces[ m ] returns a list of Faces in the Mechanism.
MechanismFaces[ m , {Face1, Face2, ...} ] returns an index with the location the specific Face would appear in a list of all Faces.";

MechanismEdges::usage="MechanismEdges[ m ] returns a list of edges in the Mechanism.
MechanismEdges[ m , {edge1, edge2, ...} ] returns an index with the location the specific edge would appear in a list of all edges.";

MechanismVertices::usage="MechanismVertices[ m ] returns a list of vertices in the Mechanism.";

MechanismCells::usage = "MechanismCells[m] lists all labels and cell types. Labels are all Strings.
MechanismCells[ m, label ] lists cells with a certain label.
MechanismCells[ m, celltype ] lists cells of a certain cell type.
MechanismCells[ m, celltype1, celltype2, ... ] lists all cells of a certain cell type.";

MechanismEdgeQ::usage="MechanismEdgeQ[m,{{v1,v2},...}] returns True if all edges are in the Mechanism m.";
MechanismFaceQ::usage="MechanismFaceQ[m,{{v1,...},...}] returns True if all Faces are in the Mechanism m.";

ValidMechanismEdgeQ::usage="ValidMechanismEdgeQ[m,{{v1,v2},...}] returns True if all edges could be an edge in Mechanism m.";
ValidMechanismFaceQ::usage="ValidMechanismFaceQ[m,{{v1,...},...}] returns True if all Faces could be a Face in Mechanism m.";

ValidMechanismCellQ::usage="ValidMechanismCellQ[ cellSpec ] returns True if cellSpec is a properly specified cell.
ValidMechanismCellQ[] returns a pattern that can be used to test if a cell has a valid specification.";
ValidMechanismCellHeadQ::usage="ValidMechanismCellHeadQ[ cell ] returns True if cell is a cell type. Use DefaultCellData[] to see a list of cell types.";


MechanismPositions::usage=
"MechanismPositions[ m ] returns the coordinates of the vertices of mechanism m.
MechanismPositions[ m -> pos ] returns a new Mechanism with coordinates given by positions.";

MechanismCellList::usage= "MechanismCellList[m] returns all the cells in Mechanism m.
MechanismCellList[ m , cellSpec ] returns a list of cells matching a certain specification.
  cellSpec can be either {cellType, {index1, index2, ...}} or a pattern.
MechanismCellList[ m, cellSpec, dataType -> function ] returns a list of cells returning True when function f returns True when applied to dataType. f must be a pure function.
MechanismCellList[ m, cellSpec, dataType -> pattern ] returns a list of cells whose dataType matches pattern.";


Linkage::usage = "Linkage[ { {x1, y1}, ...}, {cell1[ {i1, i2, ..} ], ... } ] returns a linkage in 2D with vertices at {{x1, y1}, ..} and made from the specified cells.
Linkage[ { {x1, y1, z1}, ...}, {cell1[ {i1, i2, ..} ], ... } ] returns a linkage in 3D with vertices at {{x1, y1}, ..} and made from the specified cells.\n

Use Linkage[\"Examples\"] to list some examples that can be accessed by Linkage[ example ] to generate it.

Properties:\n
Cell properties can be specified by Property[ cell1[ {i1, ...} ], {property1 -> value1, ... } ].
Properties are \"Style\", \"Shape\" (to specify a shape function), \"Label\". DefaultCellData[cell] will list additional properties that can be specified.\n

Options:\n
Linkage[] takes the option OverlapPrecision which determines how close vertices need to be in order to be mapped onto each other. This could be set to 0 (or None).";

Origami::usage=
"Origami[ { {x1, y1}, ...}, {cell1[ {i1, i2, ..} ], ... } ] returns an Origami in 3D (displayed in 2D) with vertices at {{x1, y1}, ..} and made from the specified cells.
Origami[ { {x1, y1, z1}, ...}, {cell1[ {i1, i2, ..} ], ... } ] returns an Origami in 3D with vertices at {{x1, y1}, ..} and made from the specified cells.

Use Origami[\"Examples\"] to list some examples that can be accessed by Origami[ example ].

Properties:

Cell properties can be specified by Property[ cell1[ {i1, ...} ], {property1 -> value1, ... } ].
Properties are \"Style\", \"Shape\" (to specify a shape function), \"Label\". DefaultCellData[cell] will list additional properties that can be specified.

Options:

Origami[] takes the option OverlapPrecision which determines how close vertices need to be in order to be mapped onto each other. This could be set to 0 (or None).";

MechanismQ::usage="MechanismQ[ m ] returns True if m is a Mechanism.";


OverlapPrecision::usage="OverlapPrecision is an option that indicates how close points have to be to be considered identical.";


(*cell modifiers*)
LabelCells::usage= "LabelCells[ m , label , {index1, index2,...} ] labels cells with the string label.";

LabelVertices::usage="LabelVertices[ mechanism , labels ] adds labels to the vertices of a mechanism. labels can be \"Name\", \"Index\", or None, or a list of labels, or a list of rules v -> label";

SelectCells::usage=
"SelectCells[ m, pattern ] selects a sub-Mechanism with cell specification matching pattern.
SelectCells[m, pattern, data, f] selects a sub-Mechanism with cells matching pattern and f evaluating True when applied to the data. Use dataForm[] to find data that is available for a given component.";

DeleteCells::usage="DeleteCells[ m, pattern ] deletes a sub-Mechanism with cell specification matching pattern.";

AddCells::usage="AddCells[ m, {cell1, cell2, ... } ] adds cells to a Mechanism.";

ChangeCellData::usage = "ChangeCellData[ m , pattern, { data1 -> value1, ... } ] changes the data of cells matching pattern.
ChangeCellData[ m, type, {cell1, cell2, ...}, data1 -> { value1, value2, ...} ] changes the data of one type corresponding to cells.
ChangeCellData[ m , type, {cell1, cell2, ...}, data -> value ] changes the data of a number of cells to one value.
data is either appropriate data for the cells or one of \"Style\", \"Shape\", or \"Label\".";

SubdivideCells::usage="SubdivideCells[ m, cell list, label ] adds a vertex to a cell at the centroid.";
DivideFaces::usage="DivideFaces[ m, edge list ] divides Faces along the listed edges, read from left to right.";
TriangulateFaces::usage="TriangulateFaces[m] triangulates all Faces along the shortest diagonal recursively.";

PlaceVertices::usage="PlaceVertices[m, {v1 -> new position 1, ...} ] moves vertices to new positions.
PlaceVertices[m, { pos1, pos2, ... } ] replaces all vertex positions by a list of new positions.";

DisplaceVertices::usage="DisplaceVertices[m, {v1 -> displacement 1, ...} ] displaces vertices by a displacement.
DisplaceVertices[m, { disp1, disp2, ... } ] displaces all vertices by a list of displacements.";

DeleteVertices::usage="DeleteVertices[ m, {v1, ...} ] deletes a list of vertices and all cells associated with those vertices.";

MapCells::usage="MapCells[m, f] applies a map f to a Mechanism.";

JoinMechanism::usage="JoinMechanism[ m1, m2, ... ] joins a list of Mechanisms of the same type together.";


TesselateMechanism::usage=
"TesselateMechanism[m, primitive vector, n1 ], TesselateMechanism[m, {vector 1, vector 2}, {n1, n2}],  TesselateMechanism[m, {vector 1, vector 2, vector 3}, {n1, n2, n3}] tesselates a mechanism using a set of 2D or 3D primitive vectors as an n1, n1 x n2 or n1 x n2 x n3 celled mechanism.";


PinnedFourBarLinkage::usage="PinnedFourBarLinkage[{width , height}] represents a pinned, four-bar linkage with a given width and height.";
ChainLinkage::usage="ChainLinkage[n, {start, end}] represents a chain of n bars between a start and end point.";
PolygonalLinkage::usage="PolygonalLinkage[n] represents a polygonal linkage in 2D with n equal length segments.";
WattLinkage::usage="WattLinkage represents a Watt linkage with equal length bars.
WattLinkage[ l ] represents a Watt linkage with middle bar of length l.";
ConnellyServatiusLinkage::usage="ConnellyServatiusLinkage represents the ConnellyServatius linkage with cusp singularity.";
Connelly2ndOrderRigidLinkage::usage="Connelly2ndOrderRigidLinkage represents Connelly's example of a 2nd order rigid linkage that is not prestress stable.";


RandlettBirdOrigami::usage="RandlettBirdOrigami represents an unfolded Randlett bird. RandlettBirdOrigami[\"Folded\"] represents a folded version of the Randlett bird.";
WaterBombBase::usage="WaterBombBase represents a unit cell of the water bomb base origami.";
MiuraOriCell::usage="MiuraOriCell[x] represents the unit cell of a Miura ori origami with vertical folds offset from vertex by x.
MiuraOriCell[x, \"Triangulated\"] is a triangulated Miura ori unit cell with vertical folds offset from vertex by x.";


Begin["`Private`"];


$meshRegionProperties={MeshCellStyle -> "Style", MeshCellLabel -> "Label" , MeshCellShapeFunction -> "Shape"};

packedCellPattern = {_[_,_]...};
labelPattern = {___Rule};

coordinatePattern2D = {{_, _}..};
coordinatePattern3D = {{_, _, _}..};
coordinatePattern = coordinatePattern2D | coordinatePattern3D;


mechanismPattern = (Linkage|Origami)[ coordinatePattern, _ , packedCellPattern, labelPattern ];

MechanismQ[ mechanismPattern ]:=True
MechanismQ[_]:=False


celltypePattern = RigidBar|Spring|Face|ElasticTriangle|TorsionalFold|FreeJoint|PinnedJoint|AngleJoint;
inputCellPattern = Alternatives[
	(RigidBar|Spring|TorsionalFold)[ {Except[_List],Except[_List]} ],
	(ElasticTriangle|AngleJoint)[ {Except[_List],Except[_List],Except[_List]} ],
	(FreeJoint|PinnedJoint)[ Except[_List] ],
	Face[ _?VectorQ ]
];
exactInputCellPattern = Alternatives[
	(RigidBar|Spring|TorsionalFold)[ {_Integer,_Integer} ],
	(ElasticTriangle|AngleJoint)[ {_Integer,_Integer,_Integer} ],
	(FreeJoint|PinnedJoint)[ _Integer ],
	Face[ _?(VectorQ[#,IntegerQ]&) ]
];

ValidMechanismCellHeadQ[ cell_ ] := MatchQ[ cell, celltypePattern ]

ValidMechanismCellQ[ cell_ ] := MatchQ[ cell, exactInputCellPattern ]
ValidMechanismCellQ[] := MatchQ[ exactInputCellPattern ]


(* default data in all cell types *)
DefaultCellData[] = {RigidBar, Spring, Face, ElasticTriangle , TorsionalFold, FreeJoint, PinnedJoint,AngleJoint};
DefaultCellData[RigidBar] = {"EquilibriumLength" -> Automatic, "Stiffness" -> Infinity};
DefaultCellData[Spring] = {"EquilibriumLength" -> Automatic, "Stiffness" -> Infinity, "Strain" -> "LinearStrain"};
DefaultCellData[Face] = { };
DefaultCellData[ElasticTriangle] = { "YoungsModulus" -> Infinity , "PoissonRatio" -> 1/3 };
DefaultCellData[TorsionalFold] = { "TorsionalStiffness" -> Infinity, "Angle" -> Automatic};
DefaultCellData[FreeJoint] = {};
DefaultCellData[PinnedJoint] = { "PinningStiffness" -> Infinity, "ConstraintFunction" -> Automatic };
DefaultCellData[AngleJoint] = {"AngleStiffness" -> Infinity, "Angle" -> Automatic };
DefaultCellData[_] = {};


(*data that will be stored for each of these components and in what order*)
dataForm[RigidBar] = {"Stiffness","EquilibriumLength","Shape","Label","Style"};
dataForm[Spring] = {"Stiffness","EquilibriumLength","Strain","Shape","Label","Style"};
dataForm[Face] = {"FaceStiffness","Shape","Label","Style"};
dataForm[ElasticTriangle] = {"YoungsModulus","PoissonRatio","Shape","Label","Style"};
dataForm[TorsionalFold] = {"TorsionalStiffness","Angle","Shape","Label","Style"};
dataForm[FreeJoint] = { "Shape","Label","Style" };
dataForm[PinnedJoint] = { "PinningStiffness", "ConstraintFunction","Shape","Label","Style" };
dataForm[AngleJoint] = {"AngleStiffness", "Angle","Shape","Label","Style" };
dataForm[_] := {};


packCells[ cells : {_[_,_]...} ] :=
	With[ { groupedCells = GatherBy[ Flatten[pullTags /@ cells], Head ] },
		packCellType[  Head[#[[1]]] , #[[All,1]], #[[All,2]] ]& /@ groupedCells
	]

packCellType[ s_String, indices_, data_ ] := s[ToPackedArray[indices],{}]
packCellType[ head_Symbol, indices_, data_ ] := 
	head[
		ToPackedArray[indices] , 
		compressCellData[ head, data ]
	]


pullTags[ head_[ indices_, properties_ ] ] := With[ 
{ 
rules = (# -> _ &) /@ dataForm[head], 
untagged = DeleteCases[ Flatten @ {properties}, _String ]
},
	{
		#[indices,{}]& /@ Cases[ Flatten@{properties}, _String ],
		head[indices, FilterRules[ untagged, rules ] ]
	}
]

Mechanism::badprop = "Properties `1` do not exist for cell `2`.";


combineDataList[ default_ , data_  ] := (Last /@ GatherBy[ Flatten[{default, #} ], directiveQ ])& /@ data
directiveQ[ _?ColorQ ] := "color"
directiveQ[ x_ ] := Head[x]

combineData[ dataType_, DefaultCellData_, data_ , f_ ] := 
	SparseArray[
		f /@ (dataType /. data /. {dataType -> {DefaultCellData}} /. Automatic -> "Automatic"),
		Length[data],
		DefaultCellData
	]


compressCellData[ RigidBar, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Flatten[#]&]& /@ data,
	default = Dispatch @ DefaultCellData[RigidBar]
},
	{
	combineData[ "Stiffness", Infinity, mergedData , Last],
	combineData[ "EquilibriumLength", "Automatic", mergedData , Last],
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ Spring, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[Spring]
},
	{
	combineData[ "Stiffness", Infinity, mergedData , Last],
	combineData[ "EquilibriumLength", "Automatic", mergedData , Last],
	combineData[ "Strain", ((#1-#2)/#2)&, mergedData, Last],
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ FreeJoint, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[FreeJoint]
},
	{
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ PinnedJoint, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[PinnedJoint]
},
	{
	combineData[ "PinningStiffness", Infinity, mergedData , Last],
	combineData[ "ConstraintFunction", "Automatic", mergedData , Last],
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ TorsionalFold, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[TorsionalFold]
},
	{
	combineData[ "TorsionalStiffness", Infinity, mergedData , Last],
	combineData[ "Angle", "Automatic", mergedData , Last],
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ AngleJoint, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[AngleJoint]
},
	{
	combineData[ "AngleStiffness", Infinity, mergedData , Last],
	combineData[ "Angle", "Automatic", mergedData , Last],
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ Face, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[Face]
},
	{
	combineData[ "Shape", 0 , mergedData , Last ],
	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]

compressCellData[ ElasticTriangle, data_ ] :=
With[ 
{ 
	mergedData = Merge[#, Reverse[Flatten[#]]&]& /@ data,
	default = Dispatch @ DefaultCellData[ElasticTriangle]
},
	{
	combineData[ "YoungsModulus", Infinity, mergedData , Last],
	combineData[ "PoissonRatio", 1/3, mergedData , Last],

	combineData[ "Shape", "Automatic", mergedData , Last],
	combineData[ "Label", None, mergedData , Last],
	combineDataList[ {}, "Style" /. mergedData /. default /. "Style"->{} ]
	}
]


unpackCells[ cells : packedCellPattern ] := Flatten[unpackCellType /@ cells]

unpackCellType[ s_String[ indices_ ] ] := s /@ indices

unpackCellType[ head_Symbol[ indices_, data_ ] ] :=
	MapThread[
		head,
			{
			indices,
			Transpose @ uncompressCellData[ head, indices, data ]
			}
	]

uncompressCellData[ head_ , indices_, data_ ] :=
With[ { dataNames=dataForm[head] , length = Length[indices] },
	MapThread[ Thread[ ConstantArray[#1, length] -> #2 ] &, {dataNames , data}]
]


replaceCellIndices[ cells : packedCellPattern , rules_ ] := Head[#][ ReplaceAll[ #[[1]], rules ], #[[2]] ]& /@ cells


replaceRepeatedCellIndices[ cells : packedCellPattern , rules_ ] := Head[#][ ReplaceRepeated[ #[[1]], rules ], #[[2]] ]& /@ cells


replaceCellData[ cells : packedCellPattern, rules_ ] := Head[#][ #[[1]], replaceElement[ #, rules ] & /@ #[[2]] ]& /@ cells

(*
from: https://mathematica.stackexchange.com/questions/13790/using-replaceall-on-sparsearray
*)
replaceElement[s_SparseArray, rule_] := With[
    {
    elem = ReplaceAll[s["NonzeroValues"], rule],
    def = ReplaceAll[s["Background"], rule]
    },

    Replace[
        s,
        Verbatim[SparseArray][a__, _, {b__, _}] :> SparseArray[a, def, {b, elem}]
    ]
]
(*
if the data isn't a SparseArray
*)
replaceElement[ s_, rule_ ] := ReplaceAll[ s, rule ]


sortCellType[ head_[ indices_, data_ ], sortingFunction_ : Identity ]:=With[
{ order = OrderingBy[ indices, sortingFunction ] },
	head[ indices[[ order ]], #[[ order ]]& /@ data ]
]


(*ordering is a list of integers*)
orderCellType[ head_[indices_,data_ ], ordering_ ] :=
	head[ indices[[ ordering ]], #[[ ordering ]]& /@ data ]


(*this function unpacked arrays*)
deleteDuplicateCells[ cells : packedCellPattern ] := deleteDuplicateCells /@ cells
deleteDuplicateCells[ head_[ indices_, data_ ] ] := Module[
{
indexList = DeleteDuplicatesBy[
		Transpose[ { Range[ Length[indices] ], indices } ], 
		Sort[Flatten[{Last[#]}]]& 
	][[All,1]] 
},
	head[ ToPackedArray @ indices[[ indexList ]], #[[ indexList ]]& /@ data ]
]


deleteDegenerateCells[ cells : packedCellPattern ] := deleteDegenerateCells /@ cells
deleteDegenerateCells[ head_[ indices_, data_ ] ] :=
With[ { id = Pick[ Range[Length[indices]], DuplicateFreeQ[Flatten[{#}]]& /@ indices] },
	head[ indices[[id]], #[[id]]&/@data ]
]


joinCells[ cells_ ] := joinCellType /@ GatherBy[ Flatten[cells], Head ]

joinCellType[ cells : packedCellPattern ] :=
With[ { head = Head[cells[[1]]], indices = cells[[All,1]], data = cells[[All,2]]},
	sortCellType @ deleteDegenerateCells @ deleteDuplicateCells @ head[ Join @@ indices , Join @@@ Transpose[ data ] ]
]


facePattern = Face|ElasticTriangle ;
edgePattern = RigidBar|TorsionalFold|Spring ;
vertexPattern = FreeJoint|PinnedJoint ;
triplePattern = AngleJoint ;
cellSpecPattern = facePattern|edgePattern|vertexPattern|triplePattern|_String ;

edgeLoopQ = (VectorQ[#] && Length[#]>2 &) ;
faceListQ = (VectorQ[#, VectorQ]&) ;
tripleListQ = (MatrixQ[#] &) ;

distributePropertyRules = {
	(h : cellSpecPattern)[ indices_ ] :> h[indices, {} ],
	(h : cellSpecPattern)[ indices_, properties_Rule ] :> h[indices, {properties} ],
	Property[ h_List, properties_ ] :> (Property[ #, properties ]& /@ h),
	Property[ ( h : cellSpecPattern)[ indices_, properties_ ] , newProperties_ ] :> h[indices, Flatten[ { properties, newProperties } ] ],
	(h : vertexPattern)[ indices_?VectorQ , properties_ ] :> (  h[ # , properties ]& /@ indices  ),
	(h : edgePattern)[ indices_?faceListQ , properties_ ] :> (  h[ # , properties ]& /@ indices  ),
	(h : edgePattern)[ indices_?edgeLoopQ , properties_ ] :> (  h[ # , properties ]& /@ Partition[indices,2,1] ),
	(h : facePattern)[ indices_?faceListQ , properties_ ] :> (  h[ # , properties ]& /@ indices  ),
	(h : triplePattern)[ indices_?tripleListQ , properties_ ] :> ( h[ # , properties ]& /@ indices ),
	(h : vertexPattern)[ label_ -> coordinates_ , properties_ ] :> { h[ label, properties ], add[ label , coordinates ] }
};

expansionRules = {
	Face[ i_ , properties_ ] :> { Face[ i , properties ], RigidBar[ #, properties ] & /@ Partition[ i , 2, 1, 1 ] }
};


properCellPattern= (vertexPattern)[ Except[_List], _ ] | (edgePattern)[ _?(VectorQ[#]&&Length[#]==2&), _ ] | (facePattern)[ _?(VectorQ[#]&&Length[#]>2&) , _]|(triplePattern[_?(VectorQ[#]&&Length[#]==3&),_])|_String[_,_];

parseCells[ cells_List ] := parseCells[ {{},{},{}}, cells ]
parseCells[ {oldcoordinates_, oldcells_ , oldlabels_}, cells_List ] := 
Module[{ expandedCells , goodCells, badCells, newcoordinates , labels, newlabels, coordinateCells},

	expandedCells = Flatten @ ReplaceAll[ Flatten @ ReplaceRepeated[ Flatten[cells], distributePropertyRules] , expansionRules ] ;
	coordinateCells = Cases[ expandedCells, add[_,_] ];
	goodCells = Cases[ expandedCells, properCellPattern ];

	(*if there are bad cells, report a warning*)
	badCells = DeleteCases[ expandedCells, properCellPattern|_add ];
	If[Length[badCells]>0, Message[Mechanism::badcells, Head[#][#[[1]]]&/@badCells ] ];

	(*any vertices that got added as cells get taken care of here*)
	{  labels , newcoordinates } = If[ Length[coordinateCells] > 0,
		Transpose[ List @@@ coordinateCells] ,
		{ {}, {} }
	];

	newlabels = Join[
		oldlabels,
		Thread[ labels -> Range[ Length[oldcoordinates] + 1 , Length[oldcoordinates] + Length[newcoordinates] ] ]
	];

	{
		(* new coordinates *)
		ToPackedArray @ PadRight[Join[ oldcoordinates, newcoordinates ]],
		(* cells *)
		sortCellType /@ joinCells[ Flatten @ {replaceCellIndices[ packCells[ goodCells ], Dispatch @ newlabels], oldcells } ],
		newlabels
	}
]


Mechanism::badcells = "Cells `1` cannot be parsed and will be ignored.";


unparseCells[ cells : packedCellPattern , head : _ ] := Flatten[ unparseCellType /@ Pick[ cells, Head/@cells, head ] ]

unparseCellType[ head_[ indices_ , {} ] ] := head /@ indices
unparseCellType[ head_[ indices_ , data_ ] ] :=
With[{
	dataRules = Thread[ dataForm[head] -> # ]& /@ Transpose[data]
},
	MapThread[ head[#1,#2]&, { indices, dataRules } ]
]


cellPattern[ r_ ] := With[ {groupedByHead = GatherBy[ Flatten[ recursiveCellPattern[ r ] ], Head ]},
	Head[#[[1]]][ If[ Head[#]===List&&Length[#]>1,Alternatives, Identity] @@ #[[All,1]] ]& /@ groupedByHead
]

recursiveCellPattern[ r_Alternatives ] := recursiveCellPattern /@ (List@@r)
recursiveCellPattern[ x_Blank ] /; Length[x]==0 := {_[_]}
recursiveCellPattern[ x_Blank ] := recursiveCellPattern[x[[1]][ _ ]]
recursiveCellPattern[ x_String ] := {x[_]}
recursiveCellPattern[ AnyJoint[ ind_ ] ] := recursiveCellPattern /@ {FreeJoint[ ind ], PinnedJoint[ ind ]}
recursiveCellPattern[ head_[ ind_Alternatives ] ] := recursiveCellPattern[ head[#] ]& /@ (List@@ind)
recursiveCellPattern[ head_Alternatives[ ind_ ] ] := recursiveCellPattern[ #[ind]]&/@ (List@@head)
recursiveCellPattern[ head_[ ind_ ] ] := {head[ indexPattern[ ind ] ]}

indexPattern[ x_Blank ] := x
indexPattern[ r_Alternatives ] := indexPattern /@ r
indexPattern[ n_List ] := With[ { numbers = Range[Length[n]] }, Alternatives @@ Map[ RotateRight[n,#]&, numbers ] ]
indexPattern[ n : Except[_Alternatives|_List] ] := n


casesCellType[ pattern_ , head_[ indices_ , data_ ] ] := Pick[ Range[Length[indices]], head/@indices , Alternatives @@ pattern ]
selectCellType[ f_ , head_[ indices_ , data_ ] ] := Pick[ Range[Length[indices]], f[head[#]]& /@ indices ]

identifyCellFromList[ headSpec_ , indexList_?VectorQ , head_[ indices_?VectorQ, data_ ] ] :=
With[ { table = Association @@ Thread[ Rule[indices , Range[Length[indices]] ] ]},
	DeleteMissing[ table /@ indexList ]
] /; MatchQ[head, headSpec]

identifyCellFromList[ headSpec_ , indexList : {___List} , head_[ indices : {___List}, data_ ] ] :=
With[ { table = Association @@ Thread[ Rule[ Sort /@ indices , Range[Length[indices]] ] ]},
	DeleteMissing[ table /@ (Sort /@ indexList) ]
]/; MatchQ[head, headSpec]
identifyCellFromList[ _ , indexList_, head_[ _, _ ] ] := {}

casesCellFromDataType[ dataSpec_ -> dataPattern_ , head_[ indices_, data_ ] ] := With[ { datalist = dataForm[head] },
With[ { dataIndex = dataSpec /. Thread[datalist -> Range[Length[datalist]]] },
	If[ IntegerQ[dataIndex], 
		Pick[ Range[Length[indices]], Normal@data[[ dataIndex ]] , dataPattern ],
		{}
	]
]]
selectCellFromDataType[ dataSpec_ -> selector_ , head_[ indices_, data_ ] ] := With[ { datalist = dataForm[head] },
With[ { dataIndex = datalist /. Thread[datalist -> Range[Length[datalist]]] },
	If[ IntegerQ[dataIndex], 
		Pick[ Range[Length[indices]], selector /@ data[[ dataIndex ]] ],
		{}
	]
]]


identifyCellFromList2[ headSpec_ , indexList_?VectorQ , head_[ indices_?VectorQ, data_ ] ] :=
With[ { table = Association @@ Thread[ Rule[indices , Range[Length[indices]] ] ]},
	{Pick[ Range[Length[indexList]], Not[MissingQ[#]]&/@# ],DeleteMissing[#]} & @ (table /@ indexList)
] /; MatchQ[head, headSpec]

identifyCellFromList2[ headSpec_ , indexList : {___List} , head_[ indices : {___List}, data_ ] ] :=
With[ { table = Association @@ Thread[ Rule[ Sort /@ indices , Range[Length[indices]] ] ]},
	{Pick[ Range[Length[indexList]], Not[MissingQ[#]]&/@# ],DeleteMissing[#]} & @(table /@ (Sort /@ indexList))
]/; MatchQ[head, headSpec]
identifyCellFromList2[ _ , indexList_, head_[ _, _ ] ] := {{},{}}


removeCells[ deletedCells : {___Integer}, head_[indices_, data_] ] :=
With[
{
	remainingCells = Complement[ Range[ Length[indices] ] , deletedCells ]
},
	If[ Length[remainingCells] == 0,
		{},
		head[
			indices[[remainingCells]],
			data[[All, remainingCells]]
		]
	]
]


takeCells[ remainingCells : {___Integer}, head_[indices_, data_] ] :=
	If[ Length[remainingCells] == 0,
		{},
		head[
			indices[[remainingCells]],
			data[[All, remainingCells]]
		]
	]


(*
ChangeCellData[ {c1,c2,...}, {head, dataType}, {data1, data2, ...}, cells ] replaces the old data (of type dataType) for cells indexed by c1, ... with data1, ...
ChangeCellData[ {c1,c2,...}, {head, dataType}, {data}, cells ] replaces the old data (of type dataType) for cells indexed by c1, ... all with data.
ChangeCellData[ {c1,c2,...}, {head, dataType}, dataFunc, cells ] replaces the old data (of type dataType) for cells indexed by c1, ... by applying dataFunc to the old data.
*)

changeCellDataInternal[ changingCells : {___Integer}, spec : {_ , _}, specifiedData_List, cells : packedCellPattern ] := 
	ChangeCellDataType[ changingCells, spec, specifiedData, # ]& /@ cells

ChangeCellDataType[ changingCells : {___Integer}, {headSpec_, dataSpec_}, specifiedData_ , headSpec_[ indices_ , data_ ] ] :=
With[ {
length = Length[indices],
newData = If[
	ListQ[specifiedData] && Length[specifiedData]==1,  ConstantArray[specifiedData,Length[changingCells]], 
	specifiedData
	],
	dataIndex = dataSpec /. Thread[dataForm[headSpec] -> Range[Length[dataForm[headSpec]]]]
},
	If[ IntegerQ[dataIndex],
		With[ { changedCells = Select[ changingCells, 0 < # <= length&] (*these are the cells within range*)},
			If[Length[changedCells]>0,
				headSpec[
					indices,
					ReplacePart[ data, dataIndex -> changeData[ 
							changedCells, 
							data[[ dataIndex ]], 
							(*if its a list, use it, if not map it*) If[ListQ[newData], newData, newData /@ data[[ dataIndex ]] ] 
							] ]
				],
				headSpec[ indices, data ]
			]
		],
		headSpec[indices,data]
	]
]

ChangeCellDataType[ changingCells : {___Integer}, {headSpec_, dataSpec_}, newData_ , spec : _[ _ , data_ ] ] := spec


changeData[ changedCells : {___Integer}, data_SparseArray, newdata_] :=
Module[ {
	processedData = newdata /. Automatic -> "Automatic",
	processedIndices = List /@ changedCells,
	newRules
},
	newRules = Thread[ processedIndices -> processedData ];
	SparseArray[ Join[ newRules , FilterRules[ArrayRules[ data ], Except[newRules]] ] , Length[ data ] ]
]

changeData[ changedCells : {___Integer}, data : {__List}, newdata_ ] :=
Module[ {
	processedData = ArrayRules[ "data" /@ data ],
	processedIndices = Thread[ (List /@ changedCells) -> MapThread[ "data"[mergeDataList[#1,#2]]&, {data[[ changedCells ]], newdata /. Automatic -> "Automatic"} ] ]
},
	Normal[ SparseArray[ Join[ processedIndices, processedData] , Length[data] ] ][[All,1]]
]

changeData[ changedCells : {___Integer}, data_List, newdata_ ] :=
With[ {
	processedData = ArrayRules[ "data" /@ data ],
	processedIndices = Thread[ (List /@ changedCells) -> MapThread[ "data"[#2]&, {data[[ changedCells ]], newdata /. Automatic -> "Automatic"} ] ]
},
	Normal[ SparseArray[ Join[ processedIndices, FilterRules[processedData, Except[processedIndices]]] , Length[data] ] ][[All,1]]
]



mergeDataList[ data1_ , data2_  ] := Last /@ GatherBy[ Flatten[{data1, data2} ], directiveQ ]


mapCellsInternal[ f_, cells : packedCellPattern ] := mapCellType[f, #]& /@ cells
mapCellType[ f_, celltype : _[_,_] ] := parseCells[ f /@ unparseCells[celltype] ][[2]]


outputCells[ pattern_, cells : packedCellPattern ] := outputCellType[pattern, #]& /@ cells

outputCellType[ _ , _String[ indices_, {} ] ] := Nothing
outputCellType[ _, head_[indices_,{}] ] := head[indices]
outputCellType[ None, head_[indices_, data_] ] := head[indices]
outputCellType[ pattern_, head_[indices_, data_] ] :=
With[
{
	dataRules = Transpose[ Thread /@ FilterRules[ Thread[ dataForm[head] -> data ], pattern] ] /. "Automatic"->Automatic
},
	head[indices] -> dataRules
]


replaceRules[ {}, appliedRules__ ] := {}

replaceRules[ rules_, appliedRules_ ] := replaceRules[ rules, appliedRules, 1 ]

replaceRules[ rules : {__Rule}, appliedRules_, 1 ] := 
With[ { flippedRules = Transpose[ List @@@ rules ] },
	Rule @@@ Transpose[ { Replace[ flippedRules[[1]] , appliedRules, {1} ], flippedRules[[2]] } ]
]

replaceRules[ rules : {__Rule}, appliedRules_, 2 ] := 
With[ { flippedRules = Transpose[ List @@@ rules ] },
	Rule @@@ Transpose[ { flippedRules[[1]], Replace[ flippedRules[[2]] , appliedRules, {1} ] } ]
]


removeOverlappingCoordinates[ head_[coordinateList_, mr_, cells_, labels_], None|0]:=
	head[
		ToPackedArray[coordinateList],
		mr,
		deleteDuplicateCells @ cells,
		labels
	]

removeOverlappingCoordinates[ head_[coordinateList_, mr_, cells_, labels_], precision : _?(NumericQ[N[#]] && N[#]>0 &) : 10^(-12)]:=
Module[
{
	numberedVertices=Transpose[{ Range @ Length @ coordinateList, coordinateList }],
	gatheredVertices,rules
},
	gatheredVertices=GatherBy[
		numberedVertices,
		(*they are the same if they are the same within the specified precision.*)
		Rationalize[N[#[[2]]], precision]&
	];
	rules=Dispatch @ Flatten[ (*two levels to thread through*)
		Thread /@ Thread[gatheredVertices[[All,All,1]] -> Range[Length[gatheredVertices]] ]
	];

	head[
		ToPackedArray[#[[1,2]]& /@ gatheredVertices],
		mr,
		deleteDuplicateCells @ replaceCellIndices[cells,rules],
		replaceRules[ labels, rules, 2 ]
	]
]


deleteDegenerateCellsFromMechanism[ head_[coordinates_, mr_, cells_, labels_] ] :=
	head[ coordinates, mr, deleteDegenerateCells[cells], DeleteDuplicates[Reverse[labels]] ]


missingVertices[ numberOfVertices_ , cells_ ] := With[ 
{
	freeJoints = Cases[ cells, _FreeJoint|_PinnedJoint ]
},
	If[ Length[ freeJoints ] == 0, 
		Range[numberOfVertices],
		Complement[ Range[numberOfVertices], Flatten[ freeJoints[[All,1]] ] ]
	]
]


(*choose your own dimension*)
addMeshRegion[ head_[ coordinates_?MatrixQ , _ , cells : packedCellPattern , labels_ ], Automatic, options_ ] :=
With[ {dim = chooseDimension[ head, coordinates ] },
	head[
		coordinates , 
		createMeshRegion[ head, PadRight[ coordinates, {Length[coordinates] , dim} ], cells , options, labels], 
		cells,
		labels
	]
]

addMeshRegion[ head_[ coordinates_?MatrixQ , _ , cells : packedCellPattern , labels_ ], dimSpec_, options_ ] :=
	head[
		coordinates ,
		createMeshRegion[ head, PadRight[ coordinates, {Length[coordinates], dimSpec} ], cells , options, labels], 
		cells, 
		labels
	]

chooseDimension[ Origami, coordinates_ ] := If[ coordinates[[All,3]] . coordinates[[All,3]] < 10^(-12), 2, 3 ] /; Length[ coordinates[[1]] ] == 3
chooseDimension[ head_, coordinates_ ] := Length[ coordinates[[1]] ]


stripTextWrapper[labels_] := ReplaceAll[ labels , Text[x_,___] :> x]

createMeshRegion[ head_, coordinates_?(MatrixQ[#,NumericQ]&) , cells : packedCellPattern, options_ , labels_ : {} ] := With[ {
 displayCells = toDisplayCells[head, #]& /@ cells
},
	MeshRegion[ coordinates , Flatten[ displayCells[[All,1]] ],
		Flatten @ Join[{
		MeshCellLabel ->  Join[(*Point[#[[2]]]->#[[1]]& /@ labels,*) stripTextWrapper[ Flatten[ displayCells[[All,4]] , 1 ] ]],
		MeshCellStyle ->  Flatten[ displayCells[[All,2]] , 1 ],
		MeshCellShapeFunction ->  Flatten[ displayCells[[All,3]] , 1 ],
		Method->{CoplanarityTolerance->100, DeleteDuplicateCoordinates->False},
		options
		}]
	]
]
createMeshRegion[ head_, coordinates_ , cells : packedCellPattern, options__ ] := {}


cellMeshPrimitive[ RigidBar|Spring|TorsionalFold, _ ] := Line
cellMeshPrimitive[ FreeJoint | PinnedJoint ,_] := Point
cellMeshPrimitive[ Face | ElasticTriangle ,_] := Polygon
cellMeshPrimitive[ _ ,_] := None

toDisplayCells[mechType_, head_[ indices_, data_ ] ]:=
Module[ {
dataLocations = PositionIndex[dataForm[head]] , 
primitive = cellMeshPrimitive[ head, mechType ], primitives
},
	If[ primitive =!= None,
		primitives = primitive /@ indices;
		{
			primitives,
			toMeshCellStyle[ mechType, head, primitives, data , dataLocations["Style"] ],
			toMeshCellShapeFunction[ mechType, head, primitives, data , dataLocations["Shape"] ],
			toMeshCellLabel[ mechType, head, primitives, data , dataLocations["Label"] ]
		},
			
		{{},{},{},{}}
	]
]


defaultMeshStyle[ RigidBar|TorsionalFold, _ ] := {CapForm["Round"],Black}
defaultMeshStyle[ Spring ,_] := {Black}
defaultMeshStyle[ FreeJoint | PinnedJoint ,_] := {Black}
defaultMeshStyle[ Face ,_] := {GrayLevel[0.85]}
defaultMeshStyle[ ElasticTriangle ,_] := {RGBColor[{0.85,0.85,0.95}]}
defaultMeshStyle[ _ ,_] := {}

toMeshCellStyle[ mechType_, head_, prims_, data_ , {index_}]:= With[
{
	styles = data[[ index ]],
	defaultStyle = defaultMeshStyle[ head, mechType ]
},
	DeleteCases[ MapThread[ #1 -> Directive@@Flatten[ {defaultStyle , #2} ]& , {prims, styles} ] , _ -> _?(Length[#]==0&) ]
]


toMeshCellLabel[ mechType_, head_, prims_, data_, {index_} ] := With[
{
	labels = data[[ index ]]
},
	DeleteCases[ Thread[prims -> labels] , _ -> None ]
]


defaultMeshShapeFunction[ Spring , _] := Mechanisms`graphics`SpringShape[]
defaultMeshShapeFunction[ PinnedJoint , Linkage] := Mechanisms`graphics`PinnedJointShape[]
defaultMeshShapeFunction[ _ ,_] := "Automatic"

toMeshCellShapeFunction[ mechType_, head_, prims_, data_, {index_} ] := With[
{
	labels = Normal[data[[ index ]]] /. "Automatic" -> defaultMeshShapeFunction[ head, mechType ]
},
	cleanupShapeFunction /@ DeleteCases[ Thread[prims -> labels] , _ -> "Automatic" ]
]

cleanupShapeFunction[ cell_ -> None ] := cell -> None
cleanupShapeFunction[ rule : _ -> _Function ] := rule
cleanupShapeFunction[ cell_ -> shape_ ] := cell -> (shape[#1,#2,#3]&)


Mechanism::meshfail="`1` has non-numeric coordinates.";


Format[f : Linkage[ coordinates_?(MatrixQ[#,NumericQ]&), mr_, cells : packedCellPattern, labels : labelPattern ] , StandardForm ] := 
	If[ MeshRegionQ[mr], mr, createMeshRegion[ Linkage, coordinates, cells, {} , labels] ]
Format[f : Linkage[ coordinates_, mr_, cells : packedCellPattern, labelPattern ] , StandardForm ] := 
	If[ MeshRegionQ[mr], mr , "Nondisplayable Linkage"[ "Number Vertices = "<>ToString[Length[coordinates]] ] ]

SetAttributes[Linkage,{NHoldAll}]


Options[Linkage] = Flatten[{OverlapPrecision -> 10^(-12) , Options[MeshRegion]}];

Linkage[ f : Linkage[ _?MatrixQ, _, packedCellPattern, labelPattern ] , opt : OptionsPattern[] ] :=
With[ {meshOptions = FilterRules[{opt}, Options[MeshRegion] ]},
	addMeshRegion[
		deleteDegenerateCellsFromMechanism @ removeOverlappingCoordinates[ f, OptionValue[OverlapPrecision] ],
		Automatic,
		meshOptions
	]
]

Linkage[ coordinateSpec_, cellSpec_ , opt : OptionsPattern[] ] :=
Module[
{
meshOptions = FilterRules[ {opt}, Options[MeshRegion] ],
intermediateParsing = parseCells[ {coordinateSpec, {}, {} }, {cellSpec} ],
numberOfCoordinates, coordinates, cells, labels
},
	numberOfCoordinates = Length[intermediateParsing[[1]]];
	{coordinates, cells, labels} = parseCells[ intermediateParsing, FreeJoint /@ missingVertices[numberOfCoordinates, intermediateParsing[[2]]] ];

	addMeshRegion[
		deleteDegenerateCellsFromMechanism[
			removeOverlappingCoordinates[
				Linkage[ coordinates, {}, cells, labels ],
				OptionValue[OverlapPrecision]
			]
		],
		Automatic,
		meshOptions
	]
] /; LinkagePrecisionQ[ OptionValue[ OverlapPrecision ] ]

Mechanism::precision = "Precision `1` must be a non-negative number or None.";
LinkagePrecisionQ[ None|0 ] := True
LinkagePrecisionQ[ prec_?(NumericQ[#]&&#>0 &) ] := True
LinkagePrecisionQ[ prec_ ] := (Message[Mechanism::precision, prec]; False)


Linkage[coordinates_?MatrixQ, mr : _MeshRegion|{} , packedCellPattern, labelPattern ]["Methods"] := 
	{
	"type",
	"positions", 
	"mesh", 
	"cells", 
	"labels",
	"labeledVertices", 
	"EmbeddingDimension", 
	"DisplayDimension",
	"VertexNumber",
	"edges",
	"faces"
	}

Linkage[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["type"] := Linkage
Linkage[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["positions"] := coordinates
Linkage[ coordinates_?MatrixQ, _, cells : packedCellPattern, labelPattern ]["cells"] := cells
Linkage[ coordinates_?MatrixQ, _, packedCellPattern, labels : labelPattern ]["labels"] := labels

(*can we get a mesh?*)
Linkage[ coordinates_?MatrixQ, mr_MeshRegion, packedCellPattern, labelPattern ]["mesh"] := mr
Linkage[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labels : labelPattern ]["mesh"] := 
With[{ mesh=createMeshRegion[ Linkage, coordinates, cells, {} ]},
	If[MeshRegionQ[mesh],mesh, Message[Mechanism::meshfail,Linkage]; $Failed ]
]

Linkage[ coordinates_?MatrixQ, _, packedCellPattern, labels : labelPattern ]["labeledVertices"] :=
	ReplacePart[ ConstantArray[None , Length[coordinates]], Reverse/@labels]

Linkage[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["EmbeddingDimension"] := Length[coordinates[[1]]]
Linkage[ coordinates_?MatrixQ, mr_MeshRegion, packedCellPattern, labelPattern ]["DisplayDimension"] := Last[Dimensions[MeshCoordinates[mr]]]
Linkage[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labels : labelPattern ]["DisplayDimension"] := Length[coordinates[[1]]]

Linkage[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["VertexNumber"] := Length[coordinates]

Linkage[ coordinates_?(MatrixQ[#,NumericQ]&), {}, cells : packedCellPattern, labelPattern ]["edges"] := ToPackedArray @ MeshCells[ createMeshRegion[ Linkage, PadRight[ coordinates, {Length[coordinates] , chooseDimension[ Linkage, coordinates ]} ], cells, {} ] ,1][[All,1]]
Linkage[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labelPattern ]["edges"] := ToPackedArray @ DeleteDuplicatesBy[ Flatten[Select[ cells[[All,1]], MatrixQ[#] && Last[Dimensions[#]]==2 & ],1], Sort ]
Linkage[ coordinates_?MatrixQ, m_MeshRegion, packedCellPattern, labelPattern ]["edges"] := ToPackedArray @ MeshCells[m,1][[All,1]]

Linkage[ coordinates_?(MatrixQ[#,NumericQ]&), {}, cells : packedCellPattern, labels : labelPattern ]["faces"] := ToPackedArray @ MeshCells[ createMeshRegion[ Linkage, coordinates, cells ,{}] ,2][[All,1]]
Linkage[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labels : labelPattern ]["faces"] := ToPackedArray @ DeleteDuplicatesBy[ Flatten[Cases[ cells, _Face|_ElasticFace ][[All,1]],1], Sort ]
Linkage[ coordinates_?MatrixQ, m_MeshRegion, packedCellPattern, labelPattern ]["faces"] := ToPackedArray @ MeshCells[m,2][[All,1]]


Format[f : Origami[ coordinates_?(MatrixQ[#,NumericQ]&), mr_, cells : packedCellPattern, labelPattern ] , StandardForm ] := 
	If[ MeshRegionQ[mr], mr, createMeshRegion[ Origami, PadRight[ coordinates, {Length[coordinates] , chooseDimension[ Origami, coordinates ]} ], cells, {} ] ]
Format[f : Origami[ coordinates_, mr_, cells : packedCellPattern, labelPattern ] , StandardForm ] := 
	If[ MeshRegionQ[mr], mr , "Nondisplayable Origami"[ "Number of vertices = "<>ToString[Length[coordinates]] ] ]

SetAttributes[Origami,{NHoldAll}]


Options[Origami] = Flatten[{OverlapPrecision -> 10^(-12) , Options[MeshRegion]}];

(***
This is a kludge to fix ?Origami which returns an error from PadRight otherwise for reasons that are not entirely clear.
***)
mypad[ c_, {n1_?(#>0&), n2_} ] := PadRight[c, {n1,n2}]
mypad[ c_, _ ] := c
(******)

Origami[ f : Origami[ _?MatrixQ, _, packedCellPattern, labelPattern ] , opt : OptionsPattern[] ] :=
With[ {meshOptions = FilterRules[{opt}, Options[MeshRegion] ]},
	addMeshRegion[
		deleteDegenerateCellsFromMechanism @ removeOverlappingCoordinates[ f, OptionValue[OverlapPrecision] ],
		Automatic,
		meshOptions
	]
]

Origami[ coordinateSpec_, cellSpec_ , opt : OptionsPattern[] ] :=
Module[
{
meshOptions = FilterRules[ {opt}, Options[MeshRegion] ],
intermediateParsing = parseCells[ {coordinateSpec, {}, {} }, {cellSpec} ],
numberOfCoordinates, coordinates, cells, labels
},
	numberOfCoordinates = Length[intermediateParsing[[1]]];
	{coordinates, cells, labels} = parseCells[ intermediateParsing, FreeJoint /@ missingVertices[numberOfCoordinates, intermediateParsing[[2]]] ];

	addMeshRegion[
		deleteDegenerateCellsFromMechanism[
			removeOverlappingCoordinates[
				Origami[ mypad[ coordinates , {Length[coordinates] , 3} ], {}, cells, labels ],
				OptionValue[OverlapPrecision]
			]
		],
		Automatic,
		meshOptions
	]
] /; LinkagePrecisionQ[ OptionValue[ OverlapPrecision ] ]


Origami[coordinates_?MatrixQ, mr : _MeshRegion|{}, packedCellPattern, labelPattern ]["Methods"] := 
	{
	"type",
	"positions", 
	"mesh", 
	"components", 
	"labels", "labeledVertices",
	"EmbeddingDimension", 
	"DisplayDimension",
	"VertexNumber",
	"edges",
	"faces"
	}

Origami[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["type"] := Origami

Origami[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["positions"] := coordinates

Origami[ coordinates_?MatrixQ, mr_MeshRegion, packedCellPattern, labelPattern ]["mesh"] := mr
Origami[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labels : labelPattern ]["mesh"] := 
With[ {mesh=createMeshRegion[ Origami, PadRight[ coordinates, {Length[coordinates] , chooseDimension[ Origami, coordinates ]} ], cells, {} ]},
	If[MeshRegionQ[mesh],mesh, Message[Mechanism::meshfail,Origami]; $Failed ]
]

Origami[ coordinates_?MatrixQ, _, cells : packedCellPattern, labelPattern ]["cells"] := cells

Origami[ coordinates_?MatrixQ, _, packedCellPattern, labels : labelPattern ]["labels"] := labels
Origami[ coordinates_?MatrixQ, _, packedCellPattern, labels : labelPattern ]["labeledVertices"] :=  ReplacePart[ ConstantArray[None , Length[coordinates]], Reverse/@labels]

Origami[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["EmbeddingDimension"] := Length[coordinates[[1]]]

Origami[ coordinates_?MatrixQ, mr_MeshRegion, packedCellPattern, labelPattern ]["DisplayDimension"] := Last[Dimensions[MeshCoordinates[mr]]]
Origami[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labels : labelPattern ]["DisplayDimension"] := Last[Dimensions[ MeshCoordinates[ createMeshRegion[ Origami, PadRight[ coordinates, {Length[coordinates] , chooseDimension[ Origami, coordinates ]} ], cells, {} ] ] ] ]

Origami[ coordinates_?MatrixQ, _, packedCellPattern, labelPattern ]["VertexNumber"] := Length[coordinates]

Origami[ coordinates_?(MatrixQ[#,NumericQ]&), {}, cells : packedCellPattern, labelPattern ]["edges"] := ToPackedArray @ MeshCells[ createMeshRegion[ Origami, PadRight[ coordinates, {Length[coordinates] , chooseDimension[ Origami, coordinates ]} ], cells, {} ] ,1][[All,1]]
Origami[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labelPattern ]["edges"] := ToPackedArray @ DeleteDuplicatesBy[ Flatten[Select[ cells[[All,1]], MatrixQ[#] && Last[Dimensions[#]]==2 & ],1], Sort ]
Origami[ coordinates_?MatrixQ, m_MeshRegion, packedCellPattern, labelPattern ]["edges"] := ToPackedArray @ MeshCells[m,1][[All,1]]

Origami[ coordinates_?(MatrixQ[#,NumericQ]&), {}, cells : packedCellPattern, labels : labelPattern ]["faces"] := ToPackedArray @ MeshCells[ createMeshRegion[ Origami, PadRight[ coordinates, {Length[coordinates] , chooseDimension[ Origami, coordinates ]} ], cells, {} ] ,2][[All,1]]
Origami[ coordinates_?MatrixQ, {}, cells : packedCellPattern, labels : labelPattern ]["faces"] := ToPackedArray @ DeleteDuplicatesBy[ Flatten[Cases[ cells, _Face|_ElasticFace ][[All,1]],1], Sort ]
Origami[ coordinates_?MatrixQ, m_MeshRegion, packedCellPattern, labelPattern ]["faces"] := ToPackedArray @ MeshCells[m,2][[All,1]]


dataSpecPattern = Blank[]|Alternatives[__String]|_String;

MechanismCellList[ m : mechanismPattern ] := outputCells[ _ , m[[3]] ]

MechanismCellList[ m : mechanismPattern, indexSpec_ : _ , dataSpec : _Rule : None, outputSpec_ : _ ] := 
	outputCells[  outputSpec , filterByData[m , dataSpec ] @ filterByCells[ m, indexSpec] @ m[[3]] ]

filterByCells[ m_ , {head_ , indices_List}][cells_] := Flatten[takeCells[ identifyCellFromList[ head, indices, # ], # ] & /@ cells]
filterByCells[ m_ , patt : Except[_List] ][cells_] := With[
{ pattern = cellPattern[patt] },
	Flatten[takeCells[ casesCellType[ pattern, # ], # ] & /@ cells]
]

filterByData[ m_ , None ][cells_] := cells
filterByData[ m_, dataSpec_ -> f_Function ][cells_] := Flatten[takeCells[ selectCellFromDataType[ dataSpec -> f, # ], # ]& /@ cells]
filterByData[ m_, dataSpec_ -> patt_ ][ cells_ ] := Flatten[ takeCells[ casesCellFromDataType[ dataSpec -> patt , # ], # ] & /@ cells ]


MechanismPositions[ m : mechanismPattern ] := m[[1]]
(*a quiet version*)
MechanismPositions[ m : mechanismPattern, coords_?MatrixQ ] := Head[m][coords,m[[2]],m[[3]],m[[4]]] /; Length[coords]==m["VertexNumber"]
(*actually rework the way it looks*)
MechanismPositions[ Rule[ m : mechanismPattern, coords_?MatrixQ ] ]  :=
	With[{tmp=Head[m][coords, {},m[[3]],m[[4]] ]},
		Head[m][coords,tmp["mesh"],m[[3]],m[[4]]]
	] /; Length[ coords ] == m["VertexNumber"]

MechanismPositions::coords = "Coordinates should have the same number of vertices as the mechanism.";
MechanismPositions[ Rule[ m : mechanismPattern, _ ] ] := "nothing" /; Message[ MechanismPositions::coords ]
MechanismPositions[ m : mechanismPattern, _ ] := "nothing" /; Message[ MechanismPositions::coords ]


MechanismEdges[ m : mechanismPattern ] := m["edges"]
MechanismFaces[ m : mechanismPattern ] := m["faces"]
MechanismVertices[ m : mechanismPattern ] := Range[ m["VertexNumber"] ]

MechanismEdges[ m : mechanismPattern, edgeList : {{_Integer,_Integer}...} ] :=
With[{indexedEdges=PositionIndex[Sort/@m["edges"]], sortedEdges = Sort/@edgeList}, 
	ToPackedArray @ Flatten[indexedEdges /@ sortedEdges]
]

MechanismFaces[ m : mechanismPattern, faceList : {{__Integer}...} ] :=
With[ {indexedFaces = PositionIndex[Sort/@m["faces"]], sortedFaces = Sort /@ faceList},
	ToPackedArray @ Flatten[ indexedFaces /@ sortedFaces , 1 ]
]


MechanismCells[ m : mechanismPattern ] := Sort[Head /@ m[[3]]]
MechanismCells[ m : mechanismPattern , s : _String ] := FirstCase[ m[[3]], s[__] ][[1]]
MechanismCells[ m : mechanismPattern , s : Alternatives[ __String ] ] := Cases[ m[[3]], s[__] ][[All,1]]

MechanismCells[ m : mechanismPattern , s : celltypePattern ] := FirstCase[ m[[3]], s[__] ][[1]]
MechanismCells[ m : mechanismPattern , s : celltypePattern.. ] := Head[#][#[[1]]]& /@ Cases[ m[[3]], (Alternatives@@{s})[__] ]


MechanismEdgeQ[m : mechanismPattern , edgelist_ ] := AllTrue[{MatrixQ[edgelist], Length[edgelist[[1]]]==2,ContainsAll[ Sort/@m["edges"],Sort/@edgelist]},TrueQ]
MechanismFaceQ[m : mechanismPattern , facelist_ ] := AllTrue[{ListQ[facelist],ContainsAll[ Sort/@m["faces"],Sort/@facelist ]},TrueQ]

ValidMechanismEdgeQ[m : mechanismPattern , edgelist_ ] := AllTrue[{MatrixQ[edgelist], Length[edgelist[[1]]]==2,Min[edgelist]>0,Max[edgelist]<=m["VertexNumber"]},TrueQ]
ValidMechanismEdgeQ[pos_?MatrixQ , edgelist_ ] := AllTrue[{MatrixQ[edgelist], Length[edgelist[[1]]]==2,Min[edgelist]>0,Max[edgelist]<=Length[pos]},TrueQ]

ValidMechanismFaceQ[m : mechanismPattern , facelist_ ] := AllTrue[{ ListQ[facelist], Min[facelist]>0,Max[facelist]<=m["VertexNumber"]},TrueQ]
ValidMechanismFaceQ[pos_?MatrixQ , facelist_ ] := AllTrue[{ListQ[facelist],Min[facelist]>0,Max[facelist]<=Length[pos]},TrueQ]


LabelCells[ s : _String , indexList_ ][ m : mechanismPattern ] := Head[m][ m[[1]], m[[2]], joinCells[ {packCells[ {s[indexList, {} ]} ], m[[3]] } ], m[[4]] ]
LabelCells[ m : mechanismPattern, s : _String , indexList_ ] := LabelCells[s,indexList][m]


LabelVertices[ "Name" ][m : mechanismPattern ] := ChangeCellData[ AnyJoint, Range[m["VertexNumber"]], "Label" -> m["labeledVertices"] ]@m
LabelVertices[ m : mechanismPattern , "Name" ] := ChangeCellData[ m, AnyJoint, Range[m["VertexNumber"]], "Label" -> m["labeledVertices"] ]
LabelVertices[ "Index" ][ m : mechanismPattern ] := ChangeCellData[_AnyJoint, "Label" -> "Index"]@m
LabelVertices[ m : mechanismPattern , "Index" ] := ChangeCellData[m,_AnyJoint, "Label" -> "Index"]
LabelVertices[ None ][ m : mechanismPattern ] := ChangeCellData[_AnyJoint, "Label" -> None]@m
LabelVertices[ m : mechanismPattern , None ] := ChangeCellData[m,_AnyJoint, "Label" -> None]

LabelVertices[ ruleList : {__Rule} ][m : mechanismPattern ] := ChangeCellData[AnyJoint, ruleList[[All,1]] /. m[[4]], "Label" -> ruleList[[All,2]]] @ m
LabelVertices[ m : mechanismPattern, ruleList : {__Rule} ] := ChangeCellData[m,AnyJoint, ruleList[[All,1]] /. m[[4]], "Label" -> ruleList[[All,2]]]

LabelVertices[ lst_List ][ m : mechanismPattern ] := ChangeCellData[AnyJoint, Range[m["VertexNumber"]], "Label"->lst]@m /; Length[lst]==m["VertexNumber"]
LabelVertices[ m : mechanismPattern, lst_List ] := ChangeCellData[m,AnyJoint, Range[m["VertexNumber"]], "Label"->lst] /; Length[lst]==m["VertexNumber"]
LabelVertices[ lst_List ][ m : mechanismPattern ] := "nothing" /; Message[LabelVertices::length, m["VertexNumber"]]
LabelVertices[ m : mechanismPattern , lst_List ] := "nothing" /; Message[LabelVertices::length, m["VertexNumber"]]

LabelVertices[ _ ][ m : mechanismPattern ] := "nothing" /; Message[LabelVertices::bad]
LabelVertices[ m : mechanismPattern , _ ] := "nothing" /; Message[LabelVertices::bad]

LabelVertices::length="Length of labels does not match number of vertices, `1`.";
LabelVertices::bad="First argument should be a label, list of labels, or list of rules assigning labels to a vertex.";


Options[JoinMechanism] = {OverlapPrecision -> 10^(-12)};

JoinMechanism[ meshes : __?MechanismQ|{__?MechanismQ}, opt : OptionsPattern[] ] :=
Module[{n, m = Flatten[{meshes}], meshOptions = FilterRules[{opt}, Options[MeshRegion]] },
	With[{ num = Drop[ Flatten[{{1}, #+1& /@ Accumulate[ Length /@ m[[All,1]] ] }, 1 ], -1] },
		addMeshRegion[
			deleteDegenerateCellsFromMechanism[
				removeOverlappingCoordinates[
					Head[m[[1]]][
						Flatten[m[[All,1]],1],
						{},
						joinCells[ Flatten @ MapThread[ replaceCellIndices[#1, n_Integer -> n + #2 - 1]&, { m[[All,3]], num }] ],
						Flatten @ MapThread[ replaceRules[ #1, n_Integer -> n + #2 - 1, 2 ]&, { m[[All,4]], num } ]
					],
					OptionValue[OverlapPrecision]
				]
			],
			Automatic,
			meshOptions
		]
	] /; LinkagePrecisionQ[ OptionValue[OverlapPrecision] ] && sameHeadsQ[ m ]
]

Mechanism::comb = "Cannot combine Mechanisms of different types.";
sameHeadsQ[ expr_List ] := If[ SameQ[ Head /@ expr ], True, Message[Mechanism::comb]; False ]


SelectCells[ patt_ ][ m : mechanismPattern ] := 
With[ { pattern = cellPattern[ patt ]},
	Head[m][
		m[[1]],
		{},
		Flatten[takeCells[ casesCellType[ pattern, #], # ]& /@ m[[3]]],
		m[[4]]
	]
]

SelectCells[ head_ , cellList_List ][ m : mechanismPattern ] :=
	Head[m][
		m[[1]],
		{},
		Flatten[takeCells[ identifyCellFromList[ head, cellList, #], # ]& /@ m[[3]]],
		m[[4]]
	]

SelectCells[m : mechanismPattern, patt_ ] := addMeshRegion[ SelectCells[patt][m], Automatic , {} ]
SelectCells[m : mechanismPattern, head_, cellList_ ] := addMeshRegion[ SelectCells[head, cellList][m], Automatic , {} ]


DeleteCells[ pattList_List ][ m : mechanismPattern ] := Fold[DeleteCells, m , pattList ]

DeleteCells[ patt : Except[ _List ]][ m : mechanismPattern ] := 
With[ { pattern = cellPattern[ patt ]},
	Head[m][
		m[[1]],
		{},
		Flatten[removeCells[ casesCellType[ pattern, #], # ]& /@ m[[3]]],
		m[[4]]
	]
]

DeleteCells[ head_ , cellList_List ][ m : mechanismPattern ] :=
	Head[m][
		m[[1]],
		{},
		Flatten[removeCells[ identifyCellFromList[ head, cellList, #], # ]& /@ m[[3]]],
		m[[4]]
	]

DeleteCells[m : mechanismPattern, patt_ ] := addMeshRegion[ DeleteCells[patt][m], Automatic , {} ]
DeleteCells[m : mechanismPattern, head_, cellList_ ] := addMeshRegion[ DeleteCells[head, cellList][m], Automatic , {} ]


Options[ AddCells] = Options[Linkage];

AddCells[ cells_ , opt : OptionsPattern[] ][ m : mechanismPattern ] := With[
{ parsed = parseCells[ { m[[1]], m[[3]], m[[4]] } , Flatten[{cells}]] },
	deleteDegenerateCellsFromMechanism[ removeOverlappingCoordinates[
		Head[m][parsed[[1]], {}, parsed[[2]], parsed[[3]]],
		OptionValue[OverlapPrecision]
	] ]
] /; LinkagePrecisionQ[ OptionValue[OverlapPrecision] ]

AddCells[ m : mechanismPattern, cells_, opt : OptionsPattern[] ] := addMeshRegion[ AddCells[ cells, opt ][m], Automatic, FilterRules[ {opt}, Options[MeshRegion]]]


DeleteVertices[ vertices_?VectorQ ][m : mechanismPattern] :=
With[ { pattern = Alternatives @@ ( {___,#,___}|#& /@ vertices ) },
With[ { remainingCells = Flatten[ removeCells[ casesCellType[ {Head[#][pattern]} , # ], # ]& /@ m[[3]] ] },
With[ { remainingVertices = Complement[ Range[Length[m[[1]]]], vertices /. m[[4]] ] },
With[ { remappingRules = Thread[ remainingVertices -> Range[Length[remainingVertices]]] },
	Head[m][
		m[[1, remainingVertices]],
		{},
		replaceCellIndices[ remainingCells, remappingRules ],
		replaceRules[ m[[4]], remappingRules, 2 ]
	]
]]]]


DeleteVertices[ m : mechanismPattern, vertices_ ] := addMeshRegion[ DeleteVertices[vertices][m], Automatic , {}]


ChangeCellData[ patt_, {} ][ m : mechanismPattern ] := m
ChangeCellData[ patt_, {dataRule : _Rule} ][ m : mechanismPattern ] := ChangeCellData[ patt, dataRule ][m]
ChangeCellData[ patt_ , dataRules : {__Rule} ][ m : mechanismPattern ] := ChangeCellData[ patt, Rest[dataRules] ][ ChangeCellData[patt, First[dataRules] ][m] ]

ChangeCellData[ patt_ , dataSpec_ -> data_ ][ m : mechanismPattern ] :=
With[ { pattern = cellPattern[patt] },
	Head[m][
		m[[1]],
		{},
		ChangeCellDataType[ casesCellType[ pattern, # ] , {Head[#], dataSpec} , {data}, # ] & /@ m[[3]] ,
		m[[4]]
	]
]

ChangeCellData[ headSpec_Symbol , cellList_List, Rule[ dataSpec_ , data : Except[_List] ] ][ m : mechanismPattern ] :=
	ChangeCellData[ headSpec, cellList, dataSpec -> ConstantArray[ data, Length[cellList] ]][m]

ChangeCellData[ headSpec : RigidBar|Spring|AngleJoint|TorsionalFold , cellList_?MatrixQ, dataSpec_ -> data_List ][ m : mechanismPattern ] :=
Module[{ cells , specifiedCells},
	Head[m][
		m[[1]],
		{},
		({specifiedCells,cells}=identifyCellFromList2[ headSpec , cellList /. Dispatch[m[[4]]], #];
		ChangeCellDataType[ cells , {Head[#], dataSpec}, data[[specifiedCells]] , # ]) & /@ m[[3]],
		m[[4]]
	] /; Length[data] == Length[cellList]
]

ChangeCellData[ headSpec : Face , cellList_List , dataSpec_ -> data_List ][ m : mechanismPattern ] :=
Module[{ cells , specifiedCells},
	Head[m][
		m[[1]],
		{},
		({specifiedCells,cells}=identifyCellFromList2[ headSpec , cellList /. Dispatch[m[[4]]], #];
		ChangeCellDataType[ cells , {Head[#], dataSpec}, data[[specifiedCells]] , # ]) & /@ m[[3]],
		m[[4]]
	] /; Length[data] == Length[cellList]
]

ChangeCellData[ headSpecTmp : AnyJoint|FreeJoint|PinnedJoint , cellList_?VectorQ, dataSpec_ -> data_List ][ m : mechanismPattern ] :=
With[{headSpec=If[headSpecTmp===AnyJoint,FreeJoint|PinnedJoint,headSpecTmp]},
Module[{ cells , specifiedCells},
	Head[m][
		m[[1]],
		{},
		({specifiedCells,cells}=identifyCellFromList2[ headSpec , cellList /. Dispatch[m[[4]]], #];
		ChangeCellDataType[ cells , {Head[#], dataSpec}, data[[specifiedCells]] , # ]) & /@ m[[3]],
		m[[4]]
	] /; Length[data] == Length[cellList]
]]

ChangeCellData[ cellSpec: AnyJoint|FreeJoint|PinnedJoint, cellList_?(Not[VectorQ[#]]&), _][m : mechanismPattern ] := ( Message[ChangeCellData::FreeJoint, cellSpec]; m)
ChangeCellData[ Face, _, _ ][ m : mechanismPattern ] := (Message[ChangeCellData::cell, Face]; m)
ChangeCellData[ cellSpec: RigidBar|Spring|AngleJoint|TorsionalFold, cellList_?(Not[MatrixQ[#]]&), _][m : mechanismPattern ] := ( Message[ChangeCellData::cell, cellSpec]; m)
ChangeCellData[ headSpec_Symbol, cellList_List, dataSpec_ -> data_List ][ m : mechanismPattern ] := ( Message[ChangeCellData::head, headSpec]; m ) /; Length[data]==Length[cellList]
ChangeCellData[ headSpec_Symbol , cellList_List, dataSpec_ -> data_List ][ m : mechanismPattern ] := ( Message[ChangeCellData::data]; m )

ChangeCellData::data = "Data list must be the same length as the number of cells.";
ChangeCellData::FreeJoint= "Cells of type `1` are specified as a single list.";
ChangeCellData::cell = "Cells of type `1` are specified as a list of lists.";
ChangeCellData::head= "Bad cell type, `1`.";


ChangeCellData[ m : mechanismPattern, patt_, ruleList : {___Rule}|_Rule ] := addMeshRegion[ ChangeCellData[patt,ruleList][m], Automatic, {} ]
ChangeCellData[ m : mechanismPattern, headSpec_, cellList_, dataSpec_ -> data_ ] := addMeshRegion[ ChangeCellData[headSpec, cellList, dataSpec -> data][m], Automatic, {} ]


PlaceVertices[ moveSpec : {___Rule}|_Rule ][ m : mechanismPattern ] :=
With[ { vertexMoves = Transpose[ List @@@ Flatten[{moveSpec}] ] },
	Head[m][
		newVertexPositions[ m[[1]], vertexMoves[[1]] /. m[[4]] , vertexMoves[[2]] ],
		{},
		m[[3]],
		m[[4]]
	]
]

PlaceVertices[ vertices_?(MatrixQ[#,NumericQ]&) ][m : mechanismPattern ] := Head[m][ vertices, {}, m[[3]], m[[4]] ] /; Dimensions[vertices] == Dimensions[m["positions"]]
PlaceVertices[ vertices_?(MatrixQ[#,NumericQ]&) ][m : mechanismPattern ] := (
	If[ Length[vertices] != m["VertexNumber"], Message[PlaceVertices::num] ];
	If[ Last[Dimensions[vertices]] != Last[Dimensions[m["positions"]]], Message[ PlaceVertices::pos ] ];
	m
)

PlaceVertices::num = "Number of vertices is incorrect.";
PlaceVertices::vert = "Some vertices are not valid.";
PlaceVertices::pos = "All positions are not of the same dimension as coordinates.";

newVertexPositions[ coordinates_, vertices_?(VectorQ[#,IntegerQ]&), pos_?(MatrixQ[#, NumericQ]&) ] :=
	ReplacePart[ coordinates, Thread[ vertices -> pos ] ] /; Last@Dimensions[coordinates] == Last@Dimensions[pos]
newVertexPositions[ coordinates_, _, pos_?(MatrixQ[#, NumericQ]&)] := (Message[PlaceVertices::vert]; coordinates )
newVertexPositions[ coordinates_, _ , _ ] := (Message[PlaceVertices::pos]; coordinates )


DisplaceVertices[ moveSpec : {___Rule}|_Rule ][ m : mechanismPattern ] :=
With[ { vertexMoves = Transpose[ List @@@ Flatten[{moveSpec}] ] },
	Head[m][
		moveVertexPositions[ m[[1]], vertexMoves[[1]] /. m[[4]], vertexMoves[[2]] ],
		{},
		m[[3]],
		m[[4]]
	]
]

DisplaceVertices[ vertices_?(MatrixQ[#,NumericQ]&) ][m : mechanismPattern ] := 
	Head[m][ m["positions"] + vertices, {}, m[[3]], m[[4]] ] /; Dimensions[vertices] == Dimensions[m["positions"]]
DisplaceVertices[ vertices_?(MatrixQ[#,NumericQ]&) ][m : mechanismPattern ] := (
	If[ Length[vertices] != m["VertexNumber"], Message[DisplaceVertices::num] ];
	If[ Last[Dimensions[vertices]] != Last[Dimensions[m["positions"]]], Message[ DisplaceVertices::pos ] ];
	m
)

DisplaceVertices::num = "Number of vertices is incorrect.";
DisplaceVertices::vert = "Some vertices are not valid.";
DisplaceVertices::pos = "All positions are not of the same dimension as coordinates.";

moveVertexPositions[ coordinates_, vertices_?( VectorQ[ #, IntegerQ ] & ) , pos_?( MatrixQ[ #, NumericQ ] & ) ] :=
	ReplacePart[ coordinates, Thread[ vertices -> (coordinates[[vertices]] + pos) ] ] /; Last[Dimensions[coordinates]] == Last[Dimensions[pos]]
moveVertexPositions[ coordinates_, vertices_, pos_ ] := (
	If[ Not[VectorQ[vertices, IntegerQ ]], Message[ DisplaceVertices::vert ] ];
	If[ Not[ MatrixQ[pos, NumericQ] && Last[Dimensions[coordinates]] == Last[Dimensions[pos]] ], Message[ DisplaceVertices::pos ] ];
	coordinates
)


PlaceVertices[m : mechanismPattern, moveSpec_ ] := addMeshRegion[ PlaceVertices[moveSpec][m], Automatic, {} ]
DisplaceVertices[m : mechanismPattern, moveSpec_ ] := addMeshRegion[ DisplaceVertices[moveSpec][m], Automatic, {} ]


Linkage /: Map[ f_, m : Linkage[ coordinates_, _, cells : packedCellPattern,  labels_ ] ] := With[ { newCoordinates = f /@ coordinates},
	Linkage[ newCoordinates, {}, cells, labels ] /; mapTestCoordinatesQ[ newCoordinates, coordinates ]
]

Origami /: Map[ f_, m : Origami[ coordinates_, _, cells : packedCellPattern, labels_ ] ] := With[ { newCoordinates = f /@ coordinates},
	Origami[ newCoordinates, {}, cells, labels ] /; mapTestCoordinatesQ[ newCoordinates, coordinates ]
]

Mechanism::match ="New coordinates are not numeric and of the same dimension as old coordinates.";
mapTestCoordinatesQ[ new_, old_ ] := If[ MatrixQ[ new, NumericQ ] && Dimensions[new] == Dimensions[old], True, Message[Mechanism::match]; False ]


MapCells[ f_ ][ m : mechanismPattern ] := 
Module[
{
	cells = unparseCells[m[[3]],_],
	newCells
},
	newCells = parseCells[ {m[[1]], {}, m[[4]]} , f /@ cells ];
	Head[m][ newCells[[1]], {}, newCells[[2]], newCells[[3]] ]
]
MapCells[ m : mechanismPattern , f_ ] := addMeshRegion[ MapCells[f][m], Automatic, {} ]


Options[TesselateMechanism]={OverlapPrecision->10^(-6)};


TesselateMechanism::counter="Number of cells must be positive integers corresponding to the number of basis elements.";
TesselateMechanism::dim="Basis must be `1` dimensional to match Mechanism.";
TesselateMechanism::num="Basis must be numerical with vectors of the same dimension.";
TesselateMechanism::basis="Basis must be a list of `1` numerical vectors.";

tesselationBasisQ[ m_, d_, basis_ ] := Which[
	Not[MatrixQ[basis,NumericQ]], Message[ TesselateMechanism::num]; False,
	Length[basis] != d, Message[ TesselateMechanism::basis, d ]; False,
	Last[Dimensions[basis]] != m["EmbeddingDimension"], Message[ TesselateMechanism::dim, m["EmbeddingDimension"] ]; False,
	True, True
]

tesselationCellCountsQ[ n1__Integer?(#>0&) ] := True
tesselationCellCountsQ[ __ ] := (Message[TesselateMechanism::counter]; False)


translateLabels[ n_, {} ] := {}
translateLabels[n_, ruleList : {Rule[ _, _ ]..} ]:= Module[ {labels, indices} ,  
	{labels, indices} = Transpose[ List @@@ ruleList ];
	Rule @@@ Transpose[ { Through[ labels[n] ] , indices } ]
]

translateCoordinates[ positions_, translationVector_ ] := positions + ConstantArray[translationVector, Length[positions]]


TesselateMechanism[m : mechanismPattern, basis_, n1 : Except[_List], opt : OptionsPattern[] ]:=
Module[
{
	positions = m["positions"],
	unitCellIndices = Range[n1], 
	newCoordinates, translatedMechanisms
},
	newCoordinates = translateCoordinates[ positions, basis # ]& /@ unitCellIndices;
	translatedMechanisms = MapThread[ Head[m][ #1, {}, m[[3]], translateLabels[#2, m[[4]] ] ]&, {newCoordinates, unitCellIndices} ];
	
	JoinMechanism[ translatedMechanisms, opt ]

] /; tesselationBasisQ[m, 1, {basis}] && tesselationCellCountsQ[n1,1]


TesselateMechanism[ m : mechanismPattern, basis_, {n1_, n2_}, opt : OptionsPattern[] ] :=
Module[
{
	positions = m["positions"],
	unitCellIndices = Flatten[Array[{#1,#2}&,{n1,n2}],1],
	newCoordinates, translatedMechanisms
},
	newCoordinates = translateCoordinates[ positions, # . basis ]& /@ unitCellIndices;
	translatedMechanisms = MapThread[ Head[m][#1, {}, m[[3]], translateLabels[#2, m[[4]] ]]&, {newCoordinates, unitCellIndices} ];
	
	JoinMechanism[ translatedMechanisms, opt ]
] /; tesselationBasisQ[m, 2, basis] && tesselationCellCountsQ[n1,n2]


TesselateMechanism[ m : mechanismPattern, basis_, {n1_, n2_, n3_}, opt : OptionsPattern[] ] :=
Module[
{
	positions = m["positions"],
	unitCellIndices = Flatten[Array[{#1,#2,#3}&,{n1,n2,n3}],2],
	newCoordinates, translatedMechanisms
},
	newCoordinates = translateCoordinates[ positions, # . basis ]& /@ unitCellIndices;
	translatedMechanisms = MapThread[ Head[m][#1, {}, m[[3]], translateLabels[#2, m[[4]] ]]&, {newCoordinates, unitCellIndices} ];
	
	JoinMechanism[ translatedMechanisms, opt ]
] /; tesselationBasisQ[m, 3, basis] && tesselationCellCountsQ[n1,n2,n3]


replaceElement[ f : mechanismPattern, rule_ ] := Head[f][ ReplaceAll[f[[1]],rule] ,f[[2]], replaceCellData[ f[[3]], rule ],f[[4]]]

ReplaceAll[ f_Linkage, rule_ ] ^:= replaceElement[ f, rule ]
ReplaceAll[ o_Origami, rule_] ^:= replaceElement[o, rule]


Precision[ f_Linkage ] ^:= Precision[ f["positions"] ]
Precision[ o_Origami ] ^:= Precision[ o["positions"] ]


Rationalize[ f_Linkage, dx___ ] ^:= ReplacePart[ f, 1 -> ToPackedArray[Rationalize[ f[[1]], dx ]] ]
Rationalize[ o_Origami, dx___ ] ^:= ReplacePart[ o, 1 -> ToPackedArray[Rationalize[ o[[1]], dx ]] ]


N[ f_Linkage, dx___ ] ^:= ReplacePart[ f, 1 -> ToPackedArray[N[ f[[1]], dx ]] ]
N[ o_Origami, dx___ ] ^:= ReplacePart[ o, 1 -> ToPackedArray[N[ o[[1]], dx ]] ]


SubdivideCells[ indices_?VectorQ , label_ : Unique[] ][ m : mechanismPattern ] :=
With[ {tmp = SubdivideCells[ {indices},label][m]},
	Head[tmp][ tmp[[1]], tmp[[2]],tmp[[3]], replaceRules[ tmp[[4]], {label[indices] -> label}] ]
]

SubdivideCells[ indices : {{_,__}...} , label_ : Unique[] ][ m : mechanismPattern ] := 
Module[
{
	cells = MechanismCellList[ m, {_ , indices} , _ ]
},
	AddCells[ SubdivideCellsInternal[ m[[1]], # , label] & /@ cells ] @ DeleteCells[ deletedCells[cells[[All,1]]] ] @ m
]

deletedCells[ cells_ ] := Flatten[patternTheCells /@ cells]
patternTheCells[ head_[ indices : {{_,__}..} ] ] := head /@ indices
patternTheCells[ fine : _[ {_,__} ] ] := fine

SubdivideCells[ m : mechanismPattern, indices_, label_ : Unique[] ] := addMeshRegion[SubdivideCells[indices, label][m], Automatic, {} ]

SubdivideCellsInternal[ coordinates_ , Rule[ head_[indices : {{_,_}...} ], data_ ] , label_] := With[
{ newVertices = If[Length[indices]>1,label/@indices,{label}], newVertexCoordinates = Mean[coordinates[[#]]]&/@indices},
	{
		MapThread[FreeJoint[ #1 -> #2 ]& , {newVertices, newVertexCoordinates} ],
		(*new edges*)
		MapThread[ head[ {{#1[[1]],#2},{#2, #1[[2]]}}, #3]&, { indices, newVertices, data } ]
	}
]

SubdivideCellsInternal[ coordinates_, Rule[ head_[ indices : {{_,_,__}...} ], data_ ] , label_] := With[
{
	newVertices = label/@indices, newVertexCoordinates = Mean[ coordinates[[#]] ]&/@indices
},
	{
		MapThread[ FreeJoint[ #1 -> #2 ]&, {newVertices, newVertexCoordinates} ],
		(*new Faces*)
		MapThread[ head[ splitFace[ #1, #2 ] , #3]&  , { indices , newVertices , data } ]
	}
]
splitFace[ indices_ , newVertex_ ] := {newVertex, #[[1]], #[[2]] } & /@ Partition[ indices , 2, 1, 1]


DivideFaces[ edge : {_,_} ][m : mechanismPattern ] := DivideFaces[ {edge} ][m]
DivideFaces[ edgeList_?MatrixQ ][m : mechanismPattern ] := 
Module[
{
	cells = unparseCells[ m[[3]] , Face],
	FacelessM = DeleteCells[_Face] @ m,
	newCells
},
	newCells = parseCells[ {FacelessM[[1]], FacelessM[[3]], FacelessM[[4]]}, Fold[ (cutFace[#2] /@ #1) &, cells, edgeList /. m[[4]] ] ];
	Head[m][ newCells[[1]], {}, newCells[[2]], newCells[[3]] ]
]
DivideFaces[ m : mechanismPattern, edgeList_ ] := addMeshRegion[ DivideFaces[edgeList][m], Automatic, {} ]

cutFace[ {v1_,v2_}][Face[{a___,v1_,b__,v2_,c___}, prop_] ] := {Face[{v1,b,v2},prop],Face[{v2,c,a,v1},prop]}
cutFace[ {v2_,v1_}][Face[{a___,v1_,b__,v2_,c___}, prop_] ] := {Face[{v1,b,v2},prop],Face[{v2,c,a,v1},prop]}
cutFace[ _ ][f_] := f


TriangulateFaces[ m : mechanismPattern ] :=
Module[
{
	cells = unparseCells[ m[[3]] , Face],
	newm = DeleteCells[ _Face ] @ m,
	coordinates = m[[1]],
	newcells
},
	newcells = parseCells[ {newm[[1]],newm[[3]],newm[[4]]}, decomposeFace[ coordinates, # ]& /@ cells ];
	addMeshRegion[ Head[m][newcells[[1]], {}, newcells[[2]], newcells[[3]]], Automatic, {} ]
]


decomposeFace[ coordinates_, Face[{i_,j_,k_}, prop_] ] := Face[{i,j,k}, prop]
decomposeFace[ coordinates_, Face[ i_ , prop_] ] := With[{diag = findShortestDiagonal[ coordinates, i ]},
	{TorsionalFold[diag],decomposeFace[ coordinates, # ] & /@ cutFace[diag][Face[i, prop]]}
]
decomposeFace[ coordinates_, other_ ] := other

findShortestDiagonal[ coordinates_, vertices : {__Integer} ] :=
With[ {diagonals = polygonDiagonals[ vertices ]},
	MinimalBy[ Transpose[ { diagonals , Mechanisms`geometry`DisplacementLength[ coordinates, diagonals ] } ] , N[Last[#]]& ][[1,1]]
]
polygonDiagonals[ vertices_ ] := With[ {guesses = Tuples[ Range[Length[vertices]], 2 ], l = Length[vertices]},
	DeleteDuplicatesBy[ Map[ vertices[[#]]&, Select[ guesses, Between[ Abs[ #[[1]] - #[[2]] ],  {2, Length[vertices]-2} ] & ], {2} ], Sort]
]


Linkage["Examples"]:={PinnedFourBarLinkage, PolygonalLinkage, ChainLinkage, WattLinkage,ConnellyServatiusLinkage,Connelly2ndOrderRigidLinkage};


Linkage[ PinnedFourBarLinkage[ {width_ , height_} ] ] ^:= Linkage[{{0,0},{0,height},{width,height},{width,0}},{RigidBar[{{1,2},{2,3},{3,4}}],PinnedJoint[{1,4}]}]
Linkage[ PinnedFourBarLinkage ] ^:= Linkage[{{0,0},{0,1},{1,1},{1,0}},{RigidBar[{{1,2},{2,3},{3,4}}],PinnedJoint[{1,4}]}]

Linkage[ ChainLinkage[n_Integer?Positive, {start_?(VectorQ[#,NumericQ]&), end_?(VectorQ[#,NumericQ]&)}] ] ^:= 
	Linkage[
		Join[{start,end},start + (end-start) #/n &/@ Range[n]],
		{RigidBar/@Partition[Range[2,n-1],2,1],RigidBar[{1,3}],RigidBar[{2,n}]}
	] /; Length[start] == Length[end]

Linkage[PolygonalLinkage[n_Integer?Positive]] ^:= Linkage[CirclePoints[n],RigidBar/@Partition[Range[n],2,1,1]]

Linkage[ WattLinkage ] ^:= Linkage[ {{0,0},{1,0},{1+Cos[Pi/4],Sin[Pi/4]},{2+Cos[Pi/4],Sin[Pi/4]}}, {RigidBar[{{1,2},{2,3},{3,4}}], PinnedJoint[1],PinnedJoint[4]} ]
Linkage[ WattLinkage[middle_] ] ^:= Linkage[ {{0,0},{1,0},{1+middle Cos[Pi/4],middle Sin[Pi/4]},{2+middle Cos[Pi/4],middle Sin[Pi/4]}}, {RigidBar[{{1,2},{2,3},{3,4}}], PinnedJoint[1],PinnedJoint[4]} ]

Linkage[ ConnellyServatiusLinkage] ^:= Linkage[
	{{-2-1/Sqrt[2],0},{-1-1/Sqrt[2],0},{-1-1/(2 Sqrt[2]),1/(2 Sqrt[2])},{-1,1/Sqrt[2]},{0,1/Sqrt[2]},{-1-1/Sqrt[2],1/Sqrt[2]},{2+1/Sqrt[2],0},{1+1/Sqrt[2],0},{1/4 (4+Sqrt[2]),1/(2 Sqrt[2])},{1,1/Sqrt[2]},{1+1/Sqrt[2],1/Sqrt[2]}},
	{
	RigidBar[{{1,2},{2,3},{2,6},{3,4},{3,6},{3,9},{4,5},{4,6},{7,8},{8,9},{8,11},{9,10},{9,11},{10,5},{10,11}}],
	PinnedJoint[{1,5,7}]
	}
]
Linkage[Connelly2ndOrderRigidLinkage]:=MechanismPositions[
	Linkage[
	{{0,0,0},{0,1,0},{0,0,1},{1,0,0},{1,1,0},{0,1,0.001},{1,0,0.001}},
	 {
	RigidBar[{{1,2},{1,3},{1,4},{1,6},{1,7},{2,3},{2,5},{2,7},{3,4},{3,5},{4,5},{4,6},{5,6},{5,7},{6,7}}],
	PinnedJoint[1],
	Property[PinnedJoint[2],"ConstraintFunction"->(#[[{1,3}]]&) ],
	Property[PinnedJoint[3],"ConstraintFunction"->(#[[1]]&)]
	}] ->
	{{0,0,0},{0,1,0},{0,0,1},{1,0,0},{1,1,0},{0,1,0},{1,0,0}}
]


Origami[ "Examples" ] := {RandlettBirdOrigami, WaterBombBase, MiuraOriCell };

Origami[ RandlettBirdOrigami ] ^:= 
	Origami[
		N@{{0,0,0},{1.414278593427568`,-0.00012107343015166361`,0},{1.4142678318232762`,1.4140274916158289`,0},{0.00011927863201000033`,1.414016642177331`,0},{1.4142692917736754`,1.227825699827347`,0},{1.3131744350755659`,1.211870419723824`,0},{1.2781932400501788`,1.2779508631318839`,0},{0.9659583817161155`,0.9657127724823382`,0},{1.0728360008415996`,0.7312622802432727`,0},{0.47189742974276605`,0.9427579377876374`,0},{0.353962452288944`,0.7077633593785156`,0},{1.2121120814828474`,1.312933079920714`,0},{1.2280642010424148`,1.4140260798657769`,0},{0.731505784488571`,1.0725852346780647`,0},{0.7073110670506855`,0.3537763181270138`,0},{0.9430090748133337`,0.47164965094818173`,0},{0.7074588006059522`,0.7069478151898358`,0},{1.4142724979918373`,0.8060679230868013`,0},{0.8063082321157269`,1.4140228586178498`,0},{0.23507732779834628`,1.1102230246251565`*^-16,0},{0.0001931022705954355`,0.2355241969949885`,0}},
		{
		Face[{{15,20,1},{1,17,15},{11,17,1},{1,21,11},{18,2,16},{16,9,18},{16,2,15},{15,2,20},{13,3,12},{12,3,7},{7,3,6},{6,3,5},{21,4,11},{11,4,10},{10,4,19},{19,14,10},{6,5,18},{18,9,6},{8,7,6},{6,9,8},{12,7,8},{8,14,12},{14,8,17},{17,10,14},{17,8,9},{9,16,17},{11,10,17},{19,13,12},{12,14,19},{16,15,17}}],
		"faces" /@ {{1,11},{1,15},{6,8},{6,18},{8,12},{9,17},{10,19},{12,19},{14,17},{16,18}}
		}
	]

Origami[ RandlettBirdOrigami[ "Folded" ] ] ^:=
	Origami[
		{{0.5216212272644043`,0.0041301921010017395`,-0.5215774178504944`},{-0.6376347541809082`,0.4420289695262909`,-0.16293174028396606`},{-0.435793399810791`,-0.10257021337747574`,0.4356272220611572`},{0.16264325380325317`,0.442157119512558`,0.6371942758560181`},{-0.4927003085613251`,0.07479492574930191`,0.4173307418823242`},{-0.4104839861392975`,0.11236074566841125`,0.36908411979675293`},{-0.4352164566516876`,0.0874059870839119`,0.43510642647743225`},{-0.22010493278503418`,-0.2338118851184845`,0.22003494203090668`},{-0.13188940286636353`,-0.15577159821987152`,-0.008406173437833786`},{0.11944180727005005`,0.1096508726477623`,0.06939235329627991`},{0.047771066427230835`,-0.13527639210224152`,0.09195645153522491`},{-0.3691454231739044`,0.11233522742986679`,0.410442054271698`},{-0.4174804091453552`,0.07477816939353943`,0.49260756373405457`},{0.00845295935869217`,-0.15610502660274506`,0.13212451338768005`},{-0.09123314917087555`,-0.13514138758182526`,-0.04728326201438904`},{-0.06939570605754852`,0.11028251051902771`,-0.11917997896671295`},{-0.18727263808250427`,0.1310577541589737`,0.1871875524520874`},{-0.41846323013305664`,-0.254423052072525`,0.16566649079322815`},{-0.1659073531627655`,-0.25454390048980713`,0.4185855984687805`},{0.3550601601600647`,-0.1361958533525467`,-0.4324187636375427`},{0.43267032504081726`,-0.13634231686592102`,-0.35442548990249634`}},
		{
		 Face[{{15,20,1},{1,17,15},{11,17,1},{1,21,11},{18,2,16},{16,9,18},{16,2,15},{15,2,20},{13,3,12},{12,3,7},{7,3,6},{6,3,5},{21,4,11},{11,4,10},{10,4,19},{19,14,10},{6,5,18},{18,9,6},{8,7,6},{6,9,8},{12,7,8},{8,14,12},{14,8,17},{17,10,14},{17,8,9},{9,16,17},{11,10,17},{19,13,12},{12,14,19},{16,15,17}}],
		 "faces" /@ {{1,11},{1,15},{6,8},{6,18},{8,12},{9,17},{10,19},{12,19},{14,17},{16,18}}
		}
	]


Origami[ WaterBombBase ] ^:=
	Origami[
		{{1/2,1/2},{0,0},{1/2,1/4},{1,0},{3/4,1/2},{1,1},{1/2,3/4},{0,1},{1/4,1/2},{1/2,0},{1,1/2},{1/2,1},{0,1/2}},
		{
		Face[{
		{1,2,3},{1,3,4},{1,4,5},{1,5,6},{1,6,7},{1,7,8},{1,8,9},{1,9,2},
		{2,10,3},{3,10,4},{5,4,11},{5,11,6},{7,6,12},{7,12,8},{9,8,13},{13,2,9}
		}]
		}
	]


Origami[ MiuraOriCell[x_?NumericQ] ]  /; -1/2 < x < 1/2 ^:=
	Origami[
		{
		{3/4,1/2},{0,0},{1/2+x,0},{1,0},{5/4,1/2},{1,1},{1/2+x,1},{0,1},{1/4,1/2}
		},
		{
		Face[{{1,9,2,3},{1,3,4,5},{1,5,6,7},{1,7,8,9}}]
		}
	]

Origami[ MiuraOriCell[ x_?NumericQ , "Triangulated" ] ]  /; -1/2 < x < 1/2 ^:=
		Origami[
		{
		{3/4,1/2},{0,0},{1/2+x,0},{1,0},{5/4,1/2},{1,1},{1/2+x,1},{0,1},{1/4,1/2}
		},
		{
		Face[{{1,9,3},{9,2,3},{1,3,4},{1,4,5},{1,5,6},{1,6,7},{1,7,9},{7,8,9}}],
		"faces"/@{{1,4},{1,6},{9,7},{9,3}}
		}
	]
Origami[ MiuraOriCell[x_, ___ ] ] ^:= "nothing" /; Message[MiuraOriCell::offset,x]

MiuraOriCell::offset = "Offset `1` is not strictly between -1/2 and 1/2.";


End[];

EndPackage[];


BeginPackage["Mechanisms`geometry`"];


(* 
	A snippet of code to test or a working C compiler, modified from
	https://mathematica.stackexchange.com/questions/39837/check-whether-a-working-ccompiler-is-installed
*)
If[Quiet[Check[TrueQ[Compile[{}, 0, CompilationTarget -> "C"][] == 0], False]],
  $MechanismCompilationTarget = "C",
  $MechanismCompilationTarget = "MVM"
];


$MechanismCompilationTarget::usage = "Returns \"C\" if a C compiler was detected.";


Infinitesimal::usage="Infinitesimal[ name, order ] represents an Infinitesimal of name and at a particular order.";


DisplacementRules::usage = "DisplacementRules[displacements] returns a list of rules to assign vertex displacements to their values.
DisplacementRules[ {positions1, ...} ] returns a list of rules for each set of positions. The mechanism positions must be compatible.
DisplacementRules[ periodic data ] returns displacement rules from a periodic mechanism data structure.
DisplacementRules[ periodic data, {k1, ...} ] returns displacement rules from a periodic mechanism with a different set of reciprocal wave vectors {k1, ...}.";

PositionRules::usage = "PositionRules[ m ] returns rules setting the positions of mechanism m.
PositionRules[ positions ] returns a list of rules assigning vertex positions to their values.
PositionRules[ {positions1, ...} ] returns a list of rules for each set of positions. The mechanism positions must be compatible.
PositionRules[ periodic data ] returns position rules from a periodic mechanism data structure.
PositionRules[ periodic data, lattice vectors ] returns position rules from a periodic mechanism with a different set of lattice vectors.";

PositionConstraints::usage="PositionConstraints[ periodic data, lattice vectors ] returns a set of constraints associated with periodic data provided given lattice vectors.";

OrthogonalizeDisplacements::usage=
"OrthogonalizeDisplacements[ {displacement1, ...}, (tolerance) ] returns an orthogonalized set of vertex displacements using optional tolerance.";

RandomDisplacements::usage="RandomDisplacements[ m, (n) ] returns n random displacements of a Mechanism m.
RandomDisplacements[ positions, (n) ] returns n random displacements starting from positions.
n defaults to 1 if it is omitted.";

RandomPositions::usage="RandomPositions[ m, (n) ] returns n positions that have been randomly displaced.
RandomPositions[ positions, n ] returns n random displacements starting from positions.
n defaults to 1 if it is omitted.

(see also: RandomDisplacements[])";

Distribution::usage="Distribution is an option in the Mechanisms package for RandomDisplacements and RandomPositions to specify a distribution for perturbing vertices.";
ConstraintRules::usage="ConstraintRules is an option in the Mechanisms package for RandomDisplacements and RandomPositions to specify a list of rules of the form {VertexDisplacement[n,m] -> .. , ...}."

VertexCoordinatesQ::usage= "VertexCoordinatesQ[ m, pos ] returns True if pos are valid coordinates for Mechanism m." ;
VertexCoordinateListQ::usage= "VertexCoordinateListQ[ m, pos ] returns True if pos are a list of valid positions for Mechanism m." ;
NumericCoordinatesQ::usage= "NumericCoordinatesQ[ m, pos ] returns True if pos are valid numerical coordinates for Mechanism m." ;
NumericMachinePrecisionCoordinatesQ::usage= "NumericMachinePrecisionCoordinatesQ[ m, pos ] returns True if pos are valid numerical, machine precision coordinates for Mechanism m.";


MechanismConnectivity::usage = "MechanismConnectivity[ m, s1 -> s2 ] returns a list of type s2 for each s1. Use MechanismConnectivity[\"Methods\"] to possible values for s1 or s2.";
MechanismOrientedQ::usage = "MechanismOrientedQ[ m ] returns True if all Faces in a Mechanism are oriented with their neighbors.";

BoundaryVertices::usage="BoundaryVertices[ Mechanism ] returns a list of oriented boundary vertices { component 1, ...} where each boundary component is a list of vertex indices. A boundary is defined as the boundary of a set of 2D Faces.";
BoundaryEdges::usage="BoundaryEdges[ Mechanism ] returns a list of oriented boundary edges {{v11, v12},{v21,v22},...}. A boundary is defined as the boundary of a set of 2D Faces.";
BoundaryFaces::usage="BoundaryFaces[ Mechanism ] returns a list of oriented boundary components {Face 1, Face 2, ...}, ...}. A boundary is defined as the boundary of a set of 2D Faces.";
InteriorVertices::usage="InteriorVertices[ Mechanism ] returns a list of interior vertices.";
InteriorEdges::usage="InteriorEdges[ Mechanism ] returns a list of interior edges.";

MechanismDisplayDimension::usage=
"MechanismDisplayDimension[ m ] returns the display dimension of a Mechanism m.
MechanismDisplayDimension[ d ][ m ], MechanismDisplayDimension[m -> d] changes the display dimension of a Mechanism m to d.";

MechanismEmbeddingDimension::usage=
"MechanismEmbeddingDimension[ m ] returns the embedding dimension of a Mechanism m.
MechanismEmbeddingDimension[ d ][ m ], MechanismEmbeddingDimension[m -> d] changes the embedding dimension of a Mechanism m to d.";

MechanismDeleteDanglingVertices::usage="MechanismDeleteDanglingVertices[\!\(\*StyleBox[\"m\",FontSlant->\"Italic\"]\)\!\(\*StyleBox[\",\",FontSlant->\"Italic\"]\)\!\(\*StyleBox[\"type\",FontSlant->\"Italic\"]\)] deletes all vertices of \!\(\*StyleBox[\"m\",FontSlant->\"Italic\"]\)\!\(\*StyleBox[\" \",FontSlant->\"Italic\"]\)not adjacent to a cell of a specific type, either \"Faces\" or \"edges\".
If omitted, the argument \!\(\*StyleBox[\"type\",FontSlant->\"Italic\"]\) defaults to \"Faces\" for Origami or \"edges\" for Linkages. See: DeleteVertices[]";

SaveToFOLD::usage="SaveToFOLD[\!\(\*StyleBox[\"Mechanism\",FontSlant->\"Italic\"]\), \!\(\*StyleBox[\"filename\",FontSlant->\"Italic\"]\)] saves a FOLD file from a Mechanism.";
LoadFromFOLD::usage=
"LoadFromFOLD[\!\(\*StyleBox[\"filename\",FontSlant->\"Italic\"]\)] loads a Mechanism from a FOLD. Use option \"Face\" to choose how to treat a polygon.";


PeriodicIdentification::usage=
"PeriodicIdentification[m, {f1 -> func1,..} ] applies transformation functions func1, func2, ... and creates a list of vertices identified via those transformations.
PeriodicIdentification[m, {z1,z2,...}, {vector1, vector2, ...}] identifies vertices under translation vectors {vector1, vector2,...} and returns rules identifying VertexDisplacement objects up to corresponding factors of {z1, z2,...}.
PeriodicIdentification[m, {vector1, vector2, ...}] identifies vertices under translation vectors {vector1, vector2,...} and returns rules identifying VertexDisplacement objects.";

PeriodicIdentificationData::usage="PeriodicIdentificationData is a structure for periodic Mechanisms.";
PeriodicIdentificationDataQ::usage="PeriodicIdentificationDataQ[ pi ] returns True if pi is a PeriodicIdentificationData structure.
PeriodicIdentificationDataQ[ m, pi ] returns True if pi is a PeriodicIdentificationData structure that can correspond to Mechanism m.";

MechanismUnitCell::usage=
"MechanismUnitCell[m, {vector1, vector2, ...}] reduces a Mechanism to the smallest unit cell that can be tesselated periodically according to the basis of vectors provided.
It relies on PeriodicIdentification[] but unfortunately renumbers vertices.";

LabelPeriodicVertices::usage="LabelPeriodicVertices[ m, periodic data, (style) ] labels a Mechanism consistent with its periodic data using a text style.";


DisplacementVector::usage=
"DisplacementVector[ positions, edge ] returns the vector pointing along an oriented edge.
DisplacementVector[ positions, { edge 1, edge 2, ...} ] returns list of displacement vectors.
Edges can be specified as Line[{v1,v2}] or {v1,v2}.";

DisplacementLength::usage=
"DisplacementLength[ positions, edge 1 ] returns the length of an edge.
DisplacementLength[ positions, { edge 1, edge 2, ...} ] returns lengths of a list of edges.
Edges can be specified as Line[{v1,v2}] or {v1,v2}.";

DisplacementLengthSquared::usage=
"DisplacementLengthSquared[ positions, edge 1 ] returns the squared length of an edge.
DisplacementLengthSquared[ positions, { edge 1, edge 2, ...} ] returns squared lengths of a list of edges.
Edges can be specified as Line[{v1,v2}] or {v1,v2}.";

vectorAngle::usage=
"vectorAngle[ positions, {vertex 1, vertex 2, vertex 3}] returns the angle at vertex 2 spanned by the other two points.
vectorAngle[ positions, { triple 1, triple 2, ...} ] returns the vertex angles anong a list of vertex triples.";

TurningAngle::usage=
"TurningAngle[ positions, {vertex 1, vertex 2, vertex 3}] returns the turning angle at vertex 2 spanned by the other two points.
TurningAngle[ positions, { triple 1, triple 2, ...} ] returns the turning angles anong a list of vertex triples.";

NormalVector::usage=
"NormalVector[ positions, {vertex 1, vertex 2, vertex 3} ] returns the vector normal to the plane spanned by the three points.
NormalVector[ positions, { triple 1, triple 2, ...} ] returns vectors normal to the plane spanned by the list of triples.
NormalVector[ positions, polygon ] returns the vector normal to a polygon.
NormalVector[ positions, {polygon 1, polygon 2, ...} ] returns the vectors normal to a list of polygons.";

FaceArea::usage=
"FaceArea[ positions, {vertex 1, vertex 2, vertex 3} ] returns the area of the Face spanned by the three points.
FaceArea[ positions, { triple 1, triple 2, ...} ] returns the area of the Face spanned by the list of triples.
FaceArea[ positions, polygon ] returns area of a polygon.
FaceArea[ positions, {polygon 1, polygon 2, ...} ] returns the areas of a list of polygons.";

PlaneAngle::usage=
"PlaneAngle[positions, {v1, v2, v3}] returns the oriented angle at vertex v2 spanned by the other two vertices.
PlaneAngle[positions, {triple 1, triple 2, ...}] returns the oriented angles at for each triple of verticles.";

TorsionalFoldAngle::usage=
"TorsionalFoldAngle[ Mechanism, positions, edge ] returns the TorsionalFold angle along an edge.
TorsionalFoldAngle[ Mechanism, positions,{edge 1, edge 2,...} ] returns the TorsionalFold angle of a list of edges.";

DiscreteGaussianCurvature::usage=
"DiscreteGaussianCurvature[ Mechanism, positions,vertex ] returns the discrete Gaussian curvature of vertex.
DiscreteGaussianCurvature[ Mechanism, positions,{vertex 1, vertex 2, ...} ] returns the discrete Gaussian curvature of a list of vertices.
DiscreteGaussianCurvature[ Mechanism, metric,{vertex 1, vertex 2, ...} uses a metric to explicitly compute the Gaussian curvature at each vertex.";

AlignMechanism::usage="AlignMechanism[ m1Spec, m2Spec ] returns a list of positions that best align the second Mechanism to the first.
m1Spec and m2Spec can be either be a Mechanism, a list of positions, { Mechanism, vertices }, or {positions, vertices}.
If vertices are included, only the specified vertices will be used to determine the transformation.";

CongruentMechanismQ::usage="CongruentMechanismQ[ tol_ ][ m1Spec, m2Spec ] returns True if the first and second mechsnisms are congruent up to a given tolerance.
m1Spec and m2Spec can be either be a Mechanism, a list of positions, { Mechanism, vertices }, or {positions, vertices}.
If vertices are included, only the specified vertices will be used to determine the transformation.";


Begin["`Private`"];

Needs["Mechanisms`"];


reportError[ msgHead_, msgName_ ] := ToExpression["Message["<>msgHead<>"::"<>msgName<>"]"]

reportError[ msgHead_, msgName_, arguments__ ] := With[
{ args = StringJoin@@(ToString/@{arguments}) },
	ToExpression["Message["<>msgHead<>"::"<>msgName<>","<>args<>"]"]
]


checkForError[ test_ , name_String, error__ ] := If[ TrueQ[test] , True, reportError[name, Evaluate[error] ]; False]
SetAttributes[checkForError, HoldRest ];


listParameters[expr_]:=DeleteDuplicates[(Extract[expr,#]&)/@Position[List@@expr,Infinitesimal[_,_]]]


(*
	This basically does what Series[] does in a somewhat dumber way.
	It won't handle limits as nicely as Series[] but it typically presents results that look
	more useful for analytic expressions.
*)
expandExpression[expr_,param_]:=Module[
{i,tmp},
	Total[Table[(tmp@@param)^i D[expr,{param,i}]/(i!),{i,0,param[[2]]}]/.param->0]/.tmp->Infinitesimal
]
expandExpression[expr_?NumericQ]:=expr
expandExpression[expr_]:=With[{params=listParameters[expr]},
	Fold[expandExpression,expr,Reverse@params]
]


displacementParsingRules = {
	Rule[x : {__VertexDisplacement} , y : Except[_List]] :> Thread[x->y],
	Rule[x : {__VertexDisplacement} , y_List] /; Length[x]==Length[y] :> Thread[x->y],
	Rule[ x_?(MatrixQ[#,Head[#]===VertexDisplacement&]&), y_List ] /; Length[x]==Length[y] :> Thread[x->y],
	Rule[ x_?(MatrixQ[#,Head[#]===VertexDisplacement&]&), y : Except[_List] ] :> Thread[x->y],
	pi_PeriodicIdentificationData :> (pi["DisplacementRules"] /. Thread[pi["labels"]->1])
};

parseDisplacementRules[ x_ , err_] := 
Module[{
processedRules=Flatten[ {x} //. displacementParsingRules ], badQ
},
	badQ = DeleteCases[ processedRules, VertexDisplacement[_Integer,_String] -> Except[_List] ];
	If[Length[badQ] > 0, 
		Evaluate[err]; {} , 
		processedRules
	]
]
SetAttributes[parseDisplacementRules,HoldRest];


orderPairs[{}]:={}
orderPairs[pairList_?(MatrixQ[#,IntegerQ]&)]:=
	orderPairsTopologicalSort[Graph[UndirectedEdge@@@pairList]]/; Dimensions[pairList][[2]]==2

orderPairs[pairList_,correspondingList_]:=
With[
{
association=Dispatch@Thread[pairList->correspondingList]
},
	orderPairs[pairList]/.association
]/; Length[pairList]==Length[correspondingList]


(* this does a topological sort on a graph *)
orderPairsTopologicalSort[g_?AcyclicGraphQ]:=Partition[TopologicalSort[DirectedEdge@@@EdgeList[g]],2,1]
orderPairsTopologicalSort[g_Graph]:=List@@@(First@FindCycle[g,Infinity,1])


rotateTo[s_List,v_]:=With[{pos=FirstPosition[s,v]},If[MissingQ[pos],s,RotateLeft[s,pos[[1]]-1]]]
rotateTo[h_[s_List],v_]:=h[rotateTo[s,v]]


orderFaces[FaceList_,v_Integer]:=orderPairs[{#[[2]],Last[#]}&/@FaceList,FaceList]


MechanismConnectivity["Methods"]:={{"vertices","edges","faces"}->{"vertices","edges","faces","ordered faces","ordered edges"}};


MechanismConnectivity[ m_?MechanismQ, "faces" -> "vertices" ] := m["faces"]
MechanismConnectivity[ m_?MechanismQ, "edges" -> "vertices" ] := m["edges"]


MechanismConnectivity[ m_?MechanismQ, "vertices" -> "faces" ] := With[
{ Faces = rotateThroughFaces[ MechanismConnectivity[m, "faces" -> "vertices" ] ] },
	GatherBy[(*unpacks*)
		Join[(*unpacks*)
			Transpose[{Range[Length[ m["positions"] ] ]}], (*make a list of vertices in order of the form {{1},{2},...}*)
			Faces
		],
	First][[All,2;;]]
]

rotateThroughFaces[{}]:={}
(*all faces are the same length*)
rotateThroughFaces[Faces_?MatrixQ]:=Flatten[NestList[RotateLeft[#,{0,1}]&,Faces,Last[Dimensions[Faces]]-1],1]
(*faces are different length*)
rotateThroughFaces[Faces_]:=With[
{
	groupedFaces = GatherBy[Faces, Length]
},
	Flatten[ rotateThroughFaces /@ groupedFaces, 1 ]
]


MechanismConnectivity[ m_?MechanismQ, "faces" -> "edges" ] :=
With[{cells=Transpose[ MechanismConnectivity[ m, "faces" -> "vertices" ] ]},
	Flatten[Partition[Transpose[Join[cells,{cells[[1]]}]],{1,2},1],{3}][[1]]
]


(*unpacks*)
MechanismConnectivity[m_?MechanismQ, "edges" -> "faces" ]:=With[
{ edges = MechanismConnectivity[ m, "edges" -> "vertices" ], Faces = MechanismConnectivity[ m, "faces" -> "vertices" ] },
	GatherBy[Flatten[{edges,rotateCells[Faces]},1],Sort[#[[1;;2]]]&][[All,2;;]]
]

Options[rotateCells]={"Flatten"->True};
rotateCells[cells_ , OptionsPattern[]]:=With[
{sortedCells=GatherBy[cells,Length]},
	Flatten[NestList[RotateLeft[#,{0,1}]&,#,Length[#[[1]]]-1]&/@sortedCells,2]
] /; OptionValue["Flatten"]

rotateCells[cells_ , OptionsPattern[]]:=With[
{sortedCells=GatherBy[cells,Length]},
	First[NestList[RotateLeft[#,{0,1}]&,#,Length[#[[1]]]-1]&/@sortedCells]
]


MechanismConnectivity[ m_?MechanismQ, "vertices" -> "edges" ] :=
With[ {edges = MechanismConnectivity[ m, "edges" -> "vertices" ]},
	SortBy[GatherBy[Flatten[{edges,Reverse[edges,{2}]},1],First],First]
]


MechanismConnectivity[m_?MechanismQ, "edges" -> "ordered faces" ]:=
With[
{
test1=MechanismConnectivity[m,"vertices"->"faces"],
edges=Transpose[ MechanismConnectivity[m, "edges" -> "vertices" ] ]
},
	ToPackedArray@MapThread[
		Join[#1,#2]&,
		{
		MapThread[Cases[#1,{__,#2}]&,{test1[[edges[[1]]]],edges[[2]]}],
		MapThread[Cases[#1,{_,#2,__}]&,{test1[[edges[[1]]]],edges[[2]]}]
		}
	]
]


(*
Unpacks lists.
*)
MechanismConnectivity[m_?MechanismQ, "vertices" -> "ordered faces"]:=With[
{unorderedFace=MechanismConnectivity[m,"vertices"->"faces"]},
	MapThread[orderFaces,{unorderedFace,Range[Length[unorderedFace]]}]
]


MechanismConnectivity[m_?MechanismQ, "vertices" -> "ordered edges"]:= MechanismConnectivity[m,"vertices"->"ordered faces"][[All,All,1;;2]]


MechanismConnectivity::badcombo="Methods are not recognized in this combination.";
MechanismConnectivity[m_?MechanismQ , Rule[_,_]]:="nothing" /; Message[MechanismConnectivity::badcombo]


BoundaryEdges[m_?MechanismQ] :=
Module[
{
	FacePairs=MechanismConnectivity[m,"edges" -> "faces"],
	edges=MechanismConnectivity[m, "edges" -> "vertices"],
	boundary
},
	boundary=Pick[ edges, Length /@ FacePairs, 1 ];
	If[ Length[boundary]==0,
		{},
		Map[List@@#&,FindCycle[Graph[boundary],Infinity,All],{2}]
	]
]


BoundaryVertices[m_?MechanismQ] := Map[ First@@#& , BoundaryEdges[m] , {2} ]


BoundaryFaces[m_?MechanismQ]:=
Module[{i},
	With[{
		boundary = BoundaryVertices[m],
		Faces = MechanismConnectivity[m, "faces" -> "vertices" ]
		},
		Table[
			Select[ Faces, ContainsAny[boundary[[i]],#]& ],
			{i,1,Length[boundary]}
		]
	]
]


InteriorEdges[mr_?MechanismQ]:=
	Pick[
		MechanismConnectivity[ mr, "edges" -> "vertices"],
		Length /@ MechanismConnectivity[mr, "edges" -> "faces"],
		2
	]


InteriorVertices[mr_?MechanismQ]:=
Complement[
	DeleteDuplicates[ Flatten[InteriorEdges[mr]] ],
	Flatten @ BoundaryVertices[mr]
]


MechanismOrientedQ[mr_?MechanismQ]:=
	AllTrue[
		DeleteCases[ MechanismConnectivity[mr,"edges"->"faces"] , {_} ],
		facePairOrientedQ
	]
MechanismOrientedQ[_]:=False


facePairOrientedQ[{v1_,v2_}]:=With[
{
	eg2=Take[RotateLeft[v2,#],2]&/@Range[Length[v2]],
	eg1=Reverse[Take[RotateLeft[v1,#],2]]&/@Range[Length[v1]]
},
	ContainsAny[eg1,eg2]
]
facePairOrientedQ[v1_,v2_]:=With[
{
	eg2=Take[RotateLeft[v2,#],2]&/@Range[Length[v2]],
	eg1=Reverse[Take[RotateLeft[v1,#],2]]&/@Range[Length[v1]]
},
	ContainsAny[eg1,eg2]
]


MechanismEmbeddingDimension[ m_?MechanismQ ]:= m["EmbeddingDimension"]
MechanismEmbeddingDimension[ d : 2|3 ][ m_?MechanismQ ] :=
With[ {oldPositions = m["positions"]},
	ReplacePart[m, 1 -> PadRight[ oldPositions, {Length[oldPositions],d} ] ]
]

MechanismEmbeddingDimension::dim = "Dimension `1` should be 2 or 3.";
MechanismEmbeddingDimension[ d_ ][ m_?MechanismQ ] := "nothing" /; Message[ MechanismEmbeddingDimension::dim, d ]

MechanismEmbeddingDimension[ Rule[ m_?MechanismQ, d_] ] := MechanismEmbeddingDimension[d][m]


MechanismDisplayDimension[ m_?MechanismQ ] := m["DisplayDimension"]

MechanismDisplayDimension [ d: 2|3 ][m_?MechanismQ] := 
With[ {res = Mechanisms`Private`addMeshRegion[m,d,{}]}, res /; MechanismQ[res] ]

MechanismDisplayDimension::dim = "Dimension `1` should be 2 or 3.";
MechanismDisplayDimension[ d_ ][ m_?MechanismQ ] := "nothing" /; Message[ MechanismDisplayDimension::dim, d ]

MechanismDisplayDimension[ Rule[ m_?MechanismQ, d_ ] ] := MechanismDisplayDimension[d][m]


MechanismDeleteDanglingVertices[m_Origami] := MechanismDeleteDanglingVertices[m,"faces"]
MechanismDeleteDanglingVertices[m_Linkage] := MechanismDeleteDanglingVertices[m,"edges"]
MechanismDeleteDanglingVertices[m_?MechanismQ, type: "faces"|"edges" ] := MechanismDeleteDanglingVertices[m,type]

MechanismDeleteDanglingVertices[m_?MechanismQ, "faces"]:=With[
{vertexSelector=Transpose[ { Range[m["VertexNumber"]], Length /@ MechanismConnectivity[m,"vertices"->"faces"]} ]},
	DeleteVertices[m, Select[vertexSelector,#[[2]]<1&][[All,1]] ]
]

MechanismDeleteDanglingVertices[m_?MechanismQ,"edges"]:=With[
{vertexSelector=Complement[Range[m["VertexNumber"]],MechanismConnectivity[m,"vertices"->"edges"][[All,1,1]]]},
	DeleteVertices[m,vertexSelector]
]

MechanismDeleteDanglingVertices[type_][m_?MechanismQ ] := MechanismDeleteDanglingVertices[m, type]

MechanismDeleteDanglingVertices::typ="Second argument should be either \"Faces\" or \"edges\" to whether dangling vertices are indicated by a lack of Faces or lack of edges.";
MechanismDeleteDanglingVertices[m_?MechanismQ,_String]:="nothing"/;Message[MechanismDeleteDanglingVertices::typ]


RegionEmbeddingDimension[o_Linkage] ^:= MechanismEmbeddingDimension[o]
RegionEmbeddingDimension[o_Origami] ^:= MechanismEmbeddingDimension[o]

RegionDimension[o_Linkage] ^:= RegionDimension[ o["mesh"] ]
RegionDimension[o_Origami] ^:= RegionDimension[ o["mesh"] ]

MeshCellCount[ o_Linkage, d___ ] ^:= MeshCellCount[ o["mesh"], d ]
MeshCellCount[ o_Origami, d___ ] ^:= MeshCellCount[ o["mesh"], d ]


VertexCoordinatesQ[ coord_ ] := AllTrue[ {MatrixQ[coord], Length[coord[[1]]]==2 || Length[coord[[1]]]==3}, TrueQ ]
VertexCoordinatesQ[ m_?MechanismQ, coord_ ] := AllTrue[ {MatrixQ[coord], Length[coord] == m["VertexNumber"]}, TrueQ ]


VertexCoordinateListQ[ coord_ ] := AllTrue[ {ArrayQ[coord], ArrayDepth[coord]==3 , Length[coord[[1,1]]]==2 || Length[coord[[1,1]]]==3}, TrueQ ]
VertexCoordinateListQ[ m_?MechanismQ, coord_ ] := AllTrue[ {ArrayQ[coord], ArrayDepth[coord]==3 , Length[coord[[1]]]==m["VertexNumber"]}, TrueQ ]


NumericCoordinatesQ[ coord_ ] := AllTrue[ {MatrixQ[coord, NumericQ], Length[coord[[1]]]==2 || Length[coord[[1]]]==3},TrueQ ]
NumericCoordinatesQ[ m_?MechanismQ, coord_ ] := AllTrue[ {MatrixQ[coord,NumericQ], Length[coord] == m["VertexNumber"]}, TrueQ ]


NumericMachinePrecisionCoordinatesQ[ coord_ ] := AllTrue[ {MatrixQ[coord, MachineRealQ], Length[coord[[1]]]==2 || Length[coord[[1]]]==3},TrueQ ]
NumericMachinePrecisionCoordinatesQ[ m_?MechanismQ, coord_ ] := AllTrue[ {MatrixQ[coord,MachineRealQ], Length[coord] == m["VertexNumber"]}, TrueQ ]


OrthogonalizeDisplacements[ displacements_?VertexCoordinateListQ, tol : _?NumericQ : 10^(-8) ] :=
With[{dim = Dimensions[displacements][[3]], tolsq = tol^2},
	Partition[#, dim]& /@ Select[ Orthogonalize[ Flatten /@ displacements, Tolerance -> tol ], # . # > tolsq & ]
]


OrthogonalizeDisplacements::notdispl="Not a list of valid vertex displacements of the same dimension.";
OrthogonalizeDisplacements::tol="Tolerance must be numeric.";

OrthogonalizeDisplacements[displacements_,___]:="nothing"/;Message[OrthogonalizeDisplacements::notdispl]
OrthogonalizeDisplacements[displacements_,_]:="nothing"/;Message[OrthogonalizeDisplacements::tol]


$coordinateSymbols[3]={"x","y","z"};
$coordinateSymbols[2]={"x","y"};

VertexPosition[n : {__Integer}, d_] := VertexPosition[#,d]& /@ n
VertexPosition[n_Integer, d : {__String}|{__Integer}] := VertexPosition[n,#]&/@d
VertexPosition[n_Integer, All[ d_Integer ] ] := VertexPosition[n,#]& /@ $coordinateSymbols[d]
VertexPosition[n_Integer, m_Integer] := VertexPosition[n, $coordinateSymbols[3][[m]]]
VertexPosition[m_?MechanismQ, d_] := VertexPosition[#,d]& /@ Range[m["VertexNumber"]]
VertexPosition[m_?MechanismQ] := VertexPosition[m, All[ m["EmbeddingDimension"] ] ]
VertexPosition[m_?MechanismQ, n : {__Integer} ] := VertexPosition[ n, All[ m["EmbeddingDimension"] ] ]

VertexDisplacement[n : {__Integer},d_]:=VertexDisplacement[#,d]&/@n
VertexDisplacement[n_Integer,d : {__String}|{__Integer}]:=VertexDisplacement[n,#]&/@d
VertexDisplacement[n_Integer,All[d_Integer]]:=VertexDisplacement[n,#]&/@$coordinateSymbols[d]
VertexDisplacement[n_Integer,m_Integer]:=VertexDisplacement[n,$coordinateSymbols[3][[m]]]
VertexDisplacement[m_?MechanismQ,d_]:=VertexDisplacement[#,d]&/@Range[MeshCellCount[m["mesh"],0]]
VertexDisplacement[m_?MechanismQ]:=VertexDisplacement[m,All[m["EmbeddingDimension"]]]
VertexDisplacement[m_?MechanismQ, n : {__Integer} ] := VertexDisplacement[ n, All[ m["EmbeddingDimension"] ] ]


VertexPosition[ p_PeriodicIdentificationData ] := p["VertexPosition"]
VertexDisplacement[ p_PeriodicIdentificationData ] := p["VertexDisplacement"]


PositionRules[ p_PeriodicIdentificationData ] := p["PositionRules"]
PositionRules[ p_PeriodicIdentificationData , latticeVectors_?MatrixQ ] :=
	p["LatticeVectors"->latticeVectors]["PositionRules"] /; Dimensions[latticeVectors] == Dimensions[p["LatticeVectors"]]

PositionRules::lat="Lattice vectors should have dimensions `1`.";
PositionRules[ p_PeriodicIdentificationData , latticeVectors_ ] := "nothing" /; Message[PositionRules::lat,Dimensions[p["LatticeVectors"]]]

PositionRules[ m_?MechanismQ ] := PositionRules[ m["positions"] ]
PositionRules[ positions_?VertexCoordinatesQ ] := With[{
	arbitraryPositions = Array[ VertexPosition, Dimensions[positions] ]
},
	Flatten[MapThread[Rule, {arbitraryPositions, positions}, 2]]
]
PositionRules[ positions_?VertexCoordinateListQ ] := With[{
	arbitraryPositions = Array[VertexPosition, Dimensions[positions][[2;;]] ]
},
	Flatten /@ MapThread[ Rule, { ConstantArray[ arbitraryPositions, Length[positions] ], positions } , 3 ]
]

PositionRules[ pos_ ] := "nothing" /; Message[PositionRules::pos]
PositionRules::pos = "Argument should either be a mechanism, vertex positions {pt1, pt2,...}, or a list of vertex positions { {pt1, ...}, ...} .";


PositionConstraints[ p_PeriodicIdentificationData , latticeVectors_?MatrixQ ] :=
	p["LatticeVectors"->latticeVectors]["Constraints"] /; Dimensions[latticeVectors] == Dimensions[p["LatticeVectors"]]

PositionConstraints::lat="Lattice vectors should have dimensions `1`.";
PositionConstraints[ p_PeriodicIdentificationData , latticeVectors_ ] := "nothing" /; Message[PositionConstraints::lat,Dimensions[p["LatticeVectors"]]]


DisplacementRules[ p_PeriodicIdentificationData ] := p["DisplacementRules"]
DisplacementRules[ p_PeriodicIdentificationData , wavevectors_?VectorQ ] := (p["DisplacementRules"] /. Thread[p["labels"]->wavevectors]) /; Length[wavevectors]==Length[p["labels"]]

DisplacementRules::wavevectors="You need `1` wave vectors to be provided";
DisplacementRules[ p_PeriodicIdentification, _] := "nothing" /; Message[DisplacementRules::wavevector,Length[pi["labels"]]]

DisplacementRules[ positions_?VertexCoordinatesQ ] := With[{
	arbitraryPositions = Array[ VertexDisplacement, Dimensions[positions] ]
},
	Flatten[MapThread[Rule, {arbitraryPositions, positions}, 2]]
]
DisplacementRules[ positions_?VertexCoordinateListQ] := With[{
	arbitraryPositions = Array[VertexDisplacement, Dimensions[positions][[2;;]] ]
},
	Flatten /@ MapThread[ Rule, { ConstantArray[ arbitraryPositions, Length[positions] ], positions } , 3 ]
]

DisplacementRules[ pos_ ] := "nothing" /; Message[DisplacementRules::pos]
DisplacementRules::pos = "Argument should either be vertex positions {pt1, pt2,...}, or a list of vertex positions { {pt1, ...}, ...} .";


randomDisplacementsInternal[ positions_, distribution_, precision_, {}, numDisplacements_ ] :=
Module[{
	numberOfVertices, dim, randomNumbers
},
	{numberOfVertices, dim } = Dimensions[positions];
	Check[
		RandomVariate[ distribution, {numDisplacements, numberOfVertices,dim}, WorkingPrecision -> precision ],

		$Failed
	]
]

randomDisplacementsInternal[ positions_, distribution_, precision_, rules_, numDisplacements_ ] :=
Module[
{
	numberOfVertices, dim, displacements, arbitraryDisplacements
},
	{numberOfVertices, dim } = Dimensions[positions];
	displacements = Array[VertexDisplacement[#1,#2]&, {numberOfVertices,dim}] //. rules;
	arbitraryDisplacements = Cases[ Flatten[displacements], _VertexDisplacement];
	
	Check[
		Array[(displacements /. Dispatch@Thread[arbitraryDisplacements->RandomVariate[
			distribution,
			Length[arbitraryDisplacements],
			WorkingPrecision->precision
		]] &), numDisplacements ],

		$Failed
	]
]


Options[RandomDisplacements]={
	Distribution -> NormalDistribution[0,1/10],
	WorkingPrecision -> MachinePrecision,
	ConstraintRules -> {}
};

Options[RandomPositions]={
	Distribution -> NormalDistribution[0,1/10],
	WorkingPrecision -> MachinePrecision,
	ConstraintRules -> {}
};


processRules[ _, {} ] := {}
processRules[ RandomDisplacements, r_ ] := parseDisplacementRules[r,Message[RandomDisplacements::rules]]
processRules[ RandomPositions, r_ ] := parseDisplacementRules[r,Message[RandomPositions::rules]]

precisionQ[ _, p_?(NumericQ[#] && #>0&) ] := p
precisionQ[ RandomDisplacements, p_ ] := (Message[RandomDisplacements::precision, p]; $Failed)
precisionQ[ RandomPositions, p_ ] := (Message[RandomPositions::precision, p]; $Failed)

processNumber[ _, n_Integer?Positive ] := n
processNumber[ RandomDisplacements, n_] := (Message[RandomDisplacements::num]; $Failed)
processNumber[ RandomPositions, n_] := (Message[RandomPositions::num]; $Failed)

RandomDisplacements::rules="List of rules must be of the form {VertexDisplacements[_,_]->_, ..}";
RandomDisplacements::precision="Working precision should be a real, positive number.";
RandomDisplacements::num="Number of displacements must be a positive integer.";
RandomDisplacements::pos="First argument should be positions or a mechanism.";

RandomPositions::rules="List of rules must be of the form {VertexDisplacements[_,_]->_, ..}";
RandomPositions::precision="Working precision should be a real, positive number.";
RandomPositions::num="Number of displacements must be a positive integer.";
RandomPositions::pos="First argument should be positions or a mechanism.";


RandomDisplacements[ m_, opt : OptionsPattern[] ] := With[{res=RandomDisplacements[m,1, opt]},
	res[[1]] /; Head[res] =!= RandomDisplacements
]

RandomDisplacements[ m_?MechanismQ, num : Except[_Rule] , opt : OptionsPattern[] ] := 
With[{
	precision = precisionQ[ RandomDisplacements, OptionValue[WorkingPrecision] ],
	rules = processRules[ RandomDisplacements, OptionValue[ConstraintRules] ],
	distribution = OptionValue[Distribution],
	number = processNumber[RandomDisplacements, num]
},
	With[{res=randomDisplacementsInternal[ m["positions"], distribution, precision, rules, number ]},
		res /; res =!= $Failed
	] /; precision =!= $Failed && number =!= $Failed
]

RandomDisplacements[ pos_?VertexCoordinatesQ, num : Except[_Rule] , opt : OptionsPattern[] ] := 
With[{
	precision = precisionQ[ RandomDisplacements, OptionValue[WorkingPrecision] ],
	rules = processRules[ RandomDisplacements, OptionValue[ConstraintRules] ],
	distribution = OptionValue[Distribution],
	number = processNumber[RandomDisplacements, num]
},
	With[{res=randomDisplacementsInternal[ pos, distribution, precision, rules, number ]},
		res /; res =!= $Failed
	] /; precision =!= $Failed && number =!= $Failed
]

RandomDisplacements[ a_, ___] := "nothing" /; Message[RandomDisplacements::pos]


RandomPositions[ m_ , opt : OptionsPattern[] ] := With[{res=RandomPositions[m,1,opt]}, res[[1]] /; Head[res] =!= RandomPositions]

RandomPositions[ m_?MechanismQ, num : Except[_Rule] , opt : OptionsPattern[] ] := 
With[{
	precision = precisionQ[ RandomPositions, OptionValue[WorkingPrecision] ],
	rules = processRules[ RandomPositions, OptionValue[ConstraintRules] ],
	distribution = OptionValue[Distribution],
	number = processNumber[RandomPositions, num]
},
	With[{res=randomDisplacementsInternal[ m["positions"], distribution, precision, rules, number ]},
		(ConstantArray[m["positions"],number]+res) /; res =!= $Failed
	] /; precision =!= $Failed && number =!= $Failed
]

RandomPositions[ pos_?VertexCoordinatesQ, num : Except[_Rule] , opt : OptionsPattern[] ] := 
With[{
	precision = precisionQ[RandomPositions, OptionValue[WorkingPrecision] ],
	rules = processRules[RandomPositions, OptionValue[ConstraintRules] ],
	distribution = OptionValue[Distribution],
	number = processNumber[RandomPositions, num]
},
	With[{res=randomDisplacementsInternal[ pos, distribution, precision, rules, number ]},
		(ConstantArray[pos,number]+res) /; res =!= $Failed
	] /; precision =!= $Failed && number =!= $Failed
]

RandomPositions[ a_, ___] := "nothing" /; Message[RandomPositions::pos]


PeriodicIdentificationData[dim_, f_, maps_,vertices_]["Methods"] := 
{"Identification","UniqueVertices","LatticeVectors","PeriodicMaps","labels",
"EmbeddingDimension","Constraints","VertexPosition","PositionRules","VertexDisplacements","DisplacementRules"}


(*identification method*)
PeriodicIdentificationData[dim_, f_, maps_,vertices_]["Identification"] :=
	Module[{n},
		With[{ rules = Thread[f -> ConstantArray[Identity,Length[f]]] },
			DeleteCases[ Thread[Range[Length[vertices]] -> (vertices /. rules)], n_ -> n_ ]
		]
	]


(*UniqueVertices method*)
PeriodicIdentificationData[dim_, f_, maps_,vertices_]["UniqueVertices"] := Cases[vertices, _Integer]


(*LatticeVectors method*)
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["LatticeVectors"] := With[{zeros=ConstantArray[0,dim]},#[zeros]&/@maps]
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["LatticeVectors"->basis_?MatrixQ] := 
	Module[{x},With[ {newMaps=Function[{x},x+#]&/@basis}, PeriodicIdentificationData[dim,f,newMaps,vertices]]] /; Length[basis]==dim
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["LatticeVectors"->basis_] :=
	(
	Message[PeriodicIdentification::basis,dim];
	PeriodicIdentificationData[dim,f,maps,vertices]
	)
PeriodicIdentification::basis="Lattice vectors must be provided as `1` vectors in dimension `1`.";


(*PeriodicMaps method*)
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["PeriodicMaps"]:=maps
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["PeriodicMaps" -> m_] /; Length[m]==Length[maps] := PeriodicIdentificationData[dim,f,m,vertices]
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["PeriodicMaps" -> m_] := 
	(
	Message[PeriodicIdentification::maps,Length[maps]];
	PeriodicIdentificationData[dim,f,maps,vertices]
	)
PeriodicIdentification::maps="There must be `1` maps.";


(*EmbeddingDimension method*)
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["EmbeddingDimension"]:=dim
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["EmbeddingDimension" -> d : 2|3]:=PeriodicIdentificationData[d,f,maps,vertices]
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["EmbeddingDimension" -> d_]:=
	(
	Message[PeriodicIdentification::dim,d];
	PeriodicIdentificationData[dim,f,maps,vertices]
	)
PeriodicIdentification::dim="Dimension `1` must be either 2 or 3.";


PeriodicIdentificationData[dim_, f_, maps_, vertices_]["labels"] := f

PeriodicIdentificationData[dim_,f_,maps_,vertices_]["labels" -> l_List] /; Length[l]==Length[maps] := PeriodicIdentificationData[dim,l,maps,vertices]
PeriodicIdentificationData[dim_,f_,maps_,vertices_]["labels" -> l_] :=
	(
	Message[PeriodicIdentification::label,Length[f]];
	PeriodicIdentificationData[dim,f,maps,vertices]
	)
PeriodicIdentification::label="There must be `1` labels.";


PeriodicIdentificationData[dim_,f_,maps_,vertices_]["VertexPosition"] :=
	Module[{x}, With[ { cleanupRules = Thread[f -> maps] },
		vertices /. x_Integer :> VertexPosition[x,All[dim]] /. cleanupRules
	]]

PeriodicIdentificationData[dim_,f_,maps_,vertices_]["PositionRules"] :=
	Module[{x}, DeleteCases[ With[ { cleanupRules = Thread[f -> maps] },
		Thread[
			Flatten[VertexPosition[Range[Length[vertices]], All[dim] ]] -> Flatten[vertices /. x_Integer :> VertexPosition[x,All[dim]] /. cleanupRules ]
		]] , x_ -> x_ ]
	]

PeriodicIdentificationData[dim_,f_,maps_,vertices_]["Constraints"] :=
	Module[{x}, DeleteCases[ With[ { cleanupRules = Thread[f -> maps] },
		Thread[
			Flatten[VertexPosition[Range[Length[vertices]], All[dim] ]] == Flatten[vertices /. x_Integer :> VertexPosition[x,All[dim]] /. cleanupRules ]
		]] , True ]
	]

PeriodicIdentificationData[dim_,f_,maps_,vertices_]["VertexDisplacements"] := 
Module[{x},With[{ cleanupRules = (#[x_] :> # x & /@ f) },
	vertices /. x_Integer :> VertexDisplacement[x,All[dim]] /. cleanupRules
]]

PeriodicIdentificationData[dim_,f_,maps_,vertices_]["DisplacementRules"] := 
Module[{x},
	DeleteCases[ With[{ cleanupRules = (#[x_] :> # x & /@ f) },
		Thread[
			Flatten[VertexDisplacement[Range[Length[vertices]], All[dim] ]] -> Flatten[vertices /. x_Integer :> VertexDisplacement[x,All[dim]] /. cleanupRules]
		]
	], x_ -> x_ ]
]


Format[PeriodicIdentificationData[dim_,f_,maps_,vertices_]] :=
	"PeriodicIdentificationData"[Style[ PeriodicIdentificationData[dim,f,maps,vertices]["LatticeVectors"], Italic ]]


PeriodicIdentificationDataQ[ PeriodicIdentificationData[_Integer,_,_,_] ] := True
PeriodicIdentificationDataQ[ m_?MechanismQ, PeriodicIdentificationData[d_Integer,_,_,vertices_] ] :=
	AllTrue[ {m["EmbeddingDimension"] == d , Length[vertices] == m["VertexNumber"] }, TrueQ ]
PeriodicIdentificationDataQ[ m_?MechanismQ, _ ] := False


mapQ[f_ ->  _Function]:=True
mapQ[_]:=False

PeriodicIdentification[m_?MechanismQ, basis_?(MatrixQ[#,NumericQ]&) ] :=
Module[{x}, With[ {f = ToString/@Array[Unique[],Length[basis]]},
	periodicIdentificationInternal[ m, Thread[ f -> (Function[{x},x + #]&/@basis) ] ]
]]

PeriodicIdentification[m_?MechanismQ, f_, basis_?(MatrixQ[#,NumericQ]&) ] /; Length[f] == Length[basis] :=
Module[{x},
	periodicIdentificationInternal[ m, Thread[ f -> (Function[{x},x + #]&/@basis) ] ]
]
PeriodicIdentification[m_?MechanismQ, f : {__?mapQ}]:= periodicIdentificationInternal[m,f]

periodicIdentificationInternal[m_?MechanismQ, f : {__?mapQ}]:=Module[{x},With[
{
	labels = f[[All,1]],
	maps = f[[All,2]],
	pos = MechanismPositions[m], dim = m["EmbeddingDimension"],
	ruleFunction=Function[{x}, #[[1]]->x[#[[2]]]& ],
	vertices=Range[m["VertexNumber"]]
},
	PeriodicIdentificationData[
		m["EmbeddingDimension"],
		labels,
		maps,
		vertices //. Flatten[MapThread[
			ruleFunction[#1] /@ overlappingVertices[pos, #2 /@ pos]&,
			{labels,maps}
		]]
	]
]]


(* 
	overlappingVertices returns a list of which vertices in the first set of positions can be
	identified in the second set.
	
	overlappingVertices[positions 1, positions 2] returns a list of which vertices listed in 1 can be found in 2.
*)
overlappingVertices[positions1_?MatrixQ, positions2_?MatrixQ, tol : _?NumericQ : 10^(-5)] := 
Module[
{ nearestList, selectorList },
	(*
	get the points in list 1 that are closest to each of the points in list 2
	*)
	nearestList = Nearest[ Thread[positions1 -> Range[ Length[positions1] ]], positions2 ][[All,1]] ;

	(*get the actual overlapping points according to the provided tolerance*)
	selectorList = (# . #<tol^2&) /@ ( positions2-positions1[[ nearestList ]] ) ;

	Pick[ Thread[nearestList -> Range[ Length @ positions1] ], selectorList ]

] /; Dimensions[positions1] == Dimensions[positions2]


MechanismUnitCell[m_?MechanismQ, basis_?(MatrixQ[N[#],NumericQ]&)]:=
Module[
{ pi = PeriodicIdentification[m,basis],badVertices },
	badVertices=pi["Identification"][[All,1]];
	With[{
		badEdges= Select[MechanismConnectivity[m,"edges"->"vertices"],ContainsAll[badVertices,#]&],
		badFaces = Select[MechanismConnectivity[m,"faces"->"vertices"],ContainsAll[badVertices,#]&]
	},
		MechanismDeleteDanglingVertices[
			DeleteCells[m, Alternatives @@ Join[_/@badEdges,_/@badFaces] ]
		]
	]
] /; If[ Length[basis[[1]]] == MechanismEmbeddingDimension[m], True, Message[MechanismUnitCell::dim]; False]

MechanismUnitCell[m_?MechanismQ, basis_?MatrixQ]:="nothing" /; Message[MechanismUnitCell::num]
MechanismUnitCell[m_?MechanismQ, basis : Except[_?MatrixQ] ]:="nothing" /; Message[MechanismUnitCell::num]
MechanismUnitCell[m_?MechanismQ ]:="nothing" /; Message[MechanismUnitCell::num]


MechanismUnitCell::dim="Dimension of basis must match embedding dimension of Mechanism.";
MechanismUnitCell::num="Second argument should be a numeric basis.";


LabelPeriodicVertices[ m_?MechanismQ , pi_, inputStyle_ : None] := With[{
	map = pi["Identification"], style = If[ inputStyle === None, #&, Style[#,inputStyle]& ]
},
	ChangeCellData[
		ChangeCellData[FreeJoint, map[[All,1]], "Label" -> (style/@ map[[All,2]]) ] @ m,
		FreeJoint, map[[All,2]], "Label" -> map[[All,2]]
	]
] /; PeriodicIdentificationDataQ[m,pi]


vertexArgumentsQ[ name_, vertices_ ] := checkForError[ VectorQ[vertices, IntegerQ] && Min[vertices]>0, name, "vertices"]
vertexArgumentsQ[ name_, pos_?VertexCoordinatesQ, vertices_ ] :=
	checkForError[ VectorQ[vertices, IntegerQ] && Min[vertices]>0 && Max[vertices] <= Length[pos], name, "vertices"]

(*only checks for interior vertices*)
vertexArgumentsQ[ name_, m_?MechanismQ, vertices_ ] :=
	checkForError[ VectorQ[vertices] && ContainsAll[ InteriorVertices[m], vertices ] , name, "vertices"]
vertexArgumentsQ[ name_, m_?MechanismQ, pos_, vertices_ ] := And[
	checkForError[ VectorQ[vertices, IntegerQ] && ContainsAll[ InteriorVertices[m], vertices ], name, "vertices"],
	checkForError[ MatrixQ[pos] && Length[pos] == m["VertexNumber"], name, "pos" ]
]


torsionalFoldArgumentsQ[name_, m_?MechanismQ,edgelist_]:=checkForError[
	AllTrue[{MatrixQ[edgelist,IntegerQ], Length[edgelist[[1]]]==2,Quiet @ ContainsAll[ Sort/@InteriorEdges[m],Sort/@edgelist]},TrueQ],
	name,
	"TorsionalFolds"
]
torsionalFoldArgumentsQ[name_, m_?MechanismQ,pos_,edgelist_]:=And[
	checkForError[
		AllTrue[{MatrixQ[edgelist,IntegerQ], Length[edgelist[[1]]]==2,Quiet @ ContainsAll[ Sort/@InteriorEdges[m],Sort/@edgelist]},TrueQ],
		name,
		"TorsionalFolds"
	],
	checkForError[ MatrixQ[pos] && Length[pos] == m["VertexNumber"], name, "pos" ]
]


edgeArgumentsQ[ name_ , edgelist_ ] := checkForError[ MatrixQ[edgelist,IntegerQ] && Min[edgelist]>0 , name, "edgeform" ]
edgeArgumentsQ[ name_ , m_?MechanismQ , edgelist_ ] := checkForError[ MechanismEdgeQ[m,edgelist], name, "edge" ]
edgeArgumentsQ[ name_ , pos_?VertexCoordinatesQ , edgelist_ ] := checkForError[ ValidMechanismEdgeQ[pos,edgelist], name, "edge" ]
edgeArgumentsQ[ name_, m_?MechanismQ, pos_, edgelist_] := And[
	checkForError[ MechanismEdgeQ[m,edgelist], name, "edge" ],
	checkForError[ MatrixQ[pos] && Length[pos] == m["VertexNumber"], name, "pos" ]
]


faceArgumentsQ[ name_ , Facelist_ ] := checkForError[ ArrayDepth[Facelist]==2 && Min[Facelist]>0 , name, "Faceform" ]
faceArgumentsQ[ name_ , m_?MechanismQ , facelist_ ] := checkForError[ MechanismFaceQ[m,facelist], name, "Face" ]
faceArgumentsQ[ name_ , pos_?VertexCoordinatesQ , Facelist_ ] := checkForError[ ValidMechanismFaceQ[pos,Facelist], name, "Face" ]
faceArgumentsQ[ name_, m_?MechanismQ, pos_, Facelist_] := And[
	checkForError[ MechanismFaceQ[m,Facelist], name, "Face" ],
	checkForError[ MatrixQ[pos] && Length[pos] == m["VertexNumber"], name, "pos" ]
]


tripleArgumentsQ[ name_ , triplelist_ ] := checkForError[ MatrixQ[triplelist,IntegerQ] && Length[triplelist[[1]]]==3 && Min[triplelist]>0 , name, "tripleform" ]

tripleArgumentsQ[ name_ , m_?MechanismQ , triplelist_ ] := 
	checkForError[ MatrixQ[triplelist,IntegerQ] && Length[triplelist[[1]]]==3 && Min[triplelist]>0 && Max[triplelist]<=m["VertexNumber"] , name, "triples" ]

tripleArgumentsQ[ name_ , pos_?VertexCoordinatesQ , triplelist_ ] :=
	checkForError[ MatrixQ[triplelist,IntegerQ] && Length[triplelist[[1]]]==3 && Min[triplelist]>0 && Max[triplelist]<=Length[pos] , name, "triples" ]

tripleArgumentsQ[ name_, m_?MechanismQ, pos_, triplelist_] := And[
	checkForError[ MatrixQ[triplelist,IntegerQ] && Length[triplelist[[1]]]==3 && Min[triplelist]>0 && Max[triplelist]<=Length[pos] , name, "triples" ],
	checkForError[ MatrixQ[pos] && Length[pos] == m["VertexNumber"], name, "pos" ]
]


$positionError="Vertex positions do not correspond to Mechanism.";

$edgeError="One or more provided edges are not valid.";
$edgeformError="Edges are not of the form {{v1,v2},...}.";

$tripleError="One or more provided triples involve vertices that are out of bounds.";
$tripleformError="Triples are not of the form {{v1,v2,v3},...}.";

$FaceformError="Faces are not of the form {{v1,...},...}.";
$FaceError="One or more provided Faces are not valid.";

$TorsionalFoldError="One or more provided edges are not valid TorsionalFolds.";

$vertexError="One or more vertices are not valid interior vertices.";


DisplacementVector::pos=$positionError;
DisplacementVector::edgeform = $edgeformError;
DisplacementVector::edge=$edgeError;


DisplacementVector[__, {} ] :={}

DisplacementVector[positions_?VertexCoordinatesQ, edgeList_] /; edgeArgumentsQ["DisplacementVector",positions,edgeList] :=
	DisplacementVectorInternal[positions, edgeList]
DisplacementVector[positions_?VertexCoordinateListQ, edgeList_ ] /; edgeArgumentsQ["DisplacementVector",positions[[1]],edgeList] :=
	DisplacementVectorInternal[positions, edgeList ]

DisplacementVector[m_?MechanismQ, pos_?VertexCoordinatesQ, edgeList_] /; edgeArgumentsQ["DisplacementVector",m,pos,edgeList] := 
	DisplacementVectorInternal[ pos , edgeList]
DisplacementVector[m_?MechanismQ , positions_?VertexCoordinateListQ, edgeList_ ] /; edgeArgumentsQ["DisplacementVector",positions[[1]],edgeList] :=
	DisplacementVectorInternal[positions, edgeList ]

DisplacementVector[m_?MechanismQ, edgeList_] /; edgeArgumentsQ["DisplacementVector",m,edgeList] :=
	DisplacementVectorInternal[ m["positions"] , edgeList]


DisplacementVectorInternal[positions_?(MatrixQ[#,MachineRealQ]&),edgeList_]:= DisplacementVectorCompiled[][ToPackedArray[positions],ToPackedArray[edgeList]]

DisplacementVectorInternal[positions_?MatrixQ,edgeList_]:=
With[{flippedEdgeList=Transpose[edgeList]},
	positions[[flippedEdgeList[[2]]]]-positions[[flippedEdgeList[[1]]]]
]

DisplacementVectorCompiled[]:=DisplacementVectorCompiled[]=Compile[{{positions,_Real,2},{edgeList,_Integer,2}},
	With[{flippedEdgeList=Transpose[edgeList]},
		positions[[flippedEdgeList[[2]]]]-positions[[flippedEdgeList[[1]]]]
	]
]


DisplacementVectorInternal[positions_?(ArrayQ[#,MachineRealQ]&),edgeList_]:= DisplacementVectorCompiledMultiple[][ToPackedArray[positions],ToPackedArray[edgeList]]

DisplacementVectorInternal[positions_?ArrayQ,edgeList_]:=
With[{flippedEdgeList=Transpose[edgeList]},
	Map[#[[flippedEdgeList[[2]]]]-#[[flippedEdgeList[[1]]]]&, positions]
]

DisplacementVectorCompiledMultiple[]:=DisplacementVectorCompiledMultiple[]=Compile[{{positions,_Real,3},{edgeList,_Integer,2}},
	With[{flippedEdgeList=Transpose[edgeList]},
		Map[#[[flippedEdgeList[[2]]]]-#[[flippedEdgeList[[1]]]]&, positions]
	]
]


DisplacementLength::pos=$positionError;
DisplacementLength::edgeform = $edgeformError;
DisplacementLength::edge=$edgeError;


DisplacementLength[__, {} ] :={}
DisplacementLength[positions_?VertexCoordinatesQ, edgeList_] /; edgeArgumentsQ["DisplacementLength",positions,edgeList] := 
	DisplacementLengthInternal[positions, edgeList]

DisplacementLength[m_?MechanismQ, positions_?MatrixQ, edgeList_] /; edgeArgumentsQ["DisplacementLength",m,positions,edgeList]:= 
	DisplacementLengthInternal[positions, edgeList]

DisplacementLength[m_?MechanismQ, edgeList_] /; edgeArgumentsQ["DisplacementLength",m,edgeList] :=
	DisplacementLengthInternal[ m["positions"] , edgeList]

DisplacementLengthInternal[positions_?(MatrixQ[#,MachineRealQ]&), edgelist_]:=DisplacementLengthCompiled[Length[positions[[1]]]][positions,ToPackedArray[edgelist]]
DisplacementLengthInternal[positions_,edgelist_]:=DisplacementLengthAnalytic[positions,ToPackedArray[edgelist]]


DisplacementLengthCompiled[d_Integer]:=DisplacementLengthCompiled[d]=
Compile[
{{pos,_Real,2},{edges,_Integer,2}},
	Module[{i,j},
		Table[
			With[{
			index1=Compile`GetElement[edges,i,1],
			index2=Compile`GetElement[edges,i,2]
			},
			Sqrt[Sum[(Compile`GetElement[pos,index1,j]-Compile`GetElement[pos,index2,j])^2,{j,1,d}]]
			],
		{i,1,Length[edges]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

DisplacementLengthAnalytic[positions_,edgeList_]:=
	expandExpression[Sqrt[(# . #&) /@ DisplacementVectorInternal[positions,edgeList]]]


DisplacementLengthSquared::pos=$positionError;
DisplacementLengthSquared::edgeform = $edgeformError;
DisplacementLengthSquared::edge=$edgeError;


DisplacementLengthSquared[__, {} ] :={}
DisplacementLengthSquared[positions_?VertexCoordinatesQ, edgeList_] /; edgeArgumentsQ["DisplacementLengthSquared",positions,edgeList] :=
	DisplacementLengthSquaredInternal[positions, edgeList]
DisplacementLengthSquared[m_?MechanismQ, positions_, edgeList_] /; edgeArgumentsQ["DisplacementLengthSquared",m, positions,edgeList] :=
	DisplacementLengthSquaredInternal[positions, edgeList]
DisplacementLengthSquared[m_?MechanismQ, edgeList_] /; edgeArgumentsQ["DisplacementLengthSquared",m,edgeList] :=
	DisplacementLengthSquaredInternal[ m["positions"] , edgeList]

DisplacementLengthSquaredInternal[positions_?(MatrixQ[#,MachineRealQ]&), edgelist_]:=DisplacementLengthSquaredCompiled[Length[positions[[1]]]][positions,ToPackedArray[edgelist]]
DisplacementLengthSquaredInternal[positions_,edgelist_]:=DisplacementLengthSquaredAnalytic[positions,ToPackedArray[edgelist]]


DisplacementLengthSquaredCompiled[d_Integer]:=DisplacementLengthSquaredCompiled[d]=
Compile[
{{pos,_Real,2},{edges,_Integer,2}},
	Module[{i,j},
		Table[
			With[{
			index1=Compile`GetElement[edges,i,1],
			index2=Compile`GetElement[edges,i,2]
			},
			Sum[(Compile`GetElement[pos,index1,j]-Compile`GetElement[pos,index2,j])^2,{j,1,d}]
			],
		{i,1,Length[edges]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]
DisplacementLengthSquaredAnalytic[positions_,edgeList_]:=
	expandExpression[Map[# . # &,DisplacementVectorInternal[positions,edgeList]]]


TurningAngle::pos=$positionError;
TurningAngle::tripleform=$tripleformError;
TurningAngle::triples=$tripleError;


TurningAngle[p__, {} ] :={}
TurningAngle[positions_?VertexCoordinatesQ, tripleList_] /; tripleArgumentsQ["TurningAngle", positions, tripleList ] :=
	TurningAngleInternal[positions, tripleList]
TurningAngle[m_?MechanismQ, positions_?VertexCoordinatesQ, tripleList_] /; tripleArgumentsQ["TurningAngle", m, positions, tripleList ] :=
	TurningAngleInternal[positions, tripleList]
TurningAngle[m_?MechanismQ, tripleList_] /; tripleArgumentsQ["TurningAngle", m, tripleList ]  :=
	TurningAngleInternal[ m["positions"] , tripleList]

TurningAngleInternal[positions_?(MatrixQ[#,MachineRealQ]&), tripleList_] :=
	TurningAngleCompiled[Length[positions[[1]]]][m["positions"],ToPackedArray[tripleList]]
TurningAngleInternal[positions_,tripleList_] :=
	TurningAngleAnalytic[Length[positions[[1]]],positions,ToPackedArray[tripleList]]


TurningAngleCompiled[3]:=TurningAngleCompiled[3]=
Compile[
{{pos,_Real,2},{triplets,_Integer,2}},
	Module[{i},
		With[{
			p1x=Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,1],1],
			p1y=Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,1],2],
			p1z=Compile`GetElement[pos,Compile`GetElement[triplets,i,2],3]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],3],
			p2x=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1],
			p2y=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2],
			p2z=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],3]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],3]
		},
			ArcCos[(p1x p2x+p1y p2y+p1z p2z)/(Sqrt[p1x^2+p1y^2+p1z^2] Sqrt[p2x^2+p2y^2+p2z^2])]
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

TurningAngleCompiled[2]:=TurningAngleCompiled[2]=
Compile[
{{pos,_Real,2},{triplets,_Integer,2}},
	Module[{i},
		Table[With[{
			p1x=Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,1],1],
			p1y=Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,1],2],
			p2x=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1],
			p2y=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2]
		},
			ArcTan[p1x p2x + p1y p2y,p1x p2y - p2x p1y]
		],{i,1,Length[triplets]}]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]


(*
this funny construction here works better during expansions because of the branch cuts of ArcCos[]

The problem arises in Series[ArcCos[1-xx^2],{xx,0,2}] which requires the choice of a branch. This last part
hasn't been solved but at least the answer comes out faster. The rest of the branch cut issues
are handled automatically in expandExpression[], which does what it can to make imaginary components zero.

There should be a better way to handle this but this seems to work in most cases.
*)
TurningAngleAnalytic[3,data_,tripleList_]:=
With[
{
	cosvectorAngle3D=Function[{p1,p2},
		With[
			{x=p1[[1]],y=p1[[2]],z=p1[[3]],xx=p2[[1]],yy=p2[[2]],zz=p2[[3]]},
			(x xx+y yy+z zz)/(Sqrt[x^2+y^2+z^2] Sqrt[xx^2+yy^2+zz^2])
		]
	]
},
	(expandExpression@With[{pts=data[[#]]},ArcCos[expandExpression@cosvectorAngle3D[pts[[2]]-pts[[1]],pts[[3]]-pts[[2]]]]])&/@tripleList
]

TurningAngleAnalytic[2,data_,tripleList_]:=
With[
{
	vectorAngle2D=Function[{p1,p2},
		With[
			{x=p1[[1]],y=p1[[2]],xx=p2[[1]],yy=p2[[2]]},
			ArcTan[x xx + y yy,x yy - xx y]	
		]
	]
},
	(expandExpression@With[{pts=data[[#]]},vectorAngle2D[pts[[2]]-pts[[1]],pts[[3]]-pts[[2]]]])&/@tripleList
]


NormalVector::pos=$positionError;
NormalVector::Faceform=$FaceformError;
NormalVector::Face=$FaceError;
NormalVector::norm="Option \"normalize\" must be either True or False.";


Options[NormalVector]={"Normalize"->True};

NormalVector[__, {} ] :={}
NormalVector[positions_?VertexCoordinatesQ, Faces_, OptionsPattern[]] /; faceArgumentsQ["NormalVector",positions,Faces] :=
	NormalVectorInternal[positions, Faces, OptionValue["Normalize"]] /; checkForError[ BooleanQ[OptionValue["Normalize"]] , "TurningAngle", "norm" ]
NormalVector[m_?MechanismQ, Faces_, OptionsPattern[]] /; faceArgumentsQ["NormalVector",m,Faces] :=
	NormalVectorInternal[m["positions"], Faces, OptionValue["Normalize"]] /; checkForError[ BooleanQ[OptionValue["Normalize"]] , "TurningAngle", "norm" ]
NormalVector[m_?MechanismQ, positions_?VertexCoordinatesQ, Faces_, OptionsPattern[]] /; faceArgumentsQ["NormalVector",m,positions,Faces] :=
	NormalVectorInternal[positions, Faces, OptionValue["Normalize"]] /; checkForError[ BooleanQ[OptionValue["Normalize"]] , "TurningAngle", "norm" ]

NormalVectorInternal[pos_?(MatrixQ[#,MachineRealQ]&),Faces_,normalize_ : True]:=
	NormalVectorCompiled[normalize,Length[pos[[1]]]][pos,ToPackedArray[PadRight[Faces,{Length[Faces],3}]]]
NormalVectorInternal[pos_,Faces_,normalize_ : True]:=
	NormalVectorAnalytic[normalize,Length[pos[[1]]],pos, ToPackedArray[PadRight[Faces,{Length[Faces],3}]]]


NormalVectorCompiled[True,3]:=NormalVectorCompiled[True,3]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
	Module[{i},
		Table[
			With[
			{
				x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				z=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],3]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3],
				xx=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				yy=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				zz=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],3]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3]
			},
			{
				(-yy z+y zz)/Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2],
				(xx z-x zz)/Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2],
				(-xx y+x yy)/Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2]
			}
			],
			{i,1,Length[Faces]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]
NormalVectorCompiled[False,3]:=NormalVectorCompiled[False,3]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
	Module[{i},
		Table[
			With[
			{
				x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				z=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],3]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3],
				xx=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				yy=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				zz=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],3]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3]
			},
			{
				(-yy z+y zz),
				(xx z-x zz),
				(-xx y+x yy)
			}
			],
			{i,1,Length[Faces]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]
NormalVectorCompiled[True,2]:=NormalVectorCompiled[True,2]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
	Module[{i},
		Table[
			With[
			{
				x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				xx=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				yy=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2]
			},
				Sign[-xx y+x yy]
			],
			{i,1,Length[Faces]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

NormalVectorCompiled[False,2]:=NormalVectorCompiled[False,2]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
	Module[{i},
		Table[
			With[
			{
				x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				xx=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				yy=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2]
			},
				-xx y+x yy
			],
			{i,1,Length[Faces]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

NormalVectorAnalytic[True (* normalized *),3, positions_, triples_]:=
With[
{
	data=positions,
	NormalVector3D=Function[
		{p1,p2},
		With[
			{
			x=p1[[1]],y=p1[[2]],z=p1[[3]],xx=p2[[1]],yy=p2[[2]],zz=p2[[3]]
			},
			{
			(-yy z+y zz)/Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2],
			(xx z-x zz)/Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2],
			(-xx y+x yy)/Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2]
			}
		]
	]
},
	With[{p=data[[#]]},expandExpression@NormalVector3D[p[[3]]-p[[2]],p[[1]]-p[[2]]]]&/@triples
] 

NormalVectorAnalytic[False (* normalized *),3, positions_, triples_]:=
With[
{
	data=positions,
	unNormalVector3D=Function[
		{p1,p2},
		With[
			{
			x=p1[[1]],y=p1[[2]],z=p1[[3]],xx=p2[[1]],yy=p2[[2]],zz=p2[[3]]
			},
			{
			-yy z+y zz,
			xx z-x zz,
			-xx y+x yy
			}
		]
	]
},
	With[{p=data[[#]]},expandExpression@unNormalVector3D[p[[3]]-p[[2]],p[[1]]-p[[2]]]]&/@triples
] 


FaceArea::pos=$positionError;
FaceArea::Faceform=$FaceformError;
FaceArea::Face=$FaceError;


tc=Compile[{{Face,_Integer,1}},
	With[{v1=First[Face],rest=Rest[Face]},
		Map[ Join[{v1},#]&,Partition[rest,2,1] ]
	],
	RuntimeAttributes->{Listable}
];


FaceArea[__, {} ] :={}
FaceArea[positions_?VertexCoordinatesQ, Faces_] /; faceArgumentsQ["FaceArea",positions,Faces] :=
	FaceAreaInternal[positions, Faces ]
FaceArea[m_?MechanismQ, Faces_] /; faceArgumentsQ["FaceArea",m,Faces] :=
	FaceAreaInternal[m["positions"], Faces ]
FaceArea[m_?MechanismQ, positions_?VertexCoordinatesQ, Faces_] /; faceArgumentsQ["FaceArea",m,positions,Faces] :=
	FaceAreaInternal[positions, Faces ]

FaceAreaInternal[pos_?(MatrixQ[#,MachineRealQ]&),Faces_]:=
	Total@FaceAreaCompiled[Length[pos[[1]]]][pos, #]& /@ tc[Faces]
FaceAreaInternal[pos_,Faces_]:=With[{d=Length[pos[[1]]]},
	Total@FaceAreaAnalytic[d,pos, #]& /@ tc[Faces]
]


FaceAreaCompiled[3]:=FaceAreaCompiled[3]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
	Module[{i},
		Table[
			With[
			{
				x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				z=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],3]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3],
				xx=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				yy=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				zz=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],3]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3]
			},
				Sqrt[(-yy z+y zz)^2+(xx z-x zz)^2+(-xx y+x yy)^2]
			],
			{i,1,Length[Faces]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

FaceAreaCompiled[2]:=FaceAreaCompiled[2]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
	Module[{i},
		Table[
			With[
			{
				x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
				xx=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
				yy=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2]-Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2]
			},
				Abs[-xx y+x yy]
			],
			{i,1,Length[Faces]}
		]
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

FaceAreaAnalytic[3, positions_, triples_]:=
With[
{
	data=positions,
	unNormalVector3D=Function[
		{p1,p2},
		With[
			{
			x=p1[[1]],y=p1[[2]],z=p1[[3]],xx=p2[[1]],yy=p2[[2]],zz=p2[[3]]
			},
			Sqrt[(-yy z+y zz)^2+(xx z-x zz)^2+(-xx y+x yy)^2]
		]
	]
},
	With[{p=data[[#]]},expandExpression@unNormalVector3D[p[[3]]-p[[2]],p[[1]]-p[[2]]]]&/@triples
] 

FaceAreaAnalytic[2, positions_, triples_]:=
With[
{
	data=positions,
	unNormalVector3D=Function[
		{p1,p2},
		With[
			{
			x=p1[[1]],y=p1[[2]],xx=p2[[1]],yy=p2[[2]]
			},
			Sqrt[(-xx y+x yy)^2]
		]
	]
},
	With[{p=data[[#]]},expandExpression@unNormalVector3D[p[[3]]-p[[2]],p[[1]]-p[[2]]]]&/@triples
] 


PlaneAngle::pos=$positionError;
PlaneAngle::tripleform=$tripleformError;
PlaneAngle::triples=$tripleError;


PlaneAngle[__, {} ] :={}
PlaneAngle[positions_?VertexCoordinatesQ, tripleList_] /; tripleArgumentsQ["PlaneAngle", positions, tripleList] :=
	PlaneAngleInternal[positions, tripleList]
PlaneAngle[m_?MechanismQ, positions_, tripleList_] /; tripleArgumentsQ["PlaneAngle", m, positions, tripleList]:=
	PlaneAngleInternal[positions, tripleList]
PlaneAngle[m_?MechanismQ, tripleList_ ] /; tripleArgumentsQ["PlaneAngle", m, tripleList] :=
	PlaneAngleInternal[m["positions"], tripleList]


PlaneAngleInternal[pos_?(MatrixQ[#,MachineRealQ]&),triples_] := PlaneAngleCompiled[Length[pos[[1]]]][pos,triples]
PlaneAngleInternal[pos_,triples_] := PlaneAngleAnalytic[Length[pos[[1]]],pos,triples]


PlaneAngleCompiled[3]:=PlaneAngleCompiled[3]=
Compile[
{{pos,_Real,2},{triplets,_Integer,2}},Module[{i},
	Table[
		With[{
		x=Compile`GetElement[pos,Compile`GetElement[triplets,i,1],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1],
		y=Compile`GetElement[pos,Compile`GetElement[triplets,i,1],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2],
		z=Compile`GetElement[pos,Compile`GetElement[triplets,i,1],3]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],3],
		xx=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1],
		yy=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2],
		zz=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],3]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],3]
		},
		ArcTan[x xx+y yy+z zz,Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2]]
		],
	{i,1,Length[triplets]}
	]],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]
PlaneAngleCompiled[2]:=PlaneAngleCompiled[2]=Compile[
{{pos,_Real,2},{triplets,_Real,2}},Module[{i},
	Table[With[{
		x=Compile`GetElement[pos,Compile`GetElement[triplets,i,1],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1],
		y=Compile`GetElement[pos,Compile`GetElement[triplets,i,1],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2],
		xx=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],1]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],1],
		yy=Compile`GetElement[pos,Compile`GetElement[triplets,i,3],2]-Compile`GetElement[pos,Compile`GetElement[triplets,i,2],2]
	},
			ArcTan[x xx+y yy,(xx y-x yy)^2]
	],{i,1,Length[triplets]}
	]
],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]


PlaneAngleAnalytic[3,positions_,tripleList_]:=
With[
{
	data=positions,
	angleFunc=Function[{p1,p2},
		With[
			{x=p1[[1]],y=p1[[2]],z=p1[[3]],xx=p2[[1]],yy=p2[[2]],zz=p2[[3]]},
			ArcTan[x xx+y yy+z zz,Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2]]
		]
	]
},
	With[{pts=data[[#]]},expandExpression@angleFunc[pts[[1]]-pts[[2]],pts[[3]]-pts[[2]]]]&/@tripleList
]


PlaneAngleAnalytic[2,positions_,tripleList_]:=
With[
{
	data=positions,
	angleFunc=Function[{p1,p2},
		With[
			{x=p1[[1]],y=p1[[2]],xx=p2[[1]],yy=p2[[2]]},
			ArcTan[x xx+y yy,(xx y-x yy)^2]
		]
	]
},
	With[{pts=data[[#]]},expandExpression@angleFunc[pts[[1]]-pts[[2]],pts[[3]]-pts[[2]]]]&/@tripleList
]


TorsionalFoldAngle::pos=$positionError;
TorsionalFoldAngle::TorsionalFolds=$TorsionalFoldError;


TorsionalFoldAngle[m_?MechanismQ, _?VertexCoordinatesQ, {} ] :={}
TorsionalFoldAngle[m_?MechanismQ, {} ] :={}

TorsionalFoldAngle[ m_?MechanismQ, edgeList_ ] /; torsionalFoldArgumentsQ["TorsionalFoldAngle",m,edgeList] :=
	If[ MechanismEmbeddingDimension[m]==2,
		ConstantArray[0,Length[edgeList]],
		TorsionalFoldAngleInternal[m, m["positions"], listQuadruples[m,edgeList] ]
	]

TorsionalFoldAngle[ m_?MechanismQ, pos_?MatrixQ, edgeList_ ] /; torsionalFoldArgumentsQ["TorsionalFoldAngle",m, pos, edgeList] :=
	If[ Length[pos[[1]]]==2,
		ConstantArray[0,Length[edgeList]],
		TorsionalFoldAngleInternal[m, pos, listQuadruples[m,edgeList] ]
	]

TorsionalFoldAngle[ m_?MechanismQ, positions : {__?VertexCoordinatesQ}, edgeList_ ] /; torsionalFoldArgumentsQ["TorsionalFoldAngle",m,edgeList] :=
	With[{quadruples = listQuadruples[m,edgeList]},
		If[ VertexCoordinatesQ[m,#],
			If[Length[#[[1]]]==2,ConstantArray[0,Length[edgeList]], TorsionalFoldAngleInternal[m,#,quadruples]],
			$Failed
		]& /@ positions
	]

listQuadruples[m_,edgelist_] :=
With[{
	Faces=MechanismConnectivity[m,"edges"->"ordered faces"][[ MechanismEdges[m, edgelist] ]]
},
	ToPackedArray[
		If[Length[#]==2,
			{#[[2,1]],#[[2,2]],#[[1,2]],Last@#[[2]]},
			{#[[1,1]],#[[1,2]],0,0}
		]&/@Faces
	]
]


TorsionalFoldAngleInternal[m_?MechanismQ, positions_?(MatrixQ[#,MachineRealQ]&), quadruples_]:=TorsionalFoldAngleCompiled[][positions,quadruples]
TorsionalFoldAngleInternal[m_?MechanismQ, positions_, quadruples_]:=TorsionalFoldAngleAnalytic[positions,quadruples]

TorsionalFoldAngleCompiled[]:=TorsionalFoldAngleCompiled[]=
Compile[
{{pos,_Real,2},{Faces,_Integer,2}},
Table[
	If[Compile`GetElement[Faces,i,3]==0,
	0,
	With[
		{
		p1x=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],1],
		p1y=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],2],
		p1z=Compile`GetElement[pos,Compile`GetElement[Faces,i,1],3],
		p2x=Compile`GetElement[pos,Compile`GetElement[Faces,i,2],1],
		p2y=Compile`GetElement[pos,Compile`GetElement[Faces,i,2],2],
		p2z=Compile`GetElement[pos,Compile`GetElement[Faces,i,2],3],
		p3x=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],1],
		p3y=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],2],
		p3z=Compile`GetElement[pos,Compile`GetElement[Faces,i,3],3],
		p4x=Compile`GetElement[pos,Compile`GetElement[Faces,i,4],1],
		p4y=Compile`GetElement[pos,Compile`GetElement[Faces,i,4],2],
		p4z=Compile`GetElement[pos,Compile`GetElement[Faces,i,4],3]
		},
		ArcTan[(p1y (p2x-p3x)+p2y p3x-p2x p3y+p1x (-p2y+p3y)) (-p2y p4x+p1y (-p2x+p4x)+p1x (p2y-p4y)+p2x p4y)+(p1z (p2x-p3x)+p2z p3x-p2x p3z+p1x (-p2z+p3z)) (-p2z p4x+p1z (-p2x+p4x)+p1x (p2z-p4z)+p2x p4z)+(p1z (p2y-p3y)+p2z p3y-p2y p3z+p1y (-p2z+p3z)) (-p2z p4y+p1z (-p2y+p4y)+p1y (p2z-p4z)+p2y p4z),Sqrt[(p1x-p2x)^2+(p1y-p2y)^2+(p1z-p2z)^2] (-p1x p2z p3y+p1x p2y p3z+p2z p3y p4x-p2y p3z p4x+p1x p2z p4y-p2z p3x p4y-p1x p3z p4y+p2x p3z p4y+p1z (p2x p3y-p3y p4x+p2y (-p3x+p4x)-p2x p4y+p3x p4y)+(-p1x p2y+p2y p3x+p1x p3y-p2x p3y) p4z+p1y (p2z p3x-p2x p3z-p2z p4x+p3z p4x+p2x p4z-p3x p4z))]
	]],
	{i,1,Length[Faces]}
],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]

TorsionalFoldAngleAnalytic[positions_,quadrupleList_]:=
	With[
	{
	p1x=positions[[#[[1]],1]],p1y=positions[[#[[1]],2]],p1z=positions[[#[[1]],3]],
	p2x=positions[[#[[2]],1]],p2y=positions[[#[[2]],2]],p2z=positions[[#[[2]],3]],
	p3x=positions[[#[[3]],1]],p3y=positions[[#[[3]],2]],p3z=positions[[#[[3]],3]],
	p4x=positions[[#[[4]],1]],p4y=positions[[#[[4]],2]],p4z=positions[[#[[4]],3]]
	},
	expandExpression@ArcTan[(p1y (p2x-p3x)+p2y p3x-p2x p3y+p1x (-p2y+p3y)) (-p2y p4x+p1y (-p2x+p4x)+p1x (p2y-p4y)+p2x p4y)+(p1z (p2x-p3x)+p2z p3x-p2x p3z+p1x (-p2z+p3z)) (-p2z p4x+p1z (-p2x+p4x)+p1x (p2z-p4z)+p2x p4z)+(p1z (p2y-p3y)+p2z p3y-p2y p3z+p1y (-p2z+p3z)) (-p2z p4y+p1z (-p2y+p4y)+p1y (p2z-p4z)+p2y p4z),Sqrt[(p1x-p2x)^2+(p1y-p2y)^2+(p1z-p2z)^2] (-p1x p2z p3y+p1x p2y p3z+p2z p3y p4x-p2y p3z p4x+p1x p2z p4y-p2z p3x p4y-p1x p3z p4y+p2x p3z p4y+p1z (p2x p3y-p3y p4x+p2y (-p3x+p4x)-p2x p4y+p3x p4y)+(-p1x p2y+p2y p3x+p1x p3y-p2x p3y) p4z+p1y (p2z p3x-p2x p3z-p2z p4x+p3z p4x+p2x p4z-p3x p4z))]
	]&/@quadrupleList


DiscreteGaussianCurvature::pos=$positionError;
DiscreteGaussianCurvature::vertices=$vertexError;


DiscreteGaussianCurvature[m_?MechanismQ, _?VertexCoordinatesQ, {} ] :={}
DiscreteGaussianCurvature[m_?MechanismQ, {} ] :={}

DiscreteGaussianCurvature[m_?MechanismQ, vertexList_] /; vertexArgumentsQ["DiscreteGaussianCurvature",m,vertexList] :=
	DiscreteGaussianCurvatureInternal[m, m["positions"], vertexList]
DiscreteGaussianCurvature[m_?MechanismQ, positions_, vertexList_] /; vertexArgumentsQ["DiscreteGaussianCurvature",m,positions,vertexList]:=
	DiscreteGaussianCurvatureInternal[m, positions, vertexList]


DiscreteGaussianCurvatureInternal[m_, pos_,vertexlist_]:=ConstantArray[0,Length[vertexlist]] /; MechanismEmbeddingDimension[m]==2

DiscreteGaussianCurvatureInternal[m_, pos_?(MatrixQ[#,MachineRealQ]&),vertexlist_]:=
		DiscreteGaussianCurvatureCompiled[][pos,
			ToPackedArray[PadRight[MechanismConnectivity[m,"vertices"->"ordered faces"]][[vertexlist]][[All,All,1;;3]]]
	]
DiscreteGaussianCurvatureInternal[m_, pos_,vertexlist_]:=
	DiscreteGaussianCurvatureAnalytic[
		pos,
		ToPackedArray[
			Map[RotateRight, MechanismConnectivity[m, "vertices" -> "ordered faces"][[vertexlist]],{2}][[All,All,1;;3]]
		]
	]


DiscreteGaussianCurvatureCompiled[]:=DiscreteGaussianCurvatureCompiled[]=
Compile[{{pos,_Real,2},{triplets,_Integer,3}},
	Module[{i,j},Table[2 Pi-Sum[
		If[Compile`GetElement[triplets,j,i,1]==0,
			0,
			With[{
			x=Compile`GetElement[pos,Compile`GetElement[triplets,j,i,3],1]-Compile`GetElement[pos,Compile`GetElement[triplets,j,i,1],1],
			y=Compile`GetElement[pos,Compile`GetElement[triplets,j,i,3],2]-Compile`GetElement[pos,Compile`GetElement[triplets,j,i,1],2],
			z=Compile`GetElement[pos,Compile`GetElement[triplets,j,i,3],3]-Compile`GetElement[pos,Compile`GetElement[triplets,j,i,1],3],
			xx=Compile`GetElement[pos,Compile`GetElement[triplets,j,i,2],1]-Compile`GetElement[pos,Compile`GetElement[triplets,j,i,1],1],
			yy=Compile`GetElement[pos,Compile`GetElement[triplets,j,i,2],2]-Compile`GetElement[pos,Compile`GetElement[triplets,j,i,1],2],
			zz=Compile`GetElement[pos,Compile`GetElement[triplets,j,i,2],3]-Compile`GetElement[pos,Compile`GetElement[triplets,j,i,1],3]
			},
			ArcTan[x xx+y yy+z zz,Sqrt[(xx y-x yy)^2+(xx z-x zz)^2+(yy z-y zz)^2]]
			]
		],
		{i,1,Length[Compile`GetElement[triplets,j]]}(* Sum *)
	],{j,1,Length[triplets]}] (* Table *)
	],
	RuntimeOptions->"Speed",
	CompilationTarget->$MechanismCompilationTarget
]


DiscreteGaussianCurvatureAnalytic[pos_,{triples__?(MatrixQ[#,IntegerQ]&)}]:=
	(expandExpression@(2 Pi-Total[PlaneAngle[pos,#]]))&/@{triples}


FindGeometricTransform[pts_, m_Origami, r___ ] ^:= FindGeometricTransform[ pts, m["positions"], r ]
FindGeometricTransform[m1_Origami, m2_Origami, r___] ^:= FindGeometricTransform[ m1["positions"], m2["positions"], r]

FindGeometricTransform[pts_, m_Linkage, r___ ] ^:= FindGeometricTransform[ pts, m["positions"], r ]
FindGeometricTransform[m1_Linkage, m2_Linkage, r___] ^:= FindGeometricTransform[ m1["positions"], m2["positions"], r]


Options[AlignMechanism]=Options[FindGeometricTransform];

AlignMechanism[ from_, to_, opt : OptionsPattern[] ] := 
With[{fromPos = convertToAlign[ from ], toPos = convertToAlign[ to ]},
	With[{ transform = alignTransform[ fromPos, toPos, {opt} ] },
		transform[ toPos[[1]] ] /; transform =!= $Failed
	] /; fromPos =!= $Failed && toPos =!= $Failed
]


(*convert the arguments appropriately*)
convertToAlign[ m1_?MechanismQ ] := { m1["positions"], All}
convertToAlign[ { m1_?MechanismQ , v : {__Integer} } ] := {m1["positions"], v} /; Max[v] <= m1["VertexNumber"] && Min[v]>0
convertToAlign[ {m1_?MechanismQ, v_ } ] := (Message[AlignMechanism::oob, v]; $Failed )

convertToAlign[ pos_?NumericCoordinatesQ ] := {pos,All}
convertToAlign[ {pos_?NumericCoordinatesQ, v : {__Integer} } ] := {pos,v} /; Max[v] <= Length[pos] && Min[v] > 0
convertToAlign[ {pos_?NumericCoordinatesQ, v_ } ] := (Message[AlignMechanism::oob,v]; $Failed )
convertToAlign[ {pos_, v_} ] := (Message[AlignMechanism::num]; $Failed)

AlignMechanism::oob = "Specified vertices `1` are out of bounds of Mechanism.";
AlignMechanism::num = "Vertex positions must be numerical.";


(*find the geometric transform we need*)
alignTransform[ {positionsFrom_,vFrom_}, {positionsTo_,vTo_}, options_ ] := 
With[{
transformation = FindGeometricTransform[positionsFrom[[vFrom]], positionsTo[[vTo]], Flatten[{options, {TransformationClass -> "Rigid", Method -> "Linear" }}] ]
},
	If[Head[transformation] === FindGeometricTransform, Message[AlignMechanism::gt]; $Failed, transformation[[2]] ]
] /; Dimensions[positionsFrom] == Dimensions[positionsTo]

alignTransform[ {positionsFrom_,vFrom_}, {positionsTo_,vTo_}, options_ ] := (Message[AlignMechanism::match]; $Failed)

AlignMechanism::gt = "Failed to find geometric transform.";
AlignMechanism::match = "Vertices are not of the same dimension and number.";


CongruentMechanismQ[tolerance_?(NumericQ[#]&&#>0&)][ from_, to_ ] := With[
{ fromPos = convertToAlign[from ], toPos = convertToAlign[ to ] },
	With[ { transform = alignTransform[ fromPos, toPos, {} ] },
		With[ { difference = Flatten[transform[ toPos[[1]] ] - fromPos[[1]]] },
			difference . difference < tolerance
		] /; transform =!= $Failed
	] /; fromPos =!= $Failed && toPos =!= $Failed
]


BoundingRegion[o_Origami, r___] ^:= BoundingRegion[ o["positions"], r ]
BoundingRegion[o_Linkage, r___] ^:= BoundingRegion[ o["positions"], r ]


RegionNearest[ f_Linkage, pt_ ] ^:= RegionNearest[ f["mesh"], pt ]
RegionNearest[ f_Linkage ] ^:= RegionNearest[ f["mesh"] ]

RegionNearest[ f_Origami, pt_ ] ^:= RegionNearest[ f["mesh"], pt ]
RegionNearest[ f_Origami ] ^:= RegionNearest[ f["mesh"] ]


RegionMember[ f_Linkage, pt_ ] ^:= RegionMember[ f["mesh"], pt ]
RegionMember[ f_Linkage ] ^:= RegionMember[ f["mesh"] ]

RegionMember[ f_Origami, pt_ ] ^:= RegionMember[ f["mesh"], pt ]
RegionMember[ f_Origami ] ^:= RegionMember[ f["mesh"] ]


RegionBounds[ f_Linkage, type___ ] ^:= RegionBounds[ f["mesh"], type ]
RegionBounds[ f_Origami, type___ ] ^:= RegionBounds[ f["mesh"], type ]


RandomPoint[ f_Linkage, reg___ ] ^:= RandomPoint[ f["mesh"], reg ]
RandomPoint[ f_Origami, reg___ ] ^:= RandomPoint[ f["mesh"], reg ]


RegionCentroid[ f_Linkage ] ^:= RegionCentroid[ f["mesh"] ]
RegionCentroid[ f_Origami ] ^:= RegionCentroid[ f["mesh"] ]


RegionDistance[ f_Linkage, p___ ] ^:= RegionDistance[ f["mesh"], p ]
RegionDistance[ f_Origami, p___ ] ^:= RegionDistance[ f["mesh"], p ]


SignedRegionDistance[ f_Linkage, p___ ] ^:= SignedRegionDistance[ f["mesh"], p ]
SignedRegionDistance[ f_Origami, p___ ] ^:= SignedRegionDistance[ f["mesh"], p ]


(*tierEdges::usage="tierEdges[m] partitions the edges of a 2D Mechanism into tiers so that no two edges sharing a vertex are on the same tier.";*)


selectEdgeTiers[ {edgeList_, other_} ] := 
With[{
	pass =Sort/@(List@@@FindIndependentEdgeSet[UndirectedEdge@@@edgeList])
},
	{
	DeleteCases[ Sort /@ edgeList, Alternatives@@pass],
	Join[{pass},other]
	}
]

tierEdges[ m_?MechanismQ ] := With[{edges = MechanismEdges[m]},
	Reverse[NestWhile[selectEdgeTiers,{edges,{}}, Length[First[#]]>0&][[2]]]
] /; MechanismEmbeddingDimension[m]==2

tierEdges[ m_?MechanismQ ] := "nothing" /; Message[tierEdges::embdim]

tierEdges::embdim="Mechanism embedding dimension must be 2.";


incrementVertices[d_List]:=#+1&/@d
decrementVertices[d_List]:=#-1&/@d

SaveToFOLD[m_?MechanismQ , filename_String ]:=
Export[filename,
	{
	"file_spec" -> $MechanismsVersion,
	"file_creator" -> "Mechanisms",
	"file_classes" -> {"singleModel"},
	"frame_classes" -> {"creasePattern"},
	"frame_attributes" -> {
		Switch[ MechanismDisplayDimension[m], 2, "2D", 3, "3D",_, Nothing ],
		If[ Head[m]===Origami > 0, "manifold", Nothing ],
		If[ MechanismOrientedQ[ m ], "orientable", "nonOrientable" ]
	},
	"frame_unit" -> "unit",
	"vertices_coords" -> m["positions"],
	"edges_vertices" -> decrementVertices /@ MechanismConnectivity[m, "edges" -> "vertices"],
	"Faces_vertices" -> decrementVertices /@ MechanismFaces[m, "faces" -> "vertices"]
},"JSON"]


Options[LoadFromFOLD]=Join[{ "Face"->(Face[#]&)}, Options[Origami] ];

LoadFromFOLD[filename_String,opt:OptionsPattern[]]:=Module[
{
	inputData=Import[filename,"JSON"],
	coords,edges,Faces
},
	If[inputData === $Failed,
		$Failed,

		coords="vertices_coords" /. inputData;
		edges="edges_vertices" /. inputData;
		Faces="Faces_vertices" /. inputData;

		Origami[ coords, Join[ OptionValue["Face"] /@ incrementVertices /@ Faces], FilterRules[{opt},Options[Origami]] ]
	]
]


End[];

EndPackage[];


BeginPackage["Mechanisms`rigidity`"];


ToLinkage::usage=
"ToLinkage[ obj ] attempts to create a Linkage from some other object (e.g. Graph[], MeshRegion[], etc.). It takes the same options as Linkage[].";

RandomCellNetwork::usage="RandomCellNetwork[n] returns a random network of cells contained in a periodic region Rectangle[{0,0},{1,1}].";
RandomTriangulatedNetwork::usage="RandomTriangulatedNetwork[n] returns a random triangulated network contained in a periodic region Rectangle[{0,0},{1,1}].";

Henneberg::usage="Henneberg[1][{v1,v2,...}, label -> position] is a generalized Henneberg operation of type 1 that creates a new vertex at a position and attaches it to existing vertices {v1, v2, ...}. A proper Henneberg move in 2D attaches to two vertices only.
Henneberg[2][ {v1,v2,v3},label -> position] is a Henneberg operation of type 2 that subdivides an edge {v1,v2}, places a new vertex at a position, and connects it to vertex v3.
Henneberg[t][position] is a random Henneberg operation of type t, placing the new vertex at a position.";


MechanismCellData::usage = "MechanismCellData[ m, cell pattern ] returns the equilibrium data for all cells matching a pattern.
MechanismCellData[ m, positions, cell pattern ] uses provided vertex positions to compute equilibrium data where not specified explicitly.";

MechanismConstrainedCells::usage="MechanismConstrainedCells[m] returns a list of cells with infinite stiffness.
MechanismConstrainedCells[m , {head, indices}] returns a list of cells matching the specification with infinite stiffness.
MechanismConstrainedCells[m, patt] returns a list of cells matching a pattern with infinite stiffness.";


Constraints::usage="Constraints is an option in the Mechanisms package to specify additional mechanism constraints.";
CellPattern::usage="CellPattern is an option in the Mechanisms package to specify patterns for cells.";
EliminationRules::usage="EliminationRules is an option in the Mechanisms package to specify a set of replacement rules for vertex displacements.";
AddedEnergy::usage="AddedEnergy is an option in the Mechanisms package for additional terms added to an energy.";
ConstraintOutput::usage="ConstraintOutput is an option for MechanismConstraintEquations and related functions to determine that can take the value VertexPosition or VertexDisplacement.";
StepSize::usage="StepSize is an option for MechanismIsometricTrajectory[] to set the step size used to traverse the configuration space.";
OutputVariables::usage="OutputVariables is an option for MechanismInfinitesimalMotions to specify names for the output variables.";
InitialPositions::usage="InitialPositions is an option in the mechanisms package to specify initial positions.";
VertexMass::usage="VertexMass is an option in the Mechanisms package to add mass to the vertices of a mechanism.";
VertexDrag::usage="VertexDrag is an option in the mechanisms package to add drag to the vertices of a mechanism.";
StoppingCriteria::usage="StoppingCriteria is an option for MechanismIsometricTrajectory[]. It can be either None or a function that takes the current vertex positions and the tangent direction and returns True if the calculation should contimue.";


MechanismConstraintMap::usage="MechanismConstraintMap[ m, (positions) ] returns the constraint map for a mechanism in terms of VertexPosition.
MechanismConstraintMap[ m, (positions) , n ] returns the constraint map for a mechanism up to order n in terms of VertexDisplacement.
MechanismConstraintMap[ m, (positions) , Infinity ] returns the constraint map for a mechanism up to order Infinity in terms of VertexDisplacement.";

MechanismConstraintEquations::usage=
"MechanismConstraintEquations[m\!\(\*
StyleBox[\"echanism\",\nFontSlant->\"Italic\"]\)(, \!\(\*
StyleBox[\"positions\",\nFontSlant->\"Italic\"]\)), \!\(\*
StyleBox[\"order\",\nFontSlant->\"Italic\"]\)] returns constraint equations valid to some order in the displacements.
\!\(\*
StyleBox[\"order\",\nFontSlant->\"Italic\"]\) should be 1, 2 or Infinity.

Option ConstraintOutput should be set to either VertexPosition or VertexDisplacement to determine the form of the output.";

MechanismLinearConstraints::usage="MechanismLinearConstraints[mechanism (, positions)] returns solutions to all the linear constraints provided in a mechanism.";

MechanismEnergy::usage=
"MechanismEnergy[ m\!\(\*
StyleBox[\"echanism\",\nFontSlant->\"Italic\"]\) ] returns an energy expression for a Mechanism.
MechanismEnergy[ m\!\(\*
StyleBox[\"echanism\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"positions\",\nFontSlant->\"Italic\"]\) ] returns an energy expression for vertices at arbitrary positions but using the provided positions to determine equilibrium parameters where necessary.

See $DefaultMechanismStiffnesses to determine how MechanismEnergy[] sets the default stiffnesses when the stiffness is Infinity.";

CompiledMechanismEnergy::usage=
"CompiledMechanismEnergy[ m\!\(\*
StyleBox[\"echanism\",\nFontSlant->\"Italic\"]\) ] compiles an energy and gradient for a Mechanism.
CompiledMechanismEnergy[ m\!\(\*
StyleBox[\"echanism\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"positions\",\nFontSlant->\"Italic\"]\) ] compiles an energy and gradient for a Mechanism starting from a set of positions.

Undefined symbols in the energy must be set using ReplaceAll[], /.
Options:
  \"AddToEnergy\" -> energy allows additional terms, written as a function of VertexPosition[], to be added to the energy and compiled.";

CompiledMechanismEnergyQ::usage=
"CompiledMechanismEnergyQ[ energy ] returns True if and only if energy is a CompiledMechanismEnergy[] object.";

$DefaultMechanismStiffnesses::usage="$DefaultMechanismStiffnesses[\!\(\*
StyleBox[\"component\",\nFontSlant->\"Italic\"]\)] returns the default stiffness in case the constraint is rigid.
Use $DefaultMechanismStiffnesses[\"Constraints\"] to find the stiffness of added constraints.";


MechanismConstraintMatrix::usage="MechanismConstraintMatrix[ m ] returns a constraint matrix for the first-order constraints.
MechanismConstraintMatrix[ m, positions ] returns a constraint matrix using positions to determine matrix element.";

MechanismAugmentedConstraintMatrix::usage="Experimental!
MechanismAugmentedConstraintMatrix[m , pi] returns an augmented constraint matrix for mechanism m with periodic data pi.
The augmented constraint matrix has 3 additional degrees of freedoms in 2D and 6 additional degrees of freedom in 3D corresponding to deformations of the lattice vectors.";

MechanismZeroModes::usage="MechanismZeroModes[m] returns a list of numerical zero modes (linear isometries) associated with the constraints of Mechanism m.

It takes the options of MechanismConstraintMatrix[] and Eigensystem. Option Tolerance can be used to set a numerical value to correspond to 0.";

MechanismSelfStresses::usage="MechanismSelfStresses[m] returns a list of numerical self-stresses () modes associated with the constraints of Mechanism m.

It takes the options of MechanismConstraintMatrix[] and Eigensystem. Option Tolerance can be used to set a numerical value to correspond to 0.";

MechanismInfinitesimalMotions::usage=
"MechanismInfinitesimalMotions[\!\(\*StyleBox[\"Mechanism\",FontSlant->\"Italic\"]\)(, \!\(\*StyleBox[\"positions\",FontSlant->\"Italic\"]\))] returns a list of two elements: an Infinitesimal linear motion and, if necessary, a list of quadratic constraints they must satisfy.

Use option OutputVariables to control the form of the output.";

MechanismStressMatrix::usage=
"MechanismStressMatrix[ m, vec ] returns a stress matrix associated with the stress vector vec. The (i,j) component of the stress matrix is the negative of the corresponding stress vector component (and zero otherwise) and diagonal components are chosen so that rows and columns sum to zero.

(see Connelly on generic global rigidity)";


MinimizeMechanismEnergy::usage=
"MinimizeMechanismEnergy[\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"mechanism\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)], MinimizeMechanismEnergy[ \*
StyleBox[\(m\!\(\*
StyleBox[\"echanism\",\nFontSlant->\"Italic\"]\)\)], \!\(\*
StyleBox[\"energy\",\nFontSlant->\"Italic\"]\) ] minimizes the energy of a Mechanism, returning {\!\(\*
StyleBox[\"minimum\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"energy\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"mininal\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"vertex\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"positions\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\).

The optional argument \!\(\*
StyleBox[\"energy\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)can be either an expression or a compiled Mechanism energy.";

MechanismDynamicalSystem::usage=
"MechanismDynamicalSystem[\!\(\*
StyleBox[\"m\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"{\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"initial\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"positions\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"initial\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"velocities\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"{\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"time\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"variable\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"start\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"time\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"end\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"time\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\)] returns a list of functions specifying how vertices will move as a function of \!\(\*
StyleBox[\"time\",\nFontSlant->\"Italic\"]\).

Use options VertexMass and VertexDrag to set the mass and drag coefficient for the particles.";

MechanismDynamicalSystemEquations::usage=
"MechanismDynamicalSystemEquations[\!\(\*
StyleBox[\"mech\",\nFontSlant->\"Italic\"]\), {\!\(\*
StyleBox[\"variable\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"time\",\nFontSlant->\"Italic\"]\)}] returns dynamical equations for vertex positions, using \!\(\*
StyleBox[\"variable\",\nFontSlant->\"Italic\"]\) as vertex names, and \!\(\*
StyleBox[\"time\",\nFontSlant->\"Italic\"]\) as the symbol for time.

Use options VertexMass and VertexDrag to set the mass and drag coefficient for the particles.";

MechanismMonteCarloRun::usage="MechanismMonteCarloRun[ m, \[Beta], n, distribution ] attempts n Monte Carlo steps at inverse temperature \[Beta] using a probability distribution.
The results are output as {list of energies, list of positions}.";


MechanismIsometricTrajectory::usage=
"MechanismIsometricTrajectory[\!\(\*
StyleBox[\"Mechanism\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"{\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"{\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"\[CapitalDelta]x1\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"\[CapitalDelta]y1\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"..\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"..\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"n\",\nFontSlant->\"Italic\"]\)] creates a trajectory using \!\(\*
StyleBox[\"n\",\nFontSlant->\"Italic\"]\) steps through the configuration space of a Mechanism starting in the
displacement direction \!\(\*
StyleBox[\"{\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"{\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"\[CapitalDelta]x1\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"\[CapitalDelta]y1\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"..\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\",\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"..\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"}\",\nFontSlant->\"Italic\"]\).";

MechanismFindMinimalTrajectory::usage=
"MechanismFindMinimalTrajectory[\!\(\*
StyleBox[\"Mechanism\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"start\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"end\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"n\",\nFontSlant->\"Italic\"]\)] attempts to find a valid trajectory from \!\(\*
StyleBox[\"start\",\nFontSlant->\"Italic\"]\) to \!\(\*
StyleBox[\"end\",\nFontSlant->\"Italic\"]\) configurations using \!\(\*
StyleBox[\"n\",\nFontSlant->\"Italic\"]\) intermediate steps.";

GenericGloballyRigidQ::usage=
"GenericGloballyRigidQ[m] returns True if it can be determined that the system is generically globally rigid according to Connelly's criteria based on the stress matrix.
If it returns True, the specific Mechanism may still be non-rigid.";


TangentForm::usage="TangentForm[m] computes either a scalar (for generically 0D mechanisms), a vector tangent to the configuration space (for generically 1D mechanisms) or an antisymmetric matrix (for 2D).";
CriticalSet::usage="CriticalSet[m] attempts to compute the critical configuration set of a generically 0D or 1D mechanism.
CriticalSet[m, constraints ] attempts to compute the cross-section of the critical configuration set with constraints.";
CriticalSetEquations::usage="CriticalSetEquations[m] returns equations to compute the critical configuration set of a generically 0D or 1D mechanism.
CriticalSet[m, constraints ] compute equations for the cross-section of the critical configuration set with constraints.";
CriticalSetEquations::usage="CriticalSetEquations[m], CriticalSetEquations[m, constraints] returns the equations governing the critical set subject to additional optional constraints.";
ZeroLengthEdgeQ::usage="ZeroLengthEdgeQ[m, (positions)] returns True if m has any 0 length edges.";


Begin["`Private`"];

Needs["Mechanisms`"];
Needs["Mechanisms`geometry`"];


(*
	linearMotions[m, constraint matrix] finds the right null space of the constraint matrix provided and
	collects results into a displacement vector {{dx, dy,dz}, ...} or {{dx,dy}, ... }
*)
Options[linearMotions] = Options[NullSpace];

linearMotions[ m_ , rigidityMatrix_ , opt : OptionsPattern[] ]:=
Module[{dim = MechanismEmbeddingDimension[m], lin = NullSpace[rigidityMatrix,opt]},
	If[Length[lin]>0,
		Partition[ Orthogonalize[lin], {1,dim} ][[ All, All, 1 ]],
		{}
	]
]


(*
does an expression evaluate to a number if the vertex positions or displacements are set?
*)
numericExpressionQ[positions_, expression_]:=
With[{reducedExpression = (# . #&)[ Flatten[{expression /. Dispatch@PositionRules[ N[positions] ] /. VertexDisplacement[_,_]->0.001} ] ]},
	NumericQ[reducedExpression] && Chop[Im[reducedExpression]]==0
]


(*
constraintVector[ positions, constraints ] takes arbitrary constraints, written as equations or in some other form,
parses them, and returns a standard vector map from positions to a "constraint space".
*)

equationToExpressionParser={
	a_And:>List@@a,
	Equal[a_,b_]:>a-b,
	Equal[a_,b__]:>ConstantArray[a,Length[{b}]]-{b}
};

(*get a set of user-specified constraints into the form of a vector equal to zero when constraints are satisfied*)
constraintVector[positions_,None]:={}
constraintVector[positions_,constraints_]:=With[
{
equations=Flatten[{constraints} //. equationToExpressionParser],
dimensions=Dimensions[positions]
},
	equations /. Dispatch[DisplacementRules[ VertexPosition[ Range[dimensions[[1]]],All[dimensions[[2]]] ] - positions]]
]


(*
reduceConstraintToOrder[ positions, constraintMap, order ] changes the order of the constraint map.
*)

(*express the constraints to be valid at a certain order*)
reduceConstraintToOrder[positions_,constraintVector_,order_Integer?Positive]:=
Module[
{
dimensions=Dimensions[positions],
expandedExpression,x
},
	expandedExpression = constraintVector /. Dispatch[ PositionRules[ positions + x VertexDisplacement[Range[dimensions[[1]]], All[dimensions[[2]]]]]];
	Total[ D[expandedExpression,{x,#}]/Factorial[#]& /@ Range[0,order] /. x->0]
]
reduceConstraintToOrder[positions_, constraintVector_, Infinity] :=
With[
{
dimensions=Dimensions[positions]
},
	constraintVector /. Dispatch[ PositionRules[ positions + VertexDisplacement[Range[dimensions[[1]]], All[dimensions[[2]]]]]]
]

reduceConstraintToOrder[positions_,constraintVector_,_]:=constraintVector


(*
solveLinearEquations[constraint map , variables ] solves the linear constraints from constraint map explicitly.
*)

(*Fastest way to pull out equations that are definitely linear and can be solved explicitly*)
linearEquationQ[eq_,var_]:=VectorQ[D[eq,{var}],NumericQ]

(*given a set constraints, select out the linear ones and solve them.*)
solveLinearEquations[constraintVec_, var_] := Module[ {soln},
	Quiet[ soln = Solve[ Select[ constraintVec, linearEquationQ[#, var]& ] == 0, var ] ];
	If[ Head[soln] =!= Solve, soln[[1]], {} ]
]


(*
	processConstraintEquations[ initial positions, constraint map ] 
*)
processConstraintEquations[positions_, constraintEq_] := 
Module[
{
	constraints = constraintVector[ positions, constraintEq ],
	vars = Flatten @ Array[ VertexPosition, Dimensions[positions] ],
	linearEquationSelector,
	solvedLinearConstraints,
	constrainedPositions
},
	linearEquationSelector = linearEquationQ[#, vars]& /@ constraints;
	solvedLinearConstraints = Quiet[ Solve[ Pick[ constraints, linearEquationSelector ] == 0, vars ] ];

	If[Head[solvedLinearConstraints] === Solve,
		Association[
			"linear solutions"->{}, 
			"constrained positions" -> Array[VertexPosition, Dimensions[positions] ],
			"nonlinear constraints" -> constraints
		],
	
		Association[
		(*rules that solve the linear constraints*)
		"linear solutions" -> First[solvedLinearConstraints],

		(*positions with linear solutions applied*)
		"constrained positions" -> ( constrainedPositions = Array[VertexPosition, Dimensions[positions]] /. Dispatch[First[solvedLinearConstraints]] ),

		(*remaining nonlinear constraints*)
		"nonlinear constraints" -> Pick[ constraints, linearEquationSelector, False ]
		]
	]
]

nonlinearConstraintVector[positions_, constraintEq_] := 
With[{ constraints = constraintVector[ positions, constraintEq ], vars = Flatten[ Array[VertexPosition, Dimensions[positions]] ] },
	Select[ constraints, linearEquationQ[#,vars]& /@ constraintEq]
]


(*this is an old version of the function that will be removed eventually*)
dynamicVariables[m_, pinnedVertices_,initialPositions_]:=
	DeleteDuplicatesBy[Cases[
		Transpose[{Flatten @ VertexPosition[m],Flatten @ initialPositions}] /. pinnedVertices,
		{_VertexPosition,_}
	], First ]

dynamicVariables[ positions_, processedConstraints_Association ] := dynamicVariables[ positions, processedConstraints["constrained positions"] ]
dynamicVariables[ positions_, constrainedPositions_ ] := 
	DeleteDuplicatesBy[ Cases[
		Transpose[ { Flatten @ constrainedPositions, Flatten @ positions } ],
		{_VertexPosition, _}
	], First ]


cellData[ m_, positions_ , cellSpec_ : _ ] := 
Module[ { 
cells = MechanismCellList[m, cellSpec],
activeCells, inactiveCells
},
	activeCells = Cases[cells,_Rule];
	inactiveCells = DeleteCases[cells,_Rule];
	Join[
		( #[[1]] -> mechanismCellDataComponents[ m, positions, #[[1]] -> Merge[#[[2]],Join] ] ) & /@ activeCells,
		inactiveCells
	]
]

mechanismCellDataComponents[ m_, positions_ , Rule[ RigidBar[indices_], data_Association] ] := 
Module[ { automaticCells , newlengths },
	automaticCells = Position[ data["EquilibriumLength"], Automatic ];
	newlengths = DisplacementLength[m, positions, Extract[indices,automaticCells] ];
	ReplacePart[ data, "EquilibriumLength" -> ReplacePart[ data["EquilibriumLength"], Thread[Flatten[automaticCells] -> newlengths] ] ]
]

mechanismCellDataComponents[ m_, positions_ , Rule[ Spring[indices_], data_Association] ] := 
Module[ { automaticCells , newlengths , newstrains},
	automaticCells = Position[ data["EquilibriumLength"], Automatic ];

	newlengths = ReplacePart[ data["EquilibriumLength"],Thread[ Flatten[ automaticCells ] -> DisplacementLength[m, positions, Extract[indices,automaticCells] ] ] ];	
	newstrains = data["Strain"] /. {"LinearStrain" -> ((#1-#2)/#2 &) };

	ReplacePart[ data, {
		"EquilibriumLength" -> newlengths,
		"Strain" -> newstrains
		}
	]
]

mechanismCellDataComponents[ m_, positions_, Rule[TorsionalFold[indices_], data_Association ] ] :=
Module[ {automaticCells, newangles},
	automaticCells = Position[ data["Angle"], Automatic ];
	newangles = TorsionalFoldAngle[m, positions, Extract[indices,automaticCells] ];
	ReplacePart[ data, "Angle" -> ReplacePart[ data["Angle"], Thread[Flatten[automaticCells] -> newangles] ] ]
]

mechanismCellDataComponents[ m_, positions_, Rule[AngleJoint[indices_], data_Association ] ] :=
Module[ {automaticCells, newangles},
	automaticCells = Position[ data["Angle"], Automatic ];
	newangles = TurningAngle[m, positions, Extract[indices,automaticCells] ];
	ReplacePart[ data, "Angle" -> ReplacePart[ data["Angle"], Thread[Flatten[automaticCells] -> newangles] ] ]
]

mechanismCellDataComponents[ m_, positions_, Rule[ PinnedJoint[indices_], data_Association ] ] :=
Module[ { automaticCells, pos, newconstraints},
	automaticCells = Position[ data["ConstraintFunction"], Automatic ];
	pos = positions[[ Flatten[ automaticCells ] ]];
	newconstraints = ReplacePart[ data["ConstraintFunction"], Thread[ Flatten[automaticCells] -> (#1-#2 &) ] ];
	
	ReplacePart[ data, "ConstraintFunction" -> newconstraints ]
]

mechanismCellDataComponents[ m_, positions_, Rule[ cell_, data_ ]] := data


constrainedCells[ m_, pos_, cellSpec_ ] := 
With[
{
	dataToCheck = {"Stiffness", "TorsionalStiffness", "PinningStiffness", "FaceStiffness","YoungsModulus"}
},
	Flatten[( (#[[1]] -> mechanismCellDataComponents[m, pos, #[[1]] -> Merge[#[[2]],Join]] &) /@ MechanismCellList[ m, cellSpec , #1 -> Infinity ]) & /@ dataToCheck]
]


constraintEquations[ m_ , positions_ , pattern_ , constraints_] :=
	With[
	{
		arbitraryPositions = VertexPosition[m]
	},
		Flatten[{
			componentToConstraints[m , positions, arbitraryPositions, #]& /@ constrainedCells[m, positions, pattern],
			constraintVector[ positions, constraints ]
		}]
	]


componentToConstraints[ m_, positions_, arbitraryPositions_, Rule[RigidBar[indices_], data_] ]:=
	DisplacementLengthSquared[ arbitraryPositions, indices ] - data["EquilibriumLength"]^2

componentToConstraints[ m_, positions_, arbitraryPositions_, Rule[ElasticTriangle[indices_], data_] ]:=
With[{
	edges = DeleteDuplicatesBy[Flatten[Partition[indices,{1,2},1,1],2],Sort ]
},
	DisplacementLengthSquared[ arbitraryPositions, edges ] - DisplacementLengthSquared[ positions, edges ]
]

componentToConstraints[ m_, positions_, arbitraryPositions_, Rule[Spring[indices_], data_] ] :=
	MapThread[ #1[#2,#3]&, { data["Strain"], DisplacementLength[ arbitraryPositions, indices ], data["EquilibriumLength"] } ]

componentToConstraints[ m_, positions_, arbitraryPositions_, Rule[TorsionalFold[indices_], data_ ] ] :=
	TorsionalFoldAngle[ m, arbitraryPositions, indices ] - data["Angle"]

componentToConstraints[ m_, positions_, arbitraryPositions_, Rule[PinnedJoint[indices_], data_ ] ] :=
	MapThread[ #1[#2,#3]&, { data["ConstraintFunction"], VertexPosition[ indices, All[ m["EmbeddingDimension"] ] ], positions[[ indices ]] } ]

componentToConstraints[ ___ ] := {}


eliminateZeros[ expr_ ] := Pick[ expr, Not /@ PossibleZeroQ /@ expr]


reducedLinearConstraints[ positions_ , constraints_ ] :=
With[{
	processedConstraints = processConstraintEquations[positions, constraints]
},
	{
	(*the remaining variables*)
	Variables[processedConstraints["constrained positions"]],
	(*the remaining constraints after linear ones have been solved*)
	eliminateZeros[constraints /. processedConstraints["linear solutions"] ]	
	}
]


meshCells[ mr_MeshRegion ] := Module[{i},
	Table[
		Property[ MeshCells[ mr, {i, #} ], getProperties[ mr, i, # ] ]& /@ Range[ MeshCellCount[ mr, i ] ],
		{i,0,2}
	]
]

getProperties[ mr_MeshRegion, d_, cell_ ] := # -> PropertyValue[ {mr, {d, cell}}, # ] & /@ $meshRegionProperties


Options[ToLinkage]=Options[Linkage];

ToLinkage[mr_MeshRegion , opt:OptionsPattern[]]:=Linkage[
	MeshCoordinates[mr],
	meshCells[mr] /. {Line -> RigidBar, Polygon -> Face},
	opt
]

ToLinkage[{g_Graph,data___}, opt:OptionsPattern[]]:=Linkage[
	GraphEmbedding[g,data],
	RigidBar[List@@#]& /@ EdgeList[g],
	opt
]

ToLinkage[g_Graph, opt:OptionsPattern[]]:=Linkage[
	GraphEmbedding[g],
	RigidBar[List@@#]&/@EdgeList[g],
	opt
]

ToLinkage::bad="Unable to turn `1` into a Linkage.";
ToLinkage[ obj_ , OptionsPattern[]]:="nothing" /; Message[ToLinkage::bad, Head[obj] ]


(*Build a periodic Voronoi mesh by tiling the vertices and extracting only the cells associated with the original vertices.*)

Options[RandomCellNetwork]:=Join[{ "Faces"->False, "Map"->Identity }, Options[Linkage] ];

RandomCellNetwork[numPoints_Integer?( #>0 & ), opt : OptionsPattern[]]:=Module[{i,j,selectedPrimitives, positions, mappedPositions},
With[{randomPoints=RandomReal[{-1/2,1/2},{numPoints,2}]},

	With[{expandedMesh=MeshPrimitives[VoronoiMesh[Flatten[Table[randomPoints+ConstantArray[{i,j},numPoints],{i,-1,1},{j,-1,1}],2]],2]},

		selectedPrimitives = Select[expandedMesh,Or @@ RegionMember[#, randomPoints]&];
		positions = DeleteDuplicates[Flatten[selectedPrimitives[[All,1]],1]];
		mappedPositions = OptionValue["Map"] /@ positions;

		With[
		{mesh = MeshRegion[positions, Polygon /@ Flatten /@ ( selectedPrimitives[[All,1]] /. PositionIndex[positions] )]},	

			If[ BooleanQ[OptionValue["Faces"]] && OptionValue["Faces"] == True,
				Linkage[ mappedPositions, MeshCells[mesh,2] /. Polygon->Face, FilterRules[{opt}, Options[Linkage] ] ],
				Linkage[ mappedPositions, MeshCells[mesh,1] /. Line->Spring, FilterRules[{opt}, Options[Linkage] ] ]
			]

		] /; If[ MatrixQ[mappedPositions,NumericQ] && Last[Dimensions[mappedPositions]]==2,
			True,
			Message[RandomCellNetwork::map]; False
		]
	]]
]

RandomCellNetwork[numPoints_, opt : OptionsPattern[]]:="nothing" /; Message[RandomCellNetwork::num]

RandomCellNetwork::map="Option \"Map\" is not a function taking a point in 2D to a point in 2D.";
RandomCellNetwork::num="Number of points should be a positive integer.";


(*Build a periodic Voronoi mesh by tiling the vertices and extracting only the cells associated with the original vertices.*)

Options[RandomTriangulatedNetwork]:=Join[{ "Faces"->False, "Map"->Identity }, Options[Linkage] ];

RandomTriangulatedNetwork[numPoints_Integer?( #>0 & ), opt : OptionsPattern[]]:=Module[{i,j,selectedPrimitives, positions, mappedPositions},
With[{randomPoints=RandomReal[{-1/2,1/2},{numPoints,2}]},

	With[{expandedMesh=MeshPrimitives[DelaunayMesh[Flatten[Table[randomPoints+ConstantArray[{i,j},numPoints],{i,-1,1},{j,-1,1}],2]],2]},

		selectedPrimitives = Select[expandedMesh,Or @@ RegionMember[#, randomPoints]&];
		positions = DeleteDuplicates[Flatten[selectedPrimitives[[All,1]],1]];
		mappedPositions = OptionValue["Map"] /@ positions;

		With[{mesh = MeshRegion[positions, Polygon /@ Flatten /@ ( selectedPrimitives[[All,1]] /. PositionIndex[positions] )]},	

			If[ BooleanQ[OptionValue["Faces"]] && OptionValue["Faces"] == True,
				Linkage[ mappedPositions, MeshCells[mesh,2] /. Polygon->Face, FilterRules[{opt}, Options[Linkage] ] ],
				Linkage[ mappedPositions, MeshCells[mesh,1] /. Line->Spring, FilterRules[{opt}, Options[Linkage] ] ]
			]

		] /; If[ MatrixQ[mappedPositions,NumericQ] && Last[Dimensions[mappedPositions]]==2,
			True,
			Message[RandomCellNetwork::map]; False
		]
	]]
]

RandomTriangulatedNetwork[numPoints_, opt : OptionsPattern[]]:="nothing" /; Message[RandomTriangulatedNetwork::num]

RandomTriangulatedNetwork::map="Option \"Map\" is not a function taking a point in 2D to a point in 2D.";
RandomTriangulatedNetwork::num="Number of points should be a positive integer.";


Henneberg[1][n_?VectorQ, label_ -> pos_?VectorQ][ m_?MechanismQ ] :=
	AddCells[ {FreeJoint[label -> PadRight[pos, MechanismEmbeddingDimension[m]] ], RigidBar[{#,label}]& /@ n } ][m]

Henneberg[1][pos_?VectorQ][ m_?MechanismQ ] :=
	With[{ vertices = RandomSample[ Range[ m["VertexNumber"] ], 2 ] },
		Henneberg[1][vertices, Unique[] -> pos ][m]
	]


Henneberg[2][{n1_, n2_,n3_}, label_ -> pos_][ m_?MechanismQ ] :=
	PlaceVertices[label -> pos] @ AddCells[{RigidBar[{label, n3}]}] @ SubdivideCells[ {n1,n2}, label ][ m ]

Henneberg[2][pos_?VectorQ][ m_?MechanismQ ] :=
	With[{ edge = RandomChoice[ m["edges"] ] },
		Henneberg[2][Flatten[ { edge, RandomChoice[ Complement[ Range[ m["VertexNumber"] ], edge] ] } ], Unique[] -> pos ][m]
	]

Henneberg[d:1|2][m_?MechanismQ, x__ ] := Henneberg[d][x][m]


FaceToBlock[ thickness_, label_, centroid_ , normal_ , indices_ ] := 
{
	FreeJoint[ label[1] -> (centroid + thickness normal) , {"Shape"->None,"blockJoint"}],
	FreeJoint[ label[2] -> (centroid - thickness normal) , {"Shape"->None,"blockJoint"}],
	RigidBar[ {#, label[1]} , {"Shape" -> None, "blockEdge"} ]& /@ indices,
	RigidBar[ {#, label[2]} , {"Shape" -> None, "blockEdge"} ]& /@ indices
}

blockFaces[ Faces_ ][ m_?MechanismQ ] := With[
{
	normals = NormalVector[ m , Faces , "Normalize" -> False],
	centroids = Mean[m[[1,#]]]& /@ Faces,
	labels = Unique[] /@ Range[Length[Faces]]
},
	AddCells[ MapThread[ FaceToBlock[ 0.1, #1, #2, #3, #4 ]&, { labels, centroids, normals, Faces } ] ][m]
]

blockFaces[ m_?MechanismQ ] := With[ { Faces = Select[MechanismFaces[m],Length[#]>3&] }, blockFaces[ Faces ][m] ]


MechanismCellData[ m_?MechanismQ, cellSpec_ : _] := cellData[m, m["positions"], cellSpec]
MechanismCellData[ m_?MechanismQ , pos_ , cellSpec_ : _] /; VertexCoordinatesQ[ m , pos ] := cellData[m , pos , cellSpec ]

MechanismCellData::pos = "Positions cannot be those of provided mechanism.";
MechanismCellData[ m_?MechanismQ, pos_ , ___ ] := "nothing" /; Message[MechanismCellData::pos]


MechanismConstrainedCells[ m_?MechanismQ , cellSpec_ : _ ] := constrainedCells[m , m["positions"], cellSpec ]
MechanismConstrainedCells[ m_?MechanismQ , pos_ , cellSpec_ : _ ] /; VertexCoordinatesQ[m, pos] := constrainedCells[m , pos, cellSpec ]

MechanismConstrainedCells::pos="Positions `1` cannot correspond to mechanism.";
MechanismConstrainedCells[ m_?MechanismQ , pos_ , ___ ] := "nothing" /; Message[MechanismConstrainedCells::pos, pos]


Options[MechanismLinearConstraints]={Constraints -> None, CellPattern -> _};

MechanismLinearConstraints[m_?MechanismQ, OptionsPattern[] ] := With[
{
	constraintEq = constraintEquations[m, m["positions"], OptionValue[CellPattern], OptionValue[Constraints] ]
},
	processConstraintEquations[ m["positions"] , constraintEq ]["linear solutions"]
]

MechanismLinearConstraints[m_?MechanismQ, pos_, OptionsPattern[] ] := With[
{
	constraintEq = constraintEquations[m, pos, OptionValue[CellPattern], OptionValue[Constraints] ]
},
	processConstraintEquations[ pos , constraintEq ]["linear solutions"]
] /; VertexCoordinatesQ[m,pos]

MechanismLinearConstraints[m_?MechanismQ, pos_, OptionsPattern[] ] := "nothing" /; Message[MechanismLinearConstraints::pos]

MechanismLinearConstraints::pos = "Positions do not conform to the mechanism provided.";


shouldWeReduce[True, positions_, constraints_ ] :=  reducedLinearConstraints[positions, constraints][[2]]
shouldWeReduce[False, positions_, constraints_] := constraints
shouldWeReduce[_, positions_, constraints_] := (Message[MechanismConstraintMap::red]; constraints)

orderQ[Infinity|_Integer?Positive|"nothing"] := True
orderQ[ order : Except[_Rule] ] := (Message[MechanismConstraintMap::order, order]; False)

internalConstraintMap[ m_ , positions_ , order_?orderQ , reduce_ , cellPattern_ , constraints_  ] :=
	reduceConstraintToOrder[ positions, shouldWeReduce[reduce, positions, constraintEquations[m,positions,cellPattern,constraints]], order ]

MechanismConstraintMap::red="Option \"ReducedMap\" should be Boolean.";
MechanismConstraintMap::order="Order `1` must be a positive integer or Infinity.";


Options[MechanismConstraintMap] = {
	Constraints -> None,
	CellPattern -> _,
	"ReducedMap" -> False
};

MechanismConstraintMap[ m_?MechanismQ , positions_ , order : Except[_Rule|_List] : "nothing" , OptionsPattern[] ] := With[
{ res = internalConstraintMap[m, positions, order, OptionValue["ReducedMap"], OptionValue[CellPattern], OptionValue[Constraints] ] },
	res /; Head[res] =!= internalConstraintMap
] /; VertexCoordinatesQ[ m , positions ]

MechanismConstraintMap[ m_?MechanismQ , order : Except[_Rule|_List] : "nothing" , OptionsPattern[] ] := With[
{ res = internalConstraintMap[m,m["positions"], order, OptionValue["ReducedMap"], OptionValue[CellPattern], OptionValue[Constraints] ] },
	res /; Head[res] =!= internalConstraintMap
]

MechanismConstraintMap::pos="Positions cannot correspond to the mechanism.";


Options[ MechanismConstraintEquations ] = {
	Constraints -> None,
	ConstraintOutput -> VertexDisplacement,
	CellPattern -> _
};

MechanismConstraintEquations[ m_?MechanismQ , order_, opt : OptionsPattern[] ] :=
With[{res=mechanismConstraintEquationsInternal[ m , m["positions"] , order , OptionValue[CellPattern],OptionValue[Constraints], OptionValue[ConstraintOutput] ]},
	res /; Head[res] =!= mechanismConstraintEquationsInternal
]
MechanismConstraintEquations[ m_?MechanismQ , pos_ , order_ , opt : OptionsPattern[] ] :=
With[{res=mechanismConstraintEquationsInternal[ m , pos , order , OptionValue[CellPattern],OptionValue[Constraints], OptionValue[ConstraintOutput] ]},
	res /; Head[res] =!= mechanismConstraintEquationsInternal
] /; constraintEqposQ[ m , pos ]


MechanismConstraintEquations::pos="Positions `1` cannot correspond to mechanism.";
constraintEqposQ[ m_ , pos_ ] := True /; VertexCoordinatesQ[m, pos]
constraintEqposQ[ m_ , x_ ] := Message[MechanismConstraintEquations::pos, x]

MechanismConstraintEquations::order="Order of expansion, `1`,  must be a positive integer or Infinity.";
constraintEqOrderQ[ _Integer?Positive ] := True
constraintEqOrderQ[ Infinity ] := True
constraintEqOrderQ[ x_ ] := Message[MechanismConstraintEquations::order, x]

MechanismConstraintEquations::output="Option ConstraintOutput must be either VertexDisplacement or VertexPosition.";
outputTypeQ[ VertexDisplacement|VertexPosition ] := True
outputTypeQ[ x_ ] := Message[MechanismConstraintEquations::output]


mechanismConstraintEquationsInternal[ m_, positions_, order_?constraintEqOrderQ, pattern_, constraints_, output_?outputTypeQ ] :=
Module[{tmp, tmpPositions},
	tmpPositions = positions /. VertexPosition -> tmp;
	ReplaceAll[
		reduceConstraintToOrder[
			positions,
			Flatten[{
				componentToConstraints[m , tmpPositions, VertexPosition[m], # ]& /@ constrainedCells[m , tmpPositions, pattern ],
				constraintVector[ tmpPositions, constraints ]
			}],
			order
		],

		Which[
			order === Infinity && output === VertexDisplacement, Dispatch[PositionRules[ positions + VertexDisplacement[m] ]],
			IntegerQ[order] && output === VertexPosition, Dispatch[DisplacementRules[ VertexPosition[m] - positions ]],
			True, {}
		]
	] /. tmp -> VertexPosition
]


(*
evaluateEnergy[ (m), (positions), energy ] evaluates the energy (or compiled energy) in a number of different ways.
*)

evaluateEnergy[m_?MechanismQ, positions_?MatrixQ, energy: Except[_CompiledMechanismEnergy]]:=
	energy /. Dispatch[PositionRules[positions]]

evaluateEnergy[m_?MechanismQ, positions_?VectorQ, energy: Except[_CompiledMechanismEnergy]]:=
	energy /. Dispatch[Thread[Flatten[VertexPosition[m]]->positions]]

evaluateEnergy[m_?MechanismQ, positions_?MatrixQ, energy_?CompiledMechanismEnergyQ]:=
	energy[[2]][Flatten[positions],energy["data"]]

evaluateEnergy[m_?MechanismQ, positions_?VectorQ, energy_?CompiledMechanismEnergyQ]:=
	energy[[2]][positions,energy["data"]]
	
evaluateEnergy[positions_?MatrixQ, energy: Except[_CompiledMechanismEnergy]]:=
	energy /. Dispatch[PositionRules[positions]]

evaluateEnergy[positions_?VectorQ, energy: Except[_CompiledMechanismEnergy]]:=
	energy /. Dispatch[Thread[Flatten[VertexPosition[m]]->positions]]

evaluateEnergy[positions_?MatrixQ, energy_?CompiledMechanismEnergyQ]:=
	energy[[2]][Flatten[positions],energy["data"]]

evaluateEnergy[positions_?VectorQ, energy_?CompiledMechanismEnergyQ]:=
	energy[[2]][positions,energy["data"]]


(*
analyticEnergyQ[ energy , positions ] is energy a potentially valid analytic expression for an energy?
*)
analyticEnergyQ[Automatic , positions_]:=True

analyticEnergyQ[energyExpression_ , positions_]:=
With[{
	number = N[ energyExpression /. Dispatch[PositionRules[positions]] ]
},
	Im[Chop[number]] == 0 && NumericQ[number] && Chop[Im[number]] == 0
]


$DefaultMechanismStiffnesses["Methods"]={RigidBar, Spring, TorsionalFold, FreeJoint, AngleJoint, Constraints};
$DefaultMechanismStiffnesses[RigidBar]=1;
$DefaultMechanismStiffnesses[Spring]=1;
$DefaultMechanismStiffnesses[TorsionalFold]=10^(-4);
$DefaultMechanismStiffnesses[ElasticTriangle] = 1;
$DefaultMechanismStiffnesses[PinnedJoint]=10^(-4);
$DefaultMechanismStiffnesses[AngleJoint]=10^(-4);
$DefaultMechanismStiffnesses["SmallestLength"]=10^(-6);
$DefaultMechanismStiffnesses[Constraints]=10^(-1);

$DefaultMechanismStiffnesses::err="`1` does not have a default stiffness.";
$DefaultMechanismStiffnesses[s_]:="nothing"/; Message[$DefaultMechanismStiffnesses::err,s]


Options[MechanismEnergy]={Constraints -> None, CellPattern -> _};

MechanismEnergy[ m_?MechanismQ, OptionsPattern[] ] :=
	mechanismEnergyInternal[ m , m["positions"] , OptionValue[Constraints] , OptionValue[CellPattern] ]
MechanismEnergy[ m_?MechanismQ , pos_ , OptionsPattern[] ] :=
	mechanismEnergyInternal[ m , pos , OptionValue[Constraints], OptionValue[CellPattern] ] /; VertexCoordinatesQ[ m , pos ]

MechanismEnergy::pos="Positions `1` cannot correspond to mechanism.";
MechanismEnergy[ m_?MechanismQ , pos_ , OptionsPattern[] ] := "nothing" /; Message[MechanismEnergy::pos, pos]


mechanismEnergyInternal[ m_, positions_, constraints_, pattern_ ] := 
With[
{
	components = cellData[m, positions, pattern],
	constraintData = processConstraintEquations[positions , constraints] ,
	arbitraryPositions = VertexPosition[m]
},
	Total @ Flatten[{
		componentEnergy[m, positions, arbitraryPositions, #]& /@ components,
		$DefaultMechanismStiffnesses[Constraints] (constraintData["nonlinear constraints"] . constraintData["nonlinear constraints"])/2
	}] /. constraintData["linear solutions"]
]


MechanismEnergy::EdgeLength="Warning: Edges `1` with length smaller than `2` have had their strains clipped.";

componentEnergy[m_, positions_, arbitraryPositions_, Rule[RigidBar[indices_],data_]] :=
With[{
	(*this puts a lower bound on the smallest length we use to divide the strain by*)
	smallestLengths = Clip[ data["EquilibriumLength"]^2, {$DefaultMechanismStiffnesses["SmallestLength"]^2, Infinity} ],
	(*this is to put out a warning in case some are short.*)
	shortEdges=Pick[ indices , (# < $DefaultMechanismStiffnesses["SmallestLength"])& /@ data["EquilibriumLength"]]
},
	If[Length[shortEdges]>0, Message[MechanismEnergy::EdgeLength, shortEdges, $DefaultMechanismStiffnesses["SmallestStiffness"] ] ];
	( data["Stiffness"] /. Infinity -> $DefaultMechanismStiffnesses[RigidBar] ) ( 
		(DisplacementLengthSquared[arbitraryPositions, indices ] - data["EquilibriumLength"]^2)/smallestLengths
	)^2/8
]

componentEnergy[m_, positions_, arbitraryPositions_, Rule[Spring[indices_],data_]] :=
With[{
	stiffness = data["Stiffness"] /. Infinity -> $DefaultMechanismStiffnesses[Spring]
},
	stiffness MapThread[ #1[#2,#3]^2&, { data["Strain"], DisplacementLength[arbitraryPositions, indices ], data["EquilibriumLength"] } ]
]

componentEnergy[m_, positions_, arbitraryPositions_, Rule[TorsionalFold[indices_], data_]] :=
With[{
	stiffness = data["TorsionalStiffness"] /. Infinity -> $DefaultMechanismStiffnesses[TorsionalFold]
},
	stiffness (TorsionalFoldAngle[m,arbitraryPositions, indices ] - data["Angle"])^2/2
]

componentEnergy[m_, positions_, arbitraryPositions_, Rule[ElasticTriangle[indices_], data_]] :=
With[{
	stiffness = data["YoungsModulus"] /. Infinity -> $DefaultMechanismStiffnesses[ElasticTriangle],
	poissonratio = data["PoissonRatio"]
},
	MapThread[
		elasticFaceEnergy[ #1, #2, positions[[ #3 ]], arbitraryPositions[[ #3 ]] ]&,
		{stiffness, poissonratio, indices}
	]
]

componentEnergy[m_, positions_, arbitraryPositions_, Rule[AngleJoint[indices_], data_]] :=
With[{
	stiffness = data["AngleStiffness"] /. Infinity -> $DefaultMechanismStiffnesses[TorsionalFold]
},
	stiffness (TurningAngle[m,arbitraryPositions, indices ] - data["Angle"])^2/2
]

componentEnergy[m_, positions_, arbitraryPositions_, Rule[PinnedJoint[indices_], data_]] :=
With[{
	stiffness = data["PinningStiffness"] /. Infinity -> $DefaultMechanismStiffnesses[TorsionalFold]
},
	stiffness MapThread[ #1[#2,#3]^2/2& , { data["ConstraintFunction"], arbitraryPositions[[ indices ]], positions[[ indices ]] } ]
]

componentEnergy[m_,_,_,_] := {0}


elasticFaceEnergy[Y_,\[Nu]_,{{x1_,y1_,z1_},{x2_,y2_,z2_},{x3_,y3_,z3_}},{{X1_,Y1_,Z1_},{X2_,Y2_,Z2_},{X3_,Y3_,Z3_}}] := 
1/(2 (1+\[Nu])) Y ((Sqrt[(-x1+x2)^2+(-y1+y2)^2+(-z1+z2)^2]-Sqrt[(-X1+X2)^2+(-Y1+Y2)^2+(-Z1+Z2)^2])^2/((-x1+x2)^2+(-y1+y2)^2+(-z1+z2)^2)+(Sqrt[1-(x1^2+x2 x3-x1 (x2+x3)+(y1-y2) (y1-y3)+(z1-z2) (z1-z3))^2/(((x1-x2)^2+(y1-y2)^2+(z1-z2)^2) ((x1-x3)^2+(y1-y3)^2+(z1-z3)^2))] Sqrt[(-x1+x3)^2+(-y1+y3)^2+(-z1+z3)^2]-Sqrt[1-(X1^2+X2 X3-X1 (X2+X3)+(Y1-Y2) (Y1-Y3)+(Z1-Z2) (Z1-Z3))^2/(((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2) ((X1-X3)^2+(Y1-Y3)^2+(Z1-Z3)^2))] Sqrt[(-X1+X3)^2+(-Y1+Y3)^2+(-Z1+Z3)^2])^2/((1-(x1^2+x2 x3-x1 (x2+x3)+(y1-y2) (y1-y3)+(z1-z2) (z1-z3))^2/(((x1-x2)^2+(y1-y2)^2+(z1-z2)^2) ((x1-x3)^2+(y1-y3)^2+(z1-z3)^2))) ((-x1+x3)^2+(-y1+y3)^2+(-z1+z3)^2))+((Sqrt[(-X1+X2)^2+(-Y1+Y2)^2+(-Z1+Z2)^2] (x1^2+x2 x3-x1 (x2+x3)+(y1-y2) (y1-y3)+(z1-z2) (z1-z3)) Sqrt[(-x1+x3)^2+(-y1+y3)^2+(-z1+z3)^2])/(Sqrt[((x1-x2)^2+(y1-y2)^2+(z1-z2)^2) ((x1-x3)^2+(y1-y3)^2+(z1-z3)^2)])-(Sqrt[(-x1+x2)^2+(-y1+y2)^2+(-z1+z2)^2] (X1^2+X2 X3-X1 (X2+X3)+(Y1-Y2) (Y1-Y3)+(Z1-Z2) (Z1-Z3)) Sqrt[(-X1+X3)^2+(-Y1+Y3)^2+(-Z1+Z3)^2])/(Sqrt[((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2) ((X1-X3)^2+(Y1-Y3)^2+(Z1-Z3)^2)]))^2/(4 ((-x1+x2)^2+(-y1+y2)^2+(-z1+z2)^2) (1-(x1^2+x2 x3-x1 (x2+x3)+(y1-y2) (y1-y3)+(z1-z2) (z1-z3))^2/(((x1-x2)^2+(y1-y2)^2+(z1-z2)^2) ((x1-x3)^2+(y1-y3)^2+(z1-z3)^2))) ((-x1+x3)^2+(-y1+y3)^2+(-z1+z3)^2)))+1/(2 (1-2 \[Nu]) (1+\[Nu])) Y (-((Sqrt[(-x1+x2)^2+(-y1+y2)^2+(-z1+z2)^2]-Sqrt[(-X1+X2)^2+(-Y1+Y2)^2+(-Z1+Z2)^2])/Sqrt[(-x1+x2)^2+(-y1+y2)^2+(-z1+z2)^2])-(Sqrt[1-(x1^2+x2 x3-x1 (x2+x3)+(y1-y2) (y1-y3)+(z1-z2) (z1-z3))^2/(((x1-x2)^2+(y1-y2)^2+(z1-z2)^2) ((x1-x3)^2+(y1-y3)^2+(z1-z3)^2))] Sqrt[(-x1+x3)^2+(-y1+y3)^2+(-z1+z3)^2]-Sqrt[1-(X1^2+X2 X3-X1 (X2+X3)+(Y1-Y2) (Y1-Y3)+(Z1-Z2) (Z1-Z3))^2/(((X1-X2)^2+(Y1-Y2)^2+(Z1-Z2)^2) ((X1-X3)^2+(Y1-Y3)^2+(Z1-Z3)^2))] Sqrt[(-X1+X3)^2+(-Y1+Y3)^2+(-Z1+Z3)^2])/(Sqrt[1-(x1^2+x2 x3-x1 (x2+x3)+(y1-y2) (y1-y3)+(z1-z2) (z1-z3))^2/(((x1-x2)^2+(y1-y2)^2+(z1-z2)^2) ((x1-x3)^2+(y1-y3)^2+(z1-z3)^2))] Sqrt[(-x1+x3)^2+(-y1+y3)^2+(-z1+z3)^2]))^2 \[Nu]


Options[mechanismEnergyFunction] := Options[MechanismEnergy];

mechanismEnergyFunction[m_?MechanismQ, opt : OptionsPattern[]] :=
Module[
{
energy = MechanismEnergy[m, opt ], 
symb = Array[Unique["x"]&,Length[Flatten[VertexPosition[m]]]], 
map
},
	map=Dispatch@Thread[Flatten[VertexPosition[m]]->symb];
	Function @@ { symb, energy /. map}
]


CompiledMechanismEnergy[variables_List,energy_CompiledFunction,gradient_CompiledFunction]["variables"]:=variables

Format[CompiledMechanismEnergy[variables_List,energy_CompiledFunction,gradient_CompiledFunction]]:=
StringJoin[
	"CompiledMechanismEnergy[",
	If[Length[variables]<10,
		ToString[variables],
		"{"<>ToString[First[variables]<>"..."<>Last[variables]<>"}"]
	],
	"]"
]


CompiledMechanismEnergy[variables_?VectorQ,energy_CompiledFunction,gradient_CompiledFunction]["data"]:=variables

CompiledMechanismEnergy[variables_List,energy_CompiledFunction,gradient_CompiledFunction]["energy"][pos_?(VectorQ[#,NumericQ]&),data_] := 
	energy[pos,data]
CompiledMechanismEnergy[variables_List,energy_CompiledFunction,gradient_CompiledFunction]["gradient"][pos_?(VectorQ[#,NumericQ]&),data_] := 
	gradient[pos,data]

CompiledMechanismEnergy[variables_List,energy_CompiledFunction,gradient_CompiledFunction][v_?(VectorQ[#,NumericQ]&)] := 
	CompiledMechanismEnergy[v,energy,gradient]


CompiledMechanismEnergyQ[CompiledMechanismEnergy[variables_,energy_CompiledFunction,gradient_CompiledFunction]]:=VectorQ[variables]
CompiledMechanismEnergyQ[_]:=False

compiledNumericalMechanismEnergyQ[CompiledMechanismEnergy[variables_,energy_CompiledFunction,gradient_CompiledFunction]]:=VectorQ[variables,NumericQ]
compiledNumericalMechanismEnergyQ[_]:=False


Options[CompiledMechanismEnergy]:={Constraints->None, AddedEnergy->0, CellPattern -> _};


CompiledMechanismEnergy[m_?MechanismQ, opt:OptionsPattern[]]:=
Module[{res=compiledmechanismEnergyInternal[m,m["positions"],OptionValue[Constraints],OptionValue[AddedEnergy], OptionValue[CellPattern] ]},
	res/;res=!=$Failed
]

CompiledMechanismEnergy[m_?MechanismQ, pos_, opt:OptionsPattern[]]:=
Module[{res=compiledmechanismEnergyInternal[m,pos,OptionValue[Constraints],OptionValue[AddedEnergy], OptionValue[CellPattern] ]},
	res/;res=!=$Failed
] /; VertexCoordinatesQ[m,pos]

CompiledMechanismEnergy::pos="Provided positions `1` do not correspond to Mechanism.";
CompiledMechanismEnergy[m_?MechanismQ, pos_ ,opt:OptionsPattern[]]:="nothing"/;Message[CompiledMechanismEnergy::pos, pos]


compiledmechanismEnergyInternal[m_,positions_,constraints_,additional_, pattern_]:=
Module[
{
	symbols,energy=MechanismEnergy[m,positions,Constraints->constraints, CellPattern -> _]+additional
},
	symbols=DeleteDuplicates@Select[Cases[energy,_Symbol,Infinity],Not[NumericQ[#]]&];

	Check[
		CompiledMechanismEnergy[
			symbols,
			compileEnergy[m,energy,symbols,$MechanismCompilationTarget],
			compileGradient[m,energy,symbols,$MechanismCompilationTarget]
		],
		$Failed
	]
]


compileEnergy[m_,energy_,symbols_,compiler_]:=Module[
{
body,
variables=Flatten[VertexPosition[m]],
injector,c,
dataInjector,data
},
	injector=Dispatch@Thread[variables->(Hold[c[[#]]]&/@Range[Length@variables])];
	dataInjector=Dispatch@Thread[symbols->(Hold[data[[#]]]&/@Range[Length@symbols])];
	body=energy/.injector/.dataInjector;
	ReleaseHold[Hold[Compile][
		{{c,_Real,1},{data,_Real,1}},
		body,
		RuntimeOptions->{"Speed","EvaluateSymbolically"->False},
		CompilationTarget->compiler,
		Parallelization->True
	]]
]


compileGradient[m_,energy_,symbols_,compiler_]:=Module[
{
body,
variables=Flatten[VertexPosition[m]],
injector,c,
dataInjector,data
},
	injector=Dispatch@Thread[variables->(Hold[c[[#]]]&/@Range[Length@variables])];
	dataInjector=Dispatch@Thread[symbols->(Hold[data[[#]]]&/@Range[Length@symbols])];
	body=D[energy,{variables}]/.injector/.dataInjector;
	ReleaseHold[Hold[Compile][
		{{c,_Real,1},{data,_Real,1}},
		body,
		RuntimeOptions->{"Speed","EvaluateSymbolically"->False},
		CompilationTarget->compiler,
		Parallelization->True
	]]
]


Options[MinimizeMechanismEnergy]=Join[ {InitialPositions->Automatic}, Options[MechanismEnergy], Options[FindMinimum] ];
SetAttributes[MinimizeMechanismEnergy,HoldRest];

MinimizeMechanismEnergy::badinitial="Initial conditions are not well-formed numeric positions matching the Mechanism.";
MinimizeMechanismEnergy::data="Data for compiled function not a vector of numerical values.";
MinimizeMechanismEnergy::energy="Energy is not numerical at initial condition.";


MinimizeMechanismEnergy[ m_?MechanismQ, energy : Except[_Rule] : Automatic, opt : OptionsPattern[]] :=
Module[{res = minimizeMechanismEnergyInternal[ m, energy, OptionValue[InitialPositions], {opt} ] },
	res /; res =!= $Failed
]

minimizeMechanismEnergyInternal[ m_, energyExpression_, initial_, opt_ ] := Module[
{
	energy = minimizeMechanismEnergyComputeEnergy[m, energyExpression, opt ],
	initialPositions = minimizeMechanismEnergyInitialPositions[m, initial ],
	res
},
	If[energy =!= $Failed && initialPositions =!= $Failed,
		res = minimizeMechanismEnergyMinimize[ m, energy, initialPositions, opt ];
		If[ Length[ Dimensions[initial] ] == 3, res, First[res] ],
		$Failed
	]
]


(*
	parse the arguments
*)
minimizeMechanismEnergyInitialPositions[ m_, Automatic ] := {m["positions"]}
minimizeMechanismEnergyInitialPositions[ m_, initialPositions_?MatrixQ ] := {initialPositions} /; NumericCoordinatesQ[m, initialPositions]
minimizeMechanismEnergyInitialPositions[ m_, initialPositions_?(ArrayQ[#,_,NumericQ]&) ] := initialPositions /; Dimensions[initialPositions][[2;;]] == {m["VertexNumber"],m["EmbeddingDimension"]}
minimizeMechanismEnergyInitialPositions[ m_, pos_ ] := (Message[MinimizeMechanismEnergy::badinitial]; $Failed)

(*
	parse the energy we want
*)

minimizeMechanismEnergyComputeEnergy[ m_, Automatic, options_ ] := MechanismEnergy[m, FilterRules[{options}, Options[MechanismEnergy]] ]
minimizeMechanismEnergyComputeEnergy[ m_, energy_?compiledNumericalMechanismEnergyQ, options_ ] := energy
minimizeMechanismEnergyComputeEnergy[ m_, analyticEnergy_, options_ ] := analyticEnergy /; analyticEnergyQ[analyticEnergy, m["positions"]]
minimizeMechanismEnergyComputeEnergy[ m_, energy_?CompiledMechanismEnergyQ, options_ ] := (Message[MinimizeMechanismEnergy::data]; $Failed)
minimizeMechanismEnergyComputeEnergy[ m_, badEnergy_, options_ ] := (Message[MinimizeMechanismEnergy::energy]; $Failed)


(*

energy is a compiled function

*)
minimizeMechanismEnergyMinimize[ m_, energy_?compiledNumericalMechanismEnergyQ, initialPositions_, options_ ] :=
Module[
{
	constraintData = processConstraintEquations[
		m["positions"],
		MechanismConstraintMap[m, m["positions"], Sequence@@FilterRules[options,Options[MechanismConstraintMap]]]
(*		MechanismConstraintEquations[ m, m["positions"], Infinity, Flatten @ {ConstraintOutput \[Rule] VertexPosition, FilterRules[options, Options[MechanismConstraintEquations] ] } ]*)
	],
	data = energy["data"], allVariables = Flatten[VertexPosition[m]],
	variables, variableSpecifications, linearConstraints,
	
	minimizationOptions = FilterRules[options, Options[FindMinimum]],
	minimizationResults
},
	variables = Flatten[constraintData[ "constrained positions" ]];
	variableSpecifications = Transpose[ {allVariables, Flatten @ #} ]& /@ initialPositions;

	(*turn the solved linear constraints into simple linear constraints*)
	linearConstraints = Equal @@@ constraintData[ "linear solutions" ];

	minimizationResults = Cases[
		Map[ 
			executeMinimizationCompiled[
				energy["energy"][variables, data], 
				linearConstraints, 
				#, 
				energy["gradient"][variables,data], 
				minimizationOptions 
			]&,
			variableSpecifications
		],
		
		{ _ , {__Rule} }
	];
	
	If[Length[minimizationResults]>0,
		{ #[[1]] , constraintData["constrained positions"] /. Dispatch[ #[[2]] ] }& /@ minimizationResults,
		{}
	]
]

executeMinimizationCompiled[ energy_, {}, initial_, gradient_, options_ ]:= Module[ {tmp},
	Check[
		tmp = FindMinimum @@ {
			energy, Sequence @@ initial, Gradient :> gradient, options 
		},
		If[Length[tmp]==2, {"error"@tmp[[1]], tmp[[2]] }, $Failed ]
	]
]
executeMinimizationCompiled[ energy_, constraints_, initial_, gradient_, options_ ]:= Module[ {tmp},
	tmp = Check[
		FindMinimum @@ {
			Flatten[ {energy, constraints} ], Sequence @@ initial, Gradient :> gradient, options 
		},
		If[Length[tmp]==2, {"error" @ tmp[[1]], tmp[[2]] }, $Failed ]
	]
]

(*

energy is an analytic expression

*)
minimizeMechanismEnergyMinimize[ m_, energy_, initialPositions_, options_ ] := 
Module[
{
	constraintData = processConstraintEquations[
		m["positions"],
		MechanismConstraintMap[m, m["positions"], Sequence@@FilterRules[options,Options[MechanismConstraintMap]]]
(*		MechanismConstraintEquations[ m, m["positions"], Infinity, Flatten @ {ConstraintOutput \[Rule] VertexPosition, FilterRules[options, Options[MechanismConstraintEquations] ] } ]*)
	],
	constrainedEnergy, variableSpecifications,
	minimizationOptions = FilterRules[options,Options[FindMinimum]],
	minimizationResults
},
	variableSpecifications = dynamicVariables[ #, constraintData ]& /@ initialPositions;
	constrainedEnergy = energy /. constraintData["linear solutions"];

	minimizationResults = Cases[
		Map[ executeMinimizationAnalytic[ constrainedEnergy, #, minimizationOptions ] & , variableSpecifications ],
		{ _, {__Rule}}
	];

	If[Length[minimizationResults]>0,
		{ #[[1]] , constraintData["constrained positions"] /. Dispatch[ #[[2]] ] }& /@ minimizationResults,
		{}
	]
]

executeMinimizationAnalytic[ energy_, data_, options_ ] := Module[ {tmp}, 
	Check[ tmp = FindMinimum @@ {energy, data, Sequence @@ options },
		If[Length[tmp]==2, { "error" @ tmp[[1]], tmp[[2]] } , $Failed ]
	]
]


Options[mechanismConstraintMatrixFunction]={ Constraints -> None, CellPattern -> _,   "options" -> {} };

mechanismConstraintMatrixFunction[ m_?MechanismQ , opt : OptionsPattern[] ] := 
Module[ { constraintMatrix, vars , symb = Array[Unique["x"]&,Length[Flatten[VertexPosition[m]]]], map, positions = VertexPosition[m], f, p = Unique[] },
	constraintMatrix = internalConstraintMatrix[m, positions, OptionValue[Constraints], OptionValue[CellPattern], False];
	
	map=Dispatch@Thread[Flatten[positions]->symb];
	Compile @@ { symb, Normal@constraintMatrix /. map, If[ Length[OptionValue["options"]] == 0 , Nothing , OptionValue["options"] ] }
]


Options[MechanismConstraintMatrix]={ Constraints -> None, CellPattern -> _, "ReducedMap" -> False };

$mechanismSmallestStress = 10^(-6);

reduceQ[_?BooleanQ] := True
reduceQ[ _ ] := (Message[MechanismConstraintMatrix::red]; False)

MechanismConstraintMatrix::red="Option \"ReducedMap\" should be Boolean.";
MechanismConstraintMatrix::stressed="Warning: Mechanism is stressed (residual `1` is larger than `2`). Constraint matrix may not be useful.";

internalConstraintMatrix[ m_, positions_ , constraints_ , cellPattern_ , reduce_?reduceQ ] :=
Module[{ arrays , names},With[{
	equations = If[reduce, reducedLinearConstraints[positions/.VertexPosition -> names, #]& , {Flatten[VertexPosition[m]],#}& ] @ constraintEquations[m,positions/.VertexPosition -> names,cellPattern,constraints]
},
	arrays = CoefficientArrays[ reduceConstraintToOrder[positions/.VertexPosition -> names, equations[[2]], 1] /. names ->VertexPosition, equations[[1]] /. VertexPosition -> VertexDisplacement];
	If[Length[arrays]>1,
		If[Norm[N[arrays[[1]]]] > $mechanismSmallestStress, Message[MechanismConstraintMatrix::stressed, Norm[arrays[[1]]], $mechanismSmallestStress ] ; arrays[[2]]];
		arrays[[2]],
		$Failed
	]
]]


MechanismConstraintMatrix[m_?MechanismQ, opt : OptionsPattern[] ] := 
With[{
	res=internalConstraintMatrix[m, m["positions"], OptionValue[Constraints], OptionValue[CellPattern], OptionValue["ReducedMap"]]
	},
	res /; Head[res] =!= internalConstraintMatrix && res =!= $Failed
]

MechanismConstraintMatrix[m_ , positions_ , opt : OptionsPattern[] ] :=
With[{
	res=internalConstraintMatrix[m, positions, OptionValue[Constraints], OptionValue[CellPattern], OptionValue["ReducedMap"]]
	},
	res /; Head[res] =!= internalConstraintMatrix && res =!= $Failed
] /; VertexCoordinatesQ[m, positions]


Options[MechanismAugmentedConstraintMatrix] = Options[MechanismConstraintMatrix];

MechanismAugmentedConstraintMatrix[ m_?MechanismQ, pi_?PeriodicIdentificationDataQ, opt : OptionsPattern[] ] :=
With[ { res = augmentedConstraintMatrixInternal[ m , pi, {opt} ]},
	res /; MatrixQ[ res ]
]


augmentedConstraintMatrixInternal[ m_ , pi_ , options_ ] :=
Module[{
	constraints = pi["LatticeVectors" -> designArbitraryBasis[pi,"a"]][Constraints],
	constraintEq, variables
},
	constraintEq = MechanismConstraintMap[m, 1, Normal@Merge[{{Constraints -> constraints},options},Flatten] ];
	variables = Flatten[ {VertexDisplacement[m], Array["a", numberOfLatticeElements[ m["EmbeddingDimension"] ] ] } ];
	
	CoefficientArrays[ constraintEq , variables ][[2]]
]


numberOfLatticeElements[2] = 3;
numberOfLatticeElements[3] = 6;

designArbitraryBasis[ pi_ , name_ ] := 
Module[
{
	d = pi[ "EmbeddingDimension" ],
	basis = pi[ "LatticeVectors" ]
},
	Switch[d,
		2, { basis[[1]] + {name[1], 0} , basis[[2]] + {name[2], name[3]} },
		3, { basis[[1]] + {name[1], 0, 0} , basis[[2]] + {name[2], name[3], 0} , basis[[3]] + {name[4], name[5], name[6] }},
		_ , $Failed
	]
]


Options[MechanismZeroModes] = Join[ {Constraints -> None, CellPattern -> _}, Options[Eigensystem] , {Tolerance->10^(-10)}];

MechanismZeroModes::num="The constraint matrix is not numerical.";

MechanismZeroModes[m_?MechanismQ, opt : OptionsPattern[]] := MechanismZeroModes[ m, m["positions"], opt ]
MechanismZeroModes[m_?MechanismQ, positions_, opt : OptionsPattern[]]:=
Module[{
	mat=internalConstraintMatrix[m, positions,OptionValue[Constraints], OptionValue[CellPattern], False],
	dim=MechanismEmbeddingDimension[m],
	evalues
},(
	evalues = Chop[Eigensystem[ Transpose[mat] . mat, FilterRules[ {opt}, Options[Eigenvalues] ]], OptionValue[Tolerance]];
	Partition[#, dim]& /@ Take[evalues[[2]], -Count[evalues[[1]],0]]
	) /; If[Not @ MatrixQ[mat, NumericQ],Message[MechanismZeroModes::num]; False, True]
] /; NumericCoordinatesQ[m, positions ]

MechanismZeroModes::vcoord="Vertex positions do not match Mechanism.";
MechanismZeroModes[m_?MechanismQ, Except[ __Rule ], OptionsPattern[] ] := "nothing" /; Message[MechanismZeroModes::vcoord]


Options[MechanismSelfStresses] = Join[ {Constraints -> None, CellPattern -> _}, Options[Eigensystem] , {Tolerance->10^(-10)}];

MechanismSelfStresses[m_?MechanismQ, opt : OptionsPattern[]] := MechanismSelfStresses[ m, m["positions"], opt ]
MechanismSelfStresses[m_?MechanismQ, positions_, opt : OptionsPattern[]]:=
Module[{
	mat=internalConstraintMatrix[m, positions,OptionValue[Constraints], OptionValue[CellPattern], False],
	evalues
},(
	evalues = Chop[Eigensystem[ mat . Transpose[mat] , FilterRules[ {opt}, Options[Eigenvalues] ]], OptionValue[Tolerance]];
	Take[evalues[[2]], -Count[evalues[[1]],0]]
	) /; If[Not @ MatrixQ[mat, NumericQ],Message[MechanismSelfStresses::num]; False, True]
] /; NumericCoordinatesQ[m, positions ]

MechanismSelfStresses::vcoord="Vertex positions do not match Mechanism.";
MechanismSelfStresses[m_?MechanismQ, Except[ __Rule ], OptionsPattern[] ] := "nothing" /; Message[MechanismSelfStresses::vcoord]


(*
MechanismStressMatrix[ m, vec ] returns a stress matrix associated with the stress vector vec. The (i,j) component of the stress matrix is the negative of the corresponding
stress vector component (and zero otherwise) and diagonal components are chosen so that rows sum to zero.

Columns should also sum to zero.

(see Connelly on generic global rigidity)
*)

MechanismStressMatrix[ m_?MechanismQ, stressVector_ ] :=
Module[ { offDiagonalComponents, diagonalComponents, edges = MechanismConnectivity[m, "vertices" -> "edges"] },
	offDiagonalComponents = 
	(
		(*symmetrize*)
		(# + Transpose[#] & )
	) @ SparseArray[ 
		Thread[ m["edges"] -> -stressVector ],
		{ m["VertexNumber"], m["VertexNumber"] }
	];
	diagonalComponents = SparseArray[
		Thread[ ({#,#}& /@ Range[ m["VertexNumber"] ]) -> (- Total /@ Map[ offDiagonalComponents[[ #[[1]], #[[2]] ]]&, MechanismConnectivity[m, "vertices" -> "edges"] , {2} ]) ],
		{ m["VertexNumber"], m["VertexNumber"] }
	];
	
	offDiagonalComponents + diagonalComponents
] /; MechanismStressMatrixArgumentsQ[ m, stressVector ]

MechanismStressMatrix::svec="Stress vector is not a vector.";
MechanismStressMatrix::len="Stress vector length is not the same as the number of edges.";

MechanismStressMatrixArgumentsQ[ m_, stressVector_ ] := Which[
	Not @ VectorQ[ stressVector ] ,
		Message[MechanismStressMatrix::svec]; False,
	Length[m["edges"]] != Length[stressVector],
		Message[MechanismStressMatrix::len]; False,
	True, True
]


Options[MechanismInfinitesimalMotions]=Join[{Constraints->None, OutputVariables->Automatic}, Options[NullSpace] ];


MechanismInfinitesimalMotions[m_?MechanismQ, positions: Except[_Rule] : Automatic, opt:OptionsPattern[]]:=
With[{
	actualPositions=If[positions===Automatic,m["positions"],positions]
},
	Module[
		{res=MechanismInfinitesimalMotionsInternal[m,actualPositions,OptionValue[Constraints],OptionValue[OutputVariables], FilterRules[{opt},Options[NullSpace]]]},
		res /; res=!=$Failed
	 ] /; VertexCoordinatesQ[m,actualPositions]
]

MechanismInfinitesimalMotions::failed="Failed to find an appropriate solution. Check the constraints.";
MechanismInfinitesimalMotions::pos="Positions do not match those of Mechanism.";
MechanismInfinitesimalMotions[m_?MechanismQ, positions : Except[Automatic|_Rule], OptionsPattern[]]:="nothing"/;
	Which[
		Not[VertexCoordinatesQ[m,positions]],
			Message[MechanismInfinitesimalMotions::pos]; False,
		True, False
	]


MechanismInfinitesimalMotions::var="Variables listed are not of the form VertexDisplacement[n,c].";
MechanismInfinitesimalMotions::sing="Jacobian matrix is singular. Recovering using generic variables.";
MechanismInfinitesimalMotions::jac="Jacobian matrix may not be invertible. Choosing different displacements may give better results.";
MechanismInfinitesimalMotions::lin="Found `1` linear motions so `1` displacements are needed.";


MechanismInfinitesimalMotionsInternal[m_,positions_,inputConstraints_,v_,nullspaceOptions_]:=
Module[{matrix,dependencies,solution},
With[{
variables=Flatten[VertexDisplacement[m]],

(*collect all the constraints valid to 2nd order in the displacements *)
equations = internalConstraintMap[m, positions , 2, False, _, inputConstraints]
},
	(*linearize the equations*)
	matrix=CoefficientArrays[equations,variables, "Symmetric"->True];

	(*
	Find dependencies among the linear equations.

	When the constraints arise only from edge stretching, these dependences are the self-stresses.
	*)
	dependencies=Orthogonalize[NullSpace[Transpose[matrix[[2]]],nullspaceOptions]];

	(*the linear displacements that are allowed*)
	solution=linearMotionsToDisplacementRules[m,linearMotions[m,matrix[[2]],nullspaceOptions],v];

	Which[
		solution===$Failed, $Failed,

		Length[solution]>0,
			Expand[(*analytical processing that I prefer, but should not take a long time to perform*)
				{
				VertexDisplacement[m],
				If[Length[dependencies]>0,dependencies . equations,{}]
				} /. Dispatch[solution]
			],
		
		(*no solution but no errors either*)
		True,
			Message[MechanismInfinitesimalMotions::failed];
			{{},{}}
	]
]]


linearMotionsToDisplacementRules[m_,linearMotions_,inputDisplacements_]:={} /; Length[linearMotions]==0

linearMotionsToDisplacementRules[m_,linearMotions_,inputDisplacements_List]:=
With[
(*preprocess arguments*)
{displacements=Flatten[{inputDisplacements}]},
Module[
{x,y,c,rules,jacobianMatrix,inverseJacobian,displacementIndices},
	displacementIndices=displacements/.VertexDisplacement[x_,y_]->{x,y}/.{"x"->1,"y"->2,"z"->3};
	jacobianMatrix=linearMotions[[All,Sequence@@#]]&/@displacementIndices;
	
	(*if inverting the jacobian fails entirely, we'll need to deal with that.*)
	inverseJacobian=Check[Inverse[jacobianMatrix],
		Message[MechanismInfinitesimalMotions::sing];
		Return[linearMotionsToDisplacementRules[m,linearMotions,Automatic]],
		Inverse::sing
	];
	(*warn that the jacobian matrix is almost singular*)
	If[Abs[Det[jacobianMatrix]]<10^(-16),Message[MechanismInfinitesimalMotions::jac]];

	rules=Thread[Array[c,Length[jacobianMatrix]]->inverseJacobian . Flatten[{displacements}]];
	Thread[Flatten[VertexDisplacement[m]]->Flatten[Array[c,Length[jacobianMatrix]] . linearMotions/.rules]]

]/; Length[linearMotions]==Length[displacements]&&MatchQ[displacements,{__VertexDisplacement}] ]

linearMotionsToDisplacementRules[m_,linearMotions_,Automatic]:=Module[{v=Unique[]},
	Thread[Flatten[VertexDisplacement[m]]->Flatten[Array[v,Length[linearMotions]] . linearMotions]]
]/;Length[linearMotions]>0

linearMotionsToDisplacementRules[m_,linearMotions_,c_Symbol]:=
	Thread[Flatten[VertexDisplacement[m]]->Flatten[Array[c,Length[linearMotions]] . linearMotions]]/;Length[linearMotions]>0


linearMotionsToDisplacementRules[m_,linearMotions_,displacements_]:=Which[
	Not[MatchQ[Flatten[{displacements}],{__VertexDisplacement}]],
		Message[MechanismInfinitesimalMotions::var];
		$Failed,
	Length[linearMotions]!=Length[Flatten[{displacements}]],
		Message[MechanismInfinitesimalMotions::lin,Length[linearMotions]];
		$Failed,
	True,
		$Failed
]


Options[MechanismDynamicalSystemEquations]={
	Constraints->None, (*a list of constraints*)
	VertexMass->1,
	VertexDrag->0,
	AddedEnergy->0 (*additional terms to add to the energy*)
};

Options[MechanismDynamicalSystem]=Join[
	Options[MechanismDynamicalSystemEquations],
	Options[NDSolve]
];


MechanismDynamicalSystemEquations::drag="Option VertexDrag cannot be parsed.";
MechanismDynamicalSystemEquations::mass="Option VertexMass cannot be parsed.";
MechanismDynamicalSystemEquations::pos="Not a valid set of positions.";
MechanismDynamicalSystemEquations::var="Variables should be of the form {variable, time}.";


validEquationsParamQ[value_?NumericQ]:=value >= 0
validEquationsParamQ[value : Except[_List]]:=Not[NumericQ[value]]
validEquationsParamQ[_]:=False

expandEquationDrags[m_, drags_?validEquationsParamQ]:=ConstantArray[drags, MeshCellCount[m,0] ]
expandEquationDrags[m_, drags : {__?validEquationsParamQ}]:=drags /; Length[drags]==MeshCellCount[m,0]
expandEquationDrags[m_, None]:=ConstantArray[0, MeshCellCount[m,0] ]
expandEquationDrags[m_, _]:=(Message[MechanismDynamicalSystemEquations::drag]; $Failed)

expandEquationMasses[m_, masses_?validEquationsParamQ]:=ConstantArray[masses, MeshCellCount[m,0] ]
expandEquationMasses[m_, masses : {__?validEquationsParamQ}]:=masses /; Length[masses]==MeshCellCount[m,0]
expandEquationMasses[m_, None | 0]:=None
expandEquationMasses[m_, _]:=(Message[MechanismDynamicalSystemEquations::mass]; $Failed)

evaluateEquationPositions[m_, Automatic]:=m["positions"]
evaluateEquationPositions[m_, positions_]:=positions /; VertexPositionsQ[m,positions]
evaluateEquationPositions[m_, _]:=(Message[MechanismDynamicalSystemEquations::pos]; $Failed)


MechanismDynamicalSystemEquations[m_?MechanismQ, initialPositions_ : Automatic, {variableName_Symbol, timeVariable_Symbol}, opt : OptionsPattern[] ]:=
Module[{
energy = MechanismEnergy[m, Constraints -> OptionValue[Constraints]] + OptionValue[AddedEnergy],
positions = evaluateEquationPositions[m,initialPositions],
drags = expandEquationDrags[m, OptionValue[VertexDrag]],
masses = expandEquationMasses[m, OptionValue[VertexMass]]
},
	With[{res=MechanismDynamicalSystemEquationsInternal[m, positions, variableName, timeVariable, masses, drags, energy ]},
		res /; res =!= $Failed
	] /; drags =!= $Failed && masses =!= $Failed && positions =!= $Failed && variableName =!= timeVariable
]

MechanismDynamicalSystemEquations[m_?MechanismQ, initialPositions_ : Automatic, {_,_}, OptionsPattern[]]:="nothing" /; Message[MechanismDynamicalSystemEquations::vars]
MechanismDynamicalSystemEquations[m_?MechanismQ, initialPositions_ : Automatic, Except[_List] , OptionsPattern[]]:="nothing" /; Message[MechanismDynamicalSystemEquations::vars]


MechanismDynamicalSystem::drag="Option VertexDrag cannot be parsed.";
MechanismDynamicalSystem::mass="Option VertexMass cannot be parsed.";
MechanismDynamicalSystem::timespec="The last argument should be of the form {time variable, start time, end time} with start and end times being numerical and the time variable being a Symbol.";


validDynamicalParamQ[value_]:=NumericQ[value] && value >= 0

expandDynamicalDrags[m_, drags_?validDynamicalParamQ]:=ConstantArray[drags, MeshCellCount[m,0] ]
expandDynamicalDrags[m_, drags : {__?validDynamicalParamQ}]:=drags /; Length[drags]==MeshCellCount[m,0]
expandDynamicalDrags[m_, None]:=ConstantArray[0, MeshCellCount[m,0] ]
expandDynamicalDrags[m_, _]:=(Message[MechanismDynamicalSystem::drag]; $Failed)

expandDynamicalMasses[m_, None | 0]:= None
expandDynamicalMasses[m_, masses_?validDynamicalParamQ]:= ConstantArray[masses, m["VertexNumber"] ]
expandDynamicalMasses[m_, masses : {__?validDynamicalParamQ}]:= masses /; Length[masses] == m["VertexNumber"]
expandDynamicalMasses[m_, _]:=(Message[MechanismDynamicalSystem::mass]; $Failed)


parseInitialConditions[m_, positions : _?MatrixQ | Automatic]:={parsePositions[m, positions], parseVelocities[m, None]}
parseInitialConditions[m_, {positions : _?MatrixQ | Automatic, velocities_?MatrixQ}]:={parsePositions[m, positions], parseVelocities[m, velocities]}
parseInitialConditions[m_, _]:=$Failed

parsePositions[m_, Automatic]:=m["positions"]
parsePositions[m_, positions_?MatrixQ]:=positions /; NumericCoordinatesQ[m,positions]
parsePositions[m_, positions_?MatrixQ]:=$Failed

parseVelocities[m_, velocities_]:= velocities /; NumericCoordinatesQ[m, velocities]
parseVelocities[m_, None]:= ConstantArray[0, {m["VertexNumber"], m["EmbeddingDimension"]}]


MechanismDynamicalSystem[m_?MechanismQ, initialConditions_ : Automatic, {time_Symbol, start_?NumericQ, end_?NumericQ}, opt : OptionsPattern[] ]:=
Module[{
energy = MechanismEnergy[m, Constraints -> OptionValue[Constraints]] + OptionValue[AddedEnergy],
parsedInitialConditions = parseInitialConditions[m, initialConditions],
drags = expandDynamicalDrags[m, OptionValue[VertexDrag]],
masses = expandDynamicalMasses[m, OptionValue[VertexMass]]
},
	With[{res=MechanismDynamicalSystemInternal[m, masses, drags, parsedInitialConditions, energy, {time, start, end}, FilterRules[{opt}, Options[NDSolve]] ]},
		res /; res =!= $Failed
	] /; drags =!= $Failed && masses =!= $Failed && parsedInitialConditions =!= $Failed
]

MechanismDynamicalSystem[m_?MechanismQ, _ : Automatic, {_,_,_} ]:="nothing" /; Message[MechanismDynamicalSystem::timespec]
MechanismDynamicalSystem[m_?MechanismQ, _ : Automatic, Except[_List] ]:="nothing" /; Message[MechanismDynamicalSystem::timespec]


MechanismDynamicalSystemEquationsInternal[m_, initialPositions_, v_, timeVariable_, masses_, drags_, energy_]:=
Module[{pinnedVertices = Dispatch[ solveLinearEquations[MechanismConstraintMap[m,initialPositions], Flatten[VertexPosition[m]] ] ], 
variables, gradient, equationSystem, i, j, t},
	variables=VertexPosition[m] /. pinnedVertices /. VertexPosition[i_, j_] :> v[i, j][t];
	gradient=Partition[ D[ energy, { Flatten[ VertexPosition[m] ] }], MechanismEmbeddingDimension[m] ] /. pinnedVertices /. VertexPosition[i_, j_] :> v[i, j][t];
	
	equationSystem = Flatten[
		If[masses === None, ConstantArray[0, Dimensions[initialPositions] ], DiagonalMatrix[ masses ] . D[ variables, {t, 2} ] ] 
			+ DiagonalMatrix[ drags ] . D[ variables, {t, 1} ] + gradient
	] /. t->timeVariable;

	(*we need to explicitly eliminate the equations that are predetermined by the pinned vertices or there will be too many equations*)
	Pick[ equationSystem, Not[ NumericQ[#] ]& /@ Flatten[variables] ]
]


MechanismDynamicalSystemInternal[m_, masses_, drags_, {initialPositions_, initialVelocities_}, energy_, {timeVariable_, start_, end_}, opt_]:=
Module[{equations, v, variables, processedVariables, solution, i, j, 
pinnedVertices = Dispatch[ solveLinearEquations[MechanismConstraintMap[m,initialPositions], Flatten[VertexPosition[m]] ] ]},
	(*list only the non-pinned variables*)
	variables = Flatten[ VertexPosition[m] /. pinnedVertices /. VertexPosition[i_, j_] :> v[i, j][timeVariable] ];

	equations = Flatten[{
		(*the dynamical system equations*)
		MechanismDynamicalSystemEquationsInternal[m, initialPositions, v, timeVariable, masses, drags, energy],

		(*initial positions*)
		Select[ Flatten[ (variables /. timeVariable->0) - Flatten[initialPositions]], Not[ NumericQ[ # ] ]& ],

		(*initial velocities if needed*)
		If[masses === None,
			{},
			Select[ (( D[ variables, timeVariable ] /. timeVariable->0 ) - Flatten[initialVelocities]), Not[ NumericQ[ # ] ]& ]
		]
	}];
	
	solution = NDSolve[ Thread[ equations == 0 ],  Select[ variables, Not[ NumericQ[#] ] & ], {timeVariable, start, end }, opt ];

	(*did NDSolve return a list of rules?*)
	If[MatchQ[solution,{{__Rule}}],
		VertexPosition[m] /. pinnedVertices /. VertexPosition[i_, j_] :> v[i, j][timeVariable] /. solution[[1]],
		$Failed
	]
]


Options[MechanismMonteCarloRun] = Join[
	Options[MechanismConstraintMap],
	{
		"energy"->Automatic, InitialPositions->Automatic
	}
];

MechanismMonteCarloRun[ m_?MechanismQ, beta_?temperatureQ, steps_?stepsQ , distribution_?distributionQ, options : OptionsPattern[] ] :=Module[
{ constraintData = Chop[( (#[[1]]-#[[2]] -> 0)& /@ processConstraintEquations[
		m["positions"],
		MechanismConstraintMap[ m, m["positions"], Sequence @@ FilterRules[{options}, Options[MechanismConstraintMap] ] ]
	]["linear solutions"]) /. PositionRules[ m["positions"] + VertexDisplacement[m]]],
	energy = getEnergy[m,OptionValue["energy"]],
	firstEnergy,
	firstPositions = processInitialPositions[m, OptionValue[InitialPositions]],
	results
},
	firstEnergy = evaluateEnergy[ firstPositions, energy ];
	results = Transpose @ NestList[
		monteCarloStep[ energy, #, beta, distribution, constraintData ]&, 
		{1, firstEnergy, firstPositions}, 
		steps
	];
	
	{N@Total[results[[1]]]/Length[results[[1]]], results[[2]],results[[3]] }
]

getEnergy[ m_, Automatic] := MechanismEnergy[m]
getEnergy[ m_, energy_?CompiledMechanismEnergyQ] := energy
getEnergy[ m_, expression_]:= expression /; analyticEnergyQ[expression, m["positions"]]
getEnergy[ m_, _ ] := (
	Message[MechanismMonteCarloRun::energy];
	MechanismEnergy[m]
)

processInitialPositions[m_, Automatic]:=m["positions"]
processInitialPositions[m_, pos_ ] := pos /; NumericCoordinatesQ[m,pos]
processInitialPositions[m_, pos_ ] := (Message[MechanismMonteCarloRun::pos];  m["positions"])

temperatureQ[ beta_?(NumericQ[#]&&#>0 &) ] := True
temperatureQ[ beta_?NumericQ ] := (Message[MechanismMonteCarloRun::neg]; False)
temperatureQ[ beta_ ] := (Message[MechanismMonteCarloRun::num]; False)

stepsQ[ steps_?(IntegerQ[#] && # > 0 &) ] := True
stepsQ[ steps_ ] := (Message[MechanismMonteCarloRun::steps]; False)

distributionQ[ distribution_?DistributionParameterQ ] := True
distributionQ[ distribution_ ] := (Message[MechanismMonteCarloRun::distr]; False)

MechanismMonteCarloRun::energy="Option \"energy\" should be an analytical energy, a compiled energy, or Automatic.";
MechanismMonteCarloRun::neg="Inverse temperature should be positive.";
MechanismMonteCarloRun::pos="Initial positions are not compatible with Mechanism.";
MechanismMonteCarloRun::num="Inverse temperature should be numerical and positive.";
MechanismMonteCarloRun::steps="Third argument should be a positive integer indicated the number of steps to take.";
MechanismMonteCarloRun::distr="Fourth argument should be a valid probability distribution.";


monteCarloStep[ energy_, {_, oldEnergy_, oldPositions_} , beta_, distribution_, rules_ ] := Module[
{
	newPositions = RandomPositions[ oldPositions, "distribution" -> distribution, "rules" -> rules ],
	newEnergy
},
	newEnergy = evaluateEnergy[newPositions, energy];
	
	Which[
		newEnergy <= oldEnergy, {1, newEnergy, newPositions},
		RandomReal[]<Exp[beta (oldEnergy-newEnergy) ], {1, newEnergy, newPositions},
		True, {0, oldEnergy,oldPositions}
	]
]


Options[ MechanismIsometricTrajectory ] := {
	InitialPositions -> Automatic,
	Constraints -> None,
	Method -> {"Minimization", MaxIterations -> 10^4},
	Tolerance -> 10^(-8),
	StepSize -> 0.1,
	StoppingCriteria -> None
};


MechanismIsometricTrajectory[m_?MechanismQ, direction_, steps_?stepsQ , opt : OptionsPattern[] ] :=
With[{
	initialPositions = If[OptionValue[InitialPositions]===Automatic, m["positions"], OptionValue[InitialPositions]]
},
	With[{
		res = MechanismIsometricTrajectoryInternal[m, initialPositions, direction, 
			{steps, OptionValue[StepSize], OptionValue[Tolerance], stoppingcriteriaQ[MechanismPositions[m],direction,OptionValue[StoppingCriteria]]}, 
			Flatten[{OptionValue[Method]}],
			{opt}
		]
	},
		res /; Head[res] =!= MechanismIsometricTrajectoryInternal
	] /; And[ positionsQ[m, initialPositions] , displacementsQ[m, direction] , toleranceQ[ OptionValue[Tolerance] ] ]
]

stepsQ[ steps_Integer?(#>0&) ] := True
stepsQ[ steps_ ] := (Message[MechanismIsometricTrajectory::steps]; False)

toleranceQ[ tol_?(NumericQ[#]&&#>=0&) ] := True
toleranceQ[ tol_ ] := (Message[MechanismIsometricTrajectory::tol]; False)

positionsQ[ m_ , positions_ ] := If[ NumericCoordinatesQ[m,positions], True, Message[MechanismIsometricTrajectory::pos,positions]; False]
displacementsQ[ m_ , positions_ ] := If[ NumericCoordinatesQ[m,positions], True, Message[MechanismIsometricTrajectory::disp,positions]; False]

stoppingcriteriaQ[ pos_, dir_ , None] := None
stoppingcriteriaQ[ pos_, dir_ , stopfunc_] := If[ BooleanQ[stopfunc[pos,dir]], stopfunc, Message[MechanismIsometricTrajectory::sc2]; stopfunc]

MechanismIsometricTrajectory::steps="Number of steps is not a positive integer.";
MechanismIsometricTrajectory::tol = "Tolerance should be a positive real number.";
MechanismIsometricTrajectory::stepsize = "Stepsize should be a function of positions or a positive real number.";
MechanismIsometricTrajectory::pos = "Initial positions `1` should be numerical and correspond to Mechanism..";
MechanismIsometricTrajectory::disp = "Initial displacements `1` should be numerical and correspond to Mechanism..";
MechanismIsometricTrajectory::sc = "Option StoppingCriteria is not a function or the keyword None.";
MechanismIsometricTrajectory::sc2 = "Function provided to StoppingCriteria does not return True or False.";


MechanismIsometricTrajectoryInternal[ m_, initialPositions_, direction_, {steps_, stepsize_, tolerance_,stoppingcriteria_}, {method_ , methodSpec___} , options_ ] :=
With[
{
	minimizationOptions = FilterRules[ {methodSpec}, Options[FindMinimum] ],
	nullspaceOptions = FilterRules[ {methodSpec}, Options[NullSpace] ],

	normalizedDirection = direction/Sqrt[Flatten[direction] . Flatten[direction] ],
	
	MechanismConstraintMatrixFunc = mechanismConstraintMatrixFunction[ m , FilterRules[ options, Options[mechanismConstraintMatrixFunction] ]],
	energyFunc = MechanismEnergy[m , initialPositions , FilterRules[ options, Options[MechanismEnergy] ] ]
},
	Which[
	stoppingcriteria === None,
		DeleteCases[
			FoldList[
				MechanismIsometricTrajectoryStep[ {method, stepsize, tolerance}, m, #1, #2, { Quiet[MechanismConstraintMatrixFunc @@ Flatten[#1[[1]]]] , nullspaceOptions}, {energyFunc , minimizationOptions} ] &,
				{initialPositions,normalizedDirection},
				Range[steps]
			][[All,1]],
			$Failed
		],
	Head[stoppingcriteria] === Function,
		DeleteCases[
			FoldWhileList[
				MechanismIsometricTrajectoryStep[ {method, stepsize, tolerance}, m, #1, #2, { Quiet[MechanismConstraintMatrixFunc @@ Flatten[#1[[1]]]] , nullspaceOptions}, {energyFunc , minimizationOptions} ] &,
				{initialPositions,normalizedDirection},
				Range[steps],
				stoppingcriteria[#[[1]],#[[2]]]&
			][[All,1]],
			$Failed
		],
	True, (*repeating this is kind of a silly kludge but what's the harm.*)
		Message[MechanismIsometricTrajectory::sc];
		DeleteCases[
			FoldList[
				MechanismIsometricTrajectoryStep[ {method, stepsize, tolerance}, m, #1, #2, { Quiet[MechanismConstraintMatrixFunc @@ Flatten[#1[[1]]]] , nullspaceOptions}, {energyFunc , minimizationOptions} ] &,
				{initialPositions,normalizedDirection},
				Range[steps]
			][[All,1]],
			$Failed
		]
	]
]


MechanismIsometricTrajectoryStep[ _,_,{$Failed, _},_,_]:={$Failed, $Failed}


MechanismIsometricTrajectoryStep[ {"Euler", stepsize_,tolerance_},m_,{positions_,lastDirection_}, n_, {constraintMat_, nullspaceOptions_}, {constraintEnergy_, minimizationOptions_} ] :=
Module[
{
	newDirection = projectTrialDirection[positions, lastDirection, constraintMat, nullspaceOptions ]
},
	{
		computeTrialPosition[ positions, newDirection, stepsize, n ],
		newDirection
	}
]

MechanismIsometricTrajectoryStep[ {"RungaKutta", stepsize_,tolerance_},m_,{positions_,lastDirection_}, n_, {constraintMat_, nullspaceOptions_}, {constraintEnergy_, minimizationOptions_} ] :=
Module[{k1, k2, k3, k4, k5, trialPosition},
	k1 = projectTrialDirection[ positions , lastDirection, constraintMat, nullspaceOptions ];
	k2 = projectTrialDirection[ positions + stepsize k1/2, lastDirection, constraintMat, nullspaceOptions ];
	k3 = projectTrialDirection[ positions + stepsize k2/2, lastDirection, constraintMat, nullspaceOptions ];
	k4 = projectTrialDirection[ positions + stepsize k3, lastDirection, constraintMat, nullspaceOptions ];
	k5 = (k1+2 k2 + 2 k3+k4)/6;
	trialPosition = positions + stepsize k5;
	{
		If[ evaluateEnergy[m, trialPosition, constraintEnergy] > tolerance,
			correctTrialPosition[m, trialPosition, constraintEnergy, minimizationOptions],
			trialPosition
		],
		k5/Norm[Flatten[k5]]
	}
]

MechanismIsometricTrajectoryStep[ {"Minimization", stepsize_, tolerance_} , m_, {positions_, lastDirection_}, n_, {constraintMat_, nullspaceOptions_}, {constraintEnergy_, minimizationOptions_} ] :=
Module[
{
	newDirection = projectTrialDirection[ positions, lastDirection, constraintMat, nullspaceOptions ],
	trialPosition
},
	trialPosition = computeTrialPosition[ positions, newDirection, stepsize, n ];
	{
		If[ evaluateEnergy[m, trialPosition, constraintEnergy] > tolerance,
			correctTrialPosition[m, trialPosition, constraintEnergy, minimizationOptions],
			trialPosition
		],
		newDirection
	}
]

MechanismIsometricTrajectoryStep[ {"RandomWalk", stepsize_, tolerance_} , m_, {positions_, lastDirection_}, n_, {constraintMat_, nullspaceOptions_}, {constraintEnergy_, minimizationOptions_} ] :=
Module[
{
	newDirection = projectTrialDirection[ positions, RandomPositions[m,positions], constraintMat, nullspaceOptions ],
	trialPosition
},
	trialPosition = computeTrialPosition[ positions, newDirection, stepsize, n ];
	{
		If[ evaluateEnergy[m, newDirection, constraintEnergy] > tolerance,
			correctTrialPosition[m, trialPosition, constraintEnergy, minimizationOptions],
			trialPosition
		],
		newDirection
	}
]


(* returns a normalized displacement tangent to the configuration space and as close to trialDirection as possible *)
projectTrialDirection[ positions_ , trialDirection_, constraintMat_ , nullspaceOptions_ ] :=
With[ {
nullspaceBasis = Orthogonalize[ NullSpace[constraintMat, nullspaceOptions ] ]
},
	If[ Length[nullspaceBasis] > 0,
		ArrayReshape[
			Normalize[Transpose[nullspaceBasis] . nullspaceBasis . Flatten[trialDirection]],
			Dimensions[positions]
		],

		Message[MechanismIsometricTrajectory::dirns , positions];
		ConstantArray[ 0, Dimensions[positions] ]
	]
]

MechanismIsometricTrajectory::dirns="Cannot identify tangent space at `1`.";


(* find a new set of positions from a trial step *)
computeTrialPosition[ positions_ , trialStep_ , stepsize_?NumericQ, n_ ] := 
		If[ stepsize >= 0,
			positions + stepsize trialStep,

			Message[ MechanismIsometricTrajectory::stepsize ];
			$Failed
		]

computeTrialPosition[ positions_ , trialStep_ , stepsize_, n_ ] :=
With[ { actualSize = stepsize[positions,n] },
	If[ NumericQ[ actualSize ] && actualSize >= 0,
		positions + actualSize trialStep,

		Message[ MechanismIsometricTrajectory::stepsize ];
		$Failed
	]
]


correctTrialPosition[ m_, trialPosition_ , constraintEnergy_, minimizationOptions_ ]:=
	minimizeMechanismEnergyInternal[m, constraintEnergy, trialPosition, minimizationOptions][[2]]


Options[MechanismFindMinimalTrajectory]={
	AddedEnergy->0,
	Constraints->None,
	Method->{"ElasticBand", MaxIterations->10^5, "Stiffness" -> 10^(-4)}
};


MechanismFindMinimalTrajectory["Methods"]={{"ElasticBand", Join[{"Stiffness"->10^(-4)},"Options[FindMinimum]"]}};

MechanismFindMinimalTrajectory[ m_?MechanismQ, start_, end_, steps_, opt : OptionsPattern[] ]:=
Module[{method = Flatten[{OptionValue[Method]}], res},
	res = Switch[First[method],
		"ElasticBand",
			MechanismFindMinimalTrajectoryElasticBand[m, start, end, steps, MechanismEnergy[m, Constraints->OptionValue[Constraints]]+OptionValue[AddedEnergy], Sequence @@ Rest[method] ],
		_,
			Message[MechanismFindMinimalTrajectory::meth];
			$Failed
	];
	
	res /; res =!= $Failed
]

MechanismFindMinimalTrajectory::method="`1` is not a recognized method. Only \"ElasticBand\" is currently recognized.";


MechanismFindMinimalTrajectory::ebstiff="Stiffness in \"ElasticBand\" method must be a positive numerical value.";
MechanismFindMinimalTrajectory::ebstart="Start positions are not numeric coordinates corresponding to Mechanism.";
MechanismFindMinimalTrajectory::ebend="End positions are not numeric coordinates corresponding to Mechanism.";
MechanismFindMinimalTrajectory::steps="Number of steps should be a positive integer.";

ClearAll[MechanismFindMinimalTrajectoryElasticBand];
Options[MechanismFindMinimalTrajectoryElasticBand]=Join[{"Stiffness"->10^(-4)},Options[FindMinimum]];

MechanismFindMinimalTrajectoryElasticBand[m_, startPositions_,endPositions_, steps_, energy_, opt : OptionsPattern[]]:=
Module[{
stiffness = OptionValue["Stiffness"],
fixedVertices = solveLinearEquations[MechanismConstraintMap[m,startPositions], Flatten[VertexPosition[m]] ], 
variable, internalVariables, partialEnergy, newVariables, potentialEnergy, initialConditions,
minimizationOptions = FilterRules[{opt},Options[FindMinimum]], solution
},

	Which[
		Not[ NumericQ[stiffness] && stiffness > 0 ],
			Message[MechanismFindMinimalTrajectory::ebstiff]; Return[$Failed],
		Not @ NumericCoordinatesQ[m, startPositions],
			Message[MechanismFindMinimalTrajectory::ebstart]; Return[$Failed],
		Not @ NumericCoordinatesQ[m, endPositions],
			Message[MechanismFindMinimalTrajectory::ebend]; Return[$Failed],
		Not[ IntegerQ[steps] && steps>0 ],
			Message[MechanismFindMinimalTrajectory::steps]; Return[$Failed]
	];

	internalVariables= Join[
		{PositionRules[startPositions]},
		Flatten[MapThread[ #1 -> #2&, {VertexPosition[m], #}, 2],2]& /@ Array[variable, {steps, MeshCellCount[m,0],MechanismEmbeddingDimension[m]}],
		{PositionRules[endPositions]}
	];

	newVariables = Flatten[VertexPosition[m] /. fixedVertices] /. internalVariables;

	partialEnergy = energy /. fixedVertices;

	potentialEnergy = Total @ Flatten @ {
	(*local equations*)
	(partialEnergy /. # &) /@ internalVariables,

	(*elastic band equations*)
	(stiffness (#[[2]]-#[[1]]) . (#[[2]]-#[[1]])&) /@ Partition[newVariables, 2, 1]
	};

	initialConditions = DeleteCases[ 
		Transpose @ {Flatten[ Drop[ Rest @ newVariables, -1] ], Flatten[startPositions + (endPositions - startPositions)/(steps+1) #& /@ Range[steps] ]},
		{_?NumericQ, _}
	];

	solution = FindMinimum @@ {potentialEnergy, initialConditions, minimizationOptions };
	If[Head[solution] =!= FindMinimum, Partition[#,MechanismEmbeddingDimension[m]]& /@ (newVariables /. solution[[2]]), $Failed]
]


GenericGloballyRigidQ[m_?MechanismQ] :=
Module[ {dim = m["EmbeddingDimension"], num = m["VertexNumber"]},
	If[ContainsAny[MechanismCells[m],{PinnedJoint, RigidBar}], Message[GenericGloballyRigidQ::more]];
	Or @@ (
		(MatrixRank[MechanismStressMatrix[m, #], Tolerance -> OptionValue[Tolerance] ] == num - dim - 1)& /@ MechanismSelfStresses[m, CellPattern->_RigidBar]
	)
]

GenericGloballyRigidQ::more="Mechanism containts more than joints and bars.";


toVariables[expr_] := DeleteDuplicates[Cases[expr,_Symbol|_VertexPosition,All]]


Options[TangentForm]={Constraints -> {}, CellPattern -> _ };
TangentForm[ m_?MechanismQ , opt : OptionsPattern[] ] :=
Module[{
	constraints,
	pinnedVertexLocations = MechanismLinearConstraints[m],
	equations,
	variables,
	jacobian, variableLength, i,j
},
	equations = MechanismConstraintMap[m, opt, "ReducedMap"->False] /. Dispatch[pinnedVertexLocations];
	(
		(*eliminate the trivial constraints after solving the linear constraints explicitly*)
		constraints = Pick[ equations , Not /@ PossibleZeroQ /@ equations ];
		(*what variables are left*)
		variables = DeleteCases[Flatten[VertexPosition[m]],Alternatives@@(pinnedVertexLocations[[All,1]])];
		variableLength = Length[variables];
		jacobian = D[constraints,{variables}];

		{
		variables,
		pinnedVertexLocations,
		Switch[ variableLength-Length[constraints],
			0, Det[ jacobian ],
			1, Cross @@ jacobian,
			2, Table[Det @ Join[{UnitVector[variableLength,i],UnitVector[variableLength,j]}, jacobian],{i,1,variableLength},{j,1,variableLength}],
			_, Message[TangentForm::dim, variableLength - Length[constraints]]; D[constraints,{variables}]
		]
		}

	) /; Head[equations] =!= MechanismConstraintMap (*catch an error*)
]

TangentForm::dim = "Current version cannot compute tangent form for dimensions other than 0 or 1 or 2. Dimension of configuration space is nominally `1`. Returning reduced Jacobian instead.";


Options[CriticalSet] = {
	CellPattern -> _,
	Constraints -> {}
};

CriticalSet[ m_?MechanismQ , extraConstraints : Except[_Rule] : {}, opt : OptionsPattern[]] := 
Module[
{
	tf, vars,
	pinnedVertexLocations,
	soln, positions, constraintEquations,
	extraVariables
},
	{vars,pinnedVertexLocations,tf}=Simplify[TangentForm[m, Sequence @@ FilterRules[{opt},Options[TangentForm] ]]];
	soln = Simplify[Quiet[Solve[tf==0,vars],Solve::svars]];
	positions = VertexPosition[m] /. pinnedVertexLocations;

	constraintEquations = extraConstraints /. pinnedVertexLocations;
	(*are there any extra variables declared?*)
	extraVariables = toVariables[constraintEquations];

	If[Length[soln]>0,
		With[{ fullVariableSet = DeleteDuplicates[Flatten[{vars,extraVariables}]] },
		DeleteDuplicates[
			Flatten[
				Map[(*for each entry in soln, try to solve the extra constraints*)
					With[{ constrainedSolution = Quiet[Solve[Simplify[constraintEquations /. #]==0, fullVariableSet],Solve::svars]},
						If[ Length[constrainedSolution] > 0,
							positions /. # /. constrainedSolution, 
							{{}}
						]
					] &,
					soln
			],1]
		]]
		,
		(*there are no critical points at all*)
		{}
	]
]


Options[CriticalSetEquations] = {
	CellPattern -> _,
	Constraints -> {}
};
CriticalSetEquations[m_?MechanismQ, extraConstraints : Except[_Rule] : {}, opt : OptionsPattern[]] :=
Module[{vars,pinnedLocations,tf},
	{vars,pinnedLocations,tf} = TangentForm[m,opt];
	Join[ tf , extraConstraints ] /. pinnedLocations
]


ZeroLengthEdgeQ[ m_?MechanismQ ] := ZeroLengthEdgeQ[m,MechanismPositions[m]]
ZeroLengthEdgeQ[ m_?MechanismQ , pos_ ] /; VertexCoordinatesQ[m,pos] :=
	AnyTrue[ Simplify[DisplacementLengthSquared[pos,MechanismEdges[m]]], # === 0 & ]


End[];

EndPackage[];


BeginPackage["Mechanisms`Origami`"];


OrigamiQ::usage="OrigamiQ[ obj ] returns True if obj can be turned into Origami.";


KawasakiQ::usage=
"KawasakiQ[\!\(\*
StyleBox[\"Origami\",\nFontSlant->\"Italic\"]\)] returns True if it can be determined that the Origami satisfies Kawasaki's theorem at each vertex.
Use option ZeroTest to modify how the function tests for zero. Use option WorkingPrecision to set a number of digits for the test.";


TargetFoldAngles::usage="TargetFoldAngles[m,(positions), (data)] returns an association containing a list of TorsionalFolds, their corresponding target angles, and their actual angles.
Optional argument data is a string that is either \"TorsionalFolds\", \"angles\", \"target\".";


TorsionalFoldMatrix::usage=
"TorsionalFoldMatrix[\!\(\*
StyleBox[\"Origami\",\nFontSlant->\"Italic\"]\)] returns the TorsionalFold matrix mapping linear vertex displacements to linear TorsionalFold angle changes.";

AngularFoldMatrix::usage=
"AngularFoldMatrix[\!\(\*
StyleBox[\"Origami\",\nFontSlant->\"Italic\"]\), (\!\(\*
StyleBox[\"positions\",\nFontSlant->\"Italic\"]\), ) \!\(\*
StyleBox[\"vertex\",\nFontSlant->\"Italic\"]\)] returns the angular TorsionalFold matrix of a vertex mapping the angular displacements of the TorsionalFolds from the xy plane to the TorsionalFold angle changes.";


ToOrigami::usage=
"ToOrigami[ \!\(\*
StyleBox[\"object\",\nFontSlant->\"Italic\"]\) ] converts an object to an Origami Mechanism. Effectively, this only works for some MeshRegion[] or Linkage[] objects.";

ElasticizeOrigami::usage="ElasticizeOrigami[ o ] attempts to create an elastic model of an Origami structure.";


SingleVertex::usage=
"SingleVertex[ {\!\(\*
StyleBox[\"angle\",\nFontSlant->\"Italic\"]\) 1, \!\(\*
StyleBox[\"angle\",\nFontSlant->\"Italic\"]\) 2, ...} ] returns a single vertex Origami with angles as sector angles.
SingleVertex[ {\!\(\*
StyleBox[\"angle\",\nFontSlant->\"Italic\"]\) 1, \!\(\*
StyleBox[\"angle\",\nFontSlant->\"Italic\"]\) 2, ...}, {\!\(\*
StyleBox[\"length\",\nFontSlant->\"Italic\"]\) 1, ...} ] returns a single vertex Origami with angles as sector angles and TorsionalFold lengths given by the list of lengths.
SingleVertex[ { pt1, pt2, ... } ] returns a single vertex Origami with vertex at origin and boundary at specified points in 2D or 3D.

See options \"angles\" and \"torsional stiffnesses\" to set the equilibrium angles and torsional stiffnesses.";

RandomOrigami::usage=
"RandomOrigami[ \!\(\*
StyleBox[\"n\",\nFontSlant->\"Italic\"]\) ] returns random Origami with n internal vertices.";

MiuraOri::usage=
"MiuraOri[ {length1, length2}, \!\(\*
StyleBox[\"angle\",\nFontSlant->\"Italic\"]\) ] returns a Miura ori unit cell at a particular angle and Face lengths.";


FlatOrigamiQ::usage=
"FlatOrigamiQ[ \!\(\*
StyleBox[\"Origami\",\nFontSlant->\"Italic\"]\) ] returns True if the Origami Mechanism is flat.
Use option ZeroTest to specify how to test for zero. Use option WorkingPrecision to choose the precision.";


KreslingOrigami::usage=
"KreslingOrigami[{\!\(\*
StyleBox[\"#\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"azimuthal\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"cells\",\nFontSlant->\"Italic\"]\), #\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"of\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"rings\",\nFontSlant->\"Italic\"]\)(, \!\(\*
StyleBox[\"twist\",\nFontSlant->\"Italic\"]\))},\!\(\*
StyleBox[\"overall\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"scale\",\nFontSlant->\"Italic\"]\), \!\(\*
StyleBox[\"height\",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\" \",\nFontSlant->\"Italic\"]\)\!\(\*
StyleBox[\"ratio\",\nFontSlant->\"Italic\"]\)] creates a Kresling TorsionalFold pattern of a particular radius, height, and integer twist if specified.";


MVAssignment::usage = "MVAssignment[ o (, positions), tolerance_ (, assignmentFunction)] returns a list of rules assigning a value to mountain or valley TorsionalFolds up to the value of tolerance.
The function assignmentFunction should take one of the values {-1,0,1} for valley, flat, or mountain TorsionalFolds respectively.";

FlattenOrigami::usage = "FlattenOrigami[m, tolerance] attempts to flatten an Origami structure.";


SameFoldAnglesQ::usage="SameFoldAnglesQ[ Origami, (TorsionalFolds,) (precision) ] is a function that compares the positions of two Origami structures and returns True if they have the same TorsionalFold angles.";
SameMVAnglesQ::usage="SameMVAnglesQ[ Origami, (TorsionalFolds,) (precision) ] is a function that compares the positions of two Origami structures and returns True if they have the same mountain-valley assignments.";


ButterflyDecompositionQ::usage = "ButterflyDecompositionQ[ Origami ] returns True if Origami can be decomposed into pairs of \"butterfly\" Faces.";
ButterflyDecomposition::usage = "ButterflyDecomposition[ Origami ] returns a list of \"butterfly\" Face pairs and a non-butterfly-decomposible kernel after removing butterfly Faces. You can apply MechanismDeleteDanglingVertices[] to extract the underlying Origami structure.";


DeconstructedOrigami::usage="DeconstructedOrigami[ Origami ] returns a list of vertices for polygons corresponding to the individual Faces.
Option \"edgeWeights\" allows the assignment of positive weights that allow some edges to have widths.
Option \"defaultWeight\" is the default weight on all edges.";

ExtrudedPolygon::usage="ExtrudedPolygon[coords, extrusion vector, (translation vector)] takes a Polygon and creates a 3D version.";


OrigamiData::usage="OrigamiData[ Origami ] returns an Assocation of data for various Origami structures. Use OrigamiData[] to see what is available.";


MVColor::usage="MVColor[ Origami, data, styleFunction] attempts to color data according to styles specified by styleFunction.";


Begin["`Private`"];

Needs["Mechanisms`"];
Needs["Mechanisms`geometry`"];
Needs["Mechanisms`rigidity`"];


OrigamiQ[ mr_MeshRegion ] := And[
	(*there should be at least one Face*)
	RegionDimension[mr] == 2,
	(*embedding dimension*)
	(#==2||#==3&) @ RegionEmbeddingDimension[mr]
]
OrigamiQ[ f_Linkage ] := 
	And[
		(*there should be at least one Face*)
		RegionDimension[ f["mesh"] ] == 2,
		(*embedding dimension*)
		(#==2||#==3&) @ f["EmbeddingDimension"]
	]
OrigamiQ[ o_Origami ] := True
OrigamiQ[ _ ] := False


ToOrigami::notsurFace="Input does not seem to be Origami compatible because it is not a valid surFace.";
ToOrigami::prec="Precision must be valid number of digits.";


Options[ ToOrigami ] = {Precision -> Automatic};

ToOrigami[mr_MeshRegion?OrigamiQ, OptionsPattern[] ]:=
	Origami[
		If[OptionValue[Precision] === Automatic,
			MeshCoordinates[mr],
			N[ MeshCoordinates[mr], OptionValue[Precision] ]
		],
		Flatten[{ MeshCells[mr,2] /. Polygon->Face }]
	] /; precisionQ[OptionValue[Precision]]

ToOrigami[f_Linkage?OrigamiQ, opt : OptionsPattern[]]:=
With[ {positions = f["positions"]},
	If[OptionValue[Precision] === Automatic, Origami@@f, N[ Origami@@f, OptionValue[Precision] ] ]
] /; precisionQ[ OptionValue[Precision] ]

precisionQ[ prec_ ] := Which[
	prec === Automatic, True,
	prec === MachinePrecision, True,
	NumericQ[ prec ] && prec > 0, True,
	True, Message[ ToOrigami::prec ]; False
]


ToOrigami[_]:="nothing" /; Message[ToOrigami::notsurFace]


Options[SingleVertex]=Options[Origami];


SingleVertexQ[ angles_, lengths_ ] := With[{
	deficitAngle = 2 Pi - Total[angles]
},
	And[
		If[ VectorQ[ angles, NumericQ ] && AllTrue[ angles, #>0 & ], True, Message[SingleVertex::sa]; False ],
		If[ Chop[deficitAngle] == 0, True, Message[SingleVertex::da]; False ],
		If[ VectorQ[ lengths, NumericQ ] && AllTrue[ lengths, #>0 & ], True, Message[SingleVertex::sl]; False],
		If[Length[angles] == Length[lengths] , True , Message[SingleVertex::sl]; False ]
	]
]
SingleVertexQ[ angles_ ] := With[{
	deficitAngle = 2 Pi - Total[angles]
},
	And[
		If[ VectorQ[ angles, NumericQ ] && AllTrue[ angles, #>0 & ], True, Message[SingleVertex::sa]; False ],
		If[ Chop[deficitAngle] == 0, True, Message[SingleVertex::da]; False ]
	]
]

SingleVertex::sa="Sector angles must be positive real numbers.";
SingleVertex::da="Sum of sector angles must be 2 Pi.";
SingleVertex::sl="Lengths must be positive real numbers.";
SingleVertex::n="You must have the same number of sector angles and lengths.";


SingleVertex[ pts_?VertexCoordinatesQ, opt : OptionsPattern[] ] :=
With[{
	num = Length[pts],
	dim = Length[pts[[1]]]
},
	Origami[
		pts,
		{
			FreeJoint[0 -> ConstantArray[0, dim]],
			Face[ {#[[1]],#[[2]],num+1}& /@ Partition[Range[num],2,1,1] ]
		},
		opt
	]
]

SingleVertex[ sectorAngles_, opt : OptionsPattern[] ] :=
With[{
	angles = Accumulate[sectorAngles] - ConstantArray[sectorAngles[[1]],Length[sectorAngles]],
	num = Length[sectorAngles]
},
	Origami[
		Map[ {Cos[#],Sin[#]}& , angles ],
		{
			FreeJoint[0-> {0,0}],
			Face[ {#[[1]],#[[2]],num+1}& /@ Partition[Range[num],2,1,1] ]
		},
		opt
	]
] /; SingleVertexQ[sectorAngles ]

SingleVertex[ sectorAngles_, lengths_, opt : OptionsPattern[] ] :=
With[{
	angles = Accumulate[sectorAngles] - ConstantArray[sectorAngles[[1]],Length[sectorAngles]],
	num = Length[sectorAngles]
},
	Origami[
		MapThread[ #1 {Cos[#2],Sin[#2]}& , {lengths, angles} ] ,
		{
			FreeJoint[0 -> {0,0}],
			Face[ {#[[1]],#[[2]],num+1}& /@ Partition[Range[num],2,1,1] ]
		},
		opt
	]
] /; SingleVertexQ[sectorAngles , lengths ]


Options[MiuraOri]={ "triangulate"->False };

MiuraOri[ { l1_?miuraLengthQ, l2_?miuraLengthQ } , an_?miuraAngleQ , OptionsPattern[]]:= 
Module[{ m },With[
{
	Faces = {{1,2,3,4},{1,5,6,2}},
	TorsionalFolds={{1,2},{2,3},{2,6},{2,7}},
	coordinates = {
		{0,0},
		{l1/2,0},
		{l1/2 - l2 Cot[an]/2,l2/2},
		{-l2 Cot[an]/2, l2/2},
		{-l2 Cot[an]/2, -l2/2 },
		{l1/2-l2 Cot[an]/2, -l2/2 }
	}
},
	m = Origami[coordinates,Face/@Faces];
	TesselateMechanism[
		If[ OptionValue["triangulate"], 
			AddCells[{TorsionalFold[{{1,3},{1,6}},{"passive","Shape"->None}]}] @ DivideFaces[{{1,3},{1,6}}] @ m, 
			m
		],
		{{1/2,0,0},{0,1,0}},
		{2,1}
	]
]] /; miuraOptionsQ[ OptionValue["triangulate"] ]

miuraLengthQ[ l_ ] := If[ NumericQ[l] && l > 0, True, Message[MiuraOri::length]; False ]
miuraAngleQ[ a_ ] := If[ NumericQ[a] && 0 < N[a] < Pi, True, Message[MiuraOri::angle]; False ]
miuraOptionsQ[ b_ ] := If[ BooleanQ[b], True, Message[MiuraOri::t]; False ]

MiuraOri::length = "Length must be a positive numeric value.";
MiuraOri::angle = "Angle must be a numeric value between 0 and \[Pi].";
MiuraOri::t = "Option \"triangulate\" should be a boolean variable.";


Options[KreslingOrigami]=Options[Origami];


KreslingOrigami[{numTriangles_Integer,numHeight_Integer,twist_Integer:0},opt:OptionsPattern[]]:=KreslingOrigami[{numTriangles,numHeight,twist},1,1,opt]
KreslingOrigami[{numTriangles_Integer,numHeight_Integer,twist_Integer:0},radius_?NumericQ,heightScale_?NumericQ,opt:OptionsPattern[]]:=Module[{i},
With[
{
height=heightScale Sqrt[2 (Cos[Pi/numTriangles]-Cos[2 Pi/numTriangles])]
},
	Origami[
		radius Join@@Table[
			{
			Cos[2 Pi #/numTriangles+((-1)^(i-1)) Pi/numTriangles/2],
			Sin[2 Pi #/numTriangles+((-1)^(i-1)) Pi/numTriangles/2],
			(i-1) height
			}&/@Range[numTriangles],
			{i,1,numHeight+1}],
		Table[{
			Face[
				{
				#+i numTriangles,
				Mod[#+i numTriangles+1,numTriangles,i numTriangles+1],
				Mod[#+(1+(-1)^i)/2+(i+1) numTriangles+twist,numTriangles,(i+1) numTriangles+1]
				}
			]&/@Range[numTriangles],
			Face[
				{
				#+i numTriangles,
				Mod[#+(i+1) numTriangles+(1+(-1)^i)/2+twist,numTriangles,(i+1)numTriangles+1],
				Mod[#+(i+1) numTriangles+(1+(-1)^i)/2-1+twist,numTriangles,(i+1)numTriangles+1]
				}
			]&/@Range[numTriangles]
		},{i,0,numHeight-1}],
		opt
	]
]]/;numTriangles>=2&&numHeight>=1


Options[RandomOrigami]=Join[
	Options[Origami],
	{
	Precision->Automatic,
	MaxIterations->20,
	"MinimumFolds"->4,
	"BoundaryPoints"->CirclePoints[4]
	}
];


RandomOrigami[ numberOfVertices_, opt : OptionsPattern[] ] :=
Module[ {
ctr = 1,
firstMesh = randomMesh[ numberOfVertices, OptionValue[Precision], OptionValue["BoundaryPoints"] ]
},
	(
	meshTry = ToOrigami[ firstMesh , Precision -> OptionValue[Precision] ];

	While[ Not @ validRandomOrigamiQ[ numberOfVertices, meshTry, OptionValue["MinimumFolds"], Length[OptionValue["BoundaryPoints"]] ] && ctr <= OptionValue[MaxIterations],
		meshTry = ToOrigami[ randomMesh[ numberOfVertices, OptionValue[Precision], OptionValue["BoundaryPoints"] ], Precision -> OptionValue[Precision] ]; ctr++
	];

	If[ ctr > OptionValue[MaxIterations], Message[RandomOrigami::max, ctr - 1] ];

	Origami[ meshTry, FilterRules[{opt}, Options[Origami]] ]
	) /; MeshRegionQ[firstMesh]

] /; RandomOrigamiMinFoldsQ[ OptionValue[ "MinimumFolds" ] ] && RandomOrigamiMaxIterationsQ[ OptionValue[ MaxIterations ] ]

RandomOrigami::max="Maximum number of iterations reached at `1` without finding a suitable random Origami.";

validRandomOrigamiQ[ numberOfVertices_, m_?MechanismQ, minimumFolds_, boundarySize_ ] :=
	AllTrue[
		Length /@ Drop[ MechanismConnectivity[m, "vertices" -> "edges"], boundarySize],
		# >= minimumFolds &
	] && m["VertexNumber"]-boundarySize == numberOfVertices

RandomOrigami::mTorsionalFold="Minimum number of TorsionalFolds, `1`,  must be a position integer.";
RandomOrigamiMinFoldsQ[ _Integer?(#>0&) ] := True
RandomOrigamiMinFoldsQ[ f_ ] := (Message[RandomOrigami::mTorsionalFold, f]; False)

RandomOrigami::miter="Maximum number of iterations, `1`, must be a position integer.";
RandomOrigamiMaxIterationsQ[ _Integer?(#>0&) ] := True
RandomOrigamiMaxIterationsQ[ f_ ] := (Message[RandomOrigami::miter, f]; False)


(* get a Delaunay triangulation from a set of random points *)
randomMesh[numberOfVertices_Integer?(#>0&), precision : _?(NumericQ[#] && #>0&)|Automatic|MachinePrecision, boundary_?(MatrixQ[#,NumericQ]&)]:=
Module[{points},
	points=Join[
		boundary,
		Select[
			RandomVariate[
				UniformDistribution[RegionBounds[Polygon[boundary]]],
				numberOfVertices,
				WorkingPrecision->precision
			],
			RegionMember[Polygon[boundary]]
		]
	];
	DelaunayMesh[points]
] /; Last[ Dimensions[ boundary ] ] == 2

RandomOrigami::vnum="Number of vertices `1` should be a positive integer.";
RandomOrigami::prec="Option Precision must be a positive number.";
RandomOrigami::bound="Boundary points should be a list of vertices in 2D.";
randomMesh[ numberOfVertices_, precision_, boundary_ ] := "nothing" /; (
	If[ Not[ IntegerQ[numberOfVertices] && numberOfVertices >= 1 ], Message[ RandomOrigami::vnum, numberOfVertices ] ];
	If[ Not[ NumericQ[ precision ] && precision > 0 ] && Not[ precision === Automatic ], Message[ RandomOrigami::prec, precision ] ];
	If[ Not[ MatrixQ[ boundary, NumericQ ] && Last[Dimensions[ boundary ]] == 2 ], Message[ RandomOrigami::bound ] ];
	False
)


Origami[ "RandlettBird" ] := Origami[ {"RandlettBird"} ]
Origami[{"RandlettBird", s_String : "unfolded coordinates"}] := 
With[{ data = OrigamiData[ "RandlettBird" ] },
	Origami[
		data[s],
		{
		RigidBar[ data["face folds"] , "Shape" -> None],
		Face[ data["faces"] ],
		MapThread[ TorsionalFold[#1, {"Angle"->#2,"active"}]&, {data["TorsionalFolds"], data["jihwan"]}],
		TorsionalFold[ data["face folds"] , {"passive", "Angle" -> 0}]
		}
	]
]


OrigamiData[] := {"RandlettBird"};

OrigamiData[ "RandlettBird" ] := Association[
	"TorsionalFolds"->{{16,2},{2,15},{15,16},{20,15},{3,12},{12,13},{3,7},{7,12},{3,6},{6,7},{5,6},{4,11},{11,21},{4,10},{10,11},{10,17},{17,11},{15,17},{17,16},{1,17},{9,18},{16,9},{14,10},{19,14},{9,6},{8,7},{9,8},{14,12},{8,14},{8,17}},
	"BoundaryPoints"->{{1,20},{20,2},{2,18},{18,5},{5,3},{3,13},{13,19},{19,4},{4,21},{21,1}},
	"Face TorsionalFolds"->{{1,11},{1,15},{6,8},{6,18},{8,12},{9,17},{10,19},{12,19},{14,17},{16,18}},
	"faces"->{{15,20,1},{1,17,15},{11,17,1},{1,21,11},{18,2,16},{16,9,18},{16,2,15},{15,2,20},{13,3,12},{12,3,7},{7,3,6},{6,3,5},{21,4,11},{11,4,10},{10,4,19},{19,14,10},{6,5,18},{18,9,6},{8,7,6},{6,9,8},{12,7,8},{8,14,12},{14,8,17},{17,10,14},{17,8,9},{9,16,17},{11,10,17},{19,13,12},{12,14,19},{16,15,17}},
	"face fold lengths"->{0.7913396176795269`,0.7908518374761098`,0.4256202239155842`,0.41820627040046277`,0.4256214113580315`,0.3661853242047003`,0.5778591613439372`,0.4182055804484132`,0.36642731880122054`,0.5778622627981455`},
	"fold lengths"->{0.6668302449309317`,0.7905987902232614`,0.2635292648903121`,0.5900528686107307`,0.2260243957445146`,0.10234385548891164`,0.19243997335732346`,0.07476947912792758`,0.2260251237847865`,0.07476836902333805`,0.10234618221503516`,0.7899358777822478`,0.5900530310922064`,0.6668278565928233`,0.26292795741175695`,0.33331002589557746`,0.35349728907839567`,0.3531715279617028`,0.33294017146420657`,1.0001365746515256`,0.3495350708976718`,0.2902649617356811`,0.2902613044683481`,0.3495354877728568`,0.5373516043568678`,0.44156905689970305`,0.25766229600206336`,0.5373541657262233`,0.25766207233991933`,0.3657613109073552`},
	"boundary lengths"->{0.2350773277983888`,1.1792012718458462`,0.8061889965405188`,0.42175777675288983`,0.18620179179423596`,0.18620363078624286`,0.4217559689391429`,0.8061889535079899`,1.178492447495307`,0.2355242761556322`},
	"unfolded coordinates" -> N@{{0,0,0},{1.414278593427568`,-0.00012107343015166361`,0},{1.4142678318232762`,1.4140274916158289`,0},{0.00011927863201000033`,1.414016642177331`,0},{1.4142692917736754`,1.227825699827347`,0},{1.3131744350755659`,1.211870419723824`,0},{1.2781932400501788`,1.2779508631318839`,0},{0.9659583817161155`,0.9657127724823382`,0},{1.0728360008415996`,0.7312622802432727`,0},{0.47189742974276605`,0.9427579377876374`,0},{0.353962452288944`,0.7077633593785156`,0},{1.2121120814828474`,1.312933079920714`,0},{1.2280642010424148`,1.4140260798657769`,0},{0.731505784488571`,1.0725852346780647`,0},{0.7073110670506855`,0.3537763181270138`,0},{0.9430090748133337`,0.47164965094818173`,0},{0.7074588006059522`,0.7069478151898358`,0},{1.4142724979918373`,0.8060679230868013`,0},{0.8063082321157269`,1.4140228586178498`,0},{0.23507732779834628`,1.1102230246251565`*^-16,0},{0.0001931022705954355`,0.2355241969949885`,0}},
	"folded coordinates"->{{0.5216212272644043`,0.0041301921010017395`,-0.5215774178504944`},{-0.6376347541809082`,0.4420289695262909`,-0.16293174028396606`},{-0.435793399810791`,-0.10257021337747574`,0.4356272220611572`},{0.16264325380325317`,0.442157119512558`,0.6371942758560181`},{-0.4927003085613251`,0.07479492574930191`,0.4173307418823242`},{-0.4104839861392975`,0.11236074566841125`,0.36908411979675293`},{-0.4352164566516876`,0.0874059870839119`,0.43510642647743225`},{-0.22010493278503418`,-0.2338118851184845`,0.22003494203090668`},{-0.13188940286636353`,-0.15577159821987152`,-0.008406173437833786`},{0.11944180727005005`,0.1096508726477623`,0.06939235329627991`},{0.047771066427230835`,-0.13527639210224152`,0.09195645153522491`},{-0.3691454231739044`,0.11233522742986679`,0.410442054271698`},{-0.4174804091453552`,0.07477816939353943`,0.49260756373405457`},{0.00845295935869217`,-0.15610502660274506`,0.13212451338768005`},{-0.09123314917087555`,-0.13514138758182526`,-0.04728326201438904`},{-0.06939570605754852`,0.11028251051902771`,-0.11917997896671295`},{-0.18727263808250427`,0.1310577541589737`,0.1871875524520874`},{-0.41846323013305664`,-0.254423052072525`,0.16566649079322815`},{-0.1659073531627655`,-0.25454390048980713`,0.4185855984687805`},{0.3550601601600647`,-0.1361958533525467`,-0.4324187636375427`},{0.43267032504081726`,-0.13634231686592102`,-0.35442548990249634`}},
	"jihwan" -> {-((13 \[Pi])/15),(143 \[Pi])/180,-((17 \[Pi])/30),(41 \[Pi])/60,(139 \[Pi])/180,-((149 \[Pi])/180),-((107 \[Pi])/180),-((149 \[Pi])/180),(139 \[Pi])/180,-((149 \[Pi])/180),-((149 \[Pi])/180),(143 \[Pi])/180,(41 \[Pi])/60,-((13 \[Pi])/15),-((17 \[Pi])/30),-((13 \[Pi])/15),(43 \[Pi])/60,(43 \[Pi])/60,-((13 \[Pi])/15),-((79 \[Pi])/180),\[Pi],(157 \[Pi])/180,(157 \[Pi])/180,\[Pi],-((23 \[Pi])/30),(107 \[Pi])/180,(34 \[Pi])/45,-((23 \[Pi])/30),(34 \[Pi])/45,-((91 \[Pi])/180)}
]


Origami["WaterBombBase"] := SingleVertex[ ConstantArray[2 Pi/8,8] ,Flatten[ {1,Sqrt[2]} & /@ Range[4]]  ]


ElasticizeOrigami[ m_Origami ] := Module[
{
	bars = MechanismCellData[m, _RigidBar],
	Faces = MechanismCellData[m,_Face],
	TorsionalFolds = MechanismCellData[m,_TorsionalFold],
	possibleFolds = InteriorEdges[m],
	skippedFolds
},
	AddCells[ 
		DeleteCells[ 
			Join[ 
				If[Length[bars]>0, Thread[bars[[1,1]]] , {}] , 
				If[Length[Faces]>0, Thread[Faces[[1,1]] ], {} ]
			] 
		] @ m,

		{
		If[ Length[Faces]>0, ElasticTriangle /@ Faces[[1,1,1]] , {} ],
		If[ Length[TorsionalFolds]>0,
			skippedFolds = Complement[ Sort/@possibleFolds, Sort/@TorsionalFolds[[1,1,1]]];
			If[Length[skippedFolds]>0, TorsionalFold[ skippedFolds , {"TorsionalStiffness" -> 0}], {} ],
			TorsionalFold[possibleFolds,{"TorsionalStiffness" -> 0}]
		]
		}
	] /; FacesWorkQ[ Faces ]
]

FacesWorkQ[ {} ] := True
FacesWorkQ[ {Face[ indices_ ] -> data_ } ] := AllTrue[ {MatrixQ[indices], Last[Dimensions[indices]] == 3}, TrueQ ]
FacesWorkQ[ _ ] := False

ElasticizeOrigami[ m_Origami ] := "nothing" /; Message[ElasticizeOrigami::Faces];
ElasticizeOrigami::Faces="Faces must be triangles to elasticize this Origami.";


TargetFoldAngles[ m_Origami ]:=With[ {res=TargetFoldAngles[m,m["positions"]]}, res /; Head[res] =!= TargetFoldAngles]
TargetFoldAngles[ m_Origami , s_String]:=With[ {res=TargetFoldAngles[m,m["positions"],s]}, res /; Head[res] =!= TargetFoldAngles]

TargetFoldAngles[ m_Origami, pos_ ] := 
With[{ data=MechanismCellData[m,_TorsionalFold]},
	If[Length[data]>0,
		Association[
			"TorsionalFolds" -> data[[1,1,1]],
			"target" -> data[[1,2]]["Angle"],
			"actual" -> TorsionalFoldAngle[m, pos, data[[1,1,1]]]
		],
		Association["TorsionalFolds"->{},"target"->{},"actual"->{}]
	]
] /; NumericCoordinatesQ[m,pos]

TargetFoldAngles[ m_Origami, pos_ ,"TorsionalFolds"] := 
With[{ data=MechanismCellData[m,_TorsionalFold]},
	If[Length[data]>0,data[[1,1,1]],{}]
] /; NumericCoordinatesQ[m,pos]

TargetFoldAngles[ m_Origami, pos_ , "target"] := 
With[{ data=MechanismCellData[m,_TorsionalFold]},
	If[Length[data]>0,data[[1,2]]["Angle"],{}]
] /; NumericCoordinatesQ[m,pos]

TargetFoldAngles[ m_Origami, pos_ , "actual"] := 
With[{ data=MechanismCellData[m,_TorsionalFold]},
	If[Length[data]>0,TorsionalFoldAngle[m, pos, data[[1,1,1]]],{}]
] /; NumericCoordinatesQ[m,pos]

TargetFoldAngles::data = "Data is not one of \"TorsionalFolds\", \"actual\", \"target\"";
TargetFoldAngles::pos = "Positions are not numeric or do not correspond to Mechanism.";

TargetFoldAngles[m_Origami, pos_, s_] := "nothing"/;Message[TargetFoldAngles::data]
TargetFoldAngles[m_Origami, pos_, "TorsionalFolds"|"target"|"actual"] := "nothing"/;Message[TargetFoldAngles::pos]


Options[FlatOrigamiQ]={
	ZeroTest->Automatic,
	Precision->Infinity
};


FlatOrigamiQ::precision="Precision is not a positive real number or Infinity.";
FlatOrigamiQ::zerotest="ZeroTest function must return True or False.";


computePrecision[m_,Infinity] := Precision[m]
computePrecision[m_,precision_?NumericQ] := Min[Precision[m],precision] /; precision>0
computePrecision[m_,_] := $Failed


computeZeroTestFunction[Automatic,Infinity] := PossibleZeroQ
computeZeroTestFunction[Automatic, precision : _?NumericQ] := (Abs[#]<=10^(-precision+1)&)
computeZeroTestFunction[f_,_] := f


FlatOrigamiQ[m_Origami, OptionsPattern[]]:=Module[{
	actualPrecision = computePrecision[m,OptionValue[Precision]], zerotestFunction, res
},
	zerotestFunction=computeZeroTestFunction[ OptionValue[ZeroTest], actualPrecision];

	res=flatOrigamiInternalQ[m,actualPrecision,zerotestFunction];

	res/;Which[
		Head[res]=!=flatOrigamiInternalQ,
			True,
		Not[BooleanQ[zerotestFunction[0]]],
			Message[FlatOrigamiQ::zerotest]; False,
		actualPrecision===$Failed,
			Message[FlatOrigamiQ::precision]; False,
		True,False
	]
]


flatOrigamiInternalQ[m_, precision : _?NumericQ | Infinity, zerotestFunction_?(BooleanQ[#[0]]&)]:=
	AllTrue[DiscreteGaussianCurvature[m, N[m["positions"], precision] , InteriorVertices[m] ], zerotestFunction]


Options[KawasakiQ]={
	ZeroTest->PossibleZeroQ,
	WorkingPrecision->Infinity
};


KawasakiQ[m_Origami,opt:OptionsPattern[]]:=
With[
{
	v=InteriorVertices[m],
	mNum=N[m,OptionValue[WorkingPrecision]]
},
	If[FlatOrigamiQ[m,opt],
		And@@(OptionValue[ZeroTest][kawasakiAlternatingSum[mNum,#]]&/@v),
		False
	]
]/;(
	((NumericQ[OptionValue[WorkingPrecision]]&&OptionValue[WorkingPrecision]>0)||
	OptionValue[WorkingPrecision]===Infinity)&&
	BooleanQ[OptionValue[ZeroTest][0]]
)


kawasakiAlternatingSum[m_,v_Integer]:=
With[
{
	Faces=RotateRight[#,1]&/@MechanismFaces[m,v]
},
	If[OddQ[Length[Faces]],
		1,
		Total[DiagonalMatrix[(-1)^# &/@Range[Length[Faces]]] . PlaneAngle[m,Faces]]
	]
]


KawasakiQ::workingprecision="Working precesion must be a real value larger than 0.";
KawasakiQ::zerotest="Zero test does not return True or False.";

KawasakiQ[m_Origami,opt:OptionsPattern[]]:="nothing"/;Which[
	Not[(NumericQ[OptionValue[WorkingPrecision]]&&OptionValue[WorkingPrecision]>0)||OptionValue[WorkingPrecision]===Infinity],
		Message[KawasakiQ::workingprecision];
		False,
	Not[BooleanQ[OptionValue[ZeroTest][0]]],
		Message[KawasakiQ::zerotest];
		False,
	True,False
]


SameFoldAnglesQ[m_Origami, TorsionalFolds_?(MatrixQ[#,IntegerQ]&), precision : _?(NumericQ[#]&&#>0&) : 0.1 ][ pos1_?MatrixQ, pos2_?MatrixQ ] :=
With[
{
	angles1 = TorsionalFoldAngle[ m, pos1, TorsionalFolds ],
	angles2 = TorsionalFoldAngle[ m, pos2, TorsionalFolds ]
},
	(angles1 - angles2) . (angles1 - angles2) < precision^2
] /; Dimensions[pos1]==Dimensions[pos2] && Dimensions[pos1]=={MeshCellCount[m["mesh"],0],MechanismEmbeddingDimension[m]}
SameFoldAnglesQ[m_Origami, TorsionalFolds_?(MatrixQ[#,IntegerQ]&), precision : _?(NumericQ[#]&&#>0&) : 0.1][_,_] := False

SameFoldAnglesQ[m_Origami, precision : _?(NumericQ[#]&&#>0&) : 0.1 ][ pos1_?MatrixQ, pos2_?MatrixQ ] :=
With[
{TorsionalFolds = InteriorEdges[m]},
	With[
	{
	angles1 = TorsionalFoldAngle[ m, pos1, TorsionalFolds ],
	angles2 = TorsionalFoldAngle[ m, pos2, TorsionalFolds ]
	},
	(angles1 - angles2) . (angles1 - angles2) < precision^2
	]
] /; Dimensions[pos1]==Dimensions[pos2] && Dimensions[pos1]=={MeshCellCount[m["mesh"],0],MechanismEmbeddingDimension[m]}
SameFoldAnglesQ[m_Origami, precision : _?(NumericQ[#]&&#>0&) : 0.1][_,_] := False


SameMVAnglesQ[m_Origami, TorsionalFolds_?(MatrixQ[#,IntegerQ]&), precision : _?(NumericQ[#]&&#>0 &) : 0.1 ][ pos1_?MatrixQ, pos2_?MatrixQ ] :=
With[
{
	angles1 = Sign[ If[Abs[#]<precision,0,#]& /@ TorsionalFoldAngle[ m, pos1, TorsionalFolds ] ],
	angles2 = Sign[ If[Abs[#]<precision,0,#]& /@ TorsionalFoldAngle[ m, pos2, TorsionalFolds ] ]
},
	And @@ MapThread[#1==#2&,{angles1,angles2}]
] /; Dimensions[pos1]==Dimensions[pos2] && Dimensions[pos1]=={MeshCellCount[m["mesh"],0],MechanismEmbeddingDimension[m]}
SameMVAnglesQ[m_Origami, TorsionalFolds_?(MatrixQ[#,Integer]&), precision : _?(NumericQ[#]&&#>0 &) : 0.1][_,_] := False

SameMVAnglesQ[m_Origami, precision : _?(NumericQ[#]&&#>0&) : 0.1 ][pos1_?MatrixQ, pos2_?MatrixQ ] :=
With[
{TorsionalFolds = InteriorEdges[m]},
	With[
	{
	angles1 = Sign[ If[Abs[#]<precision,0,#]& /@ TorsionalFoldAngle[ m, pos1, TorsionalFolds ] ],
	angles2 = Sign[ If[Abs[#]<precision,0,#]& /@ TorsionalFoldAngle[ m, pos2, TorsionalFolds ] ]
	},
	And @@ MapThread[#1==#2&,{angles1,angles2}]
	]
] /; Dimensions[pos1]==Dimensions[pos2] && Dimensions[pos1]=={MeshCellCount[m["mesh"],0],MechanismEmbeddingDimension[m]}
SameMVAnglesQ[m_Origami, precision : _?(NumericQ[#]&&#>0&) : 0.1][_,_] := False


TorsionalFoldMatrix[m_Origami]:=TorsionalFoldMatrixInternal[m,m["positions"],InteriorEdges[m]]
TorsionalFoldMatrix[m_Origami,positions_]:=TorsionalFoldMatrixInternal[m,m["positions"],InteriorEdges[m]]/;VertexCoordinatesQ[m,positions]
TorsionalFoldMatrix[m_Origami,positions_,indices_?edgeListQ]:=TorsionalFoldMatrixInternal[m,positions,indices]/;VertexCoordinatesQ[m,positions]


TorsionalFoldMatrixInternal[m_,positions_,indices_?(MatrixQ[#,IntegerQ]&)]:= With[
{displacements=VertexDisplacement[m]},
	D[TorsionalFoldAngle[m,positions+displacements,indices],{Flatten[displacements]}]/.Dispatch[Thread[Flatten[displacements]->0]]
]


TorsionalFoldMatrix::notorig="Mechanism is not Origami.";
TorsionalFoldMatrix::pos="Positions do not correspond to the Mechanism provided.";
TorsionalFoldMatrix::indices="Indices provided do not correspond to a list of edges.";

TorsionalFoldMatrix[m_]:="nothing"/;Message[TorsionalFoldMatrix::notorig]
TorsionalFoldMatrix[m_,positions_]:="nothing"/;Which[
	Head[m]=!=Origami,Message[TorsionalFoldMatrix::notorig],
	Not[VertexCoordinatesQ[m,positions]],Message[TorsionalFoldMatrix::pos],
	True,False
]
TorsionalFoldMatrix[m_,positions_,indices_]:="nothing"/;Which[
	Head[m]=!=Origami,Message[TorsionalFoldMatrix::notorig],
	Not[VertexCoordinatesQ[m,positions]],Message[TorsionalFoldMatrix::pos],
	Last[Dimensions[indices]]!=2,Message[TorsionalFoldMatrix::indices],
	True,False
]


AngularFoldMatrix[m_Origami,vertex_Integer]:=AngularFoldMatrix[m,m["positions"],vertex]
AngularFoldMatrix[m_Origami,positions_?MatrixQ,vertex_Integer]:=Module[
{vertices=MechanismVertices[m,vertex],TorsionalFoldLengths,angles,heights},
	TorsionalFoldLengths=DisplacementLength[m["positions"],{#,vertex}&/@vertices];
	heights=Thread[VertexDisplacement[vertices,"z"]->TorsionalFoldLengths Array[angles,Length[vertices]]];
	Last@CoefficientArrays[
		-TorsionalFoldMatrixInternal[m,positions,{#,vertex}&/@vertices] . Flatten[VertexDisplacement[m,All[3]]]/.heights/.VertexDisplacement[_,_]->0,
		Array[angles,Length[vertices]]
	]
]/;MemberQ[InteriorVertices[m],vertex]


AngularFoldMatrix::notintv="Vertex should be an interior vertex.";
AngularFoldMatrix::orig="Mechanism is not Origami.";
AngularFoldMatrix::pos="Positions do not correspond to the Mechanism provided.";

AngularFoldMatrix[m_Origami,_]:="nothing"/;Message[AngularFoldMatrix::notintv]
AngularFoldMatrix[m_,positions_,v_]:="nothing"/;Which[
	Head[m]=!=Origami,Message[AngularFoldMatrix::orig],
	Not[VertexCoordinatesQ[m,positions]],Message[AngularFoldMatrix::pos],
	Not[IntegerQ[v]]||Not[MemberQ[InteriorVertices[m],v]],Message[AngularFoldMatrix::notintv],
	True,False
]


ButterflyDecomposition[ o_Origami ] := Module[
{ results = FixedPointList[ ButterflyDecompositionStep, {{},o} ], remainingOrigami },
	remainingOrigami = Last[results][[2]];

	{ Drop[ Drop[ results[[All,1]], -1],1], remainingOrigami }
]

ButterflyDecompositionStep[ {Facepair_, o_Origami} ] := Module[
{boundary=Flatten[BoundaryVertices[o]], Faces, possibleButterflies, butterfly},
	Faces = MechanismConnectivity[ o, "vertices" -> "faces"][[ boundary ]];
	possibleButterflies = Select[ Transpose[ {boundary, Faces} ], Length[Last[#]] == 2 & ];
	If[Length[possibleButterflies]>0,
		butterfly = RandomChoice[possibleButterflies];
		{ butterfly[[2]], DeleteCells[o, Alternatives@@( Face /@ butterfly[[2]] ) ]  },

		{Facepair, o}
	]
] /; Length[o["faces"]]>=2
ButterflyDecompositionStep[ {Facepair_,o_Origami} ] := {Facepair,o}

(*this version shows little speed improvements over solving everything explicitly*)
ButterflyDecompositionQ[ o_Origami ] := FixedPoint[ ButterflyDecompositionStepQ, o ]

ButterflyDecompositionStepQ[ f_?BooleanQ ] := f
ButterflyDecompositionStepQ[ o_Origami ] := Module[
{boundary=Flatten[BoundaryVertices[o]], Faces, possibleButterflies, butterfly},
	Faces = MechanismConnectivity[ o, "vertices" -> "faces"][[ boundary ]];
	possibleButterflies = Select[ Transpose[ {boundary, Faces} ], Length[Last[#]] == 2 & ];
	If[Length[possibleButterflies]>0,
		butterfly = RandomChoice[possibleButterflies]; 
		DeleteVertices[o, {butterfly[[1]]}],

		False
	]
] /; Length[o["faces"]]>=2
ButterflyDecompositionStepQ[ o_Origami ] := True


unTorsionalFoldOrigami[ m_Origami ] := 
Module[
{
	TorsionalFolds = Property[ TorsionalFold[#], {"Angle" -> 0} ]& /@ InteriorEdges[m],
	selfflatteningOrigami, res
},
	selfflatteningOrigami = AddCells[ TorsionalFolds ] @ DeleteCells[ _PinnedJoint ] @ DeleteCells[ _TorsionalFold ] @ m;

	res = MinimizeMechanismEnergy[ selfflatteningOrigami, MaxIterations -> 10^6 ];
	If[ Head[res]===List, res, {Infinity, m["positions"]} ]
]


chooseBoundaryFace[ m_Origami ] := First @ SelectFirst[ MechanismConnectivity[m, "edges" -> "faces" ], Length[#]==1 & ]


rotateFaceToXYPlane[ positions_, Face : {__Integer} ] :=
Module[{ initialPoints = positions[[ Face[[1;;3]] ]], lengths, angle, newPointLocations, residual, transform }, 
	lengths = DisplacementLength[positions, {Face[[1;;2]],Face[[2;;3]]} ];
	angle = First @ PlaneAngle[ positions, {Face[[1;;3]]} ];	
	newPointLocations = {
		{lengths[[1]],0,0},
		{0,0,0},
		lengths[[2]] {Cos[ angle ], Sin[ angle ], 0 }
	};
	
	{ residual, transform } = FindGeometricTransform[ newPointLocations,initialPoints, TransformationClass -> "Rigid", Method -> "Linear" ];

	transform
]

FlattenOrigami::Face="Unable to identify Faces.";
rotateFaceToXYPlane[ m_Origami, _ ] := (Message[FlattenOrigami::Face]; Identity)


Options[FlattenOrigami] = Options[MeshRegion];

FlattenOrigami[ m_Origami , tol_?(NumericQ[#] && #>0&), opt : OptionsPattern[] ] := 
Module[{ unTorsionalFoldedEnergy, unTorsionalFoldedPositions, transform, xyPositions, newOrigami },
	If[ Not[FlatOrigamiQ[m, Precision -> tol ]], Message[FlattenOrigami::flat] ];

	{ unTorsionalFoldedEnergy, unTorsionalFoldedPositions } = unTorsionalFoldOrigami[m];
	If[ (# . # &) @ TorsionalFoldAngle[m, unTorsionalFoldedPositions, InteriorEdges[m]] > 10^(-4), Message[ FlattenOrigami::notflt ] ];

	transform = rotateFaceToXYPlane[ unTorsionalFoldedPositions, chooseBoundaryFace[m] ];
	xyPositions = PadRight[ transform[ unTorsionalFoldedPositions ][[All,1;;2]], {Length[unTorsionalFoldedPositions], 3 } ];
			
	Mechanisms`Private`addMeshRegion[ Origami[ xyPositions, {}, m[[3]], m[[4]] ], Automatic, opt ]
]

FlattenOrigami::notflt="Origami flat energy may be too large to get good results.";
FlattenOrigami::flat="Gaussian curvature of vertices is not flat to desired tolerance `1`.";
FlattenOrigami::tol = "Tolerance `1` should be a positive number.";
FlattenOrigami::tol2 = "Second argument should be a tolerance.";

FlattenOrigami[ m_Origami, OptionsPattern[] ] := "nothing" /; Message[FlattenOrigami::tol2 ]
FlattenOrigami[ m_Origami, tol_, OptionsPattern[] ] := "nothing" /; Message[FlattenOrigami::tol, tol ]


MVAssignment[ m_Origami, positions_ : Automatic, tol : _?NumericQ, color_ : Automatic ] := 
With[{
	pos = If[positions === Automatic, m["positions"], positions], 
	colorFunction = If[color === Automatic, MVdefaultColor, color ], 
	edges = InteriorEdges[m]
},

	With[ { angles = TorsionalFoldAngle[m, pos, edges] },
		MapThread[ (#1 -> colorFunction[ assignMVForFold[ #2, tol ] ] ) & , { edges, angles } ]
	] /; NumericCoordinatesQ[m, pos]

]

assignMVForFold[angle_, tol_]:= 1 /; angle > Abs[tol]
assignMVForFold[angle_, tol_]:= -1 /; angle < -Abs[tol]
assignMVForFold[_,_]:=0

MVdefaultColor[1] := Red
MVdefaultColor[-1] := Blue
MVdefaultColor[0] := Black


MVColor[m_Origami, s_String, colorFunction_Function ]:= Module[ {data=TargetFoldAngles[m],styles},
	(
	styles=colorFunction/@data[s];
	ChangeCellData[m, RigidBar,data["TorsionalFolds"],"Style" -> styles]
	) /; If[ MemberQ[ Keys[data], s], True, Message[MVColor::s,s,Keys[data]]; False]
]

MVColor::s="`1` is not a valid string of type `2`.";


SurfaceArea[o_Origami, r___] ^:= SurfaceArea[ o["mesh"], r ]
RegionMeasure[o_Origami, r___] ^:= RegionMeasure[ o["mesh"], r ]
RegionMoment[o_Origami, r___] ^:= RegionMoment[ o["mesh"], r ]


Options[DeconstructedOrigami]={
	"edgeWeights" -> {},
	"defaultWeight" -> 0
};

DeconstructedOrigami[ m_Origami, opt : OptionsPattern[] ]:=
	DeconstructedOrigami[m, MechanismPositions[m], opt ]

DeconstructedOrigami[ m_Origami, coords_, opt : OptionsPattern[] ]:=
With[{
	Faces = MechanismFaces[m], 
	widths = MechanismConnectivity[m,"faces" -> "edges"] /. OptionValue["edgeWeights"] /. {_Integer,_Integer} -> OptionValue["defaultWeight"]
},
	MapThread[ shrinkPolygon , {coords[[#]]& /@ Faces,widths} ]
] /; DeconstructedOrigamiQ[m , coords, OptionValue["edgeWeights"], OptionValue["defaultWeight"] ]

DeconstructedOrigamiQ[m_, positions_, rules_, default_ ]:=
	If[ VertexCoordinatesQ[m,positions], True, Message[DeconstructedOrigami::pos]; False] &&
	If[ NumericQ[default]&&default >= 0, True, Message[DeconstructedOrigami::defaultweight]; False ] && 
	If[ MatchQ[ rules, {___Rule} ], True, Message[DeconstructedOrigami::weightRules]; False ] &&
	If[ VectorQ[ rules[[All,2]] , NumericQ[#]&&#>=0& ], True, Message[DeconstructedOrigami::weightrules]; False ] &&
	If[ MatchQ[ rules[[All,1]] , {{_Integer,_Integer}...} ], True, Message[DeconstructedOrigami::weights]; False ]

DeconstructedOrigami::pos="The positions do not correspond to the Mechanism.";
DeconstructedOrigami::weights="The weights must be positive numbers.";
DeconstructedOrigami::defaultweight="The default weight must be a positive number.";
DeconstructedOrigami::edges="The left hand side of the rules must be well-formed edges.";
DeconstructedOrigami::weightrules="Option \"edgeWeights\" must be a list of rules of the form edge -> positive weight.";


shrinkPolygon[ pts_, widths_ ] := pts + Total @ MapIndexed[ shrinkPolygonSide[ pts, #2[[1]], #1 ]&, widths ]

shrinkPolygonSide[ pts_, v1_, width_ ] :=
Module[{n=Length[pts], v2, future, past, futureEdge, edge, pastEdge},
	v2=Mod[v1+1, n, 1]; future=Mod[v2+1, n, 1]; past = Mod[v1-1,n,1];
	edge=Normalize[ pts[[v2]] - pts[[v1]] ];
	futureEdge = Normalize[ pts[[future]] - pts[[v2]] ];
	pastEdge = Normalize[ pts[[past]] - pts[[v1]] ];
	
	RotateRight[Join[
		width*{
			pastEdge/Norm[ Cross[ pastEdge, edge ] ],
			futureEdge/Norm[ Cross[ futureEdge, edge ] ]
		},
		ConstantArray[{0,0,0},n-2]
	], v1-1]
]


End[];

EndPackage[];


BeginPackage["Mechanisms`graphics`"];


AngleMarker::usage="AngleMarker[m, {v1,v2,v3}, (radius) ] creates an arc around an angle spanning (v1,v2) to (v3,v2).";
AngleText::usage="angleTest[m, {v1,v2,v3}, (distance) ] adds a text label to the angle spanning (v1,v2) to (v3,v2).";

MechanismPrimitives::usage = "MechanismPrimitives[ m, (positions) ] returns a list of Mechanism primitives." ;
ToGraphicsComplex::usage = "ToGraphicsComplex[ m, (positions) ] returns a GraphicsComplex object identical to MechanismPrimitives[].";


PinnedJointShape::usage="PinnedJointShape[] is a triangular pinned joint in 2D.";
SpringShape::usage="SpringShape[] represents a Spring shape function with a particular thickness and number of jags.";
PinnedJointShape::usage="FreeJointShape[] represents a joint shape distinguishing pinned joints from free joints.";
RigidBarShape::usage="RigidBarShape[] represents the shape of a RigidBar";
FreeJointShape::usage="FreeJointShape[] represents the shape of a free joint.";
JointShape::usage="JointShape[] represents the shape of a joint that is either free or pinned.";
FaceShape::usage="FaceShape[]";


PlotMechanism::usage=
"PlotMechanism[ Mechanism, positions ] plots a Mechanism using positions.
PlotMechanism[Mechanism, {positions 1, positions 2,...}] creates a list of plotted Mechanisms with uniform size.";


StressStyle::usage="StressStyle is an option for PlotMechanism to provide a style for plotting stresses.";
DisplacementStyle::usage="DisplacementStyle is an option for PlotMechanism to provide a style for plotting displacements.";
DisplacementScale::usage="DisplacementScale is an option for PlotMechanism to provide an overall scale for displacements.";
DisplayDimension::usage="DisplayDimension is an option for PlotMechanism to choose the dimension in which a mechanism should be displayed.";
Displacements::usage="Displacements is an option for PlotMechanism to display a vertex displacement overlaid with a mechanism.";
Stresses::usage="Stresses is an option for PlotMechanism to display stresses overlaid with a mechanism.";


OrigamiTexture::usage="OrigamiTexture[ s ] returns a texture having the name s. Use $OrigamiTextures to see possible textures.";
$OrigamiTextures::usage="$OrigamiTextures returns a list of textures you can use in OrigamiTexture[] to texture the faces of origami.";


$MechanismThemes = "List of recognized graphics themes.";


Begin["`Private`"];

Needs["Mechanisms`"];
Needs["Mechanisms`geometry`"];
Needs["Mechanisms`rigidity`"];
Needs["Mechanisms`Origami`"];


defaultGraphicsPrimitive[RigidBar|Line , Linkage , {3,2}, "Standard"] := RigidBarShape["Width" -> 0.05]
defaultGraphicsPrimitive[RigidBar|Line , Linkage , _, "Standard"] := RigidBarShape[]

defaultGraphicsPrimitive[FreeJoint , Linkage , {3,2}, "Standard"] := FreeJointShape["Diameter" -> 0.05]
defaultGraphicsPrimitive[FreeJoint , Linkage , _, "Standard"] := FreeJointShape[]

defaultGraphicsPrimitive[PinnedJoint , Linkage , {3,2}, "Standard"] := PinnedJointShape["Diameter" -> 0.05]
defaultGraphicsPrimitive[PinnedJoint , Linkage , _ , "Standard"] := PinnedJointShape[]


defaultGraphicsPrimitive[RigidBar|Line , Origami , _ , "Standard"] := (Line[#]&)
defaultGraphicsPrimitive[FreeJoint , Origami , _ , "Standard"] := (Point[#]&)
defaultGraphicsPrimitive[PinnedJoint , Origami , _ , "Standard"] := (Point[#]&)
defaultGraphicsPrimitive[TorsionalFold, Origami, _ , "Standard"] := (Line[#]&)

defaultGraphicsStyle[ TorsionalFold, Origami, _ , "Standard" ] := {Black}


defaultGraphicsPrimitive[Face | ElasticTriangle, Origami, _ , "Thick"] := FaceShape["Thickness"->0.01]
defaultGraphicsPrimitive[Face | ElasticTriangle, Origami, _ , "Thick"[thickness_] ] := FaceShape["Thickness" -> thickness]


defaultGraphicsPrimitive[Face, _ , _ , "Standard"] := FaceShape[]
defaultGraphicsPrimitive[ElasticTriangle, _ , _ , "Standard"] := FaceShape["Thickness"->0.01]
defaultGraphicsPrimitive[Polygon , _ , _ , "Standard"] := (Polygon[#]&)
defaultGraphicsPrimitive[Spring , _ , _ , "Standard"] := SpringShape[]
defaultGraphicsPrimitive[_ , _ , _ , _ ] := None


defaultGraphicsPrimitive[ RigidBar|Spring|Line, _, _ , "Minimal" ] := (Line[#]&)
defaultGraphicsPrimitive[ FreeJoint|PinnedJoint|Point, _, _ , "Minimal" ] := (Point[#]&)
defaultGraphicsPrimitive[ Face|ElasticTriangle|Polygon, _, _ , "Minimal" ] := (Polygon[#]&)


defaultGraphicsStyle[ RigidBar , _ , {3,2} , "Standard"] := {RGBColor[0.25,0.45,0.95]}
defaultGraphicsStyle[ RigidBar , _ , _ , "Standard"] := {Black}

defaultGraphicsStyle[ FreeJoint , _ , _ , "Standard"] := {Black}

defaultGraphicsStyle[ PinnedJoint , _ , _ , "Standard"] := {Black}

defaultGraphicsStyle[ Face , _ , _ , "Standard"] := {GrayLevel[0.85]}

defaultGraphicsStyle[ ElasticTriangle , _ , _ , "Standard"] := {RGBColor[{0.85,0.85,0.95}]}

defaultGraphicsStyle[ Spring , _ , _ , "Standard"] := {}

defaultGraphicsStyle[ _ , _ , _ , _] := {}


dimension[ position_?MatrixQ ] := Length[position[[1]]]
dimension[ positions : {__?MatrixQ} ] := Max[Length /@ positions[[All,1]]]

sameVertexNumberQ[ m_, position_?MatrixQ ] := m["VertexNumber"] == Length[position]
sameVertexNumberQ[ m_, positions : {__?MatrixQ} ] := With[ {n = m["VertexNumber"]}, AllTrue[Length /@ positions , #==n& ]]
sameVertexNumberQ[ m_, Automatic ] := True

flatEnoughQ[ position_?MatrixQ ] := Chop[ Max[ Abs[ position[[All,3]] ] ] ] == 0
flatEnoughQ[ positions: {__?MatrixQ} ] := Chop[ Max[ Abs[ Flatten[positions[[All,All,3]]] ] ] ] == 0

recastDimension[ position_?MatrixQ , dim_ ] := PadRight[position, {Length[position],dim}]
recastDimension[ position:{__?MatrixQ} , dim_ ] := PadRight[position, {Length[position],Length[position[[1]]],dim}]


positionDimension[ m_, Automatic, dim_ ]:=
	Switch[ dim,
		Automatic,
			If[MechanismEmbeddingDimension[m]==3 && MechanismDisplayDimension[m]==2,
				recastDimension[m["positions"],2],
				m["positions"]
			],
		2, If[ MechanismEmbeddingDimension[m]==3 && Not@flatEnoughQ[m["positions"]],
			Message[Mechanism::notflat]; m["positions"],
			recastDimension[m["positions"],2]
			],
		3, recastDimension[m["positions"],3],
		_, $Failed
	]

positionDimension[ m_, positions : _?VertexCoordinatesQ|{__?VertexCoordinatesQ}, dim_ ] :=
	Switch[ dim,
		Automatic,
			recastDimension[ positions, dimension[positions] ],
		2,
			If[ dimension[positions] == 3 && Not@flatEnoughQ[positions],
				Message[Mechanism::notflat];
				recastDimension[ positions, 3],
				recastDimension[ positions, 2]
			],
		3, recastDimension[ positions, 3],
		_, $Failed
	] /; sameVertexNumberQ[ m, positions]
positionDimension[ m_, positions : _?VertexCoordinatesQ|{__?VertexCoordinatesQ}, dim_ ]:= (Message[Mechanism::vn, m["VertexNumber"] ]; $Failed)


Mechanism::notflat = "Cannot project a fully 3D Mechanism into 2D.";
Mechanism::vn = "Number of vertices `1` does not agree with number of vertex positions.";


(*translating different themes*)
$MechanismThemes={"Standard" , "Minimal", "Thick"};

themesQ[m_, Automatic] := "Standard"
themesQ[m_,theme_ ] /; Length[theme]==0 := If[ Not@MemberQ[ $MechanismThemes, theme ], Message[MechanismPrimitives::theme, theme]; "Standard" , theme]
themesQ[m_, theme_ ] := If[ Not@MemberQ[ $MechanismThemes, Head[theme] ], Message[MechanismPrimitives::theme, theme]; "Standard" , theme]

MechanismPrimitives::theme="`1` is not a valid theme.";


(*sortComponentsForGraphics[] sorts a list of components*)
sortComponentsForGraphics[ components_ ] :=
	SortBy[DeleteCases[
		{ graphicsOrder[Head[#[[1]]]] , # }& /@ components,
		{3,_}
	],First][[All,2]]

graphicsOrder[ PinnedJoint|FreeJoint|Point ]:=2
graphicsOrder[ RigidBar|Spring|Line|TorsionalFold ]:=1
graphicsOrder[ Face|ElasticTriangle|Polygon ]:=0
graphicsOrder[ _ ]:=3


Options[MechanismPrimitives] = {PlotTheme->Automatic};

MechanismPrimitives[ m_?MechanismQ, dimension : 2|3|Automatic : Automatic , OptionsPattern[]] :=With[
{pos=positionDimension[m, Automatic, dimension]},
	mechanismPrimitivesInternal[ m, pos, m["positions"] , themesQ[m,OptionValue[PlotTheme]] ] /; pos =!= $Failed
]
MechanismPrimitives[ m_?MechanismQ, positions_?NumericCoordinatesQ, dimension : 2|3|Automatic : Automatic , OptionsPattern[]] :=With[
{ pos=positionDimension[m, positions, dimension]},
	mechanismPrimitivesInternal[ m, pos, positions, themesQ[m,OptionValue[PlotTheme]] ] /; pos =!= $Failed
]

MechanismPrimitives[ m_?MechanismQ, positions_?NumericCoordinatesQ, dimension : 2|3|Automatic : Automatic , OptionsPattern[]] :=
	"nothing" /; Message[MechanismPrimitives::coord, Length[positions], m["VertexNumber"]]
MechanismPrimitives[ m_?MechanismQ, positions_?NumericCoordinatesQ, dimension_ , OptionsPattern[]] :=
	"nothing" /; Message[MechanismPrimitives::dim]
MechanismPrimitives[ m_?MechanismQ, positions_?VertexCoordinatesQ, dimension : 2|3|Automatic : Automatic , OptionsPattern[]] :=
	"nothing" /; Message[MechanismPrimitives::num]


MechanismPrimitives::coord = "Number of vertices in second argument, `1`, does not match number of vertices in Mechanism, `2`.";
MechanismPrimitives::dim = "Number of dimensions requested is not 2, 3, or Automatic.";
MechanismPrimitives::num = "Vertices should be numeric.";


timingEcho[ c_ ] := With[ {result = AbsoluteTiming[c]}, Echo[result[[1]], Head[c]]; result[[2]] ]
timingEcho[ s_String, c_ ] := With[ {result = AbsoluteTiming[c]}, Echo[result[[1]], s]; result[[2]] ]
SetAttributes[timingEcho,HoldAll];


mechanismPrimitivesInternal[ m_, positions_ , actualPositions_, theme_] :=
	Transpose[
		mechanismPrimitivesFromComponent[ m, positions, # , theme]& /@ sortComponentsForGraphics[ MechanismCellData[ m, actualPositions, _ ] ]
	]

mechanismPrimitivesFromComponent[ m_, positions_, Rule[head_[ indices_ ], display_ ] , theme_] :=
With[ {
	coordinates = ToPackedArray[N[positions[[ # ]] & /@ indices]], 
	data = Transpose[ Join[{ConstantArray[ m["EmbeddingDimension"] , Length[indices]]}, Drop[Values[ display ],-3]] ],
	style = Mechanisms`Private`combineDataList[ defaultGraphicsStyle[head, Head[m], {Length[positions[[1]]],m["EmbeddingDimension"]}, theme] ,display["Style"]],
	labels = display["Label"],
	shapes = display["Shape"] /. {Automatic -> defaultGraphicsPrimitive[head, Head[m], {Length[positions[[1]]],m["EmbeddingDimension"]},theme]} /. { None -> ({}&) }
},
	{
		MapThread[applyFunction ,
			{
			shapes, 
			coordinates, 
			data,
			head /@ indices,
			style
			}
		],

		Flatten @ MapThread[applyLabel,
			{
			If[ MatrixQ[coordinates], coordinates, Mean /@ coordinates ],
			indices,
			labels
			}
		]
	}
]

$textOffset = 0.05;
applyLabel[ coordinates_, _, None ] := {}
applyLabel[ coordinates_, label_, "Index"] := Text[label, coordinates + ConstantArray[$textOffset,Length[coordinates]]]
applyLabel[ coordinates_, _, lab_ ] := Text[ lab, coordinates + ConstantArray[$textOffset,Length[coordinates]] ] /; Head[lab] =!= Text
applyLabel[ coordinates_, _, Text[lab_, coord_, other___] ] := Text[ lab, coord + coordinates, other ]

applyFunction[ None, coordinates_, primitive_, data_, style_ ] := {}
applyFunction[ shapeFunction_, coordinates_, primitive_, data_, style_ ] := Flatten[ {style, shapeFunction[coordinates,primitive,data]} ]


Options[ToGraphicsComplex] = Join[Options[GraphicsComplex], {PlotTheme->Automatic}];

ToGraphicsComplex[ m_?MechanismQ, dimension : 2|3|Automatic : Automatic, opt : OptionsPattern[] ] :=
With[ {
res=ToGraphicsComplexInternal[ m, positionDimension[m, Automatic, dimension], m["positions"],
	dimension, OptionValue[PlotTheme], FilterRules[{opt},Options[GraphicsComplex]] ]
},
	res /; res =!= $Failed
]
ToGraphicsComplex[ m_?MechanismQ, positions_?VertexCoordinatesQ, dimension : 2|3|Automatic : Automatic, opt : OptionsPattern[] ] :=
With[ {res = ToGraphicsComplexInternal[ m, positionDimension[m, positions, dimension], positions, 
	dimension, OptionValue[PlotTheme],FilterRules[{opt},Options[GraphicsComplex]] ]
},
	res /; res =!= $Failed
 ] /; Length[positions] == m["VertexNumber"]


canonicalizePrimitiveRules = {
	p : _Polyhedron|_Parallelepiped|_Cuboid :> CanonicalizePolyhedron[p],
	p : _Polygon :> CanonicalizePolygon[p]
};

extractVertexCoordinates[primitives_] := 
DeleteDuplicates[
	Flatten[ Cases[ Flatten[primitives] ,_?RegionQ|_Text] /. {
		p_Polygon :> (Point /@ PolygonCoordinates[p]),
		p_Polyhedron :> (Point /@ PolyhedronCoordinates[p]),
		(p_ /; RegionQ[p]) :> If[ Length[Dimensions[ p[[1]] ] ] == 1, Point[p[[1]]] , Point/@p[[1]] ],
		Text[_,p_,___] :> Point[p],
		Text[_,x_,y_,___] :> Point[x+y]
	}][[All,1]]
]


ToGraphicsComplexInternal[ m_, positions_, actualPositions_, dimension_, theme_, options_ ] :=
Module[ {prims, modifiedPrimitives, coordinates, coordinateRules, x},
	prims = MechanismPrimitivesInternal[m , positions, actualPositions , themesQ[m,theme] ] /. canonicalizePrimitiveRules;

	If[ Head[prims] === MechanismPrimitives,
		$Failed,

		coordinates=extractVertexCoordinates[prims];
		coordinateRules = Dispatch[Thread[coordinates -> Range[Length[coordinates]]]];

		modifiedPrimitives = prims /. {
			Polygon[x_,y_] :> Polygon[ x[[y]]  ],
			Polyhedron[x_,y_] :> Polyhedron[ x[[#]]&/@y ]
		} /. coordinateRules /. {CanonicalizePolyhedron[Polyhedron[x_]] :> Polyhedron[x],CanonicalizePolygon[Polygon[x_]] :> Polygon[x]};

		GraphicsComplex[
			coordinates,
			modifiedPrimitives,
			Flatten[{ options,VertexTextureCoordinates -> coordinates}]
		]
	]
]


$OrigamiTextures= { "WhitePaper", "GrayPaper", "WrinkledPaper" };

$dataDirectory=FileNameJoin[{(First@PacletFind["Mechanisms"])["Location"],"Resources","Textures"}];

OrigamiTexture["WrinkledPaper"] := OrigamiTexture["WrinkledPaper"] = Texture @ Normal[Import[FileNameJoin[{$dataDirectory,"crinkled-paper.jpg"}]]]
OrigamiTexture["WhitePaper"] := OrigamiTexture["WhitePaper"] = Texture @ Normal[Import[FileNameJoin[{$dataDirectory,"white-paper.jpg"}]]]
OrigamiTexture["GrayPaper"] := OrigamiTexture["GrayPaper"] = Texture @ Normal[Import[FileNameJoin[{$dataDirectory,"blue-paper.jpeg"}]]]

OrigamiTexture::bad="Unrecognized texture `1`.";
OrigamiTexture[s_] := "nothing" /; Message[OrigamiTexture::bad, s]


coordinate2D={_?NumericQ, _?NumericQ};
coordinate3D={_?NumericQ,_?NumericQ,_?NumericQ};
meshAccessQ=_Integer|{__Integer};
plotAccessQ=_Line|_Point|_Polygon;


coordinate2DQ = VectorQ[#,NumericQ] && Length[#]==2&;
coordinate3DQ = VectorQ[#,NumericQ] && Length[#]==3&;


Options[RigidBarShape] = 
{
	"Width" -> 0.01,
	"AspectRatio" -> 0.5,
	"Thickness" -> 0.025,
	"StartOffset" -> 0,
	"EndOffset" -> 0,
	"HeightOffset" -> 0
};


(*embedded in 2D, plotted in 2D*)
RigidBarShape[ opt : OptionsPattern[] ][ { x_?coordinate2DQ, y_?coordinate2DQ}, _ , _RigidBar|_Line ] := 
With[{length = Norm[y-x], dir = Normalize[y-x] , width = OptionValue["Width"]}, With[{ normal = {{0,1},{-1,0}} . dir },
	If[length>0,
		{
			Polygon[ { x - width*normal/2, x+width*normal/2, y+width*normal/2, y-width*normal/2 } ],
			(* If we wanted hemidisks instead, we would do this...
					Disk[ {0,0} , width/2, { Pi/2, 3 Pi/2 }  ],
					Disk[ {length,0} , width/2, {-Pi/2, Pi/2} ]
			*)
			Disk[x, width/2],
			Disk[y,width/2]
		},
		{}
	]
]]


startsWith3Q=(#[[1]]==3&);
startsWith2Q=(#[[1]]==2&);

(*embedded in 3D, plotted in 3D*)
RigidBarShape[ opt : OptionsPattern[] ][ { x_?coordinate3DQ, y_?coordinate3DQ}, _?startsWith3Q, _RigidBar ] := Tube[ {x,y}, OptionValue["Width"] ]
RigidBarShape[ opt : OptionsPattern[] ][ { x_?coordinate3DQ, y_?coordinate3DQ} , _, _Line ] := Tube[ {x,y}, OptionValue["Width"] ]

(*embedded in 2D, plotted in 3D*)
RigidBarShape[ opt : OptionsPattern[] ][ { x_?coordinate3DQ, y_?coordinate3DQ}, _?startsWith2Q, _RigidBar ] :=
With[{ length = Norm[ y-x ], dir = Normalize[y-x],width = OptionValue["Width"], shift1 = OptionValue["StartOffset"], shift2=OptionValue["EndOffset"], thickness = OptionValue["Thickness"], off=OptionValue["HeightOffset"] },
With[{ x1=x[[1]],y1=x[[2]],x2=y[[1]],y2=y[[2]]},
	{
	EdgeForm[None],
	Parallelepiped[{x1+ shift1 (-x1+x2)+(width (-y1+y2))/(2 length),y1+(width (x1-x2))/(2 length)- shift1 (y1-y2),off-thickness/2},{{ (-1+shift1+shift2) (x1-x2), (-1+shift1+shift2) (y1-y2),0},{(width (y1-y2))/length,(width (-x1+x2))/length,0},{0,0,thickness}}]
	}
]]


Options[FreeJointShape] = 
{
	"Diameter" -> 0.04,
	"Thickness" -> 0.027,
	"HeightOffset" -> 0
};


(*embedded in 2D, plotted in 2D*)
FreeJointShape[ OptionsPattern[] ][x : coordinate2D, _, _]:=
	With[{circleRadius = OptionValue["Diameter"]/2}, Disk[x, circleRadius] ]


(*embedded in 2D, plotted in 3D*)
FreeJointShape[ OptionsPattern[] ][x : coordinate3D, {2,___}, _FreeJoint]:=
	With[{circleRadius = OptionValue["Diameter"]/2 , thickness = OptionValue["Thickness"],off = OptionValue["HeightOffset"]},
		{EdgeForm[None],Cylinder[{ x - {0,0,thickness/2-off}, x + {0,0,thickness/2+off} }, circleRadius]}
	]
(*embedded in 3D, plotted in 3D*)
FreeJointShape[ OptionsPattern[] ][ x : coordinate3D, {3,___} , _ ] :=
	With[ {sphereRadius = OptionValue["Diameter"]/2}, Sphere[ x, sphereRadius] ]

FreeJointShape[ OptionsPattern[] ][ x : coordinate3D, {0,_} , _Point ] :=
	With[ {sphereRadius = OptionValue["Diameter"]/2}, Sphere[ x, sphereRadius] ]


Options[PinnedJointShape] =
{
	"Diameter" -> 0.05,
	"Angle" -> 0,
	"Thickness" -> 0.027
};


PinnedJointShape[ OptionsPattern[] ][ pt : coordinate2D, _,_]:=
With[{circleRadius = OptionValue["Diameter"]/5, x = pt[[1]], y = pt[[2]], angle = OptionValue["Angle"], size = OptionValue["Diameter"]},
	With[{
		mat={{Cos[angle],-Sin[angle]},{Sin[angle],Cos[angle]}},
		translation1 = ConstantArray[{x,y},4], translation2 = ConstantArray[{x,y},2]
	},
		{
			Circle[ pt , circleRadius],
			Line[translation1 + {{0,0-circleRadius},{0-size,0-circleRadius-size},{0+size,0-circleRadius-size},{0,0-circleRadius}} . mat],
			Line[translation2 + {{0-2 size,0-circleRadius-size},{0+2 size, 0-circleRadius-size}} . mat]
		}
	]
]


PinnedJointShape[OptionsPattern[]][x : coordinate3D, {2,___}, _FreeJoint|_PinnedJoint ] :=
With[{circleRadius = OptionValue["Diameter"] + 0.02 , thickness = OptionValue["Thickness"]},
	{
	EdgeForm[None],
	Cylinder[{x -{0,0,thickness/2},x+{0,0,thickness/2}}, circleRadius/2],
	Cylinder[{x - {0,0,2 thickness},x - {0,0,thickness/2} }, circleRadius/4]
	}
]

PinnedJointShape[OptionsPattern[]][pt : coordinate3D, {3,___},  _FreeJoint|_PinnedJoint ]:= Sphere[pt,OptionValue["Diameter"]/2]
PinnedJointShape[OptionsPattern[]][pt : coordinate3D, _ , _Point ]:= Sphere[pt,OptionValue["Diameter"]/2]


Options[JointShape] =
{
	"Diameter" -> 0.01,
	"Angle" -> 0,
	"Thickness" -> 0.026,
	"HeightOffset" -> 0
};

JointShape[ opt : OptionsPattern[] ][ x : coordinate2D, a : {_, Infinity, ___ } , b : _FreeJoint|_PinnedJoint ] := PinnedJointShape[ FilterRules[{opt},Options[PinnedJointShape]] ][x,a,b]
JointShape[ opt : OptionsPattern[] ][ x : coordinate3D, a : {_, Infinity, ___ } , b : _FreeJoint|_PinnedJoint ] := PinnedJointShape[ FilterRules[{opt},Options[PinnedJointShape]] ][x,a,b]

JointShape[ opt : OptionsPattern[] ][ x : coordinate2D, a : {_, ___ } , b : _FreeJoint|_PinnedJoint ] := FreeJointShape[ FilterRules[{opt},Options[FreeJointShape]] ][x,a,b]
JointShape[ opt : OptionsPattern[] ][ x : coordinate3D, a : {_, ___ } , b : _FreeJoint|_PinnedJoint ] := FreeJointShape[ FilterRules[{opt},Options[FreeJointShape]] ][x,a,b]

JointShape[ opt : OptionsPattern[] ][ x : coordinate2D, a_, b_Point ] := FreeJointShape[ FilterRules[{opt},Options[FreeJointShape]] ][x,a,b]
JointShape[ opt : OptionsPattern[] ][ x : coordinate3D, a_, b_Point ] := FreeJointShape[ FilterRules[{opt},Options[FreeJointShape]] ][x,a,b]


Options[SpringShape] =
{
	"Width" -> 0.005,
	"number" -> 10,
	"resolution" -> 26
};


SpringShape[ OptionsPattern[] ][ { x_?coordinate2DQ, y_?coordinate2DQ }, _, _ ]:=
With[{ 
length = Norm[y-x], 
width = OptionValue["Width"], 
period = 2 OptionValue["number"], 
nPoints = Round[OptionValue["resolution"]*OptionValue["number"]],
rotMat = RotationMatrix[ N[{y-x,{1,0}}] ]
},
With[{ delta = N[length/nPoints]},
	BezierCurve[
		N@Join[
			{x},
			ConstantArray[x,nPoints-3]+({
				# + width Cos[ period Pi #/length ],
				width Sin[ period Pi #/length ]
				}& /@ Range[2 delta, length - 2 delta, delta]) . rotMat,
			{y}
		]
	]
]]


SpringShape[ OptionsPattern[] ][ {x : coordinate3D, y : coordinate3D}, _, _ ] :=
With[ {
rotMat = RotationMatrix[N[{{0,0,1},x-y}]],
length=Norm[ y-x ], 
numPoints = Round[OptionValue["resolution"]*OptionValue["number"]], 
number = OptionValue["number"],
thickness = OptionValue["Width"]/2
}, With[{ delta = N[length/numPoints] },
	BezierCurve[
		N@Join[
		{x},
		ConstantArray[x,numPoints-3] + (
			{ thickness Cos[number 2 Pi #/length],thickness Sin[number 2 Pi #/length], # }& /@ Range[2 delta,length-2 delta,delta]
		) . rotMat,
		{y}
		]
	]
]]


Options[FaceShape] = {
	"Thickness" -> 0
};


FaceShape[ OptionsPattern[] ][ x : {coordinate2D..}, _, _ ] := Polygon[ x ]

FaceShape[ OptionsPattern[] ][ x : {coordinate3D..}, {2|3, ___ } , _Face|_ElasticTriangle ] := 
	If[ OptionValue["Thickness"] == 0,
		Polygon[ x ],
		ExtrudedPolygon[{-OptionValue["Thickness"], OptionValue["Thickness"]}][x]
	]

FaceShape[ OptionsPattern[] ][ x : {coordinate3D..}, _, _Polygon ] := Polygon[ x ]


AngleText[m_?MechanismQ, {v1_Integer,v2_Integer,v3_Integer}, label_ : "",distance : _?NumericQ : 0]:=With[
{
	angleLocation=m["positions"][[v2,1;;MechanismDisplayDimension[m]]],
	vectors=-DisplacementVector[m["positions"],{{v3,v2},{v1,v2}}][[All,1;;MechanismDisplayDimension[m]]]
},
	Text[label,angleLocation + (distance+0.12) Mean[vectors]]
]


AngleMarker[m_?MechanismQ, {v1_Integer,v2_Integer,v3_Integer}, radius : _?NumericQ : 1/10]:=With[
{
	(*project the vectors making this angle to the xy-plane*)
	angleLocation=m["positions"][[v2,1;;2]],
	vectors=DisplacementVector[m["positions"],{{v2,v1},{v2,v3}}][[All,1;;2]]
},
	Circle[angleLocation,
		Abs[radius] Sqrt[Min[vectors[[1]] . vectors[[1]],vectors[[2]] . vectors[[2]]]],
		(If[Pi+#[[2]]<Pi+#[[1]],{0,2Pi}+#,#]&)[ArcTan@@@vectors]
	]
] /; MechanismDisplayDimension[m]==2 && Max[{v1,v2,v3}]<=MeshCellCount[m,0] && Min[{v1,v2,v3}]>0


(*Code borrowed from https://mathematica.stackexchange.com/questions/10957/an-efficient-circular-arc-primitive-for-graphics3d*)
ClearAll[splineCircle2];
splineCircle[m_List, r_, angles_List: {0., 2. \[Pi]}] := 
 Module[{seg, \[Phi], start, end, pts, w, k, pihalf},
   pihalf = 0.5 \[Pi];
   {start, end} = Mod[N[angles], 2. \[Pi]];
   If[end <= start, end += 2. \[Pi]];
   seg = Quotient[N[end - start], pihalf];
   \[Phi] = Mod[N[end - start], pihalf];
   If[seg == 4, seg = 3; \[Phi] = pihalf];
   With[{
     cseg = Cos[pihalf seg], sseg = Sin[pihalf seg],
     c\[Phi] = Cos[\[Phi]], s\[Phi] = Sin[\[Phi]], 
     t\[Phi] = Tan[0.5 \[Phi]],
     rcs = r Cos[start], rss = r Sin[start]
     },
    pts = Join[
       Take[{{1., 0.}, {1., 1.}, {0., 1.}, {-1., 1.}, {-1., 0.}, {-1., -1.}, {0., -1.}}, 2 seg + 1],
       {{cseg - sseg t\[Phi], sseg + cseg t\[Phi]}, {cseg c\[Phi] - sseg s\[Phi], c\[Phi] sseg + cseg s\[Phi]}}
       ] . {{rcs, rss}, {-rss, rcs}}
    ];
   pts = ConstantArray[m, Length[pts]] + 
     If[Length[m] == 2, 
      pts, 
      Join[pts, ConstantArray[{0.}, Length[pts]], 2]
     ];
   w = With[{c = 1./Sqrt[2.]}, 
     Join[Take[{1., c, 1., c, 1., c, 1.}, 2 seg + 1], {Cos[0.5 \[Phi]], 1.}]
     ];
   k = Join[{0, 0, 0}, Riffle[#, #] &@Range[seg + 1], {seg + 1}];
   BSplineCurve[pts, SplineDegree -> 2, SplineKnots -> k, SplineWeights -> w]
   ] /; Length[m] == 2 || Length[m] == 3
 
Options[circleFromPoints] = {arc -> False};

circleFromPoints[m : {q1_, q2_, q3_}, OptionsPattern[]] :=
Module[{c, r, \[Phi]1, \[Phi]2, p1, p2, p3, h, 
        rot = Quiet[RotationMatrix[{{0, 0, 1}, Cross[#1 - #2, #3 - #2]}],RotationMatrix::spln] &},
  {p1, p2, p3} = {q1, q2, q3} . rot[q1, q2, q3];
  h = p1[[3]];
  {p1, p2, p3} = {p1, p2, p3}[[All, ;; 2]];
  {c, r} = List @@ Circumsphere[{p1, p2, p3}];
  \[Phi]1 = ArcTan @@ (p3 - c);
  \[Phi]2 = ArcTan @@ (p1 - c);
  c = Append[c, h];
  If[OptionValue[arc] // TrueQ,
    MapAt[Function[{p}, rot[q1, q2, q3] . p] /@ # &, splineCircle[c, r, {\[Phi]1, \[Phi]2}], {1}],
    MapAt[Function[{p}, rot[q1, q2, q3] . p] /@ # &, splineCircle[c, r], {1}]
  ]
] /; MatrixQ[m, NumericQ] && Dimensions[m] == {3, 3}


AngleMarker[m_?MechanismQ, {v1_Integer,v2_Integer,v3_Integer}, radius : _?NumericQ : 1/10]:=With[
{
	angleLocation=m["positions"][[v2]],
	(*project the vectors making this angle to the xy-plane*)
	vectors=DisplacementVector[m["positions"],{{v2,v1},{v2,v3}}]
},
	circleFromPoints[{angleLocation+radius vectors[[1]],angleLocation+radius (vectors[[1]]+vectors[[2]])/Sqrt[2],angleLocation+radius vectors[[2]]},arc ->True]
] /; MechanismDisplayDimension[m]==3 && Max[{v1,v2,v3}]<=MeshCellCount[m,0] && Min[{v1,v2,v3}]>0


AngleMarker::bounds="Vertices are out of bounds.";
AngleMarker[m_?MechanismQ, {v1_Integer,v2_Integer,v3_Integer}]:="nothing"/;Message[AngleMarker::bounds]
AngleMarker[m_?MechanismQ, {v1_Integer,v2_Integer,v3_Integer},_]:="nothing"/;Message[AngleMarker::bounds]
AngleMarker[m_?MechanismQ, {v1_Integer,v2_Integer,v3_Integer},_,_?NumericQ]:="nothing"/;Message[AngleMarker::bounds]


Options[PlotMechanism] = Join[
	Options[Graphics3D],
	{
		PlotTheme -> Automatic,
		Displacements -> None,
		Stresses -> None,

		StressStyle -> {
			{RGBColor[{#2,0,0}] , AbsoluteThickness[1+3*#2] , Line[#]}&,
			{RGBColor[{0,0,-#2}] , AbsoluteThickness[1+3*(-#2)] ,Line[#]}&
		},

		DisplacementStyle -> {Black},
		DisplacementScale -> 0.25,

		DisplayDimension -> Automatic,
		Prolog->{}
	}
];


PlotMechanism[ m_?MechanismQ, opt : OptionsPattern[] ] :=
With[ {coord = positionDimension[ m , Automatic , OptionValue[DisplayDimension] ] },
	PlotMechanism[ m, coord, opt] /; coordQ[ coord ]
]
coordQ[ $Failed ] := (Message[ PlotMechanism::pos ]; False)
coordQ[ _ ] := True

PlotMechanism[ m_?MechanismQ, positions : _?NumericCoordinatesQ|{__?NumericCoordinatesQ}, opt : OptionsPattern[] ] := 
Module[ { 
(*organize all the coordinates into proper form*)
coordinates = If[ ArrayDepth[positions] == 2,
	positionDimension[ m , {positions} , OptionValue[DisplayDimension] ],
	positionDimension[ m , positions , OptionValue[DisplayDimension] ]
] , 
graphics, dimension, directive, options, stresses, displacements
}, 
	graphics = createMechanismGraphics[m, coordinates, Length[ coordinates[[1,1]] ], {opt} ];
	directive = graphicsDirective[ m, graphics, Length[coordinates[[1,1]]] ];
	options = graphicsOptions[ {opt}, directive, boundingRegions[m, coordinates] ];

	stresses = createStressGraphics[ MechanismEdges[m], coordinates, {OptionValue[Stresses]}, OptionValue[StressStyle] ];
	displacements = createDisplacementGraphics[ coordinates, {OptionValue[Displacements]}, OptionValue[DisplacementScale], OptionValue[DisplacementStyle] ];
	
	If[ ArrayDepth[positions]==2, First, Identity] @ MapThread[ directive[ {OptionValue[Prolog],#1, #2, #3}, options ]&, {graphics, stresses, displacements} ]
]


createMechanismGraphics[ m_, $Failed, dimension_, inputOptions_ ] := (Message[ PlotMechanism::pos ]; {})
createMechanismGraphics[ m_, positions_, dimension_, inputOptions_ ] :=
	MechanismPrimitives[ m, #, dimension, FilterRules[inputOptions, Options[MechanismPrimitives]] ] & /@ positions


PlotMechanism::pos = "Requested display coordinates are not valid.";
PlotMechanism::numstress="Stress should be a numeric list of stresses.";
PlotMechanism::stressmatch="Stresses should agree with the number of edges.";
PlotMechanism::stressstyle = "Stress style should be of the form { positive stress style, negative stress style }.";
PlotMechanism::numinst = "There must be as many stresses as positions provided.";

PlotMechanism::numdisp="Displacements must be a vector of numerical displacements.";
PlotMechanism::numdispl="Each vertex needs corresponding displacement in the correct dimension.";
PlotMechanism::displscale="The displacement scale must be a positive real number.";
PlotMechanism::numinstdisp = "There must be as many displacements as positions provided.";


(*clip the bounding box to a minimum size*)
$minsize=0.25;
minSize[{x_,y_}] := { x-$minsize, x+$minsize } /; Abs[y-x] < $minsize
minSize[{x_,y_}] := {x-$minsize, y+$minsize}

(*figure out plot range and ratios*)
plotSizes[positions_] :=
Module[{
	boundingRegion, boundingBoxRatios
},
	boundingRegion = 
		minSize /@ (Transpose @  CoordinateBoundingBox[positions]);
	boundingBoxRatios = (#[[2]] - #[[1]] &) /@ boundingRegion;

	{boundingRegion, boundingBoxRatios}
]


boundingRegions[ m_, $Failed] := $Failed
boundingRegions[ m_, graphics : {__GraphicsComplex}] := plotSizes[ Flatten[ graphics[[All,1]], 1 ] ]
boundingRegions[ m_, pos_ ] := plotSizes[pos]

graphicsDirective[ m_, $Failed, dim_ ] := Identity
graphicsDirective[ m_, graphics : {__GraphicsComplex}, expectedDim_ ] := With[ {dims = Length /@ graphics[[All,1,1]]},
	Which[
		Not[ Equal @@ dims ], Identity,
		dims[[1]]==2, Graphics,
		dims[[1]]==3, Graphics3D,
		True, Identity
	]
]
graphicsDirective[m_, graphics_, expectedDim_ ] := 
	Switch[ expectedDim,
		2, Graphics, 
		3, Graphics3D,
		_, Identity
	]

graphicsOptions[ options_, directive_, $Failed ] := {}
graphicsOptions[ options_, directive_, {boundingRegion_, boundingRatios_} ] :=
	Join[
		FilterRules[{options},Options[Graphics3D]], 
		{Boxed -> False, Lighting->"Neutral",PlotRange -> boundingRegion, BoxRatios -> boundingRatios}
	]


createStressGraphics[ edges_, coordinates_, None|{None}, _ ]:=ConstantArray[ {}, Length[coordinates] ]


createStressGraphics[ edges_ , coordinates_ , stress : _?(MatrixQ[#,NumericQ]&), style : {_,_}] :=
With[{
	stressScale = Max[ Max[stress,0], Abs @ Min[stress,0] ]
},
	MapThread[ plotStresses[ stressScale, edges, #1, #2, style ]&, { coordinates, stress } ]
] /; Dimensions[stress] == {Length[ coordinates ], Length[edges] }

createStressGraphics[ edges_, coordinates_, stress_, style_ ] := (
	If[ Not @ MatrixQ[ stress, NumericQ ], Message[PlotMechanism::numstress]];
	If[ Length[stress] != Length[ coordinates ], Message[PlotMechanism::numinst]];
	If[ Length[stress[[1]]] != Length[edges], Message[PlotMechanism::stressmatch] ];
	If[ Length[style] != 2, Message[PlotMechanism::stressstyle] ];

	ConstantArray[{}, Length[coordinates]]
)


plotStresses[ stressScale_, edges_, coordinates_, stress_, stressFunc_Function ] :=
	plotStresses[ stressScale, edges, coordinates, stress, {stressFunc, stressFunc} ]

plotStresses[ stressScale_, edges_, coordinates_, stress_, {posStress_Function, negStress_Function} ] :=
With[ {
	coordinatePairs = coordinates[[ # ]]& /@ edges,
	normalizedStresses = stress/stressScale
},
	MapThread[
		Which[#2<0, negStress[ #1, #2 ], #2>0, posStress[ #1, #2 ], True, {} ]&,
		{
		coordinatePairs,
		normalizedStresses
		}
	]
]


createDisplacementGraphics[ coordinates_, None|{None}, _ , _]:= ConstantArray[{},Length[coordinates]]


createDisplacementGraphics[ coordinates_, displacements : {__?(MatrixQ[#,NumericQ]&)}, displacementScale_?(NumericQ[#]&&#>0&), style_ ]:=
With[{ maxDisplacement = Max[ Map[Norm , displacements, {2} ] ] },
	MapThread[ plotDisplacements[ maxDisplacement, #1, #2, displacementScale, style ]&, { coordinates, displacements } ]
] /; Dimensions[ coordinates ] == Dimensions[ displacements ]

createDisplacementGraphics[ coordinates_, displacements_, displacementScale_, style_ ] :=(
	If[ Not @ (And@@( MatrixQ[#,NumericQ]& /@ displacements) ) , Message[PlotMechanism::numdisp] ];
	If[ Not @ NumericQ[displacementScale] || # <= 0&, Message[PlotMechanism::displscale] ];
	If[ Dimensions[ coordinates ] != Dimensions[ displacements ], Message[PlotMechanism::numinstdisp] ];

	ConstantArray[ {}, Length[coordinates] ]
)


plotDisplacements[ maxDisplacement_, coordinates_, displacement_, displacementScale_, style_] :=
With[{padding = 0.9},
	Flatten @ MapThread[
		displacementGraphic[ style ],
		{
			coordinates,
			displacementScale * displacement/maxDisplacement
		}
	]
]

displacementGraphic[ style_ ][ vertexLocation_, displacement_ ] := 
	{
	style,
	Arrow[ {vertexLocation, vertexLocation + displacement} ]
	}


scalePolygon[coords_ , shrinkBy_ ]:=
With[
{
	center = ConstantArray[Mean[coords],Length[coords]]
},
	center + Clip[1-shrinkBy,{0,1}] (coords-center)
]


ExtrudedPolygon[ { thickness1_?NumericQ, thickness2_?NumericQ } ][coords_] :=
With[{ extrusionVector = First @ NormalVector[coords,{{1,2,3}}]},
	With[{
		Face1 = coords + thickness1 ConstantArray[ extrusionVector, Length[coords] ],
		Face2 = coords + thickness2 ConstantArray[extrusionVector , Length[coords] ]
		},
		Polyhedron @ Join[
			{
			Face1
			},
			MapThread[ Join[#1,Reverse[#2]]& , {Partition[Face1,2,1,1],Partition[Face2,2,1,1]} ],
			{
			Reverse @ Face2
			}
		]
	] 
] /; MatrixQ[coords] && Dimensions[coords][[2]] == 3

ExtrudedPolygon[ extrusionVector : {_?NumericQ, _?NumericQ, _?NumericQ} , { thickness1_?NumericQ, thickness2_?NumericQ } ][coords_] :=
	With[{
		Face1 = coords + thickness1 ConstantArray[ extrusionVector, Length[coords] ],
		Face2 = coords + thickness2 ConstantArray[extrusionVector , Length[coords] ]
		},
		Polyhedron @ Join[
			{
			Face1
			},
			MapThread[ Join[#1,Reverse[#2]]& , {Partition[Face1,2,1,1],Partition[Face2,2,1,1]} ],
			{
			Reverse @ Face2
			}
		]
	] /; MatrixQ[coords] && Dimensions[coords][[2]] == 3

ExtrudedPolygon[{thickness1_?NumericQ, thickness2_?NumericQ}][coords_] :=
	With[{ coords2 = PadRight[ coords, {Length[coords],3} ] },With[{
		Face1 = coords2 + thickness1 ConstantArray[{0,0,1} , Length[coords] ],
		Face2 = coords2 + thickness2 ConstantArray[{0,0,1} , Length[coords] ]
		},
		Polyhedron @ Join[
			{
			Face1
			},
			MapThread[ Join[#1,Reverse[#2]]& , {Partition[Face1,2,1,1],Partition[Face2,2,1,1]} ],
			{
			Reverse @ Face2
			}
		]
	]] /; MatrixQ[coords] && Dimensions[coords][[2]] == 2


shrinkPolygon[ pts_, widths_ ] := pts + Total @ MapIndexed[ shrinkPolygonSide[ pts, #2[[1]], #1 ]&, widths ]

shrinkPolygonSide[ pts_, v1_, width_ ] :=
Module[{n=Length[pts], v2, future, past, futureEdge, edge, pastEdge},
	v2=Mod[v1+1, n, 1]; future=Mod[v2+1, n, 1]; past = Mod[v1-1,n,1];
	edge=Normalize[ pts[[v2]] - pts[[v1]] ];
	futureEdge = Normalize[ pts[[future]] - pts[[v2]] ];
	pastEdge = Normalize[ pts[[past]] - pts[[v1]] ];
	
	RotateRight[Join[
		width*{
			pastEdge/Norm[ Cross[ pastEdge, edge ] ],
			futureEdge/Norm[ Cross[ futureEdge, edge ] ]
		},
		ConstantArray[{0,0,0},n-2]
	], v1-1]
]


End[];

EndPackage[];
