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

import java.awt.BorderLayout;
import java.awt.Dimension;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.text.NumberFormat;

import javax.swing.BorderFactory;
import javax.swing.BoxLayout;
import javax.swing.ImageIcon;
import javax.swing.JButton;
import javax.swing.JPanel;
import javax.swing.JTextArea;
import javax.swing.SwingUtilities;
import javax.swing.Timer;

import jp.co.yaskawa.rtc.integratedMotionClient.toController.CarPosWithElbow;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.CarPosWithElbowHolder;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.DoubleSeqHolder;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.FrameType;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.IntegratedMotionInterface;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.RightLeft;
import jp.co.yaskawa.rtc.integratedMotionClient.toController.UnitType;


@SuppressWarnings("serial")
public class MonitorPanel extends JPanel {

	final int MONITOR_PERIOD = 500; // [ms]
	FieldListPanel jointRefPanel_;
	FieldListPanel jointRefPanel2_;
	FieldListPanel carRefPanel_;
	FieldListPanel carRefPanel2_;
	IntegratedMotionInterface imInterface_;
	Timer timer;
	JButton monitorButton_;
	ImageIcon greenBallIcon_ = new ImageIcon(getClass().getResource("/jp/co/yaskawa/rtc/integratedMotionClient/ui/resources/greenball.png"));
	ImageIcon grayBallIcon_ = new ImageIcon(getClass().getResource("/jp/co/yaskawa/rtc/integratedMotionClient/ui/resources/grayball.png"));
	StatusLabel servoLabel_;
	StatusLabel motionLabel_;
	NumberFormat nf_ = NumberFormat.getInstance();
	final String BUTTON_LABEL_START = "Monitor Start";
	final String BUTTON_LABEL_STOP = "Monitor Stop";
	
	JTextArea resultView = new JTextArea(5,40); 
	
	public MonitorPanel(IntegratedMotionInterface imif) {
		super();
		imInterface_ = imif;
		this.setLayout(new BorderLayout());
		this.setBorder(BorderFactory.createTitledBorder("Monitor"));
		
		timer = new Timer(MONITOR_PERIOD, new TimerAction());
		
		nf_.setMaximumFractionDigits(5);
		nf_.setMinimumFractionDigits(5);
		
		this.add(mkCenterPanel(), BorderLayout.CENTER);
		
	}

	public void setController(IntegratedMotionInterface imif) {
		imInterface_ = imif;
	}
	
	JPanel mkCenterPanel() {
		JPanel panel = new JPanel();
		panel.setLayout(new BoxLayout(panel, BoxLayout.Y_AXIS));
		
		JPanel centerPanel = mkFeedbackPanel();
		JPanel statusPanel = mkStatusPanel();

		int h = statusPanel.getPreferredSize().height;
		int w = centerPanel.getPreferredSize().width;
		statusPanel.setPreferredSize(new Dimension(w, h));
		statusPanel.setMaximumSize(new Dimension(w, h));
		statusPanel.setMinimumSize(new Dimension(w, h));
		
		panel.add(statusPanel);
		panel.add(mkFeedbackPanel());
		panel.add(mkButtonPanel());
		
		return panel;
	}
	
	JPanel mkButtonPanel() {
		
		monitorButton_ = new JButton(BUTTON_LABEL_START);
		monitorButton_.addActionListener(new ActionListener() {
			@Override
			public void actionPerformed(ActionEvent e) {
				String command = e.getActionCommand();
				if (command.equalsIgnoreCase(BUTTON_LABEL_START)) {
					monitorStart();
				} else {
					monitorStop();
				}				
			}
		});
		JPanel buttonPanel = new JPanel();
		buttonPanel.add(monitorButton_);
		
		return buttonPanel;
	}
	
	JPanel mkStatusPanel() {
		JPanel statusPanel = new JPanel();
		statusPanel.setBorder(BorderFactory.createTitledBorder("Status"));
		statusPanel.setLayout(new BoxLayout(statusPanel, BoxLayout.X_AXIS));
		servoLabel_ = new StatusLabel("servo", grayBallIcon_);
		motionLabel_ = new StatusLabel("motion", grayBallIcon_);
		statusPanel.add(servoLabel_);
		statusPanel.add(motionLabel_);
		
		return statusPanel;
	}
	
