package hagerty.simulator;

import coppelia.IntW;
import coppelia.IntWA;
import coppelia.remoteApi;
import hagerty.simulator.legacy.data.LegacyMotorSimData;
import hagerty.simulator.legacy.data.SimData;
import hagerty.simulator.modules.BrickSimulator;

import java.util.List;

public class CoppeliaApiClient implements Runnable {
    public static final String LOCAL_HOST = "127.0.0.1";
    public static final boolean WAIT_UNTIL_CONNECTED = true;
    public static final boolean DO_NOT_RECONNECT_ONCE_DISCONNECTED = true;
    public static final int THREAD_CYCLE_IN_MS = 50;
    private static final int MILLI_SECOND_TIMEOUT = 5000;
    final int CONNECT_PORT = 5000;
    long mStartTime;
	IntWA mObjectHandles;
	IntW mLeftMotor;
	IntW mRightMotor;
	int mClientID;
	remoteApi mVrep;
	hagerty.gui.MainApp mMainApp;
    private volatile boolean done;

	public CoppeliaApiClient(hagerty.gui.MainApp mainApp) {
		mMainApp = mainApp;
        done = false;
    }

	public boolean init() {

		mVrep = new remoteApi();
		mVrep.simxFinish(-1); // just in case, close all opened connections


        mClientID = mVrep.simxStart(LOCAL_HOST,
                CONNECT_PORT, WAIT_UNTIL_CONNECTED,
                DO_NOT_RECONNECT_ONCE_DISCONNECTED,
                MILLI_SECOND_TIMEOUT, THREAD_CYCLE_IN_MS);
        if (mClientID != -1) {
            System.out.println("Connected to remote API server");

			mObjectHandles = new IntWA(1);
			mLeftMotor = new IntW(1);
			mRightMotor = new IntW(1);

            int ret = mVrep.simxGetObjects(mClientID, remoteApi.sim_handle_all, mObjectHandles,
                    remoteApi.simx_opmode_oneshot_wait);
            if (ret == remoteApi.simx_return_ok)
                System.out.format("Number of objects in the scene: %d\n", mObjectHandles.getArray().length);
            else
				System.out.format("Remote API function call returned with error code: %d\n",ret);

			try
			{
				Thread.sleep(2000);
			}
			catch(InterruptedException ex)
			{
				Thread.currentThread().interrupt();
			}

			mVrep.simxGetObjectHandle(mClientID,"remoteApiControlledBubbleRobLeftMotor",mLeftMotor,remoteApi.simx_opmode_oneshot_wait);
			mVrep.simxGetObjectHandle(mClientID,"remoteApiControlledBubbleRobRightMotor",mRightMotor,remoteApi.simx_opmode_oneshot_wait);
			System.out.println("Left = " + mLeftMotor.getValue() + " Right = " + mRightMotor.getValue());

			mStartTime=System.currentTimeMillis();
			return true;

		} else {
			System.out.println("Failed connecting to remote API server");
			return false;
		}

	}

	public void run()
	{
		boolean done=false;
		float leftMotorSpeed=0;
		float rightMotorSpeed=0;

		/*
		 * Read the current list of modules from the GUI MainApp class
		 */
		SimData simData=null;
		List<BrickSimulator> brickList = mMainApp.getBrickData();
		for (BrickSimulator currentBrick : brickList) {
			simData = currentBrick.findSimDataName("Wheels");
			if (simData != null) break;
		}

		if (simData == null) {
			System.out.println("Failed to find a module name 'Wheels'");
			done = true;
		}

        while (!done && !Thread.currentThread().isInterrupted()) {
			simData.lock.readLock().lock();
			try {
				leftMotorSpeed = ((LegacyMotorSimData)simData).getMotor1Speed() * 3.14f;
				rightMotorSpeed = ((LegacyMotorSimData)simData).getMotor2Speed() * 3.14f;
			} finally {
				simData.lock.readLock().unlock();
			}

			mVrep.simxSetJointTargetVelocity(mClientID,mLeftMotor.getValue(),-leftMotorSpeed,remoteApi.simx_opmode_oneshot);
			mVrep.simxSetJointTargetVelocity(mClientID,mRightMotor.getValue(),rightMotorSpeed,remoteApi.simx_opmode_oneshot);
		}


		// Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
		IntW pingTime = new IntW(0);
		mVrep.simxGetPingTime(mClientID,pingTime);

		// Now close the connection to V-REP:
		mVrep.simxFinish(mClientID);
        System.out.println("Program ended");
    }

    public void requestTerminate() {
        done = true;
    }
}


@@ -722,16 +722,22 @@
 eftMotor
+Handle
 ;%0A%09IntW 
@@ -747,16 +747,22 @@
 ghtMotor
+Handle
 ;%0A%09int m
@@ -1441,24 +1441,30 @@
 %09%09mLeftMotor
+Handle
  = new IntW(
@@ -1481,16 +1481,22 @@
 ghtMotor
+Handle
  = new I
@@ -2130,24 +2130,30 @@
 %22,mLeftMotor
+Handle
 ,remoteApi.s
@@ -2269,16 +2269,22 @@
 ghtMotor
+Handle
 ,remoteA
@@ -2349,32 +2349,38 @@
 = %22 + mLeftMotor
+Handle
 .getValue() + %22 
@@ -2394,32 +2394,38 @@
  %22 + mRightMotor
+Handle
 .getValue());%0A%0A%09
@@ -3468,24 +3468,30 @@
 D,mLeftMotor
+Handle
 .getValue(),
@@ -3595,16 +3595,22 @@
 ghtMotor
+Handle
 .getValu

Rename left and right motor handles