package jp.co.yaskawa.rtc.integratedMotion.module.motion;

import jp.co.yaskawa.rtc.integratedMotion.controller.RightLeft;
import jp.co.yaskawa.rtc.integratedMotion.module.IntegratedMotionInterpolator;
import jp.co.yaskawa.rtc.integratedMotion.module.Parameters;
import jp.co.yaskawa.rtc.integratedMotion.module.motion.MoveLinearCartesianAbs.Status;
import jp.co.yaskawa.rtc.integratedMotion.module.motion.kinematics.HgTransMatrix;
import jp.co.yaskawa.rtc.integratedMotion.module.state.RTEmuContext;

public class MoveLinearCartesianAbsWithRed<T> extends AbstractMotion<T> {

	IntegratedMotionInterpolator unit_;
	
	double demandTransVelocity_;   //!< 要求並進速度
	double demandRotateVelocity_;  //!< 要求回転速度
	double demandTransAccel_;      //!< 要求並進加速度
	double demandRotateAccel_;     //!< 要求回転加速度
	double demandAccelTime_ = 1.0; //!< 要求加速時間
	double demandDecelTime_ = 1.0;	//!< 要求減速時間
	
	boolean isHoldOtherArm_;       //!< 反対アームの維持
	
    boolean isMoving_ = false;
	RightLeft rightleft_;         //!< 制御対象が右アームか左アームか
	HgTransMatrix startHgMat_ = new HgTransMatrix();  //!< 現在位置の同時変換行列
	HgTransMatrix endHgMat_ = new HgTransMatrix();    //!< 目標位置の同時変換行列
	
    double translationDistance_; // x,y,zのうちの最大移動距離
    double rotationDistance_; // alpha, beta, gamma, elbowのうちの再大回転角

	double elapsedTime_ = 0.0;
	double time_ = 0.0;
	Status status_;
	double segmentPeriod_;
    
	public MoveLinearCartesianAbsWithRed(IntegratedMotionInterpolator u, T type) {
		COMMAND = type;
		unit_ = u;
	}
    
	@Override
	public void setParameters(Parameters l) {
		// TODO Auto-generated method stub

	}

	public void doSegment(RTEmuContext<T> c) throws MotionAlarmException {
		// TODO Auto-generated method stub

	}

	public boolean isMoving() {
		// TODO Auto-generated method stub
		return isMoving_;
	}

	public void setCommand(UnitCommand<T> c) {
		System.out.println("MoveLinearCartesianAbsWithRed: setCommand");
		
		elapsedTime_ = 0.0;
		time_ = 0.0;
		status_ = Status.normal;
		segmentPeriod_ = unit_.getSegmentPeriod();
		
		double[] params = c.getParams();

		if (params[0] >= 0) {
			rightleft_ = RightLeft.RIGHT; 
		} else {
			rightleft_ = RightLeft.LEFT; 			
		}
		
		endHgMat_.m00 = params[1];
		endHgMat_.m01 = params[2];
		endHgMat_.m02 = params[3];
		endHgMat_.m03 = params[4];
		endHgMat_.m10 = params[5];
		endHgMat_.m11 = params[6];
		endHgMat_.m12 = params[7];
		endHgMat_.m13 = params[8];
		endHgMat_.m20 = params[9];
		endHgMat_.m21 = params[10];
		endHgMat_.m22 = params[11];
		endHgMat_.m23 = params[12];
		endHgMat_.m30 = 0.0;
		endHgMat_.m31 = 0.0;
		endHgMat_.m32 = 0.0;
		endHgMat_.m33 = 1.0;
			
		demandTransVelocity_ = params[13];
		demandRotateVelocity_ = params[14];
		demandTransAccel_ = params[15];
		demandRotateAccel_ = params[16];

		if (params[36] >= 0) {
			isHoldOtherArm_ = true; 
		} else {
			isHoldOtherArm_ = false; 
		}

	}

}
