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

import jp.co.yaskawa.rtc.integratedMotion.controller.FrameType;
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_;       //!< 反対アームの維持	

	double[] currentRedundantPos_;
	double[] currentPostureR_;
	double[] currentPostureL_;
	double[] currentPosture_ = new double[24];

	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_;
    
    boolean isMoving_ = false;

    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");
		
		segmentPeriod_ = unit_.getSegmentPeriod();
		
		//! 現在の冗長軸位置
		currentRedundantPos_ = unit_.getRedundantPos();
		//! 現在の制御点位置
		currentPostureR_ = unit_.getCartesianFeedbackPos(RightLeft.RIGHT, FrameType.ORIGINAL_FRAME);			
		currentPostureL_ = unit_.getCartesianFeedbackPos(RightLeft.LEFT, FrameType.ORIGINAL_FRAME);
		for(int j=0; j<12; j++) {
			currentPosture_[j] = currentPostureR_[j];
			currentPosture_[j+12] = currentPostureL_[j];
		}
		
		double[] params = c.getParams();

		//! 基準アームは左右どちらか
		if (params[0] >= 0) {
			rightleft_ = RightLeft.RIGHT; 
			elbow_fb_ = currentRedundantPos_[6];
		} else {
			rightleft_ = RightLeft.LEFT;
			elbow_fb_ = currentRedundantPos_[7];
		}
		lumbarRx_fb_ = currentRedundantPos_[3];
		lumbarRy_fb_ = currentRedundantPos_[4];
		lumbarRz_fb_ = currentRedundantPos_[5];
		vehicleX_fb_ = currentRedundantPos_[0];
		vehicleY_fb_ = currentRedundantPos_[1];
		vehicleR_fb_ = currentRedundantPos_[2];

		elbow_cmd_ = params[1];
		lumbarRx_cmd_ = params[2];
		lumbarRy_cmd_ = params[3];
		lumbarRz_cmd_ = params[4];
		vehicleX_cmd_ = params[5];
		vehicleY_cmd_ = params[6];
		vehicleR_cmd_ = params[7];

		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; 
		}

	}

}
