package com.one.vector;

public class AuxMath
{
	public static final int FP_SHIFT = 16;
	public static final int FP_MASK = (1 << FP_SHIFT) - 1;
	
	public static final double PI = 3.1415927f;
	
	protected static final double PI_L = 1.2246469E-16f;
	protected static final double ATAN_0_5H = 0.4636476f;
	protected static final double ATAN_0_5L = 2.2698777E-17f;
	protected static final double ATAN_1_5H = 0.98279375f;
	protected static final double ATAN_1_5L = 1.3903311E-17f;
	protected static final double AT0 = 0.33333334f;
	protected static final double AT1 = -0.2f;
	protected static final double AT2 = 0.14285715f;
	protected static final double AT3 = -0.111111104f;
	protected static final double AT4 = 0.09090887f;
	protected static final double AT5 = -0.07691876f;
	protected static final double AT6 = 0.06661073f;
	protected static final double AT7 = -0.058335703f;
	protected static final double AT8 = 0.04976878f;
	protected static final double AT9 = -0.03653157f;
	protected static final double AT10 = 0.01628582f;
	protected static final double TWO_29 = 5.3687091E8f;
	protected static final double TWO_60 = 1.1529215E18f;
	protected static final double TWO_66 = 7.3786976E19f;
	
	protected static double[] sintab;
	protected static int[] sintabfp;
	
	static
	{
		sintab = new double[360];
		sintabfp = new int[360];

		double mult = (double)(1 << FP_SHIFT);
		
		for(int i = 0; i < 360; i++)
		{
			sintab[i] = (double)Math.sin(Math.toRadians((double)i));
			sintabfp[i] = (int)Math.floor((double)sintab[i] * mult + 0.5);
		}
	}
	
	public static double sin(int a)
	{
		while(a < 0)
		{
			a += 360;
		}
		
		return sintab[a % 360];
	}
	
	public static double cos(int a)
	{
		while(a < 0)
		{
			a += 360;
		}
		
		return sintab[(a + 90) % 360];
	}

	public static int sinfp(int a)
	{
		while(a < 0)
		{
			a += 360;
		}

		return sintabfp[a % 360];
	}

	public static int cosfp(int a)
	{
		while(a < 0)
		{
			a += 360;
		}

		return sintabfp[(a + 90) % 360];
	}
	
	public static double sin(double x, double y)
	{
		return y / (double)Math.sqrt(x * x + y * y);
	}
	
	public static double cos(double x, double y)
	{
		return x / (double)Math.sqrt(x * x + y * y);
	}
	
	public static double[] rotate(double x, double y, double x0, double y0, int a)
	{
		double[] r = new double[2];
		
		r[0] = x0 + (x - x0) * cos(a) + (y - y0) * sin(a);
		r[1] = y0 - (x - x0) * sin(a) + (y - y0) * cos(a);
		
		return r;
	}

	public static void rotateRGB(int[] src, int[] dest, int width, int height, int a)
	{
		for(int i = 0; i < dest.length; i++)
		{
			dest[i] = 0;
		}

		int x0 = width / 2;
		int y0 = height / 2;

		int x, y;
		int rx, ry;

		for(x = 0; x < width; x++)
		{
			for(y = 0; y < height; y++)
			{
				rx = x0 + (((x - x0) * cosfp(a)) >> FP_SHIFT) + (((y - y0) * sinfp(a)) >> FP_SHIFT);
				ry = y0 - (((x - x0) * sinfp(a)) >> FP_SHIFT) + (((y - y0) * cosfp(a)) >> FP_SHIFT);

				//System.out.println("(" + x + ", " + y + ") -> (" + rx + ", " + ry + ")");

				if(rx >= 0 && rx < width && ry >= 0 && ry < height)
				{
					dest[rx + ry * width] = src[x + y * width];
				}
			}
		}
	}
	
	public static double power(double x, int n)
	{
		double r = 1.0f;
		
		for(int i = 0; i < n; i++)
		{
			r *= x;
		}
		
		return r;
	}

	public static int gcd(int m, int n)
	{
		if(m == 0 || m == n)
		{
			return n;
		}
		else if(n == 0)
		{
			return m;
		}
		else if(m == 1 || n == 1)
		{
			return 1;
		}
		else if((m & 1) == 0)
		{
			if((n & 1) == 0)
			{
				return gcd(m >>> 1, n >>> 1) << 1;
			}
			else
			{
				return gcd(m >>> 1, n);
			}
		}
		else
		{
			if((n & 1) == 0)
			{
				return gcd(m, n >>> 1);
			}
			else
			{
				return gcd(n, Math.abs(m - n));
			}
		}
	}
	
