// After (a simplified form of) https://github.com/tonyg/relax-overveld/tree/deno,
// which is MIT-licensed.

import { Vector2, Vector3 } from '@babylonjs/core/Legacy/legacy';

export interface Delta {
    isSignificant(epsilon: number): boolean;
    apply(rho: number): void;
}

export interface Constraint {
    computeDeltas(): Array<Delta>;
}

export class Relax {
    rho = 0.25;
    epsilon = 0.0001;
    constraints: Array<Constraint> = [];

    add<X extends Constraint>(constraint: X): X {
        this.constraints.push(constraint);
        return constraint;
    }

    clear(): void {
        this.constraints = [];
    }

    _iterate() {
        const allDeltas: Array<Delta> = [];

        this.constraints.forEach(t => {
            const deltas = t.computeDeltas();
            if (deltas.some(d => d.isSignificant(this.epsilon))) {
	        allDeltas.push(... deltas);
            }
        });

        // (This shouldn't be done in the loop above b/c that would affect the other delta computations.)
        allDeltas.forEach(d => d.apply(this.rho));

        return allDeltas.length > 0;
    }

    iterate(milliseconds: number): number {
        let count = 0;
        const t0 = Date.now();
        while ((Date.now() - t0) < milliseconds) {
            if (!this._iterate()) break;
            count++;
        }
        return count;
    }
}

//---------------------------------------------------------------------------

export interface Ref {
    get(): number;
    set(v: number): void;
}

export class Path implements Ref {
    path: string[];

    constructor (public obj: any, ... path: string[]) {
        this.path = path;
    }

    get(): number {
        return this.path.reduce((o, p) => o[p], this.obj) as unknown as number;
    }

    set(v: number) {
        this.path.reduce((o, p, i) => i === this.path.length - 1 ? o[p] = v : o[p], this.obj);
    }
}

export class NumericDelta implements Delta {
    constructor (public ref: Ref, public amount: number) {}
    isSignificant(epsilon: number): boolean { return Math.abs(this.amount) > epsilon; }
    apply(rho: number): void { this.ref.set(this.ref.get() + (this.amount * rho)); }
}

export class ValueConstraint implements Constraint {
    constructor (public v: Ref, public targetValue: number) {}
    computeDeltas(): Array<Delta> {
        return [new NumericDelta(this.v, this.targetValue - this.v.get())];
    }
}

export class EqualityConstraint implements Constraint {
    constructor (public p1: Ref, public p2: Ref) {}
    computeDeltas(): Array<Delta> {
        const diff = this.p1.get() - this.p2.get();
        return [new NumericDelta(this.p1, -diff / 2),
                new NumericDelta(this.p2, +diff / 2)];
    }
}

export class SumConstraint implements Constraint {
    constructor (public addend1: Ref, public addend2: Ref, public sum: Ref) {}
    computeDeltas(): Array<Delta> {
        const diff = this.sum.get() - (this.addend1.get() + this.addend2.get());
        return [new NumericDelta(this.addend1, +diff / 3),
                new NumericDelta(this.addend2, +diff / 3),
                new NumericDelta(this.sum, -diff / 3)];
    }
}

export interface Vectorish {
    length(): number;
    lengthSquared(): number;
    scale(s: number): this;
    addInPlace(v: this): void;
    add(v: this): this;
    subtract(v: this): this;
    normalize(): this; // in place!
}

export class VectorDelta<V extends Vectorish> implements Delta {
    constructor (public p: V, public delta: V) {}

    isSignificant(epsilon: number): boolean {
        return this.delta.lengthSquared() > epsilon * epsilon;
    }

    apply(rho: number): void {
        this.p.addInPlace(this.delta.scale(rho));
    }
}

export class CoordinateConstraint<V extends Vectorish> implements Constraint {
    constructor (public p: V, public c: V) {}
    computeDeltas(): Array<Delta> {
        return [new VectorDelta(this.p, this.c.subtract(this.p))];
    }
}

