// Handle conversion from older format
void VisibleObject::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
{ 
    SimTK::Array_<SimTK::String> oldGeometryFiles;
	if ( versionNumber < XMLDocument::getLatestVersion()){
		if (versionNumber<20101){
			SimTK::Xml::element_iterator visPropIter = aNode.element_begin("VisibleProperties");
			// Get geometry files and Preferences if any and set them into 
			if (visPropIter!=aNode.element_end()){
				// Move display_prference, and show_axes nodes up to VisibleObject
				SimTK::Xml::element_iterator  prefIter = visPropIter->element_begin("display_preference");
				if (prefIter!= visPropIter->element_end()){
					SimTK::Xml::Node moveNode = visPropIter->removeNode(prefIter);
					aNode.insertNodeAfter(aNode.element_end(), moveNode);
				}
				SimTK::Xml::element_iterator  showAxesIter = visPropIter->element_begin("show_axes");
				if (showAxesIter!=aNode.element_end()){
					SimTK::Xml::Node moveNode = visPropIter->removeNode(showAxesIter);
					aNode.insertNodeAfter(aNode.element_end(), moveNode);
				}
			}
			SimTK::Xml::element_iterator geometryIter = aNode.element_begin("geometry_files");
			string propValue="";
			bool hasPieces=false;
			if (geometryIter!= aNode.element_end()){
				geometryIter->getValueAs(oldGeometryFiles);
			}
        }
    }
	Object::updateFromXMLNode(aNode, versionNumber);
    if (oldGeometryFiles.size()>0){
        for(unsigned i=0; i< oldGeometryFiles.size(); i++) 
            setGeometryFileName(i, oldGeometryFiles[i]);
    }
}

void testVisModel(string fileName)
{

    Model* model = new Model(fileName, true);
    SimTK::State& si = model->initSystem();
    ModelDisplayHints mdh; 
    SimTK::Array_<SimTK::DecorativeGeometry> geometryToDisplay;
    model->generateDecorations(true, mdh, si, geometryToDisplay);
    cout << geometryToDisplay.size() << endl;
    model->generateDecorations(false, mdh, si, geometryToDisplay);
    cout << geometryToDisplay.size() << endl;
    DecorativeGeometryImplementationText dgiText;
    for (unsigned i = 0; i < geometryToDisplay.size(); i++)
        geometryToDisplay[i].implementGeometry(dgiText);

    std::string baseName = fileName.substr(0, fileName.find_last_of('.'));
    std::ifstream t("vis_" + baseName + ".txt");
    if (!t.good()) throw OpenSim::Exception("Could not open file.");
    std::stringstream buffer;
    buffer << t.rdbuf();
    std::string fromFile = buffer.str();
    std::string fromModel = dgiText.getAsString();
    cout << "From Model " << endl << "=====" << endl;
    cout << fromModel << endl;
    cout << "From File " << endl << "=====" << endl;
    cout << fromFile << endl;
    int same = fromFile.compare(fromModel);
    delete model;
    ASSERT(same == 0, __FILE__, __LINE__, "Files do not match.");
}

int main()
{
    Array<string> muscleModelNames;
    muscleModelNames.append("Thelen2003Muscle_Deprecated"); 
    muscleModelNames.append("Thelen2003Muscle");    
    muscleModelNames.append("Millard2012EquilibriumMuscle");
    muscleModelNames.append("Millard2012AccelerationMuscle");

    // Tolerances for the differences between the current models
    // and the 'standard' solution, which was closest to using 
    // Thelen2003Muscle_Deprecated musle formulation.
    double actTols[4] = {0.005, 0.025, 0.04, 0.04};
    double forceTols[4] = {0.5, 4, 5, 6};

    SimTK::Array_<std::string> failures;
    
    for(int i=0; i< muscleModelNames.getSize(); ++i){
        try { // regression test for the Thelen deprecate muscle
              // otherwise verify that SO runs with the new models
            testArm26(muscleModelNames[i], actTols[i], forceTols[i]);
        }
        catch (const std::exception& e) {
            cout << e.what() <<endl; 
            failures.push_back("testArm26_"+muscleModelNames[i]);
        }
    }
  
    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;
    return 0;
}

