/***************************************************************************
 *   Copyright (c) 2024 Shai Seger <shaise at gmail>                       *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "OpenGlWrapper.h"
#include "MillPathSegment.h"
#include "SimShapes.h"
#include "linmath.h"
#include "GlUtils.h"
#include <iostream>

#define N_MILL_SLICES 8
#define MAX_SEG_DEG (PI / 2.0f)   // 90 deg
#define NIN_SEG_DEG (PI / 90.0f)  // 2 deg
#define SWEEP_ARC_PAD 1.05f
#define PX 0
#define PY 1
#define PZ 2


namespace MillSim
{

bool IsVerticalMotion(MillMotion* m1, MillMotion* m2)
{
    return (m1->z != m2->z && EQ_FLOAT(m1->x, m2->x) && EQ_FLOAT(m1->y, m2->y));
}

bool IsArcMotion(MillMotion* m)
{
    if (m->cmd != eRotateCCW && m->cmd != eRotateCW) {
        return false;
    }
    return fabs(m->i > EPSILON) || fabs(m->j) > EPSILON;
}

float MillPathSegment::mResolution = 1;
float MillPathSegment::mSmallRadStep = (PI / 8);

MillPathSegment::MillPathSegment(EndMill* _endmill, MillMotion* from, MillMotion* to)
    : mShearMat {{1.0F, 0.0F, 0.0F, 0.0F},
                 {0.0F, 1.0F, 0.0F, 0.0F},
                 {0.0F, 0.0F, 1.0F, 0.0F},
                 {0.0F, 0.0F, 0.0F, 1.0F}}
{
    MotionPosToVec(mStartPos, from);
    MotionPosToVec(mDiff, to);
    vec3_sub(mDiff, mDiff, mStartPos);
    mXYDistance = sqrtf(mDiff[PX] * mDiff[PX] + mDiff[PY] * mDiff[PY]);
    mZDistance = fabsf(mDiff[PY]);
    mXYZDistance = sqrtf(mXYDistance * mXYDistance + mDiff[PZ] * mDiff[PZ]);
    mXYAngle = atan2f(mDiff[PY], mDiff[PX]);
    endmill = _endmill;
    mStartAngRad = mStepAngRad = 0;
    if (IsArcMotion(to)) {
        mMotionType = MTCurved;
        mRadius = sqrtf(to->j * to->j + to->i * to->i);
        mSmallRad = mRadius <= endmill->radius;

        if (mSmallRad) {
            mStepAngRad = mSmallRadStep;
        }
        else {
            mStepAngRad = asinf(mResolution / mRadius);
            if (mStepAngRad > MAX_SEG_DEG) {
                mStepAngRad = MAX_SEG_DEG;
            }
            else if (mStepAngRad < NIN_SEG_DEG) {
                mStepAngRad = NIN_SEG_DEG;
            }
        }

        MotionPosToVec(mCenter, from);
        mCenter[PX] += to->i;
        mCenter[PY] += to->j;
        mArcDir = to->cmd == eRotateCCW ? -1.f : 1.f;
        mStartAngRad = atan2f(mCenter[PX] - from->x, from->y - mCenter[PY]);
        float endAng = atan2f(mCenter[PX] - to->x, to->y - mCenter[PY]);
        mSweepAng = (mStartAngRad - endAng) * mArcDir;
        if (mSweepAng < EPSILON) {
            mSweepAng += PI * 2;
        }
        numSimSteps = (int)(mSweepAng / mStepAngRad) + 1;
        mStepAngRad = mArcDir * mSweepAng / numSimSteps;
        if (mSmallRad) {
            // when the radius is too small, we just use the tool itself to carve the stock
            mShape = endmill->toolShape;
        }
        else {
            endmill->GenerateArcSegmentDL(mRadius,
                                           mStepAngRad * SWEEP_ARC_PAD,
                                           mDiff[PZ] / numSimSteps,
                                           &mShape);
            numSimSteps++;
        }

        isMultyPart = true;
    }
    else {
        numSimSteps = (int)(mXYZDistance / mResolution);
        if (numSimSteps == 0) {
            numSimSteps = 1;
        }
        isMultyPart = false;
        mStepDistance = mXYDistance / numSimSteps;
        mStepLength[PX] = mDiff[PX];
        mStepLength[PY] = mDiff[PY];
        mStepLength[PZ] = mDiff[PZ];
        vec3_scale(mStepLength, mStepLength, 1.f / (float)numSimSteps);

        if (IsVerticalMotion(from, to)) {
            mMotionType = MTVertical;
        }
        else {
            mMotionType = MTHorizontal;
            mShearMat[0][2] = mDiff[PZ] / mXYDistance;
        }
    }
}

MillPathSegment::~MillPathSegment()
{
    mShape.FreeResources();
}


void MillPathSegment::render(int step)
{
    mStepNumber = step;
    mat4x4 mat, mat2, rmat;
    mat4x4_identity(mat);
    mat4x4_identity(rmat);
    if (mMotionType == MTCurved) {
        mat4x4_translate_in_place(mat,
                                  mCenter[PX],
                                  mCenter[PY],
                                  mCenter[PZ] + mDiff[PZ] * (step - 1) / numSimSteps);
        mat4x4_rotate_Z(mat, mat, mStartAngRad - (step - 1) * mStepAngRad);
        mat4x4_rotate_Z(rmat, rmat, mStartAngRad - (step - 1) * mStepAngRad);

        if (mSmallRad || step == numSimSteps) {
            mat4x4_translate_in_place(mat, 0, mRadius, 0);
            endmill->toolShape.Render(mat, rmat);
        }
        else {
            mShape.Render(mat, rmat);
        }
    }
    else {
        if (mMotionType == MTVertical) {
            if (mStepLength[PZ] > 0) {
                mat4x4_translate_in_place_v(mat, mStartPos);
            }
            else {
                mat4x4_translate_in_place(mat,
                                          mStartPos[PX],
                                          mStartPos[PY],
                                          mStartPos[PZ] + mStepNumber * mStepLength[PZ]);
            }
            endmill->toolShape.Render(mat, rmat);
        }
        else {
            float renderDist = step * mStepDistance;
            mat4x4_translate_in_place_v(mat, mStartPos);
            mat4x4_rotate_Z(mat, mat, mXYAngle);
            mat4x4_rotate_Z(rmat, rmat, mXYAngle);
            mat4x4_dup(mat2, mat);
            if (mDiff[PZ] != 0.0) {
                mat4x4_mul(mat2, mat2, mShearMat);
            }
            mat4x4_scale_aniso(mat2, mat2, renderDist, 1, 1);
            endmill->pathShape.Render(mat2, rmat);
            mat4x4_translate_in_place(mat, renderDist, 0, mDiff[PZ]);
            endmill->halfToolShape.Render(mat, rmat);
        }
    }
}

void MillPathSegment::GetHeadPosition(vec3 headPos)
{
    if (mMotionType == MTCurved) {
        float angRad = mStartAngRad - mStepNumber * mStepAngRad;
        vec3_set(mHeadPos, -mRadius * sinf(angRad), mRadius * cosf(angRad), 0);
        vec3_add(mHeadPos, mHeadPos, mCenter);
    }
    else {
        vec3_dup(mHeadPos, mStepLength);
        vec3_scale(mHeadPos, mHeadPos, (float)mStepNumber);
        vec3_add(mHeadPos, mHeadPos, mStartPos);
    }
    vec3_dup(headPos, mHeadPos);
}
float MillPathSegment::SetQuality(float quality, float maxStockDimension)
{
    mResolution = maxStockDimension * 0.05 / quality;
    if (mResolution > 4) {
        mResolution = 4;
    }
    if (mResolution < 0.5) {
        mResolution = 0.5;
    }
    mSmallRadStep = PI / 8;
    if (quality < 4) {
        mSmallRadStep = PI / 2;
    }
    else if (quality < 8) {
        mSmallRadStep = PI / 4;
    }
    return mResolution;
}
}  // namespace MillSim