import {
    Mesh,
    MeshBuilder,
    Plane,
    Quaternion,
    Ray,
    Scene,
    Vector3,
    VertexData,
} from '@babylonjs/core/Legacy/legacy';

import * as Cat from './cat.js';

export class PenState {
    templatePath: Vector3[] = [new Vector3()];
    paths: Vector3[][] | null = null;

    get isDown(): boolean {
        return this.paths !== null;
    }

    clear() {
        this.templatePath = [new Vector3()];
        this.paths = null;
    }

    set() {
        if (!this.paths) throw new Error("Cannot set pen with no paths");
        this.templatePath = this.paths[0];
        this.paths = null;
    }

    down() {
        this.paths = this.templatePath.map(_p => []);
    }

    push(pos: Vector3, q: Quaternion) {
        this.templatePath.forEach((p, i) => {
            const r = new Vector3();
            // p.multiplyToRef(this.templateScale, r);
            // r.rotateByQuaternionToRef(q, r);
            p.rotateByQuaternionToRef(q, r);
            r.addInPlace(pos);
            this.paths![i].push(r);
        });
    }
};

const D2R = Math.PI / 180;

export class TurtleVM extends Cat.VM<TurtleVM> {
    container: Mesh;
    meshes: Mesh[] = [];

    counter = 0;
    sideOrientation = Mesh.DEFAULTSIDE;

    pen = new PenState();
    pos = new Vector3();
    q = new Quaternion();
    prevQ = this.q;
    smooth = false;

    get euler(): Vector3 {
        return this.q.toEulerAngles();
    }

    constructor(
        name: string,
        scene: Scene | null,
        program: Cat.Input,
    ) {
        super(program, TurtlePrimitives);
        this.container = new Mesh(name, scene);
    }

    forwardBy(dist: number) {
        const v = new Vector3(0, 0, dist);
        v.rotateByQuaternionToRef(this.q, v);
        this.pos.addInPlace(v);
        if (this.pen.isDown) this.pen.push(this.pos, this.q);
    }

    setQ(newQ: Quaternion) {
        this.q = newQ;
    }

    resetQ(newQ: Quaternion) {
        this.q = this.prevQ = newQ;
    }

    rotate(y: number, x: number, z: number) {
        const e = this.euler;
        e.addInPlaceFromFloats(x * D2R, y * D2R, z * D2R);
        this.setQ(e.toQuaternion());
    }

    relativeRotate(y: number, x: number, z: number) {
        this.setQ(this.q.multiply(Quaternion.FromEulerAngles(x * D2R, y * D2R, z * D2R)));
    }

    penDown() {
        this.pen.down();
        this.pen.push(this.pos, this.q);
    }