	public static int averageColor(int[] points)
	{
		int a = 0, r = 0, g = 0, b = 0;
		
		for(int i = 0; i < points.length; i++)
		{
			a += (points[i] >> 24) & 0xFF;
			r += (points[i] >> 16) & 0xFF;
			g += (points[i] >> 8) & 0xFF;
			b += points[i] & 0xFF;
		}
		
		a /= points.length;
		r /= points.length;
		g /= points.length;
		b /= points.length;
		
		return (a << 24) | (r << 16) | (g << 8) | b;
	}
	
	public static int averageColorNoAlpha(int[] points)
	{
		int r = 0, g = 0, b = 0;
		
		for(int i = 0; i < points.length; i++)
		{
			r += (points[i] >> 16) & 0xFF;
			g += (points[i] >> 8) & 0xFF;
			b += points[i] & 0xFF;
		}
		
		r /= points.length;
		g /= points.length;
		b /= points.length;
		
		return (r << 16) | (g << 8) | b;
	}
	
	/*
	public static Image scaleImage(Image image, int destwidth, int destheight)
	{
		Image scaled = Image.createImage(destwidth, destheight);
		Graphics g = scaled.getGraphics();
		
		int relwidth = (width << FP_SHIFT) / destwidth;
		int relheight = (height << FP_SHIFT) / destheight;
		
		int scanwidth = relwidth >>> FP_SHIFT;
		int scanheight = relheight >>> FP_SHIFT;
		
		if(scanwidth < 1)
		{
			scanwidth = 1;
		}
		
		if(scanheight < 1)
		{
			scanheight = 1;
		}
		
		int[] points = new int[scanwidth * scanheight];
		
		int pointwidth = destwidth / width;
		int pointheight = destheight / height;
		
		if(pointwidth < 1)
		{
			pointwidth = 1;
		}
		
		if(pointheight < 1)
		{
			pointheight = 1;
		}
		
		for(int x = 0; x < sdata[2]; x += pointwidth)
		{
			for(int y = 0; y < sdata[3]; y += pointheight)
			{
				image.getRGB(points, 0, scanwidth, (x * relwidth) >>> FP_SHIFT, (y * relheight) >>> FP_SHIFT, scanwidth, scanheight);
				
				g.setColor(averageColorNoAlpha(points));
				g.fillRect(x, y, pointwidth, pointheight);
			}
		}
		
		return scaled;
	}
	*/
	
	public static int[] linearArrayRGB(ArrayData v, int count)
	{
		if(count < 2)
		{
			return new int[0];
		}
		
		int[] colors = new int[count--];
		
		int cr, cg, cb;
		
		int sr = (v.inta >> 16) & 0xFF;
		int sg = (v.inta >> 8) & 0xFF;
		int sb = v.inta & 0xFF;
		
		int er = (v.intb >> 16) & 0xFF;
		int eg = (v.intb >> 8) & 0xFF;
		int eb = v.intb & 0xFF;
		
		int i, j;
		
		for(i = 0; i <= count; i++)
		{
			j = count - i;
			
			cr = (sr * j + er * i) / count;
			cg = (sg * j + eg * i) / count;
			cb = (sb * j + eb * i) / count;
			
			colors[i] = (cr << 16) | (cg << 8) | cb;
		}
		
		return colors;
	}
	
	public static int[] harmonicArrayRGB(ArrayData v, int count)
	{
		int[] colors = new int[count--];
		
		int cr, cg, cb;
		
		int sr = (v.inta >> 16) & 0xFF;
		int sg = (v.inta >> 8) & 0xFF;
		int sb = v.inta & 0xFF;
		
		int er = (v.intb >> 16) & 0xFF;
		int eg = (v.intb >> 8) & 0xFF;
		int eb = v.intb & 0xFF;
		
		double delta = 1.0f / (double)count;
		int a, b;
		
		for(int i = 0; i <= count; i++)
		{
			b = (int)(delta * i * 90);
			a = 90 - b;
			
			cr = (int)(sr * sin(a) * sin(a) + er * sin(b) * sin(b));
			cg = (int)(sg * sin(a) * sin(a) + eg * sin(b) * sin(b));
			cb = (int)(sb * sin(a) * sin(a) + eb * sin(b) * sin(b));
			
			colors[i] = (cr << 16) | (cg << 8) | cb;
		}
		
		return colors;
	}
	
