/**
 * 
 */
package jp.mydns.masahase.abaqus.weldline;

import jp.mydns.masahase.InherentStrain.ElementType;
import jp.mydns.masahase.InherentStrain.InherentStrainType;
import jp.mydns.masahase.InherentStrain.NodeType;
import jp.mydns.masahase.sdes.common.FemFunc;

/**
 * 直線を溶接線
 * @author MASA.H
 * 
 */
public class StraightLine implements IWeldLine {
	/**
	 * コンストラクタ
	 * 
	 * @param pedata
	 *            固有ひずみデータ
	 * @param StartPos
	 *            溶接開始点
	 * @param EndPos
	 *            溶接終了点
	 * @param RefPos
	 *            溶接開始点より見て直上の点
	 */
	public StraightLine(InherentStrainType pedata, double[] StartPos,
			double[] EndPos, double[] RefPos) {
		this.StartPos = StartPos.clone();
		this.pedata = pedata;
		vec_a = new double[3];
		for (int i = 0; i < vec_a.length; i++) {
			vec_a[i] = EndPos[i] - StartPos[i];
		}
		double tmp = 0;
		for (double a : vec_a) {
			tmp += a * a;
		}
		tmp = Math.sqrt(tmp);
		for (int i = 0; i < vec_a.length; i++) {
			vec_a[i] = vec_a[i] / tmp;
		}
		length = tmp;
		vec_b = new double[3];
		for (int i = 0; i < vec_b.length; i++) {
			vec_b[i] = RefPos[i] - StartPos[i];
		}
		tmp = 0;
		for (double a : vec_b) {
			tmp += a * a;
		}
		tmp = Math.sqrt(tmp);
		for (int i = 0; i < vec_b.length; i++) {
			vec_b[i] = vec_b[i] / tmp;
		}
		vec_c = new double[3];
		vec_c[0] = vec_a[1] * vec_b[2] - vec_a[2] * vec_b[1];
		vec_c[1] = vec_a[2] * vec_b[0] - vec_a[0] * vec_b[2];
		vec_c[2] = vec_a[0] * vec_b[1] - vec_a[1] * vec_b[0];
		tmp = 0;
		for (double a : vec_c) {
			tmp += a * a;
		}
		tmp = Math.sqrt(tmp);
		for (int i = 0; i < vec_c.length; i++) {
			vec_c[i] = vec_c[i] / tmp;
		}
		vec_b[0] = vec_c[1] * vec_a[2] - vec_c[2] * vec_a[1];
		vec_b[1] = vec_c[2] * vec_a[0] - vec_c[0] * vec_a[2];
		vec_b[2] = vec_c[0] * vec_a[1] - vec_c[1] * vec_a[0];
		tmp = 0;
		for (double a : vec_b) {
			tmp += a * a;
		}
		tmp = Math.sqrt(tmp);
		for (int i = 0; i < vec_b.length; i++) {
			vec_b[i] = vec_b[i] / tmp;
		}
	}

	private final double[] StartPos;
	private final InherentStrainType pedata;
	private final double[] vec_a, vec_b, vec_c;
	protected final double length;

	/**
	 * 座標変換
	 * 
	 * @param x
	 * @param y
	 * @param z
	 * @return 溶接線を基準とした局所座標系での座標 double[3]
	 */
	protected double[] changeCoordinate(double x, double y, double z) {
		double[] ret = new double[3];
		double[] tmp = new double[3];
		// 平行移動
		ret[0] = x - StartPos[0];
		ret[1] = y - StartPos[1];
		ret[2] = z - StartPos[2];
		// 基底ベクトルの変換
		tmp[2] = vec_b[0] * ret[0] + vec_b[1] * ret[1] + vec_b[2] * ret[2];
		tmp[1] = vec_a[0] * ret[0] + vec_a[1] * ret[1] + vec_a[2] * ret[2];
		tmp[0] = vec_c[0] * ret[0] + vec_c[1] * ret[1] + vec_c[2] * ret[2];

		return tmp;
	}

	/*
	 * (非 Javadoc)
	 * 
	 * @see jp.mydns.masahase.abaqus.weldline.IWeldLine#getPE(double, double,
	 * double)
	 */
	@Override
	public double[] getPE(double x, double y, double z) {
		double[] ret = new double[6];
		double[] coord = changeCoordinate(x, y, z);
		for (int i = 0; i < ret.length; i++) {
			ret[i] = 0;
		}
		getPE_(ret, coord);
		ret = changeECoordinate(ret);
		return ret;
	}

