/***************************************************************************
 *   Copyright (c) 2010 Jürgen Riegel <juergen.riegel@web.de>              *
 *                                                                         *
 *   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 "PreCompiled.h"
#ifndef _PreComp_
# include <BRepAdaptor_Curve.hxx>
# include <CPnts_AbscissaPoint.hxx>
# include <TopoDS.hxx>
# include <TopoDS_Edge.hxx>
#endif

#include <Base/Sequencer.h>
#include <Mod/Part/App/edgecluster.h>
#include <Mod/Part/App/PartFeature.h>

#include "Edge2TracObject.h"
#include "Trajectory.h"
#include "Waypoint.h"


using namespace Robot;
using namespace App;

PROPERTY_SOURCE(Robot::Edge2TracObject, Robot::TrajectoryObject)


Edge2TracObject::Edge2TracObject()
{

    ADD_PROPERTY_TYPE( Source,      (nullptr)  , "Edge2Trac",Prop_None,"Edges to generate the Trajectory");
    ADD_PROPERTY_TYPE( SegValue,    (0.5), "Edge2Trac",Prop_None,"Max deviation from original geometry");
    ADD_PROPERTY_TYPE( UseRotation, (0)  , "Edge2Trac",Prop_None,"use orientation of the edge");
    NbrOfEdges = 0;
    NbrOfCluster = 0;
}

Edge2TracObject::~Edge2TracObject()
{
}

App::DocumentObjectExecReturn *Edge2TracObject::execute()
{
    App::DocumentObject* link = Source.getValue();
    if (!link)
        return new App::DocumentObjectExecReturn("No object linked");
    if (!link->getTypeId().isDerivedFrom(Part::Feature::getClassTypeId()))
        return new App::DocumentObjectExecReturn("Linked object is not a Part object");
    Part::Feature *base = static_cast<Part::Feature*>(Source.getValue());
    const Part::TopoShape& TopShape = base->Shape.getShape();

    const std::vector<std::string>& SubVals = Source.getSubValuesStartsWith("Edge");
    if (SubVals.empty())
        return new App::DocumentObjectExecReturn("No Edges specified");

    // container for all the edges 
    std::vector<TopoDS_Edge> edges;

    // run through the edge name and get the real objects from the TopoShape
    for (std::vector<std::string>::const_iterator it= SubVals.begin();it!=SubVals.end();++it) {
         TopoDS_Edge edge = TopoDS::Edge(TopShape.getSubShape(it->c_str()));
         edges.push_back(edge);
    }

    // instantiate an edge cluster sorter and get the result 
    Part::Edgecluster acluster(edges);
    Part::tEdgeClusterVector aclusteroutput = acluster.GetClusters();

    if(aclusteroutput.empty())
        return new App::DocumentObjectExecReturn("No Edges specified");

    // set the number of cluster and edges 
    NbrOfCluster = aclusteroutput.size();
    NbrOfEdges = 0;
    for(std::vector<std::vector<TopoDS_Edge> >::const_iterator it=aclusteroutput.begin();it!=aclusteroutput.end();++it)
        NbrOfEdges += it->size();

    // trajectory to fill
    Robot::Trajectory trac;
    bool first = true;

    // cycle through the cluster
    for(std::vector<std::vector<TopoDS_Edge> >::const_iterator it=aclusteroutput.begin();it!=aclusteroutput.end();++it)
    {
        // cycle through the edges of the cluster
        for(std::vector<TopoDS_Edge>::const_iterator it2=it->begin();it2!=it->end();++it2)
        {
            BRepAdaptor_Curve adapt(*it2);
            
            switch(adapt.GetType())
            {
            case GeomAbs_Line:
                {
                // get start and end point
                gp_Pnt P1 = adapt.Value(adapt.FirstParameter());
                gp_Pnt P2 = adapt.Value(adapt.LastParameter());

                Base::Rotation R1;
                Base::Rotation R2;

                // if orientation is used 
                if(UseRotation.getValue()) {
                    // here get the orientation of the start and end point...
                    //R1 = ;
                    //R2 = ;

                }

                // if reverse orintation, switch the points
                if ( it2->Orientation() == TopAbs_REVERSED )
                {
                     //switch the points and orientation
                     gp_Pnt tmpP = P1;
                     Base::Rotation tmpR = R1;
                     P1 = P2;
                     R1 = R2;
                     R2 = tmpR;
                     P2 = tmpP;
                }
                if(first){
                    Waypoint wp("Pt",Base::Placement(Base::Vector3d(P1.X(),P1.Y(),P1.Z()),R1));
                    trac.addWaypoint(wp);
                    first = false;
                }
                Waypoint wp("Pt",Base::Placement(Base::Vector3d(P2.X(),P2.Y(),P2.Z()),R2));
                trac.addWaypoint(wp);
                break;
                }
            case GeomAbs_BSplineCurve:
                {
                Standard_Real Length    = CPnts_AbscissaPoint::Length(adapt);
                Standard_Real ParLength = adapt.LastParameter()-adapt.FirstParameter();
                Standard_Real NbrSegments = Round(Length / SegValue.getValue());

                Standard_Real beg = adapt.FirstParameter();
                Standard_Real end = adapt.LastParameter();
                Standard_Real stp = ParLength / NbrSegments;
				bool reversed = false;
                if (it2->Orientation() == TopAbs_REVERSED) {
                    std::swap(beg, end);
                    stp = - stp;
					reversed = true;
                }

                if (first) 
                    first = false;
                else
                    beg += stp;
                Base::SequencerLauncher seq("Create waypoints", static_cast<size_t>((end-beg)/stp));
				if(reversed)
				{
					for (;beg > end; beg += stp) {
						gp_Pnt P = adapt.Value(beg);
                        Base::Rotation R1;
                        // if orientation is used 
                        if(UseRotation.getValue()) {
                            // here get the orientation of the start and end point...
                            //R1 = ;
                        }

						Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1));
						trac.addWaypoint(wp);
						seq.next();
					}
				}
				else
				{
					for (;beg < end; beg += stp) {
						gp_Pnt P = adapt.Value(beg);
                        Base::Rotation R1;
                        // if orientation is used 
                        if(UseRotation.getValue()) {
                            // here get the orientation of the start and end point...
                            //R1 = ;
                        }
						Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1));
						trac.addWaypoint(wp);
						seq.next();
					}
				}
                
                } break;
            case GeomAbs_Circle:
                {
                Standard_Real Length    = CPnts_AbscissaPoint::Length(adapt);
                Standard_Real ParLength = adapt.LastParameter()-adapt.FirstParameter();
                Standard_Real NbrSegments = Round(Length / SegValue.getValue());
                Standard_Real SegLength   = ParLength / NbrSegments;
				
				if ( it2->Orientation() == TopAbs_REVERSED )
				{
					//Beginning and End switch
					double i = adapt.LastParameter();
					if(first) 
						first=false;
					else
						i -= SegLength;
					for (;i>adapt.FirstParameter();i-= SegLength){
						gp_Pnt P = adapt.Value(i);
                        Base::Rotation R1;
                        // if orientation is used 
                        if(UseRotation.getValue()) {
                            // here get the orientation of the start and end point...
                            //R1 = ;
                        }
						Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1));
						trac.addWaypoint(wp);
					}
				}
				else
				{
					double i = adapt.FirstParameter();
					if(first) 
						first=false;
					else
						i += SegLength;
					for (;i<adapt.LastParameter();i+= SegLength)
					{
						gp_Pnt P = adapt.Value(i);
                        Base::Rotation R1;
                        // if orientation is used 
                        if(UseRotation.getValue()) {
                            // here get the orientation of the start and end point...
                            //R1 = ;
                        }
						Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1));
						trac.addWaypoint(wp);
					}
					
				}
				break;
                }

            default:
                throw Base::TypeError("Unknown Edge type in Robot::Edge2TracObject::execute()");
            }
           


        }
    }

    Trajectory.setValue(trac);
    
    return App::DocumentObject::StdReturn;
}


//short Edge2TracObject::mustExecute(void) const
//{
//    return 0;
//}

void Edge2TracObject::onChanged(const Property* prop)
{
 
    App::GeoFeature::onChanged(prop);
}