	public static double[] specificArrayFloat(ArrayData v, int count)
	{
		double[] r = new double[count--];
		
		double delta = 1.0f / (double)count;
		double s, t;
		
		if(v.type == ArrayData.HARMONIC)
		{
			for(int i = 0; i <= count; i++)
			{
				t = delta * i;
				s = 1.0f - t;
				
				r[i] = (v.vala * s + v.valb * t) + (v.diva * s + v.divb * t) * sin(v.inta + (int)(v.intb * t));
			}
		}
		else if(v.type == ArrayData.CUBIC)
		{
			for(int i = 0; i <= count; i++)
			{
				t = delta * i;
				s = v.vala * (1.0f - t) + v.valb * t;
				
				r[i] = ((v.diva * s + v.divb) * s + v.divc) * s + v.divd;
			}
		}
		else
		{
			for(int i = 0; i <= count; i++)
			{
				t = delta * i;
				s = 1.0f - t;
				
				r[i] = v.vala * s + v.valb * t;
			}
		}
		
		return r;
	}
	
	public static int[] specificArrayInt(ArrayData v, int count)
	{
		int[] r = new int[count--];
		
		double delta = 1.0f / (double)count;
		double s, t;
		
		if(v.type == ArrayData.HARMONIC)
		{
			for(int i = 0; i <= count; i++)
			{
				t = delta * i;
				s = 1.0f - t;
				
				r[i] = (int)((v.vala * s + v.valb * t) + (v.diva * s + v.divb * t) * sin(v.inta + (int)(v.intb * t)));
			}
		}
		else if(v.type == ArrayData.CUBIC)
		{
			for(int i = 0; i <= count; i++)
			{
				t = delta * i;
				s = v.vala * (1.0f - t) + v.valb * t;
				
				r[i] = (int)(((v.diva * s + v.divb) * s + v.divc) * s + v.divd);
			}
		}
		else
		{
			for(int i = 0; i <= count; i++)
			{
				t = delta * i;
				s = 1.0f - t;
				
				r[i] = (int)(v.inta * s + v.intb * t);
			}
		}
		
		return r;
	}
	
	public static double[] plotThickLine(double x1, double y1, double x2, double y2, double thickness, boolean processend)
	{
		double[] r = new double[processend ? 14 : 8]; // tl, bl, tr, br, e1, e2
		
		thickness /= 2;
		
		double dx, dy;
		
		if(x1 != x2 || y1 != y2)
		{
			dx = thickness * sin(x2 - x1, y2 - y1);
			dy = thickness * cos(x2 - x1, y2 - y1);
		}
		else
		{
			dx = dy = 0;
		}
		
		r[0] = x1 - dx;
		r[1] = y1 + dy;
		
		r[2] = x1 + dx;
		r[3] = y1 - dy;
		
		r[4] = x2 - dx;
		r[5] = y2 + dy;
		
		r[6] = x2 + dx;
		r[7] = y2 - dy;
		
		if(processend)
		{
			r[8] = x1;
			r[9] = y1;
			r[10] = x2;
			r[11] = y2;
			r[12] = thickness;
			r[13] = thickness;
		}
		
		return r;
	}
	
	public static double[] plotRotRect(double x, double y, double width, double height, int angle)
	{
		double[] r = new double[8]; // x1, y1, x2, y2, x3, y3, x4, y4
		
		width /= 2;
		height /= 2;
		
		double wc = width * cos(angle);
		double ws = width * sin(angle);
		double hc = height * cos(angle);
		double hs = height * sin(angle);
		
		r[0] = x - wc - hs;
		r[1] = y + ws - hc;
		
		r[2] = x - wc + hs;
		r[3] = y + ws + hc;
		
		r[4] = x + wc - hs;
		r[5] = y - ws - hc;
		
		r[6] = x + wc + hs;
		r[7] = y - ws + hc;
		
		return r;
	}
	
	public static int linearFunctionRGB(ArrayData v, double t)
	{
		double s = 1.0f - t;
		
		return ((int)(((v.inta >> 16) & 0xFF) * s + ((v.intb >> 16) & 0xFF) * t) << 16) |
		       ((int)(((v.inta >> 8) & 0xFF) * s + ((v.intb >> 8) & 0xFF) * t) << 8) |
		       (int)((v.inta & 0xFF) * s + (v.intb & 0xFF) * t);
	}
	
	public static double specificFunctionFloat(ArrayData v, double t)
	{
		if(v.type == ArrayData.HARMONIC)
		{
			return (v.vala * (1.0f - t) + v.valb * t) + (v.diva * (1.0f - t) + v.divb * t) * sin(v.inta + (int)(v.intb * t));
		}
		else if(v.type == ArrayData.CUBIC)
		{
			t = v.vala * (1.0f - t) + v.valb * t;
			return ((v.diva * t + v.divb) * t + v.divc) * t + v.divd;
		}
		else
		{
			return v.vala * (1.0f - t) + v.valb * t;
		}
	}
	
	public static int specificFunctionInt(ArrayData v, double t)
	{
		if(v.type == ArrayData.HARMONIC)
		{
			return (int)((v.vala * (1.0f - t) + v.valb * t) + (v.diva * (1.0f - t) + v.divb * t) * sin(v.inta + (int)(v.intb * t)));
		}
		else if(v.type == ArrayData.CUBIC)
		{
			t = v.vala * (1.0f - t) + v.valb * t;
			return (int)(((v.diva * t + v.divb) * t + v.divc) * t + v.divd);
		}
		else
		{
			return (int)(v.inta * (1.0f - t) + v.intb * t);
		}
	}
	
	public static double cubicMagnitude(ArrayData v)
	{
		double y1 = ((v.diva * v.vala + v.divb) * v.vala + v.divc) * v.vala;
		double y2 = ((v.diva * v.valb + v.divb) * v.valb + v.divc) * v.valb;
		
		if(v.diva != 0)
		{
			double d = 4 * v.divb * v.divb - 12 * v.diva * v.divc;
			
			if(d < 0)
			{
				return Math.abs(y2 - y1);
			}
			else
			{
				d = (double)Math.sqrt(d);
				
				double e1 = -(v.divb + d) / 3 / v.diva;
				double e2 = -(v.divb - d) / 3 / v.diva;
				
				double y3 = ((v.diva * e1 + v.divb) * e1 + v.divc) * e1;
				double y4 = ((v.diva * e2 + v.divb) * e2 + v.divc) * e2;
				
				return Math.max(y1, Math.max(y2, Math.max(y3, y4))) - Math.min(y1, Math.min(y2, Math.min(y3, y4)));
			}
		}
		else if(v.divb != 0)
		{
			double e1 = -v.divc / 2 / v.divb;
			
			double y3 = ((v.diva * e1 + v.divb) * e1 + v.divc) * e1;
			
			return Math.max(y1, Math.max(y2, y3)) - Math.min(y1, Math.min(y2, y3));
		}
		else if(v.divc != 0)
		{
			return Math.abs(y2 - y1);
		}
		else
		{
			return 0.0f;
		}
	}
	
	public static ArrayData fitCubicInRange(ArrayData v, double y1, double y2)
	{
		double k = (y2 - y1) / cubicMagnitude(v);
		double c = y1 - ((v.diva * v.vala + v.divb) * v.vala + v.divc) * v.vala;
		
		return new ArrayData(v.vala, v.valb, v.diva * k, v.divb * k, v.divc * k, c);
	}
	
	public static double[][] plotBezierCurve(double[][] points, int count)
	{
		if(points.length < 2 || count <= 0)
		{
			return new double[0][2];
		}
		
		int i, n;
		
		double[] coefs = new double[points.length];
		coefs[0] = 1.0f;
		
		for(n = 1; n < coefs.length; n++)
		{
			for(i = n; i > 0; i--)
			{
				coefs[i] = coefs[i] + coefs[i - 1];
			}
		}
		
		double delta = 1.0f / (double)count;
		double param;
		double value;
		
		int maxpower = points.length - 1;
		
		double[][] data = new double[count--][2];
		
		data[0][0] = points[0][0];
		data[0][1] = points[0][1];
		data[data.length - 1][0] = points[points.length - 1][0];
		data[data.length - 1][1] = points[points.length - 1][1];
		
		for(i = 1; i < count; i++)
		{
			data[i][0] = 0.0f;
			data[i][1] = 0.0f;
			
			param = delta * i;
			
			for(n = 0; n <= maxpower; n++)
			{
				value = coefs[n] * power(param, n) * power(1.0f - param, maxpower - n);
				
				data[i][0] += points[n][0] * value;
				data[i][1] += points[n][1] * value;
			}
		}
		
		return data;
	}
	
	public static double[][] plotPolygon(double x, double y, double width, double height, int count, int rot, int angle)
	{
		if(count <= 0)
		{
			return new double[0][2];
		}
		
		double[][] data = new double[count][2];
		
		int sec = 360 / count;
		int cur = rot;
		
		width /= 2;
		height /= 2;
		
		for(int i = 0; i < count; i++)
		{
			data[i] = rotate(x + width * cos(cur), y - height * sin(cur), x, y, angle);
			cur += sec;
		}
		
		return data;
	}
	
	public static int hypot(int x, int y)
	{
		return (int)Math.sqrt(x * x + y * y);
	}
	
	public static int normAngle(int angle)
	{
		while(angle < 0)
		{
			angle += 360;
		}
		
		angle %= 360;
		
		if(angle == 0)
		{
			angle = 360;
		}
		
		return angle;
	}
	
	public static int toDegrees(double rads)
	{
		return (int)((rads * 180) / PI);
	}
	
	public static double atan(double x)
	{
		double lo;
		double hi;
		
		boolean negative = x < 0;
		
		if(negative)
		{
			x = -x;
		}
		
		if(x >= TWO_66)
		{
			return negative ? -PI / 2 : PI / 2;
		}
		
		if(!(x >= 0.4375)) // |x|<7/16, or NaN.
		{
			if(!(x >= 1 / TWO_29)) // Small, or NaN.
			{
				return negative ? -x : x;
			}
			
			lo = hi = 0;
		}
		else if(x < 1.1875)
		{
			if(x < 0.6875) // 7/16<=|x|<11/16.
			{
				x = (2 * x - 1) / (2 + x);
				
				hi = ATAN_0_5H;
				lo = ATAN_0_5L;
			}
			else // 11/16<=|x|<19/16.
			{
				x = (x - 1) / (x + 1);
				
				hi = PI / 4;
				lo = PI_L / 4;
			}
		}
		else if(x < 2.4375) // 19/16<=|x|<39/16.
		{
			x = (x - 1.5f) / (1 + 1.5f * x);
			
			hi = ATAN_1_5H;
			lo = ATAN_1_5L;
		}
		else // 39/16<=|x|<2**66.
		{
			x = -1 / x;
			
			hi = PI / 2;
			lo = PI_L / 2;
		}
		
		// Break sum from i=0 to 10 ATi*z**(i+1) into odd and even poly.
		
		double z = x * x;
		double w = z * z;
		double s1 = z * (AT0 + w * (AT2 + w * (AT4 + w * (AT6 + w * (AT8 + w * AT10)))));
		double s2 = w * (AT1 + w * (AT3 + w * (AT5 + w * (AT7 + w * AT9))));
		
		if(hi == 0)
		{
			return negative ? x * (s1 + s2) - x : x - x * (s1 + s2);
		}
		
		z = hi - ((x * (s1 + s2) - lo) - x);
		
		return negative ? -z : z;
	}
	
	public static double atan2(double y, double x)
	{
		if(x != x || y != y)
		{
			return Double.NaN;
		}
		
		if(x == 1)
		{
			return atan(y);
		}
		
		if(x == Double.POSITIVE_INFINITY)
		{
			if(y == Double.POSITIVE_INFINITY)
			{
				return PI / 4;
			}
			
			if(y == Double.NEGATIVE_INFINITY)
			{
				return -PI / 4;
			}
			
			return 0 * y;
		}
		
		if(x == Double.NEGATIVE_INFINITY)
		{
			if(y == Double.POSITIVE_INFINITY)
			{
				return 3 * PI / 4;
			}
			
			if(y == Double.NEGATIVE_INFINITY)
			{
				return -3 * PI / 4;
			}
			
			return (1 / (0 * y) == Double.POSITIVE_INFINITY) ? PI : -PI;
		}
		
		if(y == 0)
		{
			if(1 / (0 * x) == Double.POSITIVE_INFINITY)
			{
				return y;
			}
			
			return (1 / y == Double.POSITIVE_INFINITY) ? PI : -PI;
		}
		
		if(y == Double.POSITIVE_INFINITY || y == Double.NEGATIVE_INFINITY || x == 0)
		{
			return y < 0 ? -PI / 2 : PI / 2;
		}
		
		double z = Math.abs(y / x); // Safe to do y/x.
		
		if(z > TWO_60)
		{
			z = PI / 2 + 0.5f * PI_L;
		}
		else if(x < 0 && z < 1 / TWO_60)
		{
			z = 0;
		}
		else
		{
			z = atan(z);
		}
		
		if(x > 0)
		{
			return y > 0 ? z : -z;
		}
		
		return y > 0 ? PI - (z - PI_L) : z - PI_L - PI;
	}
}
