import { Hono } from "hono";
import type { AgentConfig } from "./config.ts";

type ThreePose = {
  x: number;
  y: number;
  z: number;
  heading: number;
};

type MapCenter = {
  x: number;
  y: number;
};

type MapPose = {
  x: number;
  y: number;
  z: number;
  qx: number;
  qy: number;
  qz: number;
  qw: number;
};

function bridgeUrl(config: AgentConfig): string {
  return config.ros.bridgeUrl || `http://${config.ros.bridgeHost}:${config.ros.bridgePort}`;
}

function defaultCenter(): MapCenter {
  return {
    x: Number(process.env.CENTER_X ?? "3763.558837890625"),
    y: Number(process.env.CENTER_Y ?? "73675.99609375"),
  };
}

function validCenter(center?: MapCenter): MapCenter | undefined {
  return center && Number.isFinite(center.x) && Number.isFinite(center.y) ? center : undefined;
}

function threeToMap(pose: ThreePose, center = defaultCenter()): MapPose {
  const headingRad = (pose.heading * Math.PI) / 180;
  const yaw = Math.PI / 2 - headingRad;
  return {
    x: pose.x + center.x,
    y: center.y - pose.z,
    z: pose.y,
    qx: 0,
    qy: 0,
    qz: Math.sin(yaw / 2),
    qw: Math.cos(yaw / 2),
  };
}

function mapToThree(mx: number, my: number, mz: number, center = defaultCenter()) {
  return {
    x: mx - center.x,
    y: mz,
    z: -(my - center.y),
  };
}

function sampleTrajectory(points: number[][], minDist = 0.5): number[][] {
  if (points.length === 0) return [];
  const result: number[][] = [points[0]];
  let lx = points[0][0];
  let ly = points[0][1];

  for (const point of points.slice(1)) {
    const dx = point[0] - lx;
    const dy = point[1] - ly;
    if (Math.sqrt(dx * dx + dy * dy) >= minDist) {
      result.push(point.map((value, index) => index <= 3 ? Number(value.toFixed(3)) : value));
      lx = point[0];
      ly = point[1];
    }
  }
  return result;
}

function trimBehindStart(points: [number, number, number, number][], start: ThreePose) {
  const headingRad = (start.heading * Math.PI) / 180;
  const fwdX = Math.sin(headingRad);
  const fwdZ = -Math.cos(headingRad);

  return points.filter((point) => {
    const dx = point[0] - start.x;
    const dz = point[2] - start.z;
    return dx * fwdX + dz * fwdZ >= -0.5;
  });
}

function convertPoints(points: { x: number; y: number; z: number; t: number }[], center?: MapCenter) {
  return points.map((point) => {
    const three = mapToThree(point.x, point.y, point.z, center);
    return [three.x, three.y, three.z, point.t] as [number, number, number, number];
  });
}

async function bridgeFetch(config: AgentConfig, path: string, init?: RequestInit): Promise<Response> {
  return fetch(`${bridgeUrl(config)}${path}`, init);
}

async function bridgeJson<T>(config: AgentConfig, path: string, init?: RequestInit): Promise<T> {
  const response = await bridgeFetch(config, path, init);
  if (!response.ok) throw new Error(await response.text());
  return response.json() as Promise<T>;
}

function errorJson(error: unknown, fallback: string, status = 502): Response {
  return Response.json({
    ok: false,
    error: fallback,
    detail: error instanceof Error ? error.message : String(error),
  }, { status });
}

export function createBridgeApi(config: AgentConfig): Hono {
  const app = new Hono();

  app.get("/status", async (c) => {
    try {
      const [health, stack] = await Promise.all([
        bridgeJson(config, "/health").catch((error) => ({ ok: false, error: String(error) })),
        bridgeJson(config, "/stack").catch((error) => ({ ok: false, error: String(error) })),
      ]);
      return c.json({
        ok: true,
        configured: config.ros,
        bridgeUrl: bridgeUrl(config),
        health,
        stack,
      });
    } catch (error) {
      return c.json({ ok: false, bridgeUrl: bridgeUrl(config), message: error instanceof Error ? error.message : String(error) }, 502);
    }
  });

  app.get("/stack", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/stack"));
    } catch (error) {
      return errorJson(error, "Failed to read bridge stack.");
    }
  });

  app.post("/clear", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/clear_route", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to clear route.");
    }
  });

  app.post("/stop", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/disengage", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to stop robot.");
    }
  });

  app.post("/emergency-stop", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/emergency_stop", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to emergency stop robot.");
    }
  });

  app.post("/reset", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/reset", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to reset robot.");
    }
  });

  return app;
}

export function createViewerControlApi(config: AgentConfig): Hono {
  const app = new Hono();

  app.post("/plan", async (c) => {
    try {
      const body = await c.req.json<{ start?: ThreePose; goal?: ThreePose; center?: MapCenter }>();
      if (!body.goal) return c.json({ error: "goal is required" }, 400);

      const center = validCenter(body.center);
      const goalMap = threeToMap(body.goal, center);
      const startMap = body.start ? threeToMap(body.start, center) : null;

      if (startMap) {
        await bridgeJson(config, "/set_initial_pose", {
          method: "POST",
          headers: { "Content-Type": "application/json" },
          body: JSON.stringify(startMap),
        });
        await Bun.sleep(800);
      }

      await bridgeJson(config, "/set_route", {
        method: "POST",
        headers: { "Content-Type": "application/json" },
        body: JSON.stringify(goalMap),
      });

      const [pathData, trajData] = await Promise.all([
        bridgeJson<{ points: { x: number; y: number; z: number; t: number }[] }>(config, "/get_behavior_path"),
        bridgeJson<{ points: { x: number; y: number; z: number; t: number }[] }>(config, "/get_trajectory"),
      ]);

      const planned = sampleTrajectory(convertPoints(pathData.points, center), 0.5);
      const rawTrajectoryPoints = convertPoints(trajData.points, center);
      const visibleTrajectoryPoints = body.start ? trimBehindStart(rawTrajectoryPoints, body.start) : rawTrajectoryPoints;

      return c.json({
        planned,
        plannedTrajectory: sampleTrajectory(visibleTrajectoryPoints, 0.5),
      });
    } catch (error) {
      return errorJson(error, "Failed to plan.");
    }
  });

  app.post("/drive", async (c) => {
    try {
      const body = await c.req.json<{ goal?: ThreePose }>();
      if (!body.goal) return c.json({ error: "goal is required" }, 400);

      await bridgeJson(config, "/engage", {
        method: "POST",
        headers: { "Content-Type": "application/json" },
        body: JSON.stringify({ engage: true }),
      });
      return c.json({ ok: true });
    } catch (error) {
      return errorJson(error, "Failed to engage.");
    }
  });

  app.post("/clear", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/clear_route", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to clear route.");
    }
  });

  app.post("/stop", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/disengage", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to stop robot.");
    }
  });

  app.post("/reset", async (c) => {
    try {
      return c.json(await bridgeJson(config, "/reset", { method: "POST" }));
    } catch (error) {
      return errorJson(error, "Failed to reset robot.");
    }
  });

  return app;
}