int main() {

    SimTK::Array_<std::string> failures;

    try {testSingleRigidTendonMuscle();}
    catch (const std::exception& e)
        {  cout << e.what() <<endl; failures.push_back("testSingleRigidTendonMuscle"); }

    // redo with the Millard2012EquilibriumMuscle 
    Object::renameType("Thelen2003Muscle", "Millard2012EquilibriumMuscle");

    try {testSingleMillardRigidTendonMuscle();}
    catch (const std::exception& e)
        {   cout << e.what() <<endl;
            failures.push_back("testSingleMillardRigidTendonMuscle"); }
    
    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;

    return 0;
}

void checkMarkersReferenceConsistencyFromTool(InverseKinematicsTool& ik)
{
    MarkersReference markersReference;
    SimTK::Array_<CoordinateReference> coordinateReferences;

    ik.populateReferences(markersReference, coordinateReferences);
    const IKTaskSet& tasks = ik.getIKTaskSet();

    // Need a model to get a state, doesn't matter which model.
    Model model;
    SimTK::State& state = model.initSystem();

    SimTK::Array_<string> names = markersReference.getNames();
    SimTK::Array_<double> weights;
    markersReference.getWeights(state, weights);

    for (unsigned int i=0; i < names.size(); ++i) {
        std::cout << names[i] << ": " << weights[i];
        int ix = tasks.getIndex(names[i]);
        if (ix > -1) {
            cout << " in TaskSet: " << tasks[ix].getWeight() << endl;
            SimTK_ASSERT_ALWAYS(weights[i] == tasks[ix].getWeight(),
                "Mismatched weight to marker task");
        }
        else {
            cout << " default: " << markersReference.get_default_weight() << endl;
            SimTK_ASSERT_ALWAYS(
                weights[i] == markersReference.get_default_weight(),
                "Mismatched weight to default weight");
        }
    }
}

int main()
{
    SimTK::Array_<std::string> failures;
    
    try { 
    Millard2012EquilibriumMuscle muscle("muscle",
                    MaxIsometricForce0,
                    OptimalFiberLength0,
                    TendonSlackLength0,
                    PennationAngle0);

    MuscleFirstOrderActivationDynamicModel actMdl = muscle.getFirstOrderActivationModel();
    actMdl.setActivationTimeConstant(Activation0);
    actMdl.setDeactivationTimeConstant(Deactivation0);
    muscle.setFirstOrderActivationModel(actMdl);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(0.1, SimTK::Pi, 0);

    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion, 
        &control, 
        IntegrationAccuracy,
        CorrectnessTest,
        CorrectnessTestTolerance,
        true);
        
        
        cout << "Probes test passed" << endl; 
    }

    catch (const Exception& e) { 
        e.print(cerr);
        failures.push_back("testProbes");
    }


    printf("\n\n");
    cout <<"************************************************************"<<endl;
    cout <<"************************************************************"<<endl;

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    
    cout << "testProbes Done" << endl;
    return 0;
}

/** Update all markers weights by order in the markersReference passed in to
    construct the solver. */
void InverseKinematicsSolver::updateMarkerWeights(const SimTK::Array_<double> &weights)
{
    if(_markersReference.updMarkerWeightSet().getSize() == weights.size()){
        for(unsigned int i=0; i<weights.size(); i++){
            _markersReference.updMarkerWeightSet()[i].setWeight(weights[i]);
            _markerAssemblyCondition->changeMarkerWeight(SimTK::Markers::MarkerIx(i), weights[i]);
        }
    }
    else
        throw Exception("InverseKinematicsSolver::updateMarkerWeights: invalid size of weights.");
}

