package org.usfirst.frc.team1719.robot.sensors;

import java.util.Comparator;
import java.util.Vector;

import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class TargetVision {
    
    public static class TargetPos {
        public final double distance;
        public final double azimuth;
        public final double altitude;
        public final double targetAngle;
        
        public TargetPos(double _distance, double _azimuth, double _altitude, double _targetAngle) {
            distance = _distance;
            azimuth = _azimuth;
            altitude = _altitude;
            targetAngle = _targetAngle;
        }
    }
    
    private static class Contour implements Comparator<Contour>, Comparable<Contour> {
        final double area1, area2, width, height, posX, posY;
        Contour(double _area1, double _area2, double _width, double _height, double _posX, double _posY) {
            area1 = _area1;
            area2 = _area2;
            width = _width;
            height = _height;
            posX = _posX;
            posY = _posY;
        }
        @Override
        public int compareTo(Contour o) {
            return (int) (o.area1 - this.area1);
        }
        @Override
        public int compare(Contour o1, Contour o2) {
            return (int) (o1.area1 - o2.area1);
        }
        static Vector<Contour> getContours(double[] areas1, double[] areas2, double[] width, double[] height, double[] posX, double[] posY) {
            Vector<Contour> set = new Vector<Contour>();
            for(int i = 0; i < areas1.length; i++) {
                set.add(new Contour(areas1[i], areas2[i], width[i], height[i], posX[i], posY[i]));
            }
            set.sort(null);
            return set;
        }
    }
    
    private static final double VIEW_WIDTH_PX = 1000.0D;
    private static final double VIEW_HEIGHT_PX = 700.0D;
    private static final double VIEW_ANGLE_DEG = 49.4D; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480
    private static final double VIEW_ANGLE_HEIGHT_DEG = 50.0D;
    private static final double CAM_HEIGHT_FT = 1.0D;
    private static final double SCORE_MIN = 75.0D;
    private static final double TARGET_HEIGHT_FT = 7.583D;
    private static final double CAM_ALTITUDE_DEG = 25.0D;
    
    private static final NetworkTable table1;
    private static final NetworkTable table2;
    
    public static TargetPos detect() {
        double[] areas1 = table1.getNumberArray("area", new double[] {});
        double[] areas2 = table2.getNumberArray("area", new double[] {});
        double[] width = table2.getNumberArray("width", new double[] {});
        double[] height = table2.getNumberArray("height", new double[] {});
        double[] posX = table2.getNumberArray("centerX", new double[] {});
        double[] posY = table2.getNumberArray("centerY", new double[] {});
        System.out.println("prevector" + areas1.length);
        Vector<Contour> contours = Contour.getContours(areas1, areas2, width, height, posX, posY);
        System.out.println("Contours: " + contours.size());
        if(contours.size() > 0)
        {
            Contour contour = contours.elementAt(0);
            double areaScore = scoreArea(contour);
            SmartDashboard.putNumber("Area Score", areaScore);
            System.out.println("Area" + areaScore);
            boolean lock = areaScore > SCORE_MIN;

            //Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
            SmartDashboard.putBoolean("Target Lock", lock);
            if(!lock) {
                return null;
            }
            double azimuth = computeTargetAzimuth(contour);
            double altitude = computeTargetAltitude(contour);
            double distance = (TARGET_HEIGHT_FT - CAM_HEIGHT_FT) / Math.tan((Math.PI / 180.0D) * altitude);
            SmartDashboard.putNumber("Target Distance", distance);
            SmartDashboard.putNumber("Target Azimuth", azimuth);
            SmartDashboard.putNumber("Target Altitude", altitude);
            System.out.println("Distance" + distance);
            System.out.println("Azimuth" + azimuth);
            System.out.println("Altitude" + altitude);
            double angleToNormal = Math.acos((14.0D/20.0D) * (contour.width / contour.height) * Math.cos(CAM_ALTITUDE_DEG));
            return new TargetPos(distance, azimuth, altitude, angleToNormal);
        } else {
            SmartDashboard.putBoolean("Target Lock", false);
            return null;
        }
    }
    
    private static double ratioToScore(double ratio) {
        return (Math.max(0, Math.min(100*(1-Math.abs(1-ratio)), 100)));
    }

    private static double scoreArea(Contour contour) {
        return ratioToScore((280.0D/88.0D) * contour.area1 / contour.area2);
    }
    
    private static double computeTargetAzimuth(Contour contour) {
        double normalizedPosX = 2.0D * contour.posX / VIEW_WIDTH_PX - 1.0D;
        return (Math.atan(normalizedPosX * Math.tan(VIEW_ANGLE_DEG * Math.PI / (180*2))) * (180.0D / Math.PI));
    }
    
    private static double computeTargetAltitude(Contour contour) {
        double normalizedPosY = 2.0D * contour.posY / VIEW_HEIGHT_PX - 1.0D;
        return (Math.atan(normalizedPosY * Math.tan(VIEW_ANGLE_HEIGHT_DEG * Math.PI / (180*2))) * (180.0D / Math.PI)) + CAM_ALTITUDE_DEG;
    }
    
    static {
        table1 = NetworkTable.getTable("GRIP/table1");
        table2 = NetworkTable.getTable("GRIP/table2");
    }
}

@@ -1876,18 +1876,18 @@
 H_PX = 1
-00
+28
 0.0D;%0A  
@@ -1934,17 +1934,17 @@
 T_PX = 7
-0
+2
 0.0D;%0A  
@@ -1994,140 +1994,15 @@
 G = 
-49.4D; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480
+54.70D;
 %0A   
@@ -2058,12 +2058,13 @@
 G = 
-50.0
+18.59
 D;%0A 

Set camera specs