package org.firstinspires.ftc.teamcode.util;

import android.annotation.SuppressLint;
import android.content.Context;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.fasterxml.jackson.core.JsonFactory;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.ObjectWriter;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
import com.qualcomm.robotcore.util.RobotLog;
import com.qualcomm.robotcore.util.WebHandlerManager;

import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.SampleTankDrive;
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;

import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;
import java.text.DateFormat;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Date;
import java.util.List;
import java.util.Objects;

import fi.iki.elonen.NanoHTTPD;

public final class LogFiles {
    private static final File ROOT =
            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/");

    public static LogFile log = new LogFile("uninitialized");

    public static class LogFile {
        public String version = "quickstart1 v2";

        public String opModeName;
        public long msInit = System.currentTimeMillis();
        public long nsInit = System.nanoTime();
        public long nsStart, nsStop;

        public double ticksPerRev = DriveConstants.TICKS_PER_REV;
        public double maxRpm = DriveConstants.MAX_RPM;
        public boolean runUsingEncoder = DriveConstants.RUN_USING_ENCODER;
        public double motorP = DriveConstants.MOTOR_VELO_PID.p;
        public double motorI = DriveConstants.MOTOR_VELO_PID.i;
        public double motorD = DriveConstants.MOTOR_VELO_PID.d;
        public double motorF = DriveConstants.MOTOR_VELO_PID.f;
        public double wheelRadius = DriveConstants.WHEEL_RADIUS;
        public double gearRatio = DriveConstants.GEAR_RATIO;
        public double trackWidth = DriveConstants.TRACK_WIDTH;
        public double kV = DriveConstants.kV;
        public double kA = DriveConstants.kA;
        public double kStatic = DriveConstants.kStatic;
        public double maxVel = DriveConstants.MAX_VEL;
        public double maxAccel = DriveConstants.MAX_ACCEL;
        public double maxAngVel = DriveConstants.MAX_ANG_VEL;
        public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL;

        public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP;
        public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI;
        public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD;
        public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP;
        public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI;
        public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD;
        public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER;

        public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
        public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;
        public double tankAxialD = SampleTankDrive.AXIAL_PID.kD;
        public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP;
        public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI;
        public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD;
        public double tankHeadingP = SampleTankDrive.HEADING_PID.kP;
        public double tankHeadingI = SampleTankDrive.HEADING_PID.kI;
        public double tankHeadingD = SampleTankDrive.HEADING_PID.kD;

        public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV;
        public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
        public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;
        public double trackingLateralDistance = StandardTrackingWheelLocalizer.LATERAL_DISTANCE;
        public double trackingForwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET;

        public RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = DriveConstants.LOGO_FACING_DIR;
        public RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = DriveConstants.USB_FACING_DIR;

        public List<Long> nsTimes = new ArrayList<>();

        public List<Double> targetXs = new ArrayList<>();
        public List<Double> targetYs = new ArrayList<>();
        public List<Double> targetHeadings = new ArrayList<>();

        public List<Double> xs = new ArrayList<>();
        public List<Double> ys = new ArrayList<>();
        public List<Double> headings = new ArrayList<>();

        public List<Double> voltages = new ArrayList<>();

        public List<List<Integer>> driveEncPositions = new ArrayList<>();
        public List<List<Integer>> driveEncVels = new ArrayList<>();
        public List<List<Integer>> trackingEncPositions = new ArrayList<>();
        public List<List<Integer>> trackingEncVels = new ArrayList<>();

        public LogFile(String opModeName) {
            this.opModeName = opModeName;
        }
    }