	/**
	 * @param ret
	 * @param coord
	 */
	protected void getPE_(double[] ret, double[] coord) {
		if (pedata.getXMax() != null && coord[0] > pedata.getXMax()) {
			return;
		}
		if (pedata.getYMax() != null && coord[2] > pedata.getYMax()) {
			return;
		}
		if (pedata.getXMin() != null && coord[0] < pedata.getXMin()) {
			return;
		}
		if (pedata.getYMin() != null && coord[2] < pedata.getYMin()) {
			return;
		}

		if (coord[1] <= length && coord[1] >= 0) {
			// 指示された座標を含む要素の探索
			// 一番近いノードの探索
			NodeType near = pedata.getNodes().getNode().get(0);
			double r0 = Math.sqrt((near.getX() - coord[0])
					* (near.getX() - coord[0]) + (near.getY() - coord[2])
					* (near.getY() - coord[2]));
			for (NodeType nd : pedata.getNodes().getNode()) {
				double r1 = Math.sqrt((nd.getX() - coord[0])
						* (nd.getX() - coord[0]) + (nd.getY() - coord[2])
						* (nd.getY() - coord[2]));
				if (r1 < r0) {
					r0 = r1;
					near = nd;
				}
			}
			{
				double tmp = coord[1];
				coord[1] = coord[2];
				coord[2] = tmp;
			}
			// 一番近いノードを含む要素から対象の要素を絞り込む
			ElementType near_ele = null;
			for (ElementType et : pedata.getElements().getElement()) {
				if (et.getNodeId().contains(Integer.valueOf(near.getId()))) {
					if (near_ele == null) {
						near_ele = et;
					} else {
						double[] r1, r2;
						double tmp1, tmp2;
						double[][] value = new double[4][2];
						for (int i = 0; i < et.getNodeId().size(); i++) {
							NodeType nd = pedata.getNodes().getNode().get(
									et.getNodeId().get(i) - 1);
							if (nd.getId() != et.getNodeId().get(i)) {
								for (NodeType nt : pedata.getNodes().getNode()) {
									if (nt.getId() == et.getNodeId().get(i)) {
										nd = nt;
										break;
									}
								}
							}
							value[i][0] = nd.getX();
							value[i][1] = nd.getY();
						}
						if (value.length == 3) {
							r1 = jp.mydns.masahase.sdes.common.FemFunc
									.invertFormFunction3Triangle(coord, value);
						} else if (value.length == 4) {
							r1 = jp.mydns.masahase.sdes.common.FemFunc
									.invertFormFunction4Square(coord, value);
						} else {
							throw new java.lang.RuntimeException();
						}
						for (int i = 0; i < near_ele.getNodeId().size(); i++) {
							NodeType nd = pedata.getNodes().getNode().get(
									near_ele.getNodeId().get(i) - 1);
							if (nd.getId() != near_ele.getNodeId().get(i)) {
								for (NodeType nt : pedata.getNodes().getNode()) {
									if (nt.getId() == near_ele.getNodeId().get(
											i)) {
										nd = nt;
										break;
									}
								}
							}
							value[i][0] = nd.getX();
							value[i][1] = nd.getY();
						}
						if (value.length == 3) {
							r2 = jp.mydns.masahase.sdes.common.FemFunc
									.invertFormFunction3Triangle(coord, value);
						} else if (value.length == 4) {
							r2 = jp.mydns.masahase.sdes.common.FemFunc
									.invertFormFunction4Square(coord, value);
						} else {
							throw new java.lang.RuntimeException();
						}
						tmp1 = Math.sqrt(r1[0] * r1[0] + r1[1] * r1[1]);
						tmp2 = Math.sqrt(r2[0] * r2[0] + r2[1] * r2[1]);
						if (tmp1 < tmp2) {
							near_ele = et;
						}
					}
				}
			}
			if (near_ele != null) {
				double r[] = new double[3];
				double[][] value = new double[near_ele.getNodeId().size()][2];
				for (int i = 0; i < near_ele.getNodeId().size(); i++) {
					NodeType nd = pedata.getNodes().getNode().get(
							near_ele.getNodeId().get(i) - 1);
					if (nd.getId() != near_ele.getNodeId().get(i)) {
						for (NodeType nt : pedata.getNodes().getNode()) {
							if (nt.getId() == near_ele.getNodeId().get(i)) {
								nd = nt;
								break;
							}
						}
					}
					value[i][0] = nd.getX();
					value[i][1] = nd.getY();
				}
				if (value.length == 3) {
					r = jp.mydns.masahase.sdes.common.FemFunc
							.invertFormFunction3Triangle(coord, value);
				} else if (value.length == 4) {
					r = jp.mydns.masahase.sdes.common.FemFunc
							.invertFormFunction4Square(coord, value);
				} else {
					throw new java.lang.RuntimeException();
				}
				double tmp = Math.sqrt(r[0] * r[0] + r[1] * r[1]);
				if (tmp > 4.0) {
					near_ele = null;
				}
			} else {
				ret[0] = near.getStrainData().getX();
				ret[1] = near.getStrainData().getY();
				ret[2] = near.getStrainData().getZ();
				ret[3] = near.getStrainData().getXy();
				ret[4] = near.getStrainData().getXz();
				ret[5] = near.getStrainData().getYz();
			}
			if (near_ele != null)
			// 要素内での面積座標を算出し形状関数による内挿補間を行う
			{
				if (near_ele.getType().equalsIgnoreCase("C2D4")) {
					double[][] value = new double[4][2];
					for (int i = 0; i < 4; i++) {
						NodeType nd = pedata.getNodes().getNode().get(
								near_ele.getNodeId().get(i) - 1);
						if (nd.getId() != near_ele.getNodeId().get(i)) {
							for (NodeType nt : pedata.getNodes().getNode()) {
								if (nt.getId() == near_ele.getNodeId().get(i)) {
									nd = nt;
									break;
								}
							}
						}
						value[i][0] = nd.getX();
						value[i][1] = nd.getY();
					}
					double[] u = jp.mydns.masahase.sdes.common.FemFunc
							.invertFormFunction4Square(coord, value);
					double[][] pe = new double[6][4];
					for (int i = 0; i < 4; i++) {
						NodeType nd = pedata.getNodes().getNode().get(
								near_ele.getNodeId().get(i) - 1);
						if (nd.getId() != near_ele.getNodeId().get(i)) {
							for (NodeType nt : pedata.getNodes().getNode()) {
								if (nt.getId() == near_ele.getNodeId().get(i)) {
									nd = nt;
									break;
								}
							}
						}
						pe[0][i] = nd.getStrainData().getX();
						pe[1][i] = nd.getStrainData().getY();
						pe[2][i] = nd.getStrainData().getZ();
						pe[3][i] = nd.getStrainData().getXy();
						pe[4][i] = nd.getStrainData().getXz();
						pe[5][i] = nd.getStrainData().getYz();
					}
					for (int i = 0; i < 6; i++) {
						ret[i] = jp.mydns.masahase.sdes.common.FemFunc
								.formFunction4Square(u[0], u[1], pe[i]);
						if (ret[i] > 1 || ret[i] < -1
								|| ret[i] == java.lang.Double.NaN) {
							throw new java.lang.RuntimeException();
						}
					}
				} else if (near_ele.getType().equalsIgnoreCase("C2D3")) {
					double[][] value = new double[3][2];
					for (int i = 0; i < 3; i++) {
						NodeType nd = pedata.getNodes().getNode().get(
								near_ele.getNodeId().get(i) - 1);
						if (nd.getId() != near_ele.getNodeId().get(i)) {
							for (NodeType nt : pedata.getNodes().getNode()) {
								if (nt.getId() == near_ele.getNodeId().get(i)) {
									nd = nt;
									break;
								}
							}
						}
						value[i][0] = nd.getX();
						value[i][1] = nd.getY();
					}
					double[] u = jp.mydns.masahase.sdes.common.FemFunc
							.invertFormFunction3Triangle(coord, value);
					double[][] pe = new double[6][3];
					for (int i = 0; i < 3; i++) {
						NodeType nd = pedata.getNodes().getNode().get(
								near_ele.getNodeId().get(i) - 1);
						if (nd.getId() != near_ele.getNodeId().get(i)) {
							for (NodeType nt : pedata.getNodes().getNode()) {
								if (nt.getId() == near_ele.getNodeId().get(i)) {
									nd = nt;
									break;
								}
							}
						}
						pe[0][i] = nd.getStrainData().getX();
						pe[1][i] = nd.getStrainData().getY();
						pe[2][i] = nd.getStrainData().getZ();
						pe[3][i] = nd.getStrainData().getXy();
						pe[4][i] = nd.getStrainData().getXz();
						pe[5][i] = nd.getStrainData().getYz();
					}
					for (int i = 0; i < 6; i++) {
						ret[i] = jp.mydns.masahase.sdes.common.FemFunc
								.formFunction3Triangle(u[0], u[1], pe[i]);
						if (ret[i] > 1.0 || ret[i] < -1.0
								|| ret[i] == java.lang.Double.NaN) {
							throw new java.lang.RuntimeException();
						}
					}
				} else {
					throw new RuntimeException(
							"There is no support element type:"
									+ near_ele.getType() + "Elemid:"
									+ near_ele.getId());
				}
			}
		}
	}

