package jp.co.yaskawa.rtc.integratedMotionClient.ui;

import java.awt.BorderLayout;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;

import javax.swing.BorderFactory;
import javax.swing.BoxLayout;
import javax.swing.JButton;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.JTabbedPane;
import javax.swing.UIManager;
import javax.swing.WindowConstants;

import jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue;
import jp.co.yaskawa.rtc.integratedMotionClient.IntegratedMotionClientImpl;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.CartesianAccel;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.CartesianSpeed;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.CommandFrameType;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.HgMatrixHolder;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.IntegratedMotionInterface;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RedundantAccel;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RedundantAxesMask;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RedundantLimit;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RedundantPos;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RedundantSpeed;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.ReturnID;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RightLeft;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.UnitType;

public class IMClientUI extends JFrame {

	IntegratedMotionClientImpl imClientImpl_;
	IntegratedMotionInterface imInterface_;
	
	ControllPanel presetPanel_;
	MonitorPanel monitorPanel_;

	final String[] movLLabel_ = {"x", "y", "z", "r11", "r12", "r13", "r21", "r22", "r23",
			"r31", "r32", "r33", "t vel[mm/s]", "r vel[deg/s]", "t accel[mm/s2]", "r accel[deg/s2]"};
	final String[] movCPLabel_ = {"elbow", "lumbar_ry", "v_X", "v_Y", "v_Theta", "elbow_vel", "lumbar_ry_vel", "v_Trans_vel", "v_Rotate_vel",
			"elbow_acc", "lumbar_ry_acc", "v_Trans_acc", "v_Rotate_acc"};
	final String[] movUnitArmLabel_ = {"J1", "J2", "J3", "J4", "J5", "J6", "J7", "vel[deg/s]", "accel[deg/s2]"};
	final String[] movUnitLumbarLabel_ = {"rx", "ry", "rz", "vel[deg/s]", "accel[deg/s2]"};
	final String[] redunMaskLabel_ = {"R_elb", "L_elb", "l_ry", "v_x", "v_y", "v_th"};
	final String[] movPTPLabel_ = {"x", "y", "z", "r11", "r12", "r13", "r21", "r22", "r23",
			"r31", "r32", "r33", "time[s]"};
	
	final String[] movLDefaultValue = {"0.0", "0.0", "0.0", "1.0", "0.0", "0.0", "0.0", "1.0", "0.0",
			"0.0", "0.0", "1.0", "10.0", "10.0", "5.0", "1.0" };
	final String[] movCPDefaultValue = {"0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0",
			"1.0", "1.0", "1.0", "1.0"};
	final String[] movUnitArmDefaultValue = {"57.3", "57.3", "-68.7", "68.7", "-28.6", "-58.3", "0.0", "10.0", "10.0" };
	final String[] movUnitLArmDefaultValue = {"154.0", "-70.5", "-103.0", "-98.0", "-107.0", "100.0", "54.4", "10.0", "10.0" };
	final String[] movUnitArmRelDefaultValue = {"0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "1.0", "1.0" };
	final String[] movUnitLumDefaultValue = {"0.0", "0.0", "0.0", "1.0", "1.0" };
	final String[] movPTPDefaultValue = {"0.0", "0.0", "0.0", "1.0", "0.0", "0.0", "0.0", "1.0", "0.0",
			"0.0", "0.0", "1.0", "5.0" };
	
	final String[] dummy = {"",""};

	
	public IMClientUI(IntegratedMotionClientImpl im, IntegratedMotionInterface imif) {
		super("IntegratedMotionClient");
		imClientImpl_ = im;
		imInterface_ = imif;
		
		try {
			UIManager.getInstalledLookAndFeels();
			UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName());
		} catch (Exception e) {
			e.printStackTrace();
		}
		
		this.setDefaultCloseOperation(WindowConstants.DO_NOTHING_ON_CLOSE);
		this.addWindowListener(new WindowAdapter() {
			
			public void windowClosing(WindowEvent e) {
				if (imClientImpl_ != null) {
					
				}
				System.exit(0);
			}
		});
		
		if (imInterface_ == null) {
			System.out.println("IM null");
			imInterface_ = new DummyModule();
		}
		
		this.setLayout(new BorderLayout());
		this.getContentPane().add(makeEastPanel(), BorderLayout.EAST);
		this.getContentPane().add(makeCenterPanel(), BorderLayout.CENTER);
		this.getContentPane().add(makeSouthPanel(), BorderLayout.SOUTH);
//		this.getContentPane().add(makeEastPanel(), BorderLayout.EAST);
		
		this.pack();
		this.setResizable(true);
		this.setVisible(true);
		
	}