export class CoincidenceConstraint<V extends Vectorish> implements Constraint {
    constructor (public p1: V, public p2: V) {}
    computeDeltas(): Array<Delta> {
        const splitDiff = this.p2.subtract(this.p1).scale(0.5);
        return [new VectorDelta(this.p1, splitDiff),
                new VectorDelta(this.p2, splitDiff.scale(-1))];
    }
}

export class EquivalenceConstraint<V extends Vectorish> implements Constraint {
    constructor (public p1: V, public p2: V, public p3: V, public p4: V) {}
    computeDeltas(): Array<Delta> {
        const splitDiff = this.p2.add(this.p3)
            .subtract(this.p1.add(this.p4))
            .scale(0.25);
        return [new VectorDelta(this.p1, splitDiff),
	        new VectorDelta(this.p2, splitDiff.scale(-1)),
	        new VectorDelta(this.p3, splitDiff.scale(-1)),
	        new VectorDelta(this.p4, splitDiff)];
    }
}

export class EqualDistanceConstraint<V extends Vectorish> implements Constraint {
    constructor (public p1: V, public p2: V, public p3: V, public p4: V) {}
    computeDeltas(): Array<Delta> {
        const l12 = this.p1.subtract(this.p2).length();
        const l34 = this.p3.subtract(this.p4).length();
        const delta = (l12 - l34) / 4;
        const e12 = this.p2.subtract(this.p1).normalize().scale(delta);
        const e34 = this.p4.subtract(this.p3).normalize().scale(delta);
        return [new VectorDelta(this.p1, e12),
                new VectorDelta(this.p2, e12.scale(-1)),
                new VectorDelta(this.p3, e34.scale(-1)),
                new VectorDelta(this.p4, e34)];
    }
}

export class LengthConstraint<V extends Vectorish> implements Constraint {
    constructor (public p1: V, public p2: V, public l: number) {}
    computeDeltas(): Array<Delta> {
        const l12 = this.p1.subtract(this.p2).length();
        const delta = (l12 - this.l) / 2;
        const e12 = this.p2.subtract(this.p1).normalize().scale(delta);
        return [new VectorDelta(this.p1, e12),
	        new VectorDelta(this.p2, e12.scale(-1))];
    }
}

export function rotatedAround(p: Vector2, dTheta: number, axis: Vector2): Vector2 {
    const v = p.subtract(axis);
    v.rotateToRef(dTheta, v);
    return axis.add(v);
}

export class OrientationConstraint2 implements Constraint {
    constructor (public p1: Vector2,
                 public p2: Vector2,
                 public p3: Vector2,
                 public p4: Vector2,
                 public theta: number) {}

    computeDeltas(): Array<Delta> {
        const v12 = this.p2.subtract(this.p1);
        const a12 = Math.atan2(v12.y, v12.x);
        const m12 = this.p1.add(this.p2.subtract(this.p1).scale(0.5));

        const v34 = this.p4.subtract(this.p3);
        const a34 = Math.atan2(v34.y, v34.x);
        const m34 = this.p3.add(this.p4.subtract(this.p3).scale(0.5));

        const currTheta = a12 - a34;
        const dTheta = this.theta - currTheta;
        // TODO: figure out why setting dTheta to 1/2 times this value (as shown in the paper
        // and seems to make sense) results in jumpy/unstable behavior.

        return [new VectorDelta(this.p1, rotatedAround(this.p1, dTheta, m12).subtract(this.p1)),
	        new VectorDelta(this.p2, rotatedAround(this.p2, dTheta, m12).subtract(this.p2)),
	        new VectorDelta(this.p3, rotatedAround(this.p3, -dTheta, m34).subtract(this.p3)),
	        new VectorDelta(this.p4, rotatedAround(this.p4, -dTheta, m34).subtract(this.p4))];
    }
}