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.RTMath;
import jp.co.yaskawa.rtc.integratedMotion.module.motion.MoveLinearCartesianAbs.Status;
import jp.co.yaskawa.rtc.integratedMotion.module.motion.MoveLinearCartesianAbs.TranslationData;
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[] currentPostureMain_;
	double[] currentPostureOther_;

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

	double[] deltaPos_;
	double[] absDeltaPos_;
	int maxAxisNum_;
    double[] ratioArray_;

    double[] demandVelocity_;
	double[] demandAccel_;
	double targetVelocity_ = 0.0;

    double elbow_cmd_;
	double lumbarRx_cmd_;
	double lumbarRy_cmd_;
	double lumbarRz_cmd_;
	double vehicleX_cmd_;
	double vehicleY_cmd_;
	double vehicleR_cmd_;

	double elbow_fb_;
	double lumbarRx_fb_;
	double lumbarRy_fb_;
	double lumbarRz_fb_;
	double vehicleX_fb_;
	double vehicleY_fb_;
	double vehicleR_fb_;
	
	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;

	TranslationData transData_ = new TranslationData();

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

		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[17];
		lumbarRx_cmd_ = params[18];
		lumbarRy_cmd_ = params[19];
		lumbarRz_cmd_ = params[20];
		vehicleX_cmd_ = params[21];
		vehicleY_cmd_ = params[22];
		vehicleR_cmd_ = params[23];

		elapsedTime_ = 0.0;
		time_ = 0.0;
		status_ = Status.normal;
		segmentPeriod_ = unit_.getSegmentPeriod();
		
        deltaPos_ = new double[7];
		absDeltaPos_ = new double[7];
		
		//! 冗長軸の差分値
        deltaPos_[0] = elbow_cmd_ - elbow_fb_; 
        deltaPos_[1] = lumbarRx_cmd_ - lumbarRx_fb_;
        deltaPos_[2] = lumbarRy_cmd_ - lumbarRy_fb_;
        deltaPos_[3] = lumbarRz_cmd_ - lumbarRz_fb_;
        deltaPos_[4] = vehicleX_cmd_ - vehicleX_fb_;
        deltaPos_[5] = vehicleY_cmd_ - vehicleY_fb_;
        deltaPos_[6] = vehicleR_cmd_ - vehicleR_fb_;
       
        for (int i = 0; i < 7; i++) {
            absDeltaPos_[i] = Math.abs(deltaPos_[i]);
        }
        
		demandVelocity_ = new double[6];
		for (int i=0; i<demandVelocity_.length; i++) {
			demandVelocity_[i] = params[i+24];
		}

		demandAccel_ = new double[6];
		for (int i=0; i<demandAccel_.length; i++) {
			demandAccel_[i] = params[i+30];
		}		

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

		if(rightleft_.equals(RightLeft.RIGHT)) {
			currentPostureMain_ = unit_.getCartesianFeedbackPos(RightLeft.RIGHT, FrameType.ORIGINAL_FRAME);
			currentPostureOther_ = unit_.getCartesianFeedbackPos(RightLeft.LEFT, FrameType.ORIGINAL_FRAME);
		}
		else {
			currentPostureMain_ = unit_.getCartesianFeedbackPos(RightLeft.LEFT, FrameType.ORIGINAL_FRAME);
			currentPostureOther_ = unit_.getCartesianFeedbackPos(RightLeft.RIGHT, FrameType.ORIGINAL_FRAME);			
		}

		startHgMat_.m03 = currentPostureMain_[0];
		startHgMat_.m13 = currentPostureMain_[1];
		startHgMat_.m23 = currentPostureMain_[2];
		startHgMat_.m00 = currentPostureMain_[3];
		startHgMat_.m01 = currentPostureMain_[4];
		startHgMat_.m02 = currentPostureMain_[5];
		startHgMat_.m10 = currentPostureMain_[6];
		startHgMat_.m11 = currentPostureMain_[7];
		startHgMat_.m12 = currentPostureMain_[8];
		startHgMat_.m20 = currentPostureMain_[9];
		startHgMat_.m21 = currentPostureMain_[10];
		startHgMat_.m22 = currentPostureMain_[11];
		startHgMat_.m30 = 0.0;
		startHgMat_.m31 = 0.0;
		startHgMat_.m32 = 0.0;
		startHgMat_.m33 = 1.0;

		transData_.computeTranslationData(startHgMat_, endHgMat_);

	}

	class TranslationData {
		double	x_ = 0.0;
		double y_ = 0.0;
		double z_ = 0.0;
		double theta_ = 0.0;
		double psi_ = 0.0;
		double sinPHI_ = 0.0;
		double cosPHI_ = 0.0;
		
		void computeTranslationData(HgTransMatrix start, HgTransMatrix end) {
			
			// 並進移動量
			double dx = end.m03 - start.m03;
			double dy = end.m13 - start.m13;
			double dz = end.m23 - start.m23;
			
			x_ = start.m00 * dx + start.m10 * dy + start.m20 * dz;
			y_ = start.m01 * dx + start.m11 * dy + start.m21 * dz;
			z_ = start.m02 * dx + start.m12 * dy + start.m22 * dz;
			
	        // 回転移動量
	        double a1 = start.m01 * end.m02 + start.m11 * end.m12 + start.m21 * end.m22;
	        double a2 = start.m00 * end.m02 + start.m10 * end.m12 + start.m20 * end.m22;
	        double a3 = start.m02 * end.m02 + start.m12 * end.m12 + start.m22 * end.m22;
	        double a4 = start.m00 * end.m00 + start.m10 * end.m10 + start.m20 * end.m20;
	        double a5 = start.m01 * end.m00 + start.m11 * end.m10 + start.m21 * end.m20;
	        double a6 = start.m02 * end.m00 + start.m12 * end.m10 + start.m22 * end.m20;
	        double a7 = start.m00 * end.m01 + start.m10 * end.m11 + start.m20 * end.m21;
	        double a8 = start.m01 * end.m01 + start.m11 * end.m11 + start.m21 * end.m21;
	        double a9 = start.m02 * end.m01 + start.m12 * end.m11 + start.m22 * end.m21;
	        
	        theta_ = RTMath.atan3(Math.sqrt(a1 * a1 + a2 * a2), a3);
	        
	        double phi = Math.atan2(a1, a2);
	        sinPHI_ = Math.sin(phi);
	        cosPHI_ = Math.cos(phi);
	        
	        double sct0 = Math.sin(theta_);
	        double sct1 = Math.cos(theta_);
	        double vh = 1.0 - sct1;
	        
	        double b1 = -sinPHI_ * cosPHI_ * vh;
	        double b2 =  cosPHI_ * cosPHI_ * vh + sct1;
	        double b3 =  sinPHI_ * sct0;
	        
	        double sp = b1 * a4 + b2 * a5 - b3 * a6;
	        double cp = b1 * a7 + b2 * a8 - b3 * a9;
	        
	        psi_ = Math.atan2(sp, cp);
	        
	        if (false) {
	        	System.out.println();
	        	System.out.println("setTraslationData");
	        	System.out.println("\tdx: " + dx + " dy: " + dy + " dz: " + dz);
	        	System.out.println("\tx: " + x_ + " y: " + y_ + " z: " + z_);
	        	System.out.println("\ttheta: " + theta_);
	        	System.out.println("\tpsi: " + psi_);
	        	System.out.println("\tphi: " + phi);
	        	System.out.println("\tsinPHI: " + sinPHI_ + " cosPHI: " + cosPHI_);
	        	
	        }
		}
		
	}	

}