    penUp(close: boolean) {
        if (!this.pen.isDown) return;

        if (close) {
            throw new Error('todo');
        }

        const m = new Mesh(this.container.name + this.counter++);
        // TODO: this.sideOrientation

        const vertexData = new VertexData();
        const positions: number[] = [];
        const indices: number[] = [];
        const normals: number[] = [];
        const uvs: number[] = [];

        const paths = this.pen.paths!;
        const pathCount = paths.length;
        const stepCount = paths[0].length;

        const us: number[][] = [];
        const vs: number[][] = [];
        const uTotal: number[] = [];
        const vTotal: number[] = [];

        for (let pathIndex = 0; pathIndex < pathCount; pathIndex++) {
            uTotal.push(0);
            us.push([]);
            let prev = paths[pathIndex][0];
            for (let stepIndex = 0; stepIndex < stepCount; stepIndex++) {
                const curr = paths[pathIndex][stepIndex];
                uTotal[pathIndex] += curr.subtract(prev).length();
                us[pathIndex][stepIndex] = uTotal[pathIndex];
                prev = curr;
            }
        }

        for (let stepIndex = 0; stepIndex < stepCount; stepIndex++) {
            vTotal.push(0);
            vs.push([]);
            let prev = paths[0][stepIndex];
            for (let pathIndex = 0; pathIndex < pathCount; pathIndex++) {
                const curr = paths[pathIndex][stepIndex];
                vTotal[stepIndex] += curr.subtract(prev).length();
                vs[stepIndex][pathIndex] = vTotal[stepIndex];
                prev = curr;
            }
        }

        function pushPoint(pathIndex: number, stepIndex: number): number {
            const pointIndex = positions.length / 3;
            positions.push(... paths[pathIndex][stepIndex].asArray());
            uvs.push(us[pathIndex][stepIndex] / uTotal[pathIndex],
                     vs[stepIndex][pathIndex] / vTotal[stepIndex]);
            return pointIndex;
        }

        const cachedIndices: { [key: string]: number } = {};
        function cachedPoint(pathIndex: number, stepIndex: number): number {
            const v = paths[pathIndex][stepIndex].asArray().map(n => n.toFixed(3)).toString();
            if (!(v in cachedIndices)) cachedIndices[v] = pushPoint(pathIndex, stepIndex);
            return cachedIndices[v];
        }

        const computePointIndex = this.smooth ? cachedPoint : pushPoint;
        for (let pathIndex = 1; pathIndex < pathCount; pathIndex++) {
            for (let stepIndex = 1; stepIndex < stepCount; stepIndex++) {
                indices.push(computePointIndex(pathIndex - 1, stepIndex - 1),
                             computePointIndex(pathIndex - 1, stepIndex),
                             computePointIndex(pathIndex, stepIndex - 1));

                indices.push(computePointIndex(pathIndex - 1, stepIndex),
                             computePointIndex(pathIndex, stepIndex),
                             computePointIndex(pathIndex, stepIndex - 1));
            }
        }

        VertexData.ComputeNormals(positions, indices, normals);
        VertexData._ComputeSides(this.sideOrientation, positions, indices, normals, uvs);
        vertexData.positions = new Float32Array(positions);
        vertexData.indices = new Int32Array(indices);
        vertexData.normals = new Float32Array(normals);
        vertexData.uvs = new Float32Array(uvs);

        vertexData.applyToMesh(m);

        // const m = MeshBuilder.CreateRibbon(this.container.name + this.counter++, {
        //     pathArray: this.pen.paths!,
        //     sideOrientation: this.sideOrientation,
        // });

        m.parent = this.container;
        this.meshes.push(m);
    }
}

export const TurtlePrimitives: Cat.Environment<TurtleVM> = Object.assign({}, Cat.Primitives, {
    'Home'() { this.pos = new Vector3(); this.resetQ(new Quaternion()); return []; },

    'GetPos'() { return [this.pos.asArray()]; },
    'GetX'() { return [this.pos.x]; },
    'GetY'() { return [this.pos.y]; },
    'GetZ'() { return [this.pos.z]; },
    'SetX'(v) { this.pos.x = v as number; return []; },
    'SetY'(v) { this.pos.y = v as number; return []; },
    'SetZ'(v) { this.pos.z = v as number; return []; },
    'SetPos'(x, y, z) {
        this.pos.set(x as number, y as number, z as number);
        return [];
    },

    'GetHeading'() { return [this.euler.asArray().map(v => v / D2R)]; },
    'GetRX'() { return [this.euler.x / D2R]; },
    'GetRY'() { return [this.euler.y / D2R]; },
    'GetRZ'() { return [this.euler.z / D2R]; },
    'SetRX'(v) { this.q.x = v as number * D2R; return []; },
    'SetRY'(v) { this.q.y = v as number * D2R; return []; },
    'SetRZ'(v) { this.q.z = v as number * D2R; return []; },
    'SetHeading'(x, y, z) {
        this.resetQ(Quaternion.FromEulerAngles(x as number * D2R, y as number * D2R, z as number * D2R));
        return [];
    },

    'F'(dist) { this.forwardBy(dist as number); return []; },
    'B'(dist) { this.forwardBy(-(dist as number)); return []; },

    'RX'(degrees) { this.rotate(0, degrees as number, 0); return []; },
    'RY'(degrees) { this.rotate(degrees as number, 0, 0); return []; },
    'RZ'(degrees) { this.rotate(0, 0, degrees as number); return []; },

    'U'(degrees) { this.relativeRotate(0, -(degrees as number), 0); return []; },
    'D'(degrees) { this.relativeRotate(0, degrees as number, 0); return []; },
    'L'(degrees) { this.relativeRotate(-(degrees as number), 0, 0); return []; },
    'R'(degrees) { this.relativeRotate(degrees as number, 0, 0); return []; },
    'CW'(degrees) { this.relativeRotate(0, 0, -(degrees as number)); return []; },
    'CCW'(degrees) { this.relativeRotate(0, 0, degrees as number); return []; },

    'ClearPen'() { this.pen.clear(); return []; },
    'SetPen'() { this.pen.set(); return []; },

    'PenDown'() { this.penDown(); return []; },
    'PenUp'() { this.penUp(false); return []; },
    'Close'() { this.penUp(true); return []; },

    'SetSmooth'(b) { this.smooth = b as boolean; return []; },
    'SetSideOrientation'(s) {
        switch (s) {
            case "default": this.sideOrientation = Mesh.DEFAULTSIDE; break;
            case "front": this.sideOrientation = Mesh.FRONTSIDE; break;
            case "back": this.sideOrientation = Mesh.BACKSIDE; break;
            case "double": this.sideOrientation = Mesh.DOUBLESIDE; break;
            default: throw new TypeError("Invalid SideOrientation: " + s);
        }
        return [];
    },
} satisfies Cat.Environment<TurtleVM>);