	JPanel makeSouthPanel() {
		presetPanel_ = new ControllPanel(imInterface_);
		presetPanel_.setMonitor(monitorPanel_);
		return presetPanel_;
	}
	
	JPanel makeServoPanel() {
		JPanel panel = new JPanel();
		panel.setBorder(BorderFactory.createTitledBorder("Servo"));
		
		JButton servoOnButton = new JButton("ON");
		JButton servoOffButton = new JButton("OFF");
		
		servoOnButton.addActionListener(new ActionListener() {
			@Override
			public void actionPerformed(ActionEvent e) {
				ReturnID rtn = imInterface_.servoOn();
				monitorPanel_.setMessage(Integer.toString(rtn.id) + rtn.comment);				
				if(rtn.id == 0);
			}
		});
		servoOffButton.addActionListener(new ActionListener() {
			@Override
			public void actionPerformed(ActionEvent e) {
				imInterface_.servoOff();
			}
		});
		
		panel.add(servoOnButton);
		panel.add(servoOffButton);
		
		return panel;
	}
	
	JPanel makeCenterPanel() {
		JPanel panel = new JPanel();
		panel.setLayout(new BoxLayout(panel, BoxLayout.Y_AXIS));
	
		MovePanel movLCarAbsPanel = new MovePanel("MoveLinearAbs",movLLabel_) {
			@Override
			protected void doAction() {
				try {
					
					double[][] mat = new double[3][4];
					HgMatrixHolder absHgMat = new HgMatrixHolder(mat);
					
					double[] tmp = getValue();
					CartesianSpeed speed = new CartesianSpeed();
					CartesianAccel accel = new CartesianAccel();
					
					absHgMat.value[0][3] = tmp[0]; //!< x
					absHgMat.value[1][3] = tmp[1]; //!< y
					absHgMat.value[2][3] = tmp[2]; //!< z
					
					int index = 3;
					for(int i=0; i<3; i++) {
						for(int j=0; j<3; j++) {
							absHgMat.value[i][j] = tmp[index];
							index++;
						}
					}
					
					speed.translation = tmp[12];
					speed.rotation = tmp[13];
					accel.translation = tmp[14];
					accel.rotation = tmp[15];
					
					boolean[] stat = getCheckStatus();
					
					if(stat[0] == true)
					{
						imInterface_.moveLinearCartesianAbs(RightLeft.RIGHT, absHgMat.value, speed, accel, stat[2]);						
					}
					else if(stat[1] == true)
					{
						imInterface_.moveLinearCartesianAbs(RightLeft.LEFT, absHgMat.value, speed, accel, stat[2]);												
					}
					else
					{
						System.out.println("Arm type is not selected.Right or Left?");
						return;
					}
										
				} catch(Exception e) {
					return;
				}
			}
		};
		movLCarAbsPanel.setValue(movLDefaultValue);

		MovePanel movLCarRelPanel = new MovePanel("MoveLinearRel",movLLabel_) {
			@Override
			protected void doAction() {
				try {
					
					double[][] mat = new double[3][4];
					HgMatrixHolder relHgMat = new HgMatrixHolder(mat);
					
					double[] tmp = getValue();
					CartesianSpeed speed = new CartesianSpeed();
					CartesianAccel accel = new CartesianAccel();
					
					relHgMat.value[0][3] = tmp[0]; //!< x
					relHgMat.value[1][3] = tmp[1]; //!< y
					relHgMat.value[2][3] = tmp[2]; //!< z
					
					int index = 3;
					for(int i=0; i<3; i++) {
						for(int j=0; j<3; j++) {
							relHgMat.value[i][j] = tmp[index];
							index++;
						}
					}
					
					speed.translation = tmp[12];
					speed.rotation = tmp[13];
					accel.translation = tmp[14];
					accel.rotation = tmp[15];

					boolean[] stat = getCheckStatus();
					
					if(stat[0] == true)
					{
						imInterface_.moveLinearCartesianRel(RightLeft.RIGHT, CommandFrameType.BASE_FRAME, relHgMat.value, speed, accel, stat[2]);
					}
					else if(stat[1] == true)
					{
						imInterface_.moveLinearCartesianRel(RightLeft.RIGHT, CommandFrameType.BASE_FRAME, relHgMat.value, speed, accel, stat[2]);
					}
					else
					{
						System.out.println("Arm type is not selected.Right or Left?");
						return;
					}					
										
				} catch(Exception e) {
					e.printStackTrace();
				}
			}
		};
		movLCarRelPanel.setValue(movLDefaultValue);

		MovePanel movCPAbsPanel = new MovePanel("MoveCPHoldAbs",movCPLabel_) {
			@Override
			protected void doAction() {
				try {
					RedundantPos redAbsPos = new RedundantPos();
					RedundantSpeed redSpd = new RedundantSpeed();
					RedundantAccel redAcc = new RedundantAccel();
					double[] tmp = getValue();
					
					redAbsPos.elbow = tmp[0];
					redAbsPos.lumbar1 = tmp[1];
					redAbsPos.lumbar2 = 0.0;
					redAbsPos.lumbar3 = 0.0;
					redAbsPos.vehicleX = tmp[2];
					redAbsPos.vehicleY = tmp[3];
					redAbsPos.vehicleTheta = tmp[4];
					
					redSpd.elbow = tmp[5];
					redSpd.lumbar1 = tmp[6];
					redSpd.lumbar2 = 0.0;
					redSpd.lumbar3 = 0.0;
					redSpd.vehicleTranslation = tmp[7];
					redSpd.vehicleRotation = tmp[8];

					redAcc.elbow = tmp[9];
					redAcc.lumbar1 = tmp[10];
					redAcc.lumbar2 = 0.0;
					redAcc.lumbar3 = 0.0;
					redAcc.vehicleTranslation = tmp[11];
					redAcc.vehicleRotation = tmp[12];

					boolean[] stat = getCheckStatus();
					
					if(stat[0] == true)
					{
						imInterface_.moveCPHoldAbs(RightLeft.RIGHT, redAbsPos, redSpd, redAcc);
					}
					else if(stat[1] == true)
					{
						imInterface_.moveCPHoldAbs(RightLeft.LEFT, redAbsPos, redSpd, redAcc);
					}
					else
					{
						System.out.println("Arm type is not selected.Right or Left?");
						return;
					}										
					
										
				} catch(Exception e) {
					return;
				}
			}
		};
		movCPAbsPanel.setValue(movCPDefaultValue);
		
		MovePanel movCPRelPanel = new MovePanel("MoveCPHoldRel",movCPLabel_) {
			@Override
			protected void doAction() {
				try {
					RedundantPos redRelPos = new RedundantPos();
					RedundantSpeed redSpd = new RedundantSpeed();
					RedundantAccel redAcc = new RedundantAccel();
					double[] tmp = getValue();
					
					redRelPos.elbow = tmp[0];
					redRelPos.lumbar1 = 0.0;
					redRelPos.lumbar2 = tmp[1];
					redRelPos.lumbar3 = 0.0;
					redRelPos.vehicleX = tmp[2];
					redRelPos.vehicleY = tmp[3];
					redRelPos.vehicleTheta = tmp[4];
					
					redSpd.elbow = tmp[5];
					redSpd.lumbar1 = 0.0;
					redSpd.lumbar2 = tmp[6];
					redSpd.lumbar3 = 0.0;
					redSpd.vehicleTranslation = tmp[7];
					redSpd.vehicleRotation = tmp[8];

					redAcc.elbow = tmp[9];
					redAcc.lumbar1 = 0.0;
					redAcc.lumbar2 = tmp[10];
					redAcc.lumbar3 = 0.0;
					redAcc.vehicleTranslation = tmp[11];
					redAcc.vehicleRotation = tmp[12];

					boolean[] stat = getCheckStatus();
					
					if(stat[0] == true)
					{
						imInterface_.moveCPHoldRel(RightLeft.RIGHT, redRelPos, redSpd, redAcc);
					}
					else if(stat[1] == true)
					{
						imInterface_.moveCPHoldRel(RightLeft.LEFT, redRelPos, redSpd, redAcc);
					}
					else
					{
						System.out.println("Arm type is not selected.Right or Left?");
						return;
					}										
					
										
				} catch(Exception e) {
					return;
				}
			}
		};
		movCPRelPanel.setValue(movCPDefaultValue);

		MovePanel movUnitR_ArmAbsPanel = new MovePanel("MoveUnitR_ArmAbs", movUnitArmLabel_) {	
			@Override
			protected void doAction() {
				try {
					double[] absPosition = new double[7];
					double[] tmp = getValue();
					for (int i = 0; i < absPosition.length; i++) {
						absPosition[i] = tmp[i];
					}
					double speed[] = new double[1];
					speed[0] = tmp[7];
					double accel[] = new double[1];
					accel[0] = tmp[8];
					
					imInterface_.moveUnitAbs(UnitType.RIGHT_ARM, absPosition, speed, accel);
				} catch(Exception e) {
					return;
				}
			}
		};
		movUnitR_ArmAbsPanel.setValue(movUnitArmDefaultValue);
		
		MovePanel movUnitR_ArmRelPanel = new MovePanel("MoveUnitR_ArmRel", movUnitArmLabel_) {	
			@Override
			protected void doAction() {
				try {
					double[] relPosition = new double[7];
					double[] tmp = getValue();
					for (int i = 0; i < relPosition.length; i++) {
						relPosition[i] = tmp[i];
					}
					double speed[] = new double[1];
					speed[0] = tmp[7];
					double accel[] = new double[1];
					accel[0] = tmp[8];
					
					imInterface_.moveUnitRel(UnitType.RIGHT_ARM, relPosition, speed, accel);
				} catch(Exception e) {
					return;
				}
			}
		};
		movUnitR_ArmRelPanel.setValue(movUnitArmRelDefaultValue);

		MovePanel movUnitL_ArmAbsPanel = new MovePanel("MoveUnitL_ArmAbs", movUnitArmLabel_) {	
			@Override
			protected void doAction() {
				try {
					double[] absPosition = new double[7];
					double[] tmp = getValue();
					for (int i = 0; i < absPosition.length; i++) {
						absPosition[i] = tmp[i];
					}
					double speed[] = new double[1];
					speed[0] = tmp[7];
					double accel[] = new double[1];
					accel[0] = tmp[8];
					
					imInterface_.moveUnitAbs(UnitType.LEFT_ARM, absPosition, speed, accel);
				} catch(Exception e) {
					return;
				}
			}
		};
		movUnitL_ArmAbsPanel.setValue(movUnitLArmDefaultValue);
		
		MovePanel movUnitL_ArmRelPanel = new MovePanel("MoveUnitL_ArmRel", movUnitArmLabel_) {	
			@Override
			protected void doAction() {
				try {
					double[] relPosition = new double[7];
					double[] tmp = getValue();
					for (int i = 0; i < relPosition.length; i++) {
						relPosition[i] = tmp[i];
					}
					double speed[] = new double[1];
					speed[0] = tmp[7];
					double accel[] = new double[1];
					accel[0] = tmp[8];
					
					imInterface_.moveUnitRel(UnitType.LEFT_ARM, relPosition, speed, accel);
				} catch(Exception e) {
					return;
				}
			}
		};
		movUnitL_ArmRelPanel.setValue(movUnitArmRelDefaultValue);
		
		
		MovePanel movUnitLumbarAbsPanel = new MovePanel("MoveUnitLumbarAbs", movUnitLumbarLabel_) {	
			@Override
			protected void doAction() {
				try {
					double[] absPosition = new double[3];
					double[] tmp = getValue();
					for (int i = 0; i < absPosition.length; i++) {
						absPosition[i] = tmp[i];
					}
					double speed[] = new double[1];
					speed[0] = tmp[3];
					double accel[] = new double[1];
					accel[0] = tmp[4];
					
					imInterface_.moveUnitAbs(UnitType.LUMBER, absPosition, speed, accel);
				} catch(Exception e) {
					return;
				}
			}
		};
		movUnitLumbarAbsPanel.setValue(movUnitLumDefaultValue);
		
		SelectPanel selectRedPanel = new SelectPanel("selectRed",dummy) {
			@Override
			protected void doAction() {
				try {
					boolean[] tmp = getCheckStatus();
					
					RedundantAxesMask mask = new RedundantAxesMask();
					mask.elbow = tmp[0];
					mask.lumbar1 = tmp[2];
					mask.lumbar2 = tmp[3];
					mask.lumbar3 = tmp[4];
					mask.vehicleX = tmp[5];
					mask.vehicleY = tmp[6];
					mask.vehicleTheta = tmp[7];
					
					RedundantLimit lim = new RedundantLimit();
			        lim.rightElbow = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.leftElbow = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.lumbar1 = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.lumbar2 = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.lumbar3 = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.vehicleX = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.vehicleY = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
			        lim.vehicleTheta = new jp.co.yaskawa.rtc.integratedMotionClient.toController.LimitValue();
					
					lim.rightElbow.upper = 180.0;
					lim.rightElbow.lower = -180.0;
					lim.leftElbow.upper = 180.0;
					lim.leftElbow.lower = -180.0;
					lim.lumbar1.upper = 0.0;
					lim.lumbar1.lower = 0.0;
					lim.lumbar2.upper = 90.0;
					lim.lumbar2.lower = -90.0;
					lim.lumbar3.upper = 0.0;
					lim.lumbar3.lower = 0.0;
					lim.vehicleX.upper = 0.0;
					lim.vehicleX.lower = 0.0;
					lim.vehicleY.upper = 0.0;
					lim.vehicleY.lower = 0.0;
					lim.vehicleTheta.upper = 180.0;
					lim.vehicleTheta.lower = -180.0;
					
					imInterface_.selectRedundantAxes(mask, lim);
					
				} catch(Exception e) {
					e.printStackTrace();
				}
			}
		};
		
		MovePanel movPTPAbsPanel = new MovePanel("MovePTPAbs",movPTPLabel_) {
			@Override
			protected void doAction() {
				try {
					
					double[][] mat = new double[3][4];
					HgMatrixHolder absHgMat = new HgMatrixHolder(mat);
					
					double[] tmp = getValue();
					double movetime = 0.0;
					
					absHgMat.value[0][3] = tmp[0]; //!< x
					absHgMat.value[1][3] = tmp[1]; //!< y
					absHgMat.value[2][3] = tmp[2]; //!< z
					
					int index = 3;
					for(int i=0; i<3; i++) {
						for(int j=0; j<3; j++) {
							absHgMat.value[i][j] = tmp[index];
							index++;
						}
					}
					
					movetime = tmp[12];
					
					boolean[] stat = getCheckStatus();
					
					if(stat[0] == true)
					{
						imInterface_.movePTPCartesianAbs(RightLeft.RIGHT, absHgMat.value, movetime, stat[2]);						
					}
					else if(stat[1] == true)
					{
						imInterface_.movePTPCartesianAbs(RightLeft.LEFT, absHgMat.value, movetime, stat[2]);						
					}
					else
					{
						System.out.println("Arm type is not selected.Right or Left?");
						return;
					}
										
				} catch(Exception e) {
					return;
				}
			}
		};
		movPTPAbsPanel.setValue(movPTPDefaultValue);

		MovePanel movPTPRelPanel = new MovePanel("MovePTPRel",movPTPLabel_) {
			@Override
			protected void doAction() {
				try {
					
					double[][] mat = new double[3][4];
					HgMatrixHolder relHgMat = new HgMatrixHolder(mat);
					
					double[] tmp = getValue();
					double movetime = 0.0;
					
					relHgMat.value[0][3] = tmp[0]; //!< x
					relHgMat.value[1][3] = tmp[1]; //!< y
					relHgMat.value[2][3] = tmp[2]; //!< z
					
					int index = 3;
					for(int i=0; i<3; i++) {
						for(int j=0; j<3; j++) {
							relHgMat.value[i][j] = tmp[index];
							index++;
						}
					}
					
					movetime = tmp[12];

					boolean[] stat = getCheckStatus();
					
					if(stat[0] == true)
					{
						imInterface_.movePTPCartesianRel(RightLeft.RIGHT, relHgMat.value, movetime, stat[2]);						
					}
					else if(stat[1] == true)
					{
						imInterface_.movePTPCartesianRel(RightLeft.LEFT, relHgMat.value, movetime, stat[2]);						
					}
					else
					{
						System.out.println("Arm type is not selected.Right or Left?");
						return;
					}					
										
				} catch(Exception e) {
					e.printStackTrace();
				}
			}
		};
		movPTPRelPanel.setValue(movPTPDefaultValue);
		
		JTabbedPane pane = new JTabbedPane();
		pane.setTabLayoutPolicy(JTabbedPane.SCROLL_TAB_LAYOUT);
		pane.addTab("selectRedun", selectRedPanel);
		pane.addTab("movLinearAbs", movLCarAbsPanel);
		pane.addTab("movLinearRel", movLCarRelPanel);
		pane.addTab("movCPHoldAbs", movCPAbsPanel);
		pane.addTab("movCPHoldRel", movCPRelPanel);
		pane.addTab("movJ_RArmAbs", movUnitR_ArmAbsPanel);
		pane.addTab("movJ_RArmRel", movUnitR_ArmRelPanel);
		pane.addTab("movJ_LArmAbs", movUnitL_ArmAbsPanel);
		pane.addTab("movJ_LArmRel", movUnitL_ArmRelPanel);
		pane.addTab("movLumbarAbs", movUnitLumbarAbsPanel);
		pane.addTab("movPTPAbs", movPTPAbsPanel);
		pane.addTab("movPTPRel", movPTPRelPanel);
		
		panel.add(makeServoPanel());
		panel.add(pane);
		
		return panel;
	}
	
	JPanel makeEastPanel() {
		monitorPanel_ = new MonitorPanel(imInterface_);
		return monitorPanel_;
	}
	
	public void setController(IntegratedMotionInterface c) {
		if (c == null) {
			imInterface_ = new DummyModule();
		} else {
			imInterface_ = c;
		}
		monitorPanel_.setController(imInterface_);
		presetPanel_.setController(imInterface_);
		
	}

	public void monitorStart() {
		monitorPanel_.monitorStart();
	}
	
	public void monitorStop() {
		monitorPanel_.monitorStop();
	}
	
	
}
