#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChSystemSMC.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono/physics/ChBody.h"
#include "chrono/physics/ChLinkMate.h"
#include "chrono/physics/ChLinkRSDA.h"
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
#include "chrono/physics/ChLinkMotorLinearPosition.h"
#include "chrono/physics/ChLinkMotorRotationAngle.h"
#include "chrono/physics/ChLinkMotorLinearForce.h"
#include <vector>
#include <cstdio>
#include <cmath>
#include <string>
#include "chrono/assets/ChVisualShapeTriangleMesh.h"
#include "chrono_irrlicht/ChVisualSystemIrrlicht.h"
#include "chrono/core/ChRealtimeStep.h"
#include <chrono/solver/ChSolverBB.h>
#include "chrono/geometry/ChTriangleMeshConnected.h"
using namespace chrono;
using namespace chrono::irrlicht;
using namespace irr;
double sphereswept_r = 0.001;
bool draw_coll_shapes = true;
ChVisualSystem::Type vis_type = ChVisualSystem::Type::IRRLICHT;
ChContactMethod contact_method = ChContactMethod::NSC;
ChCollisionSystem::Type collision_type = ChCollisionSystem::Type::BULLET;
void get_body_add(std::shared_ptr<ChBodyAuxRef>& body, std::string name, ChFramed frame, ChFramed frame_COM, std::string link_vis, std::string link_collision, double mass, chrono::ChVector3d InertiaXX, chrono::ChVector3d InertXY, std::shared_ptr<chrono::ChContactMaterialNSC> mat_1, bool colison) {
	std::cout << "2" << " " << "\n";
	std::shared_ptr<chrono::ChVisualShapeModelFile> body_shape;
	chrono::ChMatrix33<> mr;
	body = chrono_types::make_shared<ChBodyAuxRef>();
	body->SetName(name); std::cout << "2" << " " << "\n";
	body->SetPos(frame.GetPos());
	body->SetRot(frame.GetRot());

	body->SetMass(mass);
	body->SetInertiaXX(InertiaXX);
	body->SetInertiaXY(InertXY);
	body->SetFrameCOMToRef(frame_COM);
	std::cout << "2" << " " << "\n";

	body_shape = chrono_types::make_shared<chrono::ChVisualShapeModelFile>();
	body_shape->SetFilename(link_vis);
	body->AddVisualShape(body_shape, chrono::ChFramed(chrono::ChVector3d(0, 0, 0), chrono::ChQuaternion<>(1, 0, 0, 0)));
	std::cout << "2" << " " << "\n";

	// Collision Model
	body->AddCollisionModel(chrono_types::make_shared<chrono::ChCollisionModel>());

	// Collision material

	// Collision shape
	std::shared_ptr<chrono::ChCollisionShape> collshape_1;

	// Triangle mesh collision shape
	//auto body_1_1_collision_mesh = chrono_types::make_shared<chrono::ChTriangleMeshConnected>();
	//body_1_1_collision_mesh->LoadWavefrontMesh(link_vis, false, true);
	auto body_1_1_collision_mesh = ChTriangleMeshConnected::CreateFromWavefrontFile(link_collision,
		false, true);
	mr(0, 0) = 1; mr(1, 0) = 0; mr(2, 0) = 0;
	mr(0, 1) = 0; mr(1, 1) = 1; mr(2, 1) = 0;
	mr(0, 2) = 0; mr(1, 2) = 0; mr(2, 2) = 1;
	body_1_1_collision_mesh->Transform(chrono::ChVector3d(0, 0, 0), ChMatrix33<>(1));
	collshape_1 = chrono_types::make_shared<chrono::ChCollisionShapeTriangleMesh>(mat_1, body_1_1_collision_mesh, false, false, sphereswept_r);
	body->GetCollisionModel()->AddShape(collshape_1);
	body->EnableCollision(colison);

	// Visualization shape

}

// Drawer for collision shapes
class DebugDrawer : public ChCollisionSystem::VisualizationCallback {
public:
	explicit DebugDrawer(ChVisualSystemIrrlicht* vis) : m_vis(vis) {}
	~DebugDrawer() {}