	/**
	 * ひずみの座標変換. ひずみテンソルe<sub>ij</sub>が既知の時、x<sub>i</sub>=g<sub>ji</sub>*x
	 * '<sub>j</sub>となるようg<sub>ji</sub>を置くと
	 * e'<sub>ij</sub>=g<sub>il</sub>*g<sub>jm</sub>*e<sub>lm</sub>
	 * 
	 * @param data
	 *            double[6]
	 * @return double[6]
	 */
	protected double[] changeECoordinate(double[] data) {
		double[] ret = new double[6];
		double g11, g12, g13, g21, g22, g23, g31, g32, g33;
		g11 = vec_c[0];
		g12 = vec_c[1];
		g13 = vec_c[2];
		g21 = vec_a[0];
		g22 = vec_a[1];
		g23 = vec_a[2];
		g31 = vec_b[0];
		g32 = vec_b[1];
		g33 = vec_b[2];
		ret[0] = g11 * g11 * data[0] + g12 * g12 * data[1] + g13 * g13
				* data[2] + g11 * g12 * data[3] + g11 * g13 * data[4] + g12
				* g13 * data[5];
		ret[1] = g21 * g21 * data[0] + g22 * g22 * data[1] + g23 * g23
				* data[2] + g21 * g22 * data[3] + g21 * g23 * data[4] + g22
				* g23 * data[5];
		ret[2] = g31 * g31 * data[0] + g32 * g32 * data[1] + g33 * g33
				* data[2] + g31 * g32 * data[3] + g31 * g33 * data[4] + g32
				* g33 * data[5];
		ret[3] = g11 * g21 * data[0] + g12 * g22 * data[1] + g13 * g23
				* data[2] + (g11 * g22 + g21 * g12) * data[3]
				+ (g11 * g23 + g21 * g13) * data[4] + (g12 * g23 + g22 * g13)
				* data[5];
		ret[4] = g11 * g31 * data[0] + g12 * g32 * data[1] + g13 * g33
				* data[2] + (g11 * g32 + g31 * g12) * data[3]
				+ (g11 * g33 + g31 * g13) * data[4] + (g12 * g33 + g32 * g13)
				* data[5];
		ret[5] = g21 * g31 * data[0] + g22 * g32 * data[1] + g23 * g33
				* data[2] + (g21 * g32 + g31 * g22) * data[3]
				+ (g21 * g33 + g31 * g23) * data[4] + (g22 * g33 + g32 * g23)
				* data[5];

		return ret;
	}

