/*
 * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


/*
 * Author: Andrey Pokidov
 * Date: 20 Oct 2024
 */

namespace Geometry
{
    public struct DPVersor
    {
        public static readonly DPVersor IDLE = new DPVersor(1.0, 0.0, 0.0, 0.0, 1.0);

        private double s0;
        private double x1;
        private double x2;
        private double x3;
        private double corrector;

        private DPVersor(double s0, double x1, double x2, double x3, double corrector)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
            this.corrector = corrector;
        }

        public DPVersor()
        {
            this.s0 = 1.0;
            this.x1 = 0.0;
            this.x2 = 0.0;
            this.x3 = 0.0;
            this.corrector = 1.0;
        }

        public DPVersor(double s0, double x1, double x2, double x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
            
            double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);

            this.corrector = 2.0 - squareModule;

            if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule)
            {
                this.Normalize(squareModule);
            }
        }

        public DPVersor(in DPVersor versor)
        {
            this.s0 = versor.s0;
            this.x1 = versor.x1;
            this.x2 = versor.x2;
            this.x3 = versor.x3;
            this.corrector = versor.corrector;
        }

        public DPVersor(in SPVersor versor)
        {
            this.s0 = versor.GetScalar();
            this.x1 = versor.GetX1();
            this.x2 = versor.GetX2();
            this.x3 = versor.GetX3();

            double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3);

            this.corrector = 2.0 - squareModule;

            if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule)
            {
                this.Normalize(squareModule);
            }
        }

        public readonly double GetScalar()
        {
            return this.s0;
        }

        public readonly double GetX1()
        {
            return this.x1;
        }

        public readonly double GetX2()
        {
            return this.x2;
        }

        public readonly double GetX3()
        {
            return this.x3;
        }

        public readonly double GetCorrector()
        {
            return this.corrector;
        }

        public readonly bool IsIdle()
        {
            return this.s0 <= -(1.0 - DPUtility.EPSYLON) || (1.0 - DPUtility.EPSYLON) <= this.s0;
        }

        public void Reset()
        {
            this.s0 = 1.0;
            this.x1 = 0.0;
            this.x2 = 0.0;
            this.x3 = 0.0;
            this.corrector = 1.0;
        }

        public void Invert()
        {
            this.x1 = -this.x1;
            this.x2 = -this.x2;
            this.x3 = -this.x3;
        }

        public readonly double GetAngle(AngleUnit unit)
        {
            if (this.s0 <= -(1.0 - DPUtility.TWO_EPSYLON) || 1.0 - DPUtility.TWO_EPSYLON <= this.s0) {
                return 0.0;
            }

            if (-DPUtility.EPSYLON <= this.s0 && this.s0 <= DPUtility.EPSYLON)
            {
                return DPAngle.GetHalfCircle(unit);
            }

            return DPAngle.ConvertFromRadians(2.0 * Math.Acos(s0), unit);
        }

        public readonly void MakeRotationMatrix(out DPMatrix3x3 matrix)
        {
            double s0s0 = this.s0 * this.s0;
            double x1x1 = this.x1 * this.x1;
            double x2x2 = this.x1 * this.x2;
            double x3x3 = this.x1 * this.x3;


            double s0x1 = this.s0 * this.x1;
            double s0x2 = this.s0 * this.x2;
            double s0x3 = this.s0 * this.x3;

            double x1x2 = this.x1 * this.x2;
            double x1x3 = this.x1 * this.x3;
            double x2x3 = this.x2 * this.x3;

            double corrector2 = 2.0 * this.corrector;

            matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3));
            matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3));
            matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2));

            matrix.r1c2 = corrector2 * (x1x2 - s0x3);
            matrix.r2c3 = corrector2 * (x2x3 - s0x1);
            matrix.r3c1 = corrector2 * (x1x3 - s0x2);

            matrix.r2c1 = corrector2 * (x1x2 + s0x3);
            matrix.r3c2 = corrector2 * (x2x3 + s0x1);
            matrix.r1c3 = corrector2 * (x1x3 + s0x2);
        }

        public readonly void MakeReverseMatrix(out DPMatrix3x3 matrix)
        {
            double s0s0 = this.s0 * this.s0;
            double x1x1 = this.x1 * this.x1;
            double x2x2 = this.x1 * this.x2;
            double x3x3 = this.x1 * this.x3;


            double s0x1 = this.s0 * this.x1;
            double s0x2 = this.s0 * this.x2;
            double s0x3 = this.s0 * this.x3;

            double x1x2 = this.x1 * this.x2;
            double x1x3 = this.x1 * this.x3;
            double x2x3 = this.x2 * this.x3;

            double corrector2 = 2.0 * this.corrector;

            matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3));
            matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3));
            matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2));

            matrix.r1c2 = corrector2 * (x1x2 + s0x3);
            matrix.r2c3 = corrector2 * (x2x3 + s0x1);
            matrix.r3c1 = corrector2 * (x1x3 + s0x2);

            matrix.r2c1 = corrector2 * (x1x2 - s0x3);
            matrix.r3c2 = corrector2 * (x2x3 - s0x1);
            matrix.r1c3 = corrector2 * (x1x3 - s0x2);
        }

        public void SetValues(double s0, double x1, double x2, double x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
            
            double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);

            this.corrector = 2.0 - squareModule;

            if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule)
            {
                this.Normalize(squareModule);
            }
        }

        public void SetValues(in DPVersor versor)
        {
            this.s0 = versor.s0;
            this.x1 = versor.x1;
            this.x2 = versor.x2;
            this.x3 = versor.x3;
            this.corrector = versor.corrector;
        }

        public void SetValues(in SPVersor versor)
        {
            this.s0 = versor.GetScalar();
            this.x1 = versor.GetX1();
            this.x2 = versor.GetX2();
            this.x3 = versor.GetX3();

            double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3);

            this.corrector = 2.0 - squareModule;

            if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule)
            {
                this.Normalize(squareModule);
            }
        }

        public void SetInverted(in DPVersor versor)
        {
            this.s0 = versor.s0;
            this.x1 = -versor.x1;
            this.x2 = -versor.x2;
            this.x3 = -versor.x3;
            this.corrector = versor.corrector;
        }

        public void SetInverted(in SPVersor versor)
        {
            this.s0 = versor.GetScalar();
            this.x1 = -versor.GetX1();
            this.x2 = -versor.GetX2();
            this.x3 = -versor.GetX3();

            double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3);

            this.corrector = 2.0 - squareModule;

            if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule)
            {
                this.Normalize(squareModule);
            }
        }

        public readonly void Turn(in DPVector3 vector, out DPVector3 result)
        {
            double multiplier = 2.0 * this.corrector;
            double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2);
            double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3);
            double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1);

            double x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2);
            double x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
            double x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1);

            result.x1 = x1;
            result.x2 = x2;
            result.x3 = x3;
        }

        public readonly void TurnBack(in DPVector3 vector, out DPVector3 result)
        {
            double multiplier = 2.0 * this.corrector;
            double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2);
            double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3);
            double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1);

            double x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2);
            double x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
            double x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1);

            result.x1 = x1;
            result.x2 = x2;
            result.x3 = x3;
        }

        private void Normalize(double squareModule)
        {
            if (squareModule <= DPUtility.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= DPUtility.SQUARE_EPSYLON * squareModule)
            {
                this.Reset();
                return;
            }

            double module = Math.Sqrt(squareModule);

            this.s0 /= module;
            this.x1 /= module;
            this.x2 /= module;
            this.x3 /= module;

            this.corrector = 2.0 - (this.s0 * this.s0 + this.x1 * this.x1) - (this.x2 * this.x2 + this.x3 * this.x3);
        }

        public static void Combine(in DPVersor second, in DPVersor first, out DPVersor result)
        {
            double s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3);
            double x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3);
            double x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1);
            double x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2);

            result.s0 = s0;
            result.x1 = x1;
            result.x2 = x2;
            result.x3 = x3;

            double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);

            result.corrector = 2.0 - squareModule;

            if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule)
            {
                result.Normalize(squareModule);
            }
        }
    }
}