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.motion.kinematics.Posture;
import jp.co.yaskawa.rtc.integratedMotion.module.state.RTEmuContext;

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

	IntegratedMotionInterpolator unit_;
	
    double totalTime_;	
	double elapsedTime_ = 0.0;
	double time_ = 0.0;
    double ratio_ = 1.0;
    double prevDTime_ = 0.0;

	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[] currentPos_;         //!< elbow, lumber1-3, vehicle x,y,theta
	double[] targetPos_;         //!< elbow, lumber1-3, vehicle x,y,theta

	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のうちの再大回転角

	Status status_;
	double segmentPeriod_;

	double velocity_ = 0.0;
	double accel_ = 0.0;

    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 {
        elapsedTime_ += segmentPeriod_;
        
        if (totalTime_ < 5 * RTMath.EPSILON || status_ == Status.completed) {
            unit_.setCartesianPos(endHgMat_.toArray()); //!< TODO:右だけじゃなく左も
            isMoving_ = false;
            return;
        }
        
        setTransitions();
        
        double r = time_ / totalTime_;
        double sinRTheta = Math.sin(r * transData_.theta_);
        double cosRTheta = Math.cos(r * transData_.theta_);
        double sinRPsi = Math.sin(r * transData_.psi_);
        double cosRPsi = Math.cos(r * transData_.psi_);
        double vh = 1.0 - cosRTheta;
        double a1 = transData_.sinPHI_ * transData_.sinPHI_ * vh + cosRTheta;
        double a2 = transData_.sinPHI_ * transData_.cosPHI_ * vh;
        double a3 = transData_.cosPHI_ * sinRTheta;
        double a4 = transData_.cosPHI_ * transData_.cosPHI_ * vh + cosRTheta;
        double a5 = transData_.sinPHI_ * sinRTheta;
        
        HgTransMatrix tRaRoHgMat = new HgTransMatrix();
        tRaRoHgMat.m00 =  cosRPsi * a1 - sinRPsi * a2;
        tRaRoHgMat.m01 = -sinRPsi * a1 - cosRPsi * a2;
        tRaRoHgMat.m02 =  a3;
        tRaRoHgMat.m10 = -cosRPsi * a2 + sinRPsi * a4;
        tRaRoHgMat.m11 =  sinRPsi * a2 + cosRPsi * a4;
        tRaRoHgMat.m12 =  a5;
        tRaRoHgMat.m20 = -cosRPsi * a3 - sinRPsi * a5;
        tRaRoHgMat.m21 =  sinRPsi * a3 - cosRPsi * a5;
        tRaRoHgMat.m22 =  cosRTheta;
        
        tRaRoHgMat.m03 = r * transData_.x_;
        tRaRoHgMat.m13 = r * transData_.y_;
        tRaRoHgMat.m23 = r * transData_.z_;
        
        HgTransMatrix tmpMat = (HgTransMatrix)startHgMat_.clone();
        tmpMat.mul(tRaRoHgMat);

        double[] commandPosture = new double[24];
        
        if(rightleft_.equals(RightLeft.RIGHT)) {

        	commandPosture[0] = tmpMat.m03;
            commandPosture[1] = tmpMat.m13;
            commandPosture[2] = tmpMat.m23;
            commandPosture[3] = tmpMat.m00;
            commandPosture[4] = tmpMat.m01;
            commandPosture[5] = tmpMat.m02;
            commandPosture[6] = tmpMat.m10;
            commandPosture[7] = tmpMat.m11;
            commandPosture[8] = tmpMat.m12;
            commandPosture[9] = tmpMat.m20;
            commandPosture[10] = tmpMat.m21;
            commandPosture[11] = tmpMat.m22;        

            for(int i=0; i<12; i++) {
                commandPosture[i+12] = currentPostureOther_[i];                    	
            }
        	
        } else {
            for(int i=0; i<12; i++) {
                commandPosture[i] = currentPostureOther_[i];                    	
            }
        	commandPosture[12] = tmpMat.m03;
            commandPosture[13] = tmpMat.m13;
            commandPosture[14] = tmpMat.m23;
            commandPosture[15] = tmpMat.m00;
            commandPosture[16] = tmpMat.m01;
            commandPosture[17] = tmpMat.m02;
            commandPosture[18] = tmpMat.m10;
            commandPosture[19] = tmpMat.m11;
            commandPosture[20] = tmpMat.m12;
            commandPosture[21] = tmpMat.m20;
            commandPosture[22] = tmpMat.m21;
            commandPosture[23] = tmpMat.m22;        	
        }
                
		velocity_ = velocity_ + accel_ * segmentPeriod_;
		velocity_ = velocity_ > targetVelocity_ ? targetVelocity_ : velocity_;

		double remainPos = Math.abs(targetPos_[maxAxisNum_] - currentPos_[maxAxisNum_]);
		double velocityReducePoint = Math.sqrt( 2 * accel_ * remainPos) - accel_ * segmentPeriod_ * 0.5d;
		velocityReducePoint = velocityReducePoint < 0 ? 0 : velocityReducePoint;
		
		velocity_ = velocity_ > velocityReducePoint ? velocityReducePoint : velocity_;
		
		if (velocity_ * segmentPeriod_ == 0) {
			isMoving_ = false;
			currentPos_= targetPos_;
		} else {
			double incrementPos = 0.0d;
            for (int i = 0; i < currentPos_.length; i++) {
    			incrementPos = velocity_ * ratioArray_[i] * segmentPeriod_;
    			currentPos_[i] = currentPos_[i] + incrementPos;            	
            }
		}
		
		outputPos_ = new double[8];
		outputPos_[0] = currentPos_[4];
		outputPos_[1] = currentPos_[5];
		outputPos_[2] = currentPos_[6];
		outputPos_[3] = currentPos_[1];
		outputPos_[4] = currentPos_[2];
		outputPos_[5] = currentPos_[3];
		
		if(rightleft_ == RightLeft.RIGHT) {
			outputPos_[6] = currentPos_[0];
			outputPos_[7] = currentRedundantPos_[1];
		} else {
			outputPos_[6] = currentRedundantPos_[0];
			outputPos_[7] = currentPos_[0];		
		}
		
		unit_.setRedundantPos(outputPos_);

		unit_.setCartesianPos(commandPosture);		

	}

    void setTransitions() {
        double tmpTime = 0.0;
        double tmpDTime = 0.0;
        double dtOfTarget;
        
//        setSpeedRatio();
        
        dtOfTarget = segmentPeriod_ * ratio_;
        
        double increment = getDtIncrement();
        double decrement = getDtDecrement();
        
        if (dtOfTarget > (prevDTime_ + increment)) {
            tmpDTime = prevDTime_ + increment;
        } else {
            if ((prevDTime_ - decrement) > dtOfTarget) {
                tmpDTime = prevDTime_ - decrement;
            } else {
                tmpDTime = dtOfTarget;
            }
        }
        
        double slowDown = setSlowDown(decrement);
        if (tmpDTime > slowDown) {
            double remainDTime = totalTime_ - time_;
            tmpDTime = (remainDTime < decrement) ? remainDTime : slowDown;
        }
        
        tmpDTime = tmpDTime < 0.0 ? 0.0 : tmpDTime;
        Status status;
        if (tmpDTime > prevDTime_) {
            status = Status.incrementing;
        } else if (tmpDTime < prevDTime_) {
            status = Status.decrementing;
        } else {
            status = Status.normal;
        }
        
        tmpTime = time_ + tmpDTime;
        if (tmpTime >= totalTime_) {
            tmpTime = totalTime_;
            tmpDTime = tmpTime - time_;
            status = Status.completed;
        }
        
        time_ = tmpTime;
        prevDTime_ = tmpDTime;
        status_ = status;
        
    }

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

		double totalTime[] = new double[7];
		
		for (int i=0; i<totalTime.length; i++) {
			
			if((i==5) || (i==6)){
				if(demandVelocity_[i-1] != 0)
					totalTime[i] = absDeltaPos_[i]/demandVelocity_[i-1];
				else
					totalTime[i] = 0.0;					
			}
			else{
				if(demandVelocity_[i] != 0)
					totalTime[i] = absDeltaPos_[i]/demandVelocity_[i];
				else
					totalTime[i] = 0.0;									
			}	
		}

		double maxTime = RTMath.max(totalTime.clone());
        maxAxisNum_ = RTMath.getIndex(maxTime, totalTime);
		
		targetVelocity_ = demandVelocity_[maxAxisNum_];
		accel_ = demandAccel_[maxAxisNum_];

        ratioArray_ = new double[7];
        for (int i = 0; i < ratioArray_.length; i++) {
            ratioArray_[i] = totalTime[i] / totalTime[maxAxisNum_];
            if( deltaPos_[i] <0 )
            {
            	ratioArray_[i] = ratioArray_[i]*(-1);
            }
        }
		
		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_);

		setProfile();
        
        isMoving_ = true;
	}

    void setProfile() {

    	Posture startPosture = new Posture(startHgMat_);
    	Posture endPosture = new Posture(endHgMat_);
    	
        double maxTransDistance = RTMath.max(Math.abs(endHgMat_.m03 - startHgMat_.m03), Math.abs(endHgMat_.m13 - startHgMat_.m13), Math.abs(endHgMat_.m23 - startHgMat_.m23));
        double maxRotateDistance = RTMath.max(Math.abs(endPosture.alpha - startPosture.alpha), Math.abs(endPosture.beta - startPosture.beta), Math.abs(endPosture.gamma - startPosture.gamma));
        
        boolean isTransVelocityZero = demandTransVelocity_ < 5 * RTMath.EPSILON;
        boolean isRotationVelocityZero = demandRotateVelocity_ < 5 * RTMath.EPSILON;
        
        double transTime = isTransVelocityZero ? 0.0 : Math.abs(maxTransDistance / demandTransVelocity_);
        double rotateTime = isRotationVelocityZero ? 0.0 : Math.abs(maxRotateDistance / demandRotateVelocity_);
        
        totalTime_ = transTime > rotateTime ? transTime : rotateTime;
        demandAccelTime_ = transTime > rotateTime ? (demandTransVelocity_/demandTransAccel_) : (demandRotateVelocity_/demandRotateAccel_);
        demandDecelTime_ = demandAccelTime_;
        translationDistance_ = maxTransDistance;
        rotationDistance_ = maxRotateDistance;
        
    }

    double getDtIncrement() {
        double accelTime = demandAccelTime_;
        return accelTime > 0 ? ( segmentPeriod_ * segmentPeriod_ / accelTime) : segmentPeriod_;
    }
    
    double getDtDecrement() {
        double decelTime = demandDecelTime_;
        return decelTime > 0 ? (segmentPeriod_ * segmentPeriod_ / decelTime) : segmentPeriod_;
    }

    double setSlowDown(double dec) {
        return Math.sqrt(2.0 * dec * (totalTime_ - time_)) - (dec * 0.50);
    }

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

}