	// for debug
	public static void main(String[] args) {
		double[] start, end, ref;
		start = new double[3];
		end = new double[3];
		ref = new double[3];
		System.out.println("1)座標変換の確認");
		System.out.println("1-1)そのまま");
		start[0] = 0.0;
		start[1] = 0.0;
		start[2] = 0.0;
		end[0] = 0.0;
		end[1] = 10.0;
		end[2] = 0.0;
		ref[0] = 0.0;
		ref[1] = 0.0;
		ref[2] = 1.0;
		StraightLine sl1 = new StraightLine(null, start, end, ref);
		double[] tmp = sl1.changeCoordinate(1.0, 0.0, 0.0);
		System.out
				.printf("(1.0,0.0,0.0)->(%f,%f,%f)\n", tmp[0], tmp[1], tmp[2]);
		tmp = sl1.changeCoordinate(-1.0, 0.0, 0.0);
		System.out.printf("(-1.0, 0.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl1.changeCoordinate(0.0, 2.0, 0.0);
		System.out.printf("(0.0, 2.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl1.changeCoordinate(0.0, -2.0, 0.0);
		System.out.printf("(0.0, -2.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl1.changeCoordinate(0.0, 0.0, 3.0);
		System.out.printf("(0.0, 0.0, 3.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl1.changeCoordinate(0.0, 0.0, -3.0);
		System.out.printf("(0.0, 0.0, -3.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		System.out.println("1-2)元のx軸がy軸になるように回転");
		start[0] = 0.0;
		start[1] = 0.0;
		start[2] = 0.0;
		end[0] = 10.0;
		end[1] = 0.0;
		end[2] = 0.0;
		ref[0] = 0.0;
		ref[1] = 0.0;
		ref[2] = 1.0;
		StraightLine sl2 = new StraightLine(null, start, end, ref);
		tmp = sl2.changeCoordinate(1.0, 0.0, 0.0);
		System.out
				.printf("(1.0,0.0,0.0)->(%f,%f,%f)\n", tmp[0], tmp[1], tmp[2]);
		tmp = sl2.changeCoordinate(-1.0, 0.0, 0.0);
		System.out.printf("(-1.0, 0.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl2.changeCoordinate(0.0, 2.0, 0.0);
		System.out.printf("(0.0, 2.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl2.changeCoordinate(0.0, -2.0, 0.0);
		System.out.printf("(0.0, -2.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl2.changeCoordinate(0.0, 0.0, 3.0);
		System.out.printf("(0.0, 0.0, 3.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl2.changeCoordinate(0.0, 0.0, -3.0);
		System.out.printf("(0.0, 0.0, -3.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		System.out.println("1-3)元のx軸がz軸になるように回転");
		start[0] = 0.0;
		start[1] = 0.0;
		start[2] = 0.0;
		end[0] = 0.0;
		end[1] = 10.0;
		end[2] = 0.0;
		ref[0] = 10.0;
		ref[1] = 0.0;
		ref[2] = 0.0;
		StraightLine sl3 = new StraightLine(null, start, end, ref);
		tmp = sl3.changeCoordinate(1.0, 0.0, 0.0);
		System.out
				.printf("(1.0,0.0,0.0)->(%f,%f,%f)\n", tmp[0], tmp[1], tmp[2]);
		tmp = sl3.changeCoordinate(-1.0, 0.0, 0.0);
		System.out.printf("(-1.0, 0.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl3.changeCoordinate(0.0, 2.0, 0.0);
		System.out.printf("(0.0, 2.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl3.changeCoordinate(0.0, -2.0, 0.0);
		System.out.printf("(0.0, -2.0, 0.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl3.changeCoordinate(0.0, 0.0, 3.0);
		System.out.printf("(0.0, 0.0, 3.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		tmp = sl3.changeCoordinate(0.0, 0.0, -3.0);
		System.out.printf("(0.0, 0.0, -3.0)->(%f,%f,%f)\n", tmp[0], tmp[1],
				tmp[2]);
		System.out.println("2)ひずみの座標変換の確認");
		System.out.println("溶接線方向が0.1");
		System.out.println("板幅方向が0.2");
		System.out.println("板厚方向が0.3");
		double[] tmp_e = new double[6];
		double[] tmp_e2;
		double tmp_e1 = FemFunc.calcEstimateStrain(tmp_e);
		tmp_e[0] = 0.2;
		tmp_e[1] = 0.1;
		tmp_e[2] = 0.3;
		tmp_e[3] = 0.4;
		tmp_e[4] = 0.5;
		tmp_e[5] = 0.6;
		System.out.println("2-1)溶接線がx軸と同一でxy平面上");
		tmp_e2 = sl2.changeECoordinate(tmp_e);
		System.out.printf("(%f,%f,%f)\n", tmp_e2[0], tmp_e2[1], tmp_e2[2]);
		// System.out.printf("Error:%f%%\n",Math.abs((tmp_e1-FemFunc.calcEstimateStrain(tmp_e2)/tmp_e1))*100);
		System.out.println("2-2)溶接線がy軸と同一でxy平面上");
		tmp_e2 = sl1.changeECoordinate(tmp_e);
		System.out.printf("(%f,%f,%f)\n", tmp_e2[0], tmp_e2[1], tmp_e2[2]);
		// System.out.printf("Error:%f%%\n",Math.abs((tmp_e1-FemFunc.calcEstimateStrain(tmp_e2)/tmp_e1))*100);
		System.out.println("2-3)溶接線がy軸と同一でyz平面上");
		tmp_e2 = sl3.changeECoordinate(tmp_e);
		System.out.printf("(%f,%f,%f)\n", tmp_e2[0], tmp_e2[1], tmp_e2[2]);
		// System.out.printf("Error:%f%%\n",Math.abs((tmp_e1-FemFunc.calcEstimateStrain(tmp_e2)/tmp_e1))*100);

	}
}