	virtual void DrawLine(const ChVector3d& from, const ChVector3d& to, const ChColor& color) override {
		m_vis->GetVideoDriver()->draw3DLine(irr::core::vector3dfCH(from), irr::core::vector3dfCH(to),
			irr::video::SColor(255, 55, 55, 255));
	}

	virtual double GetNormalScale() const override { return 1.0; }

	void Draw(int flags, bool use_zbuffer = true) {
		m_vis->GetVideoDriver()->setTransform(irr::video::ETS_WORLD, irr::core::matrix4());
		irr::video::SMaterial mattransp;
		mattransp.ZBuffer = use_zbuffer;
		mattransp.Lighting = false;
		m_vis->GetVideoDriver()->setMaterial(mattransp);

		m_vis->GetSystem(0).GetCollisionSystem()->Visualize(flags);
	}

private:
	ChVisualSystemIrrlicht* m_vis;
};

void gym() {
    // Create a Chrono system
    ChSystemNSC sys;
    ChCollisionModel::SetDefaultSuggestedEnvelope(0.001);
    ChCollisionModel::SetDefaultSuggestedMargin(0.001);
	sys.SetCollisionSystemType(collision_type);
    auto material = chrono_types::make_shared<ChContactMaterialNSC>();
	auto floor = chrono_types::make_shared<ChBodyEasyBox>(8, 0.3, 8, 1000, material);
	floor->SetFixed(true);
	floor->EnableCollision(true);
	sys.Add(floor);
	double l = 2;
	//double l2 = l;
	//_1
	auto pole = chrono_types::make_shared<ChBodyEasyCylinder>(ChAxis::Y, 0.048 / 2, l, 1000, material);
	pole->EnableCollision(true);
	pole->SetPos(ChVector3d(0, l / 2, 0));
	sys.Add(pole);
	pole->SetFixed(true);
	// defining Parameters:
	double dis = 0.3;
	double l00_z_offset = 0.3;
	double l0_x = 0.1;
	double l0_y = 0.2;
	double l1_1 = 0.2;
	double l1_2 = 0.1;
	double l2 = 0.35;
	double l3 = 0.35;
	double ls = 0.1;
	double l0_rear = 0.25;
	double l1_rear = 0.1;
	double l2_rear = 0.2;
	double l3_rear = 0.3;

	double l4_gripper_rear = ls;
	// defining frames
	auto initial_frame = pole->GetVisualModelFrame();
	auto frame_block_ee_1 = initial_frame * ChFrame(ChVector3d(0, dis, 0));
	auto frame_block_ee_2 = initial_frame * ChFrame(ChVector3d(0, -dis, 0), QuatFromAngleY(CH_PI));
	auto frame_base_body=initial_frame* ChFrame(ChVector3d(0, 0, l00_z_offset));

	double initial_angle_shl_r = 0;//CH_PI*0.3;// at CH_PI error occurr
	double initial_angle_plane_body_r = CH_PI_2*0.5;//0;//
	double initial_angle_plane_angle_r = 0;//0;//
	//
	double initial_angle_shl_l = 0;// CH_PI * 0.3;//
	double initial_angle_plane_body_l = 0;//CH_PI * -0.1;//
	double initial_angle_plane_angle_l = 0;//CH_PI * 0.2;// right_arm
	// frames
	// right
	auto frame_shr_pos_r= frame_base_body * ChFrame(ChVector3d(l0_x, l0_y, 0), QuatFromAngleY(CH_PI_4));
	auto frame_shr_r_helper = frame_shr_pos_r * ChFrame(VNULL, QuatFromAngleZ(initial_angle_shl_r));
	auto frame_shlr_r = frame_shr_r_helper * ChFrame(VNULL, QuatFromAngleY(-CH_PI_2) * QuatFromAngleZ(-CH_PI_2));
	auto frame_joint_plane_body_r = ChFrame(frame_shlr_r * ChVector3d(0, l1_1, l1_2), frame_shlr_r.GetRot() * QuatFromAngleY(-CH_PI) * QuatFromAngleZ(initial_angle_plane_body_r));
	auto frame_joint_plane_angle_r = ChFrame(frame_joint_plane_body_r * ChVector3d(0, l2, 0), frame_joint_plane_body_r.GetRot() * QuatFromAngleZ(initial_angle_plane_angle_r));
	auto frame_joint_uni_r = ChFrame(frame_joint_plane_angle_r * ChVector3d(0, l3 + 0.025, 0), frame_joint_plane_angle_r.GetRot() * Q_ROTATE_Z_TO_Y);
	auto frame_joint_after_u_r = frame_joint_uni_r * ChFrame(ChVector3d(0, 0, 0.025));
	// left

	// right
	// // bodies
	auto body = chrono_types::make_shared<ChBodyEasyBox>(0.1, 0.4, 0.05, 1000, material);
	body->SetFixed(true);
	body->SetPos(frame_base_body.GetPos());
	body->EnableCollision(true);
	sys.Add(body);
	bool all_fixed_r = true;
	auto body_conector_r = chrono_types::make_shared<ChBodyEasyBox>(0.05, l1_1, 0.05, 1000, material); body_conector_r->SetFixed(all_fixed_r); body_conector_r->SetPos(frame_shlr_r * ChVector3d(0, l1_1 / 2, 0)); body_conector_r->SetRot(frame_shlr_r.GetRot()); sys.AddBody(body_conector_r); body_conector_r->EnableCollision(false);
	auto body_link_arm_2_r = chrono_types::make_shared<ChBodyEasyBox>(0.05, l2, 0.05, 1000, material);  body_link_arm_2_r->SetFixed(all_fixed_r);  body_link_arm_2_r->SetPos(frame_joint_plane_body_r * ChVector3d(0, l2 / 2, 0));  body_link_arm_2_r->SetRot(frame_joint_plane_body_r.GetRot()); sys.AddBody( body_link_arm_2_r);  body_link_arm_2_r->EnableCollision(false);
	auto body_link_arm_3_r = chrono_types::make_shared<ChBodyEasyBox>(0.05, l3, 0.05, 1000, material);  body_link_arm_3_r->SetFixed(all_fixed_r);  body_link_arm_3_r->SetPos(frame_joint_plane_angle_r * ChVector3d(0, l2 / 2, 0));  body_link_arm_3_r->SetRot(frame_joint_plane_angle_r.GetRot()); sys.AddBody(body_link_arm_3_r);  body_link_arm_3_r->EnableCollision(false);
	auto  body_arm_r_h_u= chrono_types::make_shared<ChBodyEasyCylinder>(ChAxis::Z,0.01, 0.002, 1000, material);  body_arm_r_h_u->SetFixed(false); body_arm_r_h_u->SetPos(frame_joint_after_u_r * ChVector3d(0, 0, -0.001)); body_arm_r_h_u->SetRot(frame_joint_after_u_r.GetRot()); sys.AddBody(body_arm_r_h_u);  body_arm_r_h_u->EnableCollision(false);
	//joint
	auto joint_r_1 = chrono_types::make_shared<ChLinkLockRevolute>();
	auto joint_r_2 = chrono_types::make_shared<ChLinkLockRevolute>();
	auto joint_r_3 = chrono_types::make_shared<ChLinkLockRevolute>();
	auto joint_r_u = chrono_types::make_shared<ChLinkMateGeneric>(true,true,true,false,false,true);//ChLinkUniversal
	auto joint_r_4 = chrono_types::make_shared<ChLinkLockRevolute>();
	if (!all_fixed_r){
	joint_r_1->Initialize(body, body_conector_r, frame_shr_r_helper); sys.AddLink(joint_r_1);
	joint_r_2->Initialize(body_conector_r,body_link_arm_2_r, frame_joint_plane_body_r); sys.AddLink(joint_r_2);
	joint_r_3->Initialize(body_link_arm_2_r, body_link_arm_3_r, frame_joint_plane_angle_r); sys.AddLink(joint_r_3);}
	joint_r_u->Initialize(body_link_arm_3_r, body_arm_r_h_u, frame_joint_uni_r); sys.AddLink(joint_r_u); // muss evtl geändert werden
	//motors
	auto set_f_r_1 = chrono_types::make_shared<chrono::ChFunctionSetpoint>();
	auto set_f_r_2 = chrono_types::make_shared<chrono::ChFunctionSetpoint>();
	auto set_f_r_3 = chrono_types::make_shared<chrono::ChFunctionSetpoint>();
	auto set_f_r_4 = chrono_types::make_shared<chrono::ChFunctionSetpoint>();
	auto motor_r_1 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
	auto motor_r_2 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
	auto motor_r_3 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
	auto motor_r_4 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
	if (!all_fixed_r) {
		motor_r_1->Initialize(body, body_conector_r, frame_shr_r_helper); motor_r_1->SetAngleFunction(set_f_r_1); sys.AddLink(motor_r_1); //motor_r_1->SetAngleOffset(-initial_angle_shl_r);
		motor_r_2->Initialize(body_conector_r, body_link_arm_2_r, frame_joint_plane_body_r);   sys.AddLink(motor_r_2); motor_r_2->SetAngleFunction(set_f_r_2); //motor_r_2->SetAngleOffset(-initial_angle_plane_body_r);
		motor_r_3->Initialize(body_link_arm_2_r, body_link_arm_3_r, frame_joint_plane_angle_r);   sys.AddLink(motor_r_3); motor_r_3->SetAngleFunction(set_f_r_3); motor_r_3->SetAngleOffset(-initial_angle_plane_angle_r);
	}
	// springs
	double rest_length = 0.08;
	double spring_coef = 500;// 50;
	double damping_coef = 10;
	auto spring_1_r = chrono_types::make_shared<ChLinkTSDA>();
	spring_1_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true, ChVector3d(0, l3 / 2 - 0.05, -0.02), ChVector3d(0, 0.05, 0));
	spring_1_r->SetRestLength(rest_length);
	spring_1_r->SetSpringCoefficient(spring_coef);
	spring_1_r->SetDampingCoefficient(damping_coef);


	// --- 2. Feder ---
	auto spring2_r = chrono_types::make_shared<ChLinkTSDA>();
	spring2_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true, ChVector3d(0.02, l3 / 2 - 0.05, 0), ChVector3d(0.05, 0, 0));
	spring2_r->SetSpringCoefficient(spring_coef);
	spring2_r->SetDampingCoefficient(damping_coef);
	spring2_r->SetRestLength(rest_length);


	// --- 3. Feder ---
	auto spring3_r = chrono_types::make_shared<ChLinkTSDA>();

	spring3_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true, ChVector3d(-0.02, l3 / 2 - 0.05, 0), ChVector3d(-0.05, 0, 0));
	spring3_r->SetSpringCoefficient(spring_coef);
	spring3_r->SetDampingCoefficient(damping_coef);
	spring3_r->SetRestLength(rest_length);


	// --- 4. Feder ---
	auto spring4_r = chrono_types::make_shared<ChLinkTSDA>();
	spring4_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true, ChVector3d(0, l3 / 2 - 0.05, 0.02), ChVector3d(0, -0.05, 0));
	spring4_r->SetSpringCoefficient(spring_coef);
	spring4_r->SetDampingCoefficient(damping_coef);
	spring4_r->SetRestLength(rest_length);
	sys.AddLink(spring_1_r);
	sys.AddLink(spring2_r);
	sys.AddLink(spring3_r);
	sys.AddLink(spring4_r);
	// Attach a visualization asset.
	spring_1_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01, 40, 8));
	spring2_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01, 40, 8));
	spring3_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01, 40, 8));
	spring4_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01, 40, 8));
	// grippers
	auto material_friction = chrono_types::make_shared<ChContactMaterialNSC>();
	material_friction->SetFriction(0.5);
	material_friction->SetRollingFriction(0.5);
	material_friction->SetSlidingFriction(0.5);
	std::string shapes_dir = "C:\\workspace\\climbing_101\\gripper_export_shapes\\";
	std::shared_ptr<ChBodyAuxRef> finger_1_r;
	std::shared_ptr<ChBodyAuxRef> finger_2_r;
	std::shared_ptr<ChBodyAuxRef> base_r;
	auto frame_base_com_to_ref = chrono::ChFramed(chrono::ChVector3d(1.88969192858969e-18, 0.0466317534677547, 4.71445555287807e-19), chrono::ChQuaternion<>(1, 0, 0, 0));
	auto frame_finger_1_com_to_ref = chrono::ChFramed(chrono::ChVector3d(0.000953801080669733, -0.021322686682018, 5.3115175771791e-19), chrono::ChQuaternion<>(1, 0, 0, 0));
	auto frame_finger_2_com_to_ref = chrono::ChFramed(chrono::ChVector3d(0.000953801080669733, -0.021322686682018, 5.3115175771791e-19), chrono::ChQuaternion<>(1, 0, 0, 0));

	auto frame_gripper_r = frame_joint_after_u_r * ChFrame(ChVector3d(0, 0, 0), QuatFromAngleX(CH_PI_2) * QuatFromAngleY(CH_PI_2));
	auto frame_finger_1_r = frame_gripper_r * ChFrame(ChVector3d(0.0366618867860707, -0.003116191915022, 0.0591554353444595)).GetInverse() * ChFrame(ChVector3d(-0.00148250962951285, 0.137595735538563, 0.0591554353444594));
	auto frame_finger_2_r = frame_gripper_r * ChFrame(ChVector3d(0.0366618867860707, -0.003116191915022, 0.0591554353444595)).GetInverse() * ChFrame(chrono::ChVector3d(0.0743899226468857, 0.137595735538563, 0.0591554353444595), chrono::ChQuaternion<>(7.85000728502012e-17, -1.17368952349279e-16, 1, 2.77555756156289e-17));
	get_body_add(base_r, std::string("base"), frame_gripper_r, frame_base_com_to_ref, shapes_dir + "body_3_1.obj", shapes_dir + "body_3_1_collision.obj", 0.406216270935814, chrono::ChVector3d(0.000352637121668769, 0.000451309361507021, 0.000370829111668769), chrono::ChVector3d(-1.06257411112441e-20, -1.28013920144965e-20, -8.61559598459534e-21), material_friction, true);
	sys.AddBody(base_r);
	/*get_body_add(finger_1_r, std::string("finger_1"), frame_finger_1_r, frame_finger_1_com_to_ref, shapes_dir + "body_1_1_collision_mini.obj", shapes_dir + "body_1_1_collision_mini.obj", 0.0373760124348794, chrono::ChVector3d(4.54111393779954e-05, 5.14947357705548e-06, 4.11685501697624e-05), chrono::ChVector3d(-3.20274862402208e-07, 1.8446335448084e-22, -1.03000641621034e-21), material_friction, false);// Inertia and COM and position are done by solidworks export
	get_body_add(finger_2_r, std::string("finger_2"), frame_finger_2_r, frame_finger_2_com_to_ref, shapes_dir + "body_1_1_collision_mini.obj", shapes_dir + "body_1_1_collision_mini.obj", 0.0373760124348794, chrono::ChVector3d(4.54111393779954e-05, 5.14947357705548e-06, 4.11685501697624e-05), ChVector3d(3.20274862402217e-07, -4.816237693587e-22, -3.59090401274159e-21), material_friction, false);
	sys.AddBody(finger_1_r); sys.AddBody(finger_2_r); finger_1_r->EnableCollision(true); finger_2_r->EnableCollision(true);
	

	double limit_neg = -0.01;
	double limit_pos = 0.03;
	//right
	auto frame_link_1_r = frame_finger_1_r * ChFrame(ChVector3d(0, -0.065, 0), QuatFromAngleY(CH_PI_2));
	auto frame_link_2_r = frame_finger_2_r * ChFrame(ChVector3d(0, -0.065, 0), QuatFromAngleY(CH_PI_2));
	auto prismatic_finger_1_r = chrono_types::make_shared<ChLinkLockPrismatic>();
	prismatic_finger_1_r->Initialize(finger_1_r, base_r, frame_link_1_r);	prismatic_finger_1_r->LimitZ().SetActive(true);	prismatic_finger_1_r->LimitZ().SetMin(limit_neg); prismatic_finger_1_r->LimitZ().SetMax(limit_pos);
	sys.AddLink(prismatic_finger_1_r);
	// finger_2 base
	auto prismatic_finger_2_r = chrono_types::make_shared<ChLinkLockPrismatic>();	prismatic_finger_2_r->Initialize(finger_2_r, base_r, frame_link_2_r);	prismatic_finger_2_r->LimitZ().SetActive(true);	prismatic_finger_2_r->LimitZ().SetMin(limit_pos);	prismatic_finger_2_r->LimitZ().SetMax(limit_pos);
	sys.AddLink(prismatic_finger_2_r);
	auto motfun_finger_1_r = chrono_types::make_shared<chrono::ChFunctionSetpoint>();
	auto motfun_finger_2_r = chrono_types::make_shared<chrono::ChFunctionSetpoint>();
	
	auto motor_finger_1_r = chrono_types::make_shared<chrono::ChLinkMotorLinearForce>(); motor_finger_1_r->Initialize(finger_1_r, base_r, frame_link_1_r); sys.AddLink(motor_finger_1_r); motor_finger_1_r->SetMotorFunction(motfun_finger_1_r);
	auto motor_finger_2_r = chrono_types::make_shared<chrono::ChLinkMotorLinearForce>(); motor_finger_2_r->Initialize(finger_2_r, base_r, frame_link_2_r); sys.AddLink(motor_finger_2_r); motor_finger_2_r->SetMotorFunction(motfun_finger_2_r);
	
	prismatic_finger_1_r->Lock(true);
	prismatic_finger_2_r->Lock(true);*/
	joint_r_4->Initialize(body_arm_r_h_u, base_r, body_arm_r_h_u->GetVisualModelFrame()); sys.AddLink(joint_r_4);
	motor_r_4->Initialize(body_arm_r_h_u, base_r, body_arm_r_h_u->GetVisualModelFrame()); motor_r_4->SetAngleFunction(set_f_r_4); sys.AddLink(motor_r_4);

	//left
	
	
		auto solver = chrono_types::make_shared<ChSolverBB>();//ChSolverMINRES, ChSolverPJacobi,ChSolverBB,ADMM

		// Set maximum iterations and tolerance
		//solver->SetMaxIterations(400);           // number of solver iterations
		//solver->SetTolerance(1e-10);             // constraint tolerance
		//solver->EnableDiagonalPreconditioner(true); // often helps


		// Attach solver to system
		sys.SetSolver(solver);
		sys.SetTimestepperType(ChTimestepper::Type::EULER_IMPLICIT_LINEARIZED);//EULER_IMPLICIT_LINEARIZED

		// Configure the integrator
		auto stepper = std::dynamic_pointer_cast<ChTimestepperHHT>(sys.GetTimestepper());
		if (stepper) {
			stepper->SetAlpha(-0.2);    // numerical damping (-0.3..0 recommended)
			stepper->SetMaxIters(100);  // iterations per step
			stepper->SetAbsTolerances(1e-12, 1e-12);  // state & constraint tolerance

	}


	double timestep = 0.001;
	ChRealtimeStepTimer realtime_timer;
	auto vis = chrono_types::make_shared<ChVisualSystemIrrlicht>();
	vis->AttachSystem(&sys);
	vis->SetWindowSize(800, 600);
	vis->SetWindowTitle("testing_body_system");
	vis->Initialize();
	vis->AddLogo();
	vis->AddSkyBox();
	vis->AddTypicalLights();
	vis->AddCamera(ChVector3d(0, 4, 4));
	vis->AddLightWithShadow(ChVector3d(20.0, 35.0, -25.0), ChVector3d(0, 0, 0), 55, 20, 55, 35, 512,
		ChColor(0.6f, 0.8f, 1.0f));
	vis->EnableShadows();
	auto drawer = chrono_types::make_shared<DebugDrawer>(vis.get());
	sys.GetCollisionSystem()->RegisterVisualizationCallback(drawer);
	int mode = ChCollisionSystem::VIS_Shapes;
	bool use_zbuffer = true;

	double start_angle = motor_r_4->GetAngleOffset();
	while (vis->Run()) {
		vis->BeginScene();
		vis->Render();
		if (draw_coll_shapes)
			drawer->Draw(mode, use_zbuffer);
		tools::drawCoordsys(vis.get(), frame_shr_r_helper.GetCoordsys());
		std::cout << spring_1_r->GetLength()<<"\n";
		vis->EndScene();
		double t = sys.GetChTime();
		std::cout << t << "\n";
		if (t > 0.2) {
			
			
		}
		else {
double val = start_angle + t * 2;
		set_f_r_4->SetSetpoint(val, t);
		
		}

		sys.DoStepDynamics(timestep);
		realtime_timer.Spin(timestep);
	}

    

}

int main() {
	gym();
	return 0;
}