    public static void record(
            Pose2d targetPose, Pose2d pose, double voltage,
            List<Integer> lastDriveEncPositions, List<Integer> lastDriveEncVels, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels
    ) {
        long nsTime = System.nanoTime();
        if (nsTime - log.nsStart > 3 * 60 * 1_000_000_000L) {
            return;
        }

        log.nsTimes.add(nsTime);

        log.targetXs.add(targetPose.getX());
        log.targetYs.add(targetPose.getY());
        log.targetHeadings.add(targetPose.getHeading());

        log.xs.add(pose.getX());
        log.ys.add(pose.getY());
        log.headings.add(pose.getHeading());

        log.voltages.add(voltage);

        while (log.driveEncPositions.size() < lastDriveEncPositions.size()) {
            log.driveEncPositions.add(new ArrayList<>());
        }
        while (log.driveEncVels.size() < lastDriveEncVels.size()) {
            log.driveEncVels.add(new ArrayList<>());
        }
        while (log.trackingEncPositions.size() < lastTrackingEncPositions.size()) {
            log.trackingEncPositions.add(new ArrayList<>());
        }
        while (log.trackingEncVels.size() < lastTrackingEncVels.size()) {
            log.trackingEncVels.add(new ArrayList<>());
        }

        for (int i = 0; i < lastDriveEncPositions.size(); i++) {
            log.driveEncPositions.get(i).add(lastDriveEncPositions.get(i));
        }
        for (int i = 0; i < lastDriveEncVels.size(); i++) {
            log.driveEncVels.get(i).add(lastDriveEncVels.get(i));
        }
        for (int i = 0; i < lastTrackingEncPositions.size(); i++) {
            log.trackingEncPositions.get(i).add(lastTrackingEncPositions.get(i));
        }
        for (int i = 0; i < lastTrackingEncVels.size(); i++) {
            log.trackingEncVels.get(i).add(lastTrackingEncVels.get(i));
        }
    }

    private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {
        @SuppressLint("SimpleDateFormat")
        final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS");

        final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory())
                .writerWithDefaultPrettyPrinter();

        @Override
        public void onOpModePreInit(OpMode opMode) {
            log = new LogFile(opMode.getClass().getCanonicalName());

            // clean up old files
            File[] fs = Objects.requireNonNull(ROOT.listFiles());
            Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified()));
            long totalSizeBytes = 0;
            for (File f : fs) {
                totalSizeBytes += f.length();
            }

            int i = 0;
            while (i < fs.length && totalSizeBytes >= 32 * 1000 * 1000) {
                totalSizeBytes -= fs[i].length();
                if (!fs[i].delete()) {
                    RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath());
                }
                ++i;
            }
        }

        @Override
        public void onOpModePreStart(OpMode opMode) {
            log.nsStart = System.nanoTime();
        }

        @Override
        public void onOpModePostStop(OpMode opMode) {
            log.nsStop = System.nanoTime();

            if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) {
                //noinspection ResultOfMethodCallIgnored
                ROOT.mkdirs();

                String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json";
                File file = new File(ROOT, filename);
                try {
                    jsonWriter.writeValue(file, log);
                } catch (IOException e) {
                    RobotLog.setGlobalErrorMsg(new RuntimeException(e),
                            "Unable to write data to " + file.getAbsolutePath());
                }
            }
        }
    };

    @WebHandlerRegistrar
    public static void registerRoutes(Context context, WebHandlerManager manager) {
        //noinspection ResultOfMethodCallIgnored
        ROOT.mkdirs();

        // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves
        // don't use @OnCreateEventLoop because it's unreliable
        OpModeManagerImpl.getOpModeManagerOfActivity(
                AppUtil.getInstance().getActivity()
        ).registerListener(notifHandler);

        manager.register("/logs", session -> {
            final StringBuilder sb = new StringBuilder();
            sb.append("<!doctype html><html><head><title>Logs</title></head><body><ul>");
            File[] fs = Objects.requireNonNull(ROOT.listFiles());
            Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified()));
            for (File f : fs) {
                sb.append("<li><a href=\"/logs/download?file=");
                sb.append(f.getName());
                sb.append("\" download=\"");
                sb.append(f.getName());
                sb.append("\">");
                sb.append(f.getName());
                sb.append("</a></li>");
            }
            sb.append("</ul></body></html>");
            return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK,
                    NanoHTTPD.MIME_HTML, sb.toString());
        });

        manager.register("/logs/download", session -> {
            final String[] pairs = session.getQueryParameterString().split("&");
            if (pairs.length != 1) {
                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
                        NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length);
            }

            final String[] parts = pairs[0].split("=");
            if (!parts[0].equals("file")) {
                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
                        NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]);
            }

            File f = new File(ROOT, parts[1]);
            if (!f.exists()) {
                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
                        NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist");
            }

            return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
                    "application/json", new FileInputStream(f));
        });
    }
}