void Geometry::generateDecorations(bool fixed, const ModelDisplayHints& hints, const SimTK::State& state,
    SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const
{
    if (!fixed) return; // serialized Geometry is assumed fixed
    SimTK::Array_<SimTK::DecorativeGeometry> decos;
    implementCreateDecorativeGeometry(decos);
    if (decos.size() == 0) return;
    setDecorativeGeometryTransform(decos, state);
    for (unsigned i = 0; i < decos.size(); i++){
        setDecorativeGeometryAppearance(decos[i]);
        appendToThis.push_back(decos[i]);
    }
}

int main()
{
    SimTK::Array_<std::string> failures;

    try { testBody(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testBody");
    }
        
    try { testPhysicalOffsetFrameOnBody(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; 
        failures.push_back("testPhysicalOffsetFrameOnBody");
    }

    try { testPhysicalOffsetFrameOnBodySerialize(); }
    catch (const std::exception& e){
        cout << e.what() << endl; 
        failures.push_back("testPhysicalOffsetFrameOnBodySerialize");
    }
    
    try { testPhysicalOffsetFrameOnPhysicalOffsetFrame(); }
    catch (const std::exception& e){
        cout << e.what() << endl;
        failures.push_back("testPhysicalOffsetFrameOnPhysicalOffsetFrame");
    }

    try { testPhysicalOffsetFrameOnPhysicalOffsetFrameOrder(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testPhysicalOffsetFrameOnPhysicalOffsetFrameOrder");
    }

    try { testFilterByFrameType(); }
    catch (const std::exception& e){
        cout << e.what() << endl;
        failures.push_back("testFilterByFrameType");
    }

    try { testVelocityAndAccelerationMethods(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testVelocityAndAccelerationMethods");
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done. All cases passed." << endl;

    return 0;
}

int SegmentedQuinticBezierToolkit::calcIndex(double x, 
                                             const SimTK::Array_<SimTK::Vector>& bezierPtsX)
{
    int idx = 0;
    bool flag_found = false;

    int n = bezierPtsX.size(); 
    for(int i=0; i<n; i++){
        if( x >= bezierPtsX[i](0) && x < bezierPtsX[i](5) ){
            idx = i;
            flag_found = true;
            break; 
        }
    }

    //Check if the value x is identically the last point
    if(!flag_found && x == bezierPtsX[n-1](5)){
        idx = n-1;
        flag_found = true;
    }

    SimTK_ERRCHK_ALWAYS( (flag_found == true), 
        "SegmentedQuinticBezierToolkit::calcIndex", 
        "Error: A value of x was used that is not within the Bezier curve set.");

    return idx;
}

void Brick::implementCreateDecorativeGeometry(SimTK::Array_<SimTK::DecorativeGeometry>& decoGeoms) const
{
    const Vec3 netScale = get_scale_factors();
    DecorativeBrick deco(get_half_lengths());
    deco.setScaleFactors(netScale);
    decoGeoms.push_back(deco);
}

// Implement generateDecorations by WrapCylinder to replace the previous out of place implementation
// in ModelVisualizer
void WrapCylinder::generateDecorations(bool fixed, const ModelDisplayHints& hints, const SimTK::State& state,
                                       SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const
{

    Super::generateDecorations(fixed, hints, state, appendToThis);
    if (fixed) return;

    if (hints.get_show_wrap_geometry()) {
        const Appearance& defaultAppearance = get_Appearance();
        if (!defaultAppearance.get_visible()) return;
        const Vec3 color = defaultAppearance.get_color();

        SimTK::Transform ztoy;
        // Make transform that takes z axis to y axis due to different
        // assumptions between DecorativeCylinder aligned with y  and
        // WrapCylinder aligned with z
        ztoy.updR().setRotationFromAngleAboutX(SimTK_PI / 2);
        const SimTK::Transform& X_GB = getFrame().getTransformInGround(state);
        SimTK::Transform X_GW = X_GB*getTransform()*ztoy;
        appendToThis.push_back(
            SimTK::DecorativeCylinder(get_radius(),
                                      get_length() / 2)
            .setTransform(X_GW).setResolution(2.0)
            .setColor(color).setOpacity(defaultAppearance.get_opacity())
            .setScale(1).setRepresentation(defaultAppearance.get_representation()));
    }
}

int main()
{
    try {
        Storage result("Arm26_optimized_states.sto"),
                standard("std_Arm26_optimized_states.sto");
        CHECK_STORAGE_AGAINST_STANDARD(result, standard, Array<double>(0.01, 16),
            __FILE__, __LINE__, "Arm26 states failed comparison test");
        cout << "Arm26 states comparison test passed\n";

        // Ensure the optimization result acheived a velocity of at least
        // REF_MAX_VEL, and that the control values are within either 20% of the
        // reference values or 0.05 in absolute value.
        ifstream resFile;
        resFile.open("Arm26_optimization_result");
        ASSERT(resFile.is_open(), __FILE__, __LINE__,
               "Can't open optimization result file" );

        SimTK::Array_<double> resVec;
        for ( ; ; ) {
            double tmp;
            resFile >> tmp;
            if (!resFile.good())
                break;
            resVec.push_back(tmp);
        }

        ASSERT(resVec.size() == ARM26_DESIGN_SPACE_DIM+1, __FILE__, __LINE__,
               "Optimization result size mismatch" );
        
        // Ensure the optimizer found a local minimum we expect.
        for (int i = 0; i < ARM26_DESIGN_SPACE_DIM-1; ++i) {
            ASSERT(fabs(resVec[i] - refControls[i])/refControls[i] < 0.2 ||
                   fabs(resVec[i] - refControls[i]) < 0.05, __FILE__, __LINE__,
                   "Control value does not match reference" );
        }
        ASSERT(resVec[ARM26_DESIGN_SPACE_DIM] > REF_MAX_VEL, __FILE__, __LINE__,
               "Optimized velocity smaller than reference" );

        cout << "Arm26 optimization results passed\n";
    }
    catch (const Exception& e) {
        e.print(cerr);
        return 1;
    }
    cout << "Done" << endl;
    return 0;
}

int main()
{
	Array<string> muscleModelNames;
    muscleModelNames.append("Thelen2003Muscle_Deprecated");	
    muscleModelNames.append("Thelen2003Muscle");	
	muscleModelNames.append("Millard2012EquilibriumMuscle");
	muscleModelNames.append("Millard2012AccelerationMuscle");


    cout << "=========================================================" << endl;
    cout << "                       WARNING                           " << endl;
    cout << "Athough this file says it testsStaticOptimization, it is" << endl;
    cout << "not a valid test. It merely checks to see if the current" << endl;
    cout << "static results agree with past static results.          " << endl;
    cout << endl;
    cout << "                This is not a test                      " << endl;
    cout << endl;
    cout << "A valid test might be done by instead checking that " <<endl;
    cout << "1. IPOPT can correctly solve a quadradic problem " <<endl;
    cout << "2. That the muscle forces provided to static are in fact   "<<endl;
    cout << "   linear with activation, as is assumed in a static " << endl;
    cout << "   optimization.                       M.Millard 2012" << endl;
    cout << "=========================================================" << endl;
	
	SimTK::Array_<std::string> failures;
	
	for(int i=0; i< muscleModelNames.getSize(); ++i){
		try { // regression test for the Thelen deprecate muscle
			  // otherwise verify that SO runs with the new models
            testArm26(muscleModelNames[i], i<1);
		}
		catch (const std::exception& e) {
			cout << e.what() <<endl; 
			failures.push_back("testArm26_"+muscleModelNames[i]);
		}
	}
  
    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;
    return 0;
}

int main()
{
    SimTK::Array_<std::string> failures;

    try { testTorqueActuator(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testTorqueActuator");
    }
    try { testClutchedPathSpring(); }
    catch (const std::exception& e){
        cout << e.what() <<endl; failures.push_back("testClutchedPathSpring");
    }
    try { testMcKibbenActuator(); }
    catch (const std::exception& e) {
        cout << e.what() << endl; failures.push_back("testMcKibbenActuator");
    }
    try { testBodyActuator(); }
    catch (const std::exception& e) {
        cout << e.what() << endl; failures.push_back("testBodyActuator");
    }
    try { testActuatorsCombination(); }
    catch (const std::exception& e) {
        cout << e.what() << endl; failures.push_back("testActuatorsCombination");
    }
    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done, testActuators passed." << endl;
}

int main()
{
    SimTK::Array_<std::string> failures;

    try { testMarkersReference(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testMarkersReference");
    }
    try { testOrientationsReference(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testOrientationsReference");
    }
    try { testAccuracy(); }
    catch (const std::exception& e) {
        cout << e.what() << endl; failures.push_back("testAccuracy");
    }
    try { testUpdateMarkerWeights(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testUpdateMarkerWeights");
    }
    try { testTrackWithUpdateMarkerWeights(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTrackWithUpdateMarkerWeights");
    }

    try { testNumberOfMarkersMismatch(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testNumberOfMarkersMismatch");
    }

    try { testNumberOfOrientationsMismatch(); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testNumberOfOrientationsMismatch");
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done. All cases passed." << endl;

    return 0;
}

	double calcDerivative(const SimTK::Array_<int>& derivComponents, const SimTK::Vector& x) const {
		if (derivComponents.size() == 1){
			if (derivComponents[0]==0){
				SimTK::Vector x1(1);
				x1[0] = x[0];
				return scale*f1->calcDerivative(derivComponents, x1);
			}
			else if (derivComponents[0]==1)
				return -1;
		}
		else if(derivComponents.size() == 2){
			if (derivComponents[0]==0 && derivComponents[1] == 0){
				SimTK::Vector x1(1);
				x1[0] = x[0];
				return scale*f1->calcDerivative(derivComponents, x1);
			}
		}
        return 0;
    }

int main() {
    Object::renameType("Thelen2003Muscle", "Thelen2003Muscle_Deprecated");
    //Object::renameType("Thelen2003Muscle", "Millard2012AccelerationMuscle");
    //Object::renameType("Thelen2003Muscle", "Millard2012EquilibriumMuscle");
    SimTK::Array_<std::string> failures;

    try {testGait2354();}
    catch (const std::exception& e)
        {  cout << e.what() <<endl; failures.push_back("testGait2354"); }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;

    return 0;
}

// See if we can find the given file. The rules are
//  - if it is an absolute pathname, we only get one shot, else:
//  - define "modelDir" to be the absolute pathname of the 
//      directory from which we read in the .osim model, if we did,
//      otherwise modelDir="." (current directory).
//  - look for the geometry file in modelDir
//  - look for the geometry file in modelDir/Geometry
//  - look for the geometry file in installDir/Geometry
bool ModelVisualizer::
findGeometryFile(const Model& aModel, 
                 const std::string&          geoFile,
                 bool&                       geoFileIsAbsolute,
                 SimTK::Array_<std::string>& attempts)
{
    attempts.clear();
    std::string geoDirectory, geoFileName, geoExtension; 
    SimTK::Pathname::deconstructPathname(geoFile, 
        geoFileIsAbsolute, geoDirectory, geoFileName, geoExtension);

    bool foundIt = false;
    if (geoFileIsAbsolute) {
        attempts.push_back(geoFile);
        foundIt = Pathname::fileExists(attempts.back());
    } else {  
        const string geoDir = "Geometry" + Pathname::getPathSeparator();
        string modelDir;
        if (aModel.getInputFileName() == "Unassigned") 
            modelDir = Pathname::getCurrentWorkingDirectory();
        else {
            bool isAbsolutePath; string directory, fileName, extension; 
            SimTK::Pathname::deconstructPathname(
                aModel.getInputFileName(),
                isAbsolutePath, directory, fileName, extension);
            modelDir = isAbsolutePath 
                ? directory
                : Pathname::getCurrentWorkingDirectory() + directory;
        }

        attempts.push_back(modelDir + geoFile);
        foundIt = Pathname::fileExists(attempts.back());

        if (!foundIt) {
            attempts.push_back(modelDir + geoDir + geoFile); 
            foundIt = Pathname::fileExists(attempts.back());
        }

        if (!foundIt) {
            const string installDir = 
                Pathname::getInstallDir("OPENSIM_HOME", "OpenSim");
            attempts.push_back(installDir + geoDir + geoFile);
            foundIt = Pathname::fileExists(attempts.back());
        }
    }

    return foundIt;
}

int main() {

    SimTK::Array_<std::string> failures;

    try{
        testCMCEMGDrivenArm();
    } catch(const std::exception& e) {  
        cout << e.what() <<endl; failures.push_back("testCMCEMGDrivenArm"); 
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;

    return 0;
}