package baseclasses;

/*
 *  Adapted from code for Computer Integrated Surgery I - Fall 2014
 *  Original Authors: 
 *  Alex Mathews, amathews@jhu.edu
 *  Pranav Lakshminarayanan, plakshm1@jhu.edu
 */

import java.math.RoundingMode;
import java.text.DecimalFormat;
import org.apache.commons.math3.linear.BlockRealMatrix;
import java.awt.Color;
import java.util.ArrayList;

/**
 *
 * @author Alex, Pranav
 * Point class 
 */
public class Point {
    
    //Global Variables========================================================//
    
    /**
     * X coordinate of point in 3D space.
     */
    private final double xCor;
    
    /**
     * Y coordinate of point in 3D space.
     */
    private final double yCor;
    
    /**
     * Z coordinate of point in 3D space.
     */
    private final double zCor;
    
    
    /**
     * Z coordinate of point in 3D space.
     */
    private final double[] rgb;
    
    /**
     * Z coordinate of point in 3D space.
     */
    private final float[] vertColor;
    
    /**
     * Default value for coordinates gives origin.
     */
    private final double defaultValue = 0.0;
    
    /**
     * Working in 3 space.
     */
    private final int dimension = 3;
    
    /**
     * Neighbors of the Point.
     */
    private ArrayList<Point> neighbors;
    
    /**
     * Neighbors of the Point.
     */
    private ArrayList<Integer> neighborsInd;
    //========================================================================//
    
    
    //Constructors============================================================//
    
    /**
     * Point - Default constructor for a point. 
     * Defaults to origin. 
     */
    public Point() {
        this.xCor = this.defaultValue;
        this.yCor = this.defaultValue;
        this.zCor = this.defaultValue;
        this.vertColor = null;
        this.rgb = null;
        this.neighbors = new ArrayList<Point>();
        this.neighborsInd = new ArrayList<Integer>();
    }
    
    /**
     * Point - Point constructor for points given as variable. 
     * @param xCorParam - X coordinate in 3D space.
     * @param yCorParam - Y coordinate in 3D space.
     * @param zCorParam - Z coordinate in 3D space.
     */
    public Point(double xCorParam, double yCorParam, double zCorParam) {
        this.xCor = xCorParam;
        this.yCor = yCorParam;
        this.zCor = zCorParam;
        double[] colors = {0.0, 0.0, 0.0};
        this.rgb = colors;
        this.vertColor = this.hsbColorConvert(0.0, 0.0, 0.0);
        this.neighbors = new ArrayList<Point>();
        this.neighborsInd = new ArrayList<Integer>();
    }
    
    /**
     * Point - Point constructor for points given as variable. 
     * @param xCorParam - X coordinate in 3D space.
     * @param yCorParam - Y coordinate in 3D space.
     * @param zCorParam - Z coordinate in 3D space.
     * @param rIn - X coordinate in 3D space.
     * @param gIn - Y coordinate in 3D space.
     * @param bIn - Z coordinate in 3D space.
     */
    public Point(double xCorParam, double yCorParam, double zCorParam, 
            double rIn, double gIn, double bIn) {
        this.xCor = xCorParam;
        this.yCor = yCorParam;
        this.zCor = zCorParam;
        double[] colors = {rIn, gIn, bIn};
        this.rgb = colors;
        this.vertColor = this.hsbColorConvert(rIn, gIn, bIn);
        this.neighbors = new ArrayList<Point>();
        this.neighborsInd = new ArrayList<Integer>();
    }
    //========================================================================//
    
    
    //Accessors===============================================================//
    
    /**
     * getCoor - returns an array of coordinates.
     * @return array of doubles.
     * index 0 - x coordinate.
     * index 1 - y coordinate.
     * index 2 - z coordinate.
     */
    public double[] getCoor() {
        double[] coor = new double[this.dimension];
        coor[0] = this.xCor;
        coor[1] = this.yCor;
        coor[2] = this.zCor;
        return coor;
    }
   
    /**
     * Access x coordinate.
     * @return x coordinate
     */
    public double getX() {
        return this.xCor;
    }
    
    /**
     * Access y coordinate.
     * @return y coordinate
     */
    public double getY() {
        return this.yCor;
    }
  
    /**
     * Access z coordinate.
     * @return z coordinate
     */
    public double getZ() {
        return this.zCor;
    }
    
    /**
     * Return the associated color of this vertex.
     * @return a Java Color object.
     */
    public float[] getColor() {
        return this.vertColor;
    }
    
    /**
     * Return the associated color of this vertex.
     * @return a Java Color object.
     */
    public double[] getRGBColor() {
        return this.rgb;
    }
    
    /**
     * Return the neighbors of this point.
     * @return an ArrayList of Point objects.
     */
    public ArrayList<Point> getNeighbors() {
        return this.neighbors;
    }
    
    /**
     * Accessor for neighbors.
     * @param neigh ArrayList of Point objects.
     */
    public void setNeighbors(ArrayList<Point> neigh) {
        this.neighbors = neigh;
    }
    
    /**
     * Add a neighbor to this Point object.
     * @param p point to be added.
     */
    public void addNeighbor(Point p) {
        if (!this.neighbors.contains(p)) {
            this.neighbors.add(p);
        }
    }
    
    /**
     * Return the neighbors of this point.
     * @return an ArrayList of Point objects.
     */
    public ArrayList<Integer> getNeighborsInd() {
        return this.neighborsInd;
    }
    
    /**
     * Accessor for neighbors.
     * @param neigh ArrayList of Point objects.
     */
    public void setNeighborsInd(ArrayList<Integer> neigh) {
        this.neighborsInd = neigh;
    }
    
    /**
     * Add a neighbor to this Point object.
     * @param p point to be added.
     */
    public void addNeighborInd(Integer p) {
        if (!this.neighborsInd.contains(p)) {
            this.neighborsInd.add(p);
        }
    }
    
    //========================================================================//
    
    
    //Static Methods==========================================================//
    
    /** 
     * Add a point to another.
     * @param a first point to be added
     * @param b second point to be added
     * @return shifted point
     */    
    public static Point add(Point a, Point b) {
        return new Point(a.getX() + b.getX(), 
                a.getY() + b.getY(), 
                a.getZ() + b.getZ());
    }

    /** 
     * Subtract a point from another.
     * @param a first point to be added
     * @param b second point to be added
     * @return shifted point
     */
    public static Point subtract(Point a, Point b) {
        return new Point(a.getX() - b.getX(), 
                a.getY() - b.getY(), 
                a.getZ() - b.getZ());
    }

    /** 
     * Scales point with scalar multiplication.
     * @param a Point to be scaled
     * @param s scalar to be used
     * @return scaled point
     */    
    public static Point scale(Point a, double s) {
        return new Point(a.getX() * s, a.getY() * s, a.getZ() * s);
    }

    /** 
     * Element-wise scaling of points.
     * @param a first factor
     * @param b second factor
     * @return product point
     */    
    public static Point multiply(Point a, Point b) {
        return new Point(a.getX() * b.getX(), 
                a.getY() * b.getY(), 
                a.getZ() * b.getZ());
    }
    
    /** 
     * Element-wise division of points.
     * @param a numerator
     * @param b denominator
     * @return product point
     */    
    public static Point divide(Point a, Point b) {
        return new Point(a.getX() / b.getX(), 
                a.getY() / b.getY(), 
                a.getZ() / b.getZ());
    }

    /** 
     * Dot product of two points.
     * @param a first factor
     * @param b second factor
     * @return product point
     */    
    public static double dot(Point a, Point b) {
        double dp = a.getX() * b.getX()
                + a.getY() * b.getY()
                + a.getZ() * b.getZ();
        return dp;
    }
    
    //========================================================================//
    
    
    //Methods=================================================================//
     
    /** 
     * Scales point with element-wise division.
     * @param a point to be divided into this
     * @return scaled point
     */    
    public Point invScale(Point a) {
        return this.invScale(a.getX(), a.getY(), a.getZ());
    }
    
    /** 
     * Scales point with element-wise division.
     * If dividing by 0, scale by 1.
     * @param xDiv change in x coordinate
     * @param yDiv change in y coordinate
     * @param zDiv change in z coordinate
     * @return shifted point
     */    
    public Point invScale(double xDiv, double yDiv, double zDiv) {
        double xd = xDiv;
        double yd = yDiv;
        double zd = zDiv;
        
        if (xDiv == 0) {
            xd = 1;
        }
        if (yDiv == 0) {
            yd = 1;
        }
        if (zDiv == 0) {
            zd = 1;
        }
        
        return new Point(this.xCor / xd, this.yCor / yd, this.zCor / zd);
    }

    /**
     * toString - to print out a point.
     * @return String of x,y,z
     */
    @Override
    public String toString() {
        DecimalFormat df = new DecimalFormat("####.##");
        df.setRoundingMode(RoundingMode.DOWN);
        String printable; 
        printable = "\nCoordinates: " + df.format(this.xCor) + ", " 
                + df.format(this.yCor) + ", " + df.format(this.zCor);
        printable += "\nNeighbors: " + this.neighborsInd.toString();
        return printable;
    }
    
    /**
     * Pretty white space formatted toString.
     * @return a pretty string.
     */
    public String prettyToString() {
        DecimalFormat df = new DecimalFormat("###0.00");
        df.setRoundingMode(RoundingMode.UP);
        String[] coord = new String[]{df.format(this.xCor), 
            df.format(this.yCor), df.format(this.zCor)};
        for (int i = 0; i < 3; i++) {
            int len = 8 - coord[i].length();
            for (int j = 0; j < len; j++) {
                coord[i] = " " + coord[i];
            }
        }
        return coord[0] + " " + coord[1] + " " + coord[2];
    }
    
    /**
     * equals - determine if two points are equivalent.
     * @param testPoint - other point to compare to.
     * @return boolean, true if equal.
     */
    @Override
    public boolean equals(Object testPoint) {
        if (testPoint instanceof Point) {        
            double[] thisPoints = this.getCoor();
            double[] testPoints = ((Point) testPoint).getCoor();
            for (int i = 0; i < this.dimension; i++) {
                if (thisPoints[i] != testPoints[i]) {
                    return false;
                }
            }
            return true;
        } else {
            return false;
        }

    }
    
    /**
     * equalsWTolerance - determine if two points are equivalent, within 
     * a specified tolerance.
     * @param testPoint - other point to compare to.
     * @param tol - double with tolerance of equality.
     * @return boolean, true if equal.
     */
    public boolean equalsWTolerance(Point testPoint, double tol) {
        double[] thisPoints = this.getCoor();
        double[] testPoints = testPoint.getCoor();
        for (int i = 0; i < this.dimension; i++) {
            if (Math.abs(thisPoints[i] - testPoints[i]) > tol) {
                return false;
            }
        }
        return true;
    }
    /**
     * hashCode - method to return a hash code for the point.
     * @return integer hash code.
     */
    @Override
    public int hashCode() {
        int hash = 29;
        hash = 37 * hash + (int) (Double.doubleToLongBits(this.xCor) 
                ^ (Double.doubleToLongBits(this.xCor) >>> 37));
        hash = 29 * hash + (int) (Double.doubleToLongBits(this.yCor) 
                ^ (Double.doubleToLongBits(this.yCor) >>> 29));
        hash = 37 * hash + (int) (Double.doubleToLongBits(this.zCor) 
                ^ (Double.doubleToLongBits(this.zCor) >>> 9));
        hash = 29 * hash + this.dimension;
        return hash;
    }
    //========================================================================//
        
    
    //Static Utility Methods==================================================//
    
    /**
     * calculateAvgPt - find the average point.
     * assuming 3 space.
     * @param points - points to be averaged.
     * @return - Point that represents the average.
     */
    public static Point calculateAvgPt(Point[] points) {
        double xbar = 0.0;
        double ybar = 0.0;
        double zbar = 0.0;
        
        //add all x,y,z coordinates
        for (Point point : points) {
            double[] coor = point.getCoor();
            xbar += coor[0];
            ybar += coor[1];
            zbar += coor[2];
        }
        
        //divide by length
        xbar /= points.length;
        ybar /= points.length;
        zbar /= points.length;
        
        return new Point(xbar, ybar, zbar);
    }

    /**
     * relativePoint - return relative point from one point for centroid calc.
     * @param pointA - start point, usually centroid.
     * @param pointB - end point, usually new point.
     * @return pointB relative to pointA.
     */
    public static Point relativePoint(Point pointA, Point pointB) {
        double[] aPoints = pointA.getCoor();
        double[] bPoints = pointB.getCoor();
        
        //calculate b coordinates realtive to b coordinates
        double xPrime = bPoints[0] - aPoints[0];
        double yPrime = bPoints[1] - aPoints[1];
        double zPrime = bPoints[2] - aPoints[2];
        
        //return new relative point using x,y,zPrime
        Point relPoint; 
        relPoint = new Point(xPrime, yPrime, zPrime); 
        return relPoint;
    }
    
    /**
     * vectorBetweenPoints - find the relPoint from b to b in 3 space.
     * @param pA - first point, start.
     * @param pB - second point, end.
     * @return 3x1 relPoint 
     */
    public static BlockRealMatrix vectorBetweenPoints(Point pA, Point pB) {
        BlockRealMatrix vector = new BlockRealMatrix(3, 1);
        double[] aPoints = pA.getCoor();
        double[] bPoints = pB.getCoor();
        vector.setEntry(0, 0, bPoints[0] - aPoints[0]); 
        vector.setEntry(1, 0, bPoints[1] - aPoints[1]); 
        vector.setEntry(2, 0, bPoints[2] - aPoints[2]); 
        return vector;
    }
    
    /**
     * Find the magnitude of a vector from origin to the Point.
     * @param pt Point to calculate for.
     * @return magnitude of vector as a double.
     */
    public static double magnitude(Point pt) {
        double mag = Math.pow(pt.getX(), 2.0) + Math.pow(pt.getY(), 2.0) 
                + Math.pow(pt.getZ(), 2.0);
        return Math.sqrt(mag);
    }
    
    /**
     * Find the distance between two Points.
     * @param a first Point.
     * @param b second Point.
     * @return the distance between Points.
     */
    public static double distance(Point a, Point b) {
        double[] aCor = a.getCoor();
        double[] bCor = b.getCoor();
        double x = Math.pow(bCor[0] - aCor[0], 2);
        double y = Math.pow(bCor[1] - aCor[1], 2);
        double z = Math.pow(bCor[2] - aCor[2], 2);
        return Math.sqrt(x + y + z);
    }
    
    /**
     * Color conversion.
     * @param rIn red value.
     * @param gIn green value.
     * @param bIn blue value.
     * @return a float of HSB values.
     */
    private float[] hsbColorConvert(double rIn, double gIn, double bIn) {
        float[] tVertColor = new float[3];
        tVertColor = Color.RGBtoHSB((int) (255.0 * rIn), 
                (int) (255.0 * gIn), (int) (255.0 * bIn), null);
        tVertColor[0] = tVertColor[0] * 360.0f;
        tVertColor[1] = tVertColor[1] * 100.0f;
        tVertColor[2] = tVertColor[2] * 100.0f;
        return tVertColor;
    }
}