	JPanel mkFeedbackPanel() {
		JPanel panel = new JPanel();
		panel.setBorder(BorderFactory.createTitledBorder("Feedback"));
		panel.setLayout(new BoxLayout(panel, BoxLayout.Y_AXIS));
		
		JPanel centerPanel = new JPanel();
		centerPanel.setLayout(new BoxLayout(centerPanel, BoxLayout.X_AXIS));
		String[] l1 = {"J1", "J2", "J3", "J4", "J5", "J6", "J7", "ry", "x", "y", "th"};
//		String[] l2 = {"X", "Y", "Z", "roll", "pitch", "yaw", "elbow"};
		String[] l2 = {"x", "y", "z", "r11", "r12", "r13", "r21", "r22", "r23",
		"r31", "r32", "r33", "elb"};	
		String[] l3 = {"J1", "J2", "J3", "J4", "J5", "J6", "J7", "--", "--", "--", "--"};
		
		jointRefPanel_ = new FieldListPanel("R_Joint", l1) {
			@Override
			public String format(double d) {
				return nf_.format(d);
			}
		};

		jointRefPanel2_ = new FieldListPanel("L_Joint", l3) {
			@Override
			public String format(double d) {
				return nf_.format(d);
			}
		};

		carRefPanel_ = new FieldListPanel("R_Cartesian", l2) {
			@Override
			public String format(double d) {
				return nf_.format(d);
			}
		};
		carRefPanel2_ = new FieldListPanel("L_Cartesian", l2) {
			@Override
			public String format(double d) {
				return nf_.format(d);
			}
		};

		centerPanel.add(jointRefPanel_);
		centerPanel.add(jointRefPanel2_);
		centerPanel.add(carRefPanel_);
		centerPanel.add(carRefPanel2_);

		JPanel southPanel = new JPanel();
		southPanel.add(resultView);
		
		panel.add(centerPanel);
		panel.add(southPanel);
		
		return panel;
	}
	
	public void monitorStart() {
		SwingUtilities.invokeLater(new Runnable() {
			@Override
			public void run() {
				monitorButton_.setText(BUTTON_LABEL_STOP);
			}
		});
		timer.start();
	}
	
	public void monitorStop() {
		SwingUtilities.invokeLater(new Runnable() {
			@Override
			public void run() {
				monitorButton_.setText(BUTTON_LABEL_START);
			}
		});
		timer.stop();
	}
	
	class TimerAction implements ActionListener {
		@Override
		public void actionPerformed(ActionEvent e) {
			
			DoubleSeqHolder jPosition = new DoubleSeqHolder(); 
			double[] position = new double[7];
			jPosition.value = position;

			DoubleSeqHolder jLPosition = new DoubleSeqHolder(); 
			double[] lposition = new double[7];
			jLPosition.value = lposition;	
			
			DoubleSeqHolder lPosition = new DoubleSeqHolder(); 
			double[] lpos = new double[3];
			lPosition.value = lpos;
			
			CarPosWithElbowHolder cPosition = new CarPosWithElbowHolder();
			double[][] carPos = new double[3][4];
			CarPosWithElbow cPosElbow = new CarPosWithElbow();
			cPosElbow.carPos = carPos;
			cPosition.value = cPosElbow;

			CarPosWithElbowHolder cLPosition = new CarPosWithElbowHolder();
			double[][] lcarPos = new double[3][4];
			CarPosWithElbow lcPosElbow = new CarPosWithElbow();
			lcPosElbow.carPos = lcarPos;
			cLPosition.value = lcPosElbow;
			
//			double[] d1 = new double[7];
			double[] d1 = new double[11];
			double[] d2 = new double[13];
			double[] d3 = new double[11];
			double[] d4 = new double[13];
			
			try {
				imInterface_.getFeedbackPosJoint(UnitType.RIGHT_ARM, jPosition);
				
				for (int i = 0; i < 7; i++) {
					d1[i] = jPosition.value[i];
				}

				imInterface_.getFeedbackPosJoint(UnitType.LUMBER, lPosition);
				d1[7] = lPosition.value[1];

				jointRefPanel_.setValue(d1);

				
				imInterface_.getFeedbackPosCartesian(RightLeft.RIGHT, FrameType.ORIGINAL_FRAME, cPosition);
				d2[0] = cPosition.value.carPos[0][3];
				d2[1] = cPosition.value.carPos[1][3];
				d2[2] = cPosition.value.carPos[2][3];
				
				int k = 3;
				for (int i = 0; i < 3; i++) {
					for (int j = 0; j < 3; j++) {
						d2[k] = cPosition.value.carPos[i][j];
						k++;
					}
				}
				d2[k] = cPosition.value.elbow;
				carRefPanel_.setValue(d2);
				
				imInterface_.getFeedbackPosJoint(UnitType.LEFT_ARM, jLPosition);
				for (int i = 0; i < 7; i++) {
					d3[i] = jLPosition.value[i];
				}
				
				jointRefPanel2_.setValue(d3);
				
				imInterface_.getFeedbackPosCartesian(RightLeft.LEFT, FrameType.ORIGINAL_FRAME, cLPosition);
				d4[0] = cLPosition.value.carPos[0][3];
				d4[1] = cLPosition.value.carPos[1][3];
				d4[2] = cLPosition.value.carPos[2][3];
				
				int l = 3;
				for (int i = 0; i < 3; i++) {
					for (int j = 0; j < 3; j++) {
						d4[l] = cLPosition.value.carPos[i][j];
						l++;
					}
				}
				d4[k] = cLPosition.value.elbow;
				carRefPanel2_.setValue(d4);
				
				boolean servo = imInterface_.isServoOn();
				if (servo) {
					servoLabel_.setIcon(greenBallIcon_);
				} else {
					servoLabel_.setIcon(grayBallIcon_);
				}
				
				boolean moving = imInterface_.isMoving();
				if (moving) {
					motionLabel_.setIcon(greenBallIcon_);
				} else {
					motionLabel_.setIcon(grayBallIcon_);
				}
			} catch(Exception ex) {
				System.err.println(ex);
				return;
			}
			
		}
	}
	
}