function mitredExtrude(
    name: string,
    options: { shape: Vector3[], path: Vector3[], close?: boolean },
    scene: Scene,
): Mesh {
    const shape = options.shape;
    const path = options.path;
    const closed = options.close ?? false;

    function miterNormal(v1: Vector3, v2: Vector3): Vector3 {
        return Vector3.Cross(Vector3.Cross(v2, v1), v2.subtract(v1));
    }

    var allPaths = [];

    for (var s = 0; s < shape.length; s++) {
        let axisZ = path[1].subtract(path[0]).normalize();
        const axisX = Vector3.Cross(scene.activeCamera!.position, axisZ).normalize();
        const axisY = Vector3.Cross(axisZ, axisX);
        let startPoint = path[0].add(axisX.scale(shape[s].x)).add(axisY.scale(shape[s].y));
        const ribbonPath = [startPoint];
        for (var p = 0; p < path.length - 2; p++) {
            const nextAxisZ = path[p + 2].subtract(path[p + 1]).normalize();

            startPoint = startPoint.add(axisZ.scale(new Ray(startPoint, axisZ).intersectsPlane(Plane.FromPositionAndNormal(path[p + 1], miterNormal(axisZ, nextAxisZ)))!));

            ribbonPath.push(startPoint);
            axisZ = nextAxisZ;
        }
        // Last Point
        if (closed) {
            let nextAxisZ = path[0].subtract(path[path.length - 1]).normalize();
            startPoint = startPoint.add(axisZ.scale(new Ray(startPoint, axisZ).intersectsPlane(Plane.FromPositionAndNormal(path[path.length - 1], miterNormal(axisZ, nextAxisZ)))!));

            ribbonPath.push(startPoint);
            axisZ = nextAxisZ;

            nextAxisZ = path[1].subtract(path[0]).normalize();
            startPoint = startPoint.add(axisZ.scale(new Ray(startPoint, axisZ).intersectsPlane(Plane.FromPositionAndNormal(path[0], miterNormal(axisZ, nextAxisZ)))!));

            ribbonPath.shift();
            ribbonPath.unshift(startPoint);
        } else {
            startPoint = startPoint.add(axisZ.scale(new Ray(startPoint, axisZ).intersectsPlane(Plane.FromPositionAndNormal(path[path.length - 1], axisZ))!));

            ribbonPath.push(startPoint);
        }
        allPaths.push(ribbonPath);
    }

    return MeshBuilder.CreateRibbon(name, {
        pathArray: allPaths,
        sideOrientation: Mesh.DOUBLESIDE,
        closeArray: true,
        closePath: closed,
    }, scene);
}