/*
 * KiRouter - a push-and-(sometimes-)shove PCB router
 *
 * Copyright (C) 2013-2017 CERN
 * Copyright (C) 2016-2020 KiCad Developers, see AUTHORS.txt for contributors.
 * Author: Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * This program 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
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <core/optional.h>
#include <memory>

#include "pns_arc.h"
#include "pns_debug_decorator.h"
#include "pns_line_placer.h"
#include "pns_node.h"
#include "pns_router.h"
#include "pns_shove.h"
#include "pns_topology.h"
#include "pns_walkaround.h"


namespace PNS {

LINE_PLACER::LINE_PLACER( ROUTER* aRouter ) :
    PLACEMENT_ALGO( aRouter )
{
    m_initial_direction = DIRECTION_45::N;
    m_world = NULL;
    m_shove = NULL;
    m_currentNode = NULL;
    m_idle = true;

    // Init temporary variables (do not leave uninitialized members)
    m_lastNode = NULL;
    m_placingVia = false;
    m_currentNet = 0;
    m_currentLayer = 0;
    m_currentMode = RM_MarkObstacles;
    m_startItem = NULL;
    m_chainedPlacement = false;
    m_orthoMode = false;
    m_placementCorrect = false;
}


LINE_PLACER::~LINE_PLACER()
{
}


void LINE_PLACER::setWorld( NODE* aWorld )
{
    m_world = aWorld;
}


const VIA LINE_PLACER::makeVia( const VECTOR2I& aP )
{
    const LAYER_RANGE layers( m_sizes.GetLayerTop(), m_sizes.GetLayerBottom() );

    return VIA( aP, layers, m_sizes.ViaDiameter(), m_sizes.ViaDrill(), -1, m_sizes.ViaType() );
}


bool LINE_PLACER::ToggleVia( bool aEnabled )
{
    m_placingVia = aEnabled;

    if( !aEnabled )
        m_head.RemoveVia();

    return true;
}


void LINE_PLACER::setInitialDirection( const DIRECTION_45& aDirection )
{
    m_initial_direction = aDirection;

    if( m_tail.SegmentCount() == 0 )
            m_direction = aDirection;
}


bool LINE_PLACER::handleSelfIntersections()
{
    SHAPE_LINE_CHAIN::INTERSECTIONS ips;
    SHAPE_LINE_CHAIN& head = m_head.Line();
    SHAPE_LINE_CHAIN& tail = m_tail.Line();

    // if there is no tail, there is nothing to intersect with
    if( tail.PointCount() < 2 )
        return false;

    if( head.PointCount() < 2 )
        return false;

    // completely new head trace? chop off the tail
    if( tail.CPoint(0) == head.CPoint(0) )
    {
        m_p_start = tail.CPoint( 0 );
        m_direction = m_initial_direction;
        tail.Clear();
        return true;
    }

    tail.Intersect( head, ips );

    // no intesection points - nothing to reduce
    if( ips.empty() )
        return false;

    int n = INT_MAX;
    VECTOR2I ipoint;

    // if there is more than one intersection, find the one that is
    // closest to the beginning of the tail.
    for( const SHAPE_LINE_CHAIN::INTERSECTION& i : ips )
    {
        if( i.our.Index() < n )
        {
            n = i.our.Index();
            ipoint = i.p;
        }
    }

    // ignore the point where head and tail meet
    if( ipoint == head.CPoint( 0 ) || ipoint == tail.CPoint( -1 ) )
        return false;

    // Intersection point is on the first or the second segment: just start routing
    // from the beginning
    if( n < 2 )
    {
        m_p_start = tail.CPoint( 0 );
        m_direction = m_initial_direction;
        tail.Clear();
        head.Clear();

        return true;
    }
    else
    {
        // Clip till the last tail segment before intersection.
        // Set the direction to the one of this segment.
        const SEG last = tail.CSegment( n - 1 );
        m_p_start = last.A;
        m_direction = DIRECTION_45( last );
        tail.Remove( n, -1 );
        return true;
    }

    return false;
}


bool LINE_PLACER::handlePullback()
{
    SHAPE_LINE_CHAIN& head = m_head.Line();
    SHAPE_LINE_CHAIN& tail = m_tail.Line();

    if( head.PointCount() < 2 )
        return false;

    int n = tail.PointCount();

    if( n == 0 )
    {
        return false;
    }
    else if( n == 1 )
    {
        m_p_start = tail.CPoint( 0 );
        tail.Clear();
        return true;
    }

    DIRECTION_45 first_head( head.CSegment( 0 ) );
    DIRECTION_45 last_tail( tail.CSegment( -1 ) );
    DIRECTION_45::AngleType angle = first_head.Angle( last_tail );

    // case 1: we have a defined routing direction, and the currently computed
    // head goes in different one.
    bool pullback_1 = false;    // (m_direction != DIRECTION_45::UNDEFINED && m_direction != first_head);

    // case 2: regardless of the current routing direction, if the tail/head
    // extremities form an acute or right angle, reduce the tail by one segment
    // (and hope that further iterations) will result with a cleaner trace
    bool pullback_2 = ( angle == DIRECTION_45::ANG_RIGHT || angle == DIRECTION_45::ANG_ACUTE );

    if( pullback_1 || pullback_2 )
    {
        const SEG last = tail.CSegment( -1 );
        m_direction = DIRECTION_45( last );
        m_p_start = last.A;

        wxLogTrace( "PNS", "Placer: pullback triggered [%d] [%s %s]",
                n, last_tail.Format().c_str(), first_head.Format().c_str() );

        // erase the last point in the tail, hoping that the next iteration will
        // result with a head trace that starts with a segment following our
        // current direction.
        if( n < 2 )
            tail.Clear(); // don't leave a single-point tail
        else
            tail.Remove( -1, -1 );

        if( !tail.SegmentCount() )
            m_direction = m_initial_direction;

        return true;
    }

    return false;
}


bool LINE_PLACER::reduceTail( const VECTOR2I& aEnd )
{
    SHAPE_LINE_CHAIN& head = m_head.Line();
    SHAPE_LINE_CHAIN& tail = m_tail.Line();

    int n = tail.SegmentCount();

    if( head.SegmentCount() < 1 )
        return false;

    // Don't attempt this for too short tails
    if( n < 2 )
        return false;

    // Start from the segment farthest from the end of the tail
    // int start_index = std::max(n - 1 - ReductionDepth, 0);

    DIRECTION_45 new_direction;
    VECTOR2I new_start;
    int reduce_index = -1;

    for( int i = tail.SegmentCount() - 1; i >= 0; i-- )
    {
        const SEG s = tail.CSegment( i );
        DIRECTION_45 dir( s );

        // calculate a replacement route and check if it matches
        // the direction of the segment to be replaced
        SHAPE_LINE_CHAIN replacement = dir.BuildInitialTrace( s.A, aEnd );

        if( replacement.SegmentCount() < 1 )
            continue;

        LINE tmp( m_tail, replacement );

        if( m_currentNode->CheckColliding( &tmp, ITEM::ANY_T ) )
            break;

        if( DIRECTION_45( replacement.CSegment( 0 ) ) == dir )
        {
            new_start = s.A;
            new_direction = dir;
            reduce_index = i;
        }
    }

    if( reduce_index >= 0 )
    {
        wxLogTrace( "PNS", "Placer: reducing tail: %d", reduce_index );
        SHAPE_LINE_CHAIN reducedLine = new_direction.BuildInitialTrace( new_start, aEnd );

        m_p_start = new_start;
        m_direction = new_direction;
        tail.Remove( reduce_index + 1, -1 );
        head.Clear();
        return true;
    }

    if( !tail.SegmentCount() )
        m_direction = m_initial_direction;

    return false;
}


bool LINE_PLACER::checkObtusity( const SEG& aA, const SEG& aB ) const
{
    const DIRECTION_45 dir_a( aA );
    const DIRECTION_45 dir_b( aB );

    return dir_a.IsObtuse( dir_b ) || dir_a == dir_b;
}


bool LINE_PLACER::mergeHead()
{
    SHAPE_LINE_CHAIN& head = m_head.Line();
    SHAPE_LINE_CHAIN& tail = m_tail.Line();

    const int ForbiddenAngles = DIRECTION_45::ANG_ACUTE
                                    | DIRECTION_45::ANG_HALF_FULL
                                    | DIRECTION_45::ANG_UNDEFINED;

    head.Simplify();
    tail.Simplify();

    int n_head  = head.SegmentCount();
    int n_tail  = tail.SegmentCount();

    if( n_head < 3 )
    {
        wxLogTrace( "PNS", "Merge failed: not enough head segs." );
        return false;
    }

    if( n_tail && head.CPoint( 0 ) != tail.CPoint( -1 ) )
    {
        wxLogTrace( "PNS", "Merge failed: head and tail discontinuous." );
        return false;
    }

    if( m_head.CountCorners( ForbiddenAngles ) != 0 )
        return false;

    DIRECTION_45 dir_tail, dir_head;

    dir_head = DIRECTION_45( head.CSegment( 0 ) );

    if( n_tail )
    {
        dir_tail = DIRECTION_45( tail.CSegment( -1 ) );

        if( dir_head.Angle( dir_tail ) & ForbiddenAngles )
            return false;
    }

    tail.Append( head );
    tail.Remove( -1 );

    tail.Simplify();

    SEG last = tail.CSegment( -1 );

    m_p_start = last.B;
    m_direction = DIRECTION_45( last ).Right();

    head.Remove( 0, -1 );

    wxLogTrace( "PNS", "Placer: merge %d, new direction: %s", n_head, m_direction.Format().c_str() );

    head.Simplify();
    tail.Simplify();

    return true;
}


VECTOR2I closestProjectedPoint( const SHAPE_LINE_CHAIN& line, const VECTOR2I& p )
{
    int min_dist = INT_MAX;
    VECTOR2I closest;

    for(int i = 0; i < line.SegmentCount(); i++ )
    {
        const auto& s = line.CSegment(i);
        auto a = s.NearestPoint( p );
        auto d = (a - p).EuclideanNorm();

        if( d < min_dist )
        {
            min_dist = d;
            closest = a;
        }
    }

    return closest;
}


bool LINE_PLACER::rhWalkOnly( const VECTOR2I& aP, LINE& aNewHead )
{
    LINE initTrack( m_head );
    LINE walkFull( m_head );
    int effort = 0;
    bool rv = true, viaOk;

    viaOk = buildInitialLine( aP, initTrack );

    WALKAROUND walkaround( m_currentNode, Router() );

    walkaround.SetSolidsOnly( false );
    walkaround.SetDebugDecorator( Dbg() );
    walkaround.SetLogger( Logger() );
    walkaround.SetIterationLimit( Settings().WalkaroundIterationLimit() );

    WALKAROUND::RESULT wr = walkaround.Route( initTrack );
    //WALKAROUND::WALKAROUND_STATUS wf = walkaround.Route( initTrack, walkFull, false );

    auto l_cw = wr.lineCw.CLine();
    auto l_ccw = wr.lineCcw.CLine();

    if( wr.statusCcw == WALKAROUND::ALMOST_DONE || wr.statusCw == WALKAROUND::ALMOST_DONE )
    {

        auto p_cw = closestProjectedPoint( l_cw, aP );
        auto p_ccw = closestProjectedPoint( l_ccw, aP );

        int idx_cw = l_cw.Split( p_cw );
        int idx_ccw = l_ccw.Split( p_ccw );

        l_cw = l_cw.Slice( 0, idx_cw );
        l_ccw = l_ccw.Slice( 0, idx_ccw );

        //Dbg()->AddLine( wr.lineCw.CLine(), 3, 40000 );

        //Dbg()->AddPoint( p_cw, 4 );
        //Dbg()->AddPoint( p_ccw, 5 );

        Dbg()->AddLine( wr.lineCw.CLine(), 4, 1000 );
        Dbg()->AddLine( wr.lineCcw.CLine(), 5, 1000 );

    }

    walkFull.SetShape( l_ccw.Length() < l_cw.Length() ? l_ccw : l_cw );

    Dbg()->AddLine( walkFull.CLine(), 2, 100000, "walk-full" );

    switch( Settings().OptimizerEffort() )
    {
    case OE_LOW:
        effort = 0;
        break;

    case OE_MEDIUM:
    case OE_FULL:
        effort = OPTIMIZER::MERGE_SEGMENTS;
        break;
    }

    if( Settings().SmartPads() )
        effort |= OPTIMIZER::SMART_PADS;

    if( wr.statusCw == WALKAROUND::STUCK || wr.statusCcw == WALKAROUND::STUCK )
    {
        walkFull = walkFull.ClipToNearestObstacle( m_currentNode );
        rv = true;
    }
    else if( m_placingVia && viaOk )
    {
        walkFull.AppendVia( makeVia( walkFull.CPoint( -1 ) ) );
    }

    OPTIMIZER::Optimize( &walkFull, effort, m_currentNode );

    if( m_currentNode->CheckColliding( &walkFull ) )
    {
        aNewHead = m_head;
        return false;
    }

    m_head = walkFull;
    aNewHead = walkFull;

    return rv;
}


bool LINE_PLACER::rhMarkObstacles( const VECTOR2I& aP, LINE& aNewHead )
{
    LINE newHead( m_head ), bestHead( m_head );
    bool hasBest = false;

    buildInitialLine( aP, newHead );

    NODE::OBSTACLES obstacles;

    m_currentNode->QueryColliding( &newHead, obstacles );

    // If we are allowing DRC violations, we don't push back to the hull
    if( !Settings().CanViolateDRC() )
    {
        for( OBSTACLE& obs : obstacles )
        {
            int cl = m_currentNode->GetClearance( obs.m_item, &newHead );
            const SHAPE_LINE_CHAIN hull = obs.m_item->Hull( cl, newHead.Width(), newHead.Layer() );

            VECTOR2I nearest = hull.NearestPoint( aP );
            Dbg()->AddLine( hull, 2, 10000 );

            if( ( nearest - aP ).EuclideanNorm() < newHead.Width() + cl )
            {
                buildInitialLine( nearest, newHead );

                // We want the shortest line here to ensure we don't break a clearance
                // rule on larger, overlapping items (e.g. vias)
                if( newHead.CLine().Length() < bestHead.CLine().Length() )
                {
                    bestHead = newHead;
                    hasBest = true;
                }
            }
        }
    }

    if( hasBest )
        m_head = bestHead;
    else
        m_head = newHead;

    aNewHead = m_head;

    return static_cast<bool>( m_currentNode->CheckColliding( &m_head ) );
}


const LINE LINE_PLACER::reduceToNearestObstacle( const LINE& aOriginalLine )
{
    const auto& l0 = aOriginalLine.CLine();

    if ( !l0.PointCount() )
        return aOriginalLine;

    int l = l0.Length();
    int step = l / 2;
    VECTOR2I target;

    LINE l_test( aOriginalLine );

    while( step > 0 )
    {
        target = l0.PointAlong( l );
        SHAPE_LINE_CHAIN l_cur( l0 );

        int index = l_cur.Split( target );

        l_test.SetShape( l_cur.Slice( 0, index ) );

        if ( m_currentNode->CheckColliding( &l_test ) )
            l -= step;
        else
            l += step;

        step /= 2;
    }

    l = l_test.CLine().Length();

    while( m_currentNode->CheckColliding( &l_test ) && l > 0 )
    {
        l--;
        target = l0.PointAlong( l );
        SHAPE_LINE_CHAIN l_cur( l0 );

        int index = l_cur.Split( target );

        l_test.SetShape( l_cur.Slice( 0, index ) );
    }

    return l_test;
}


bool LINE_PLACER::rhStopAtNearestObstacle( const VECTOR2I& aP, LINE& aNewHead )
{
    LINE l0;
    l0 = m_head;

    buildInitialLine( aP, l0 );

    LINE l_cur = reduceToNearestObstacle( l0 );

    const SHAPE_LINE_CHAIN l_shape = l_cur.CLine();

    if( l_shape.SegmentCount() == 0 )
        return false;

    if( l_shape.SegmentCount() == 1 )
    {
        SEG s = l_shape.CSegment( 0 );

        VECTOR2I dL( DIRECTION_45( s ).Left().ToVector() );
        VECTOR2I dR( DIRECTION_45( s ).Right().ToVector() );

        SEG leadL( s.B, s.B + dL );
        SEG leadR( s.B, s.B + dR );

        SEG segL( s.B, leadL.LineProject( aP ) );
        SEG segR( s.B, leadR.LineProject( aP ) );

        LINE finishL( l0, SHAPE_LINE_CHAIN( { segL.A, segL.B } ) );
        LINE finishR( l0, SHAPE_LINE_CHAIN( { segR.A, segR.B } ) );

        LINE reducedL = reduceToNearestObstacle( finishL );
        LINE reducedR = reduceToNearestObstacle( finishR );

        int lL = reducedL.CLine().Length();
        int lR = reducedR.CLine().Length();

        if( lL > lR )
            l_cur.Line().Append( reducedL.CLine() );
        else
            l_cur.Line().Append( reducedR.CLine() );

        l_cur.Line().Simplify();
    }

    m_head = l_cur;
    aNewHead = m_head;
    return true;
}


bool LINE_PLACER::rhShoveOnly( const VECTOR2I& aP, LINE& aNewHead )
{
    LINE initTrack( m_head );
    LINE walkSolids, l2;

    bool viaOk = buildInitialLine( aP, initTrack );

    m_currentNode = m_shove->CurrentNode();

    m_shove->SetLogger( Logger() );
    m_shove->SetDebugDecorator( Dbg() );

    OPTIMIZER optimizer( m_currentNode );

    WALKAROUND walkaround( m_currentNode, Router() );

    walkaround.SetSolidsOnly( true );
    walkaround.SetIterationLimit( 10 );
    walkaround.SetDebugDecorator( Dbg() );
    walkaround.SetLogger( Logger() );
    WALKAROUND::WALKAROUND_STATUS stat_solids = walkaround.Route( initTrack, walkSolids );

    optimizer.SetEffortLevel( OPTIMIZER::MERGE_SEGMENTS );
    optimizer.SetCollisionMask( ITEM::SOLID_T );
    optimizer.Optimize( &walkSolids );

    if( stat_solids == WALKAROUND::DONE )
        l2 = walkSolids;
    else
        l2 = initTrack.ClipToNearestObstacle( m_shove->CurrentNode() );

    LINE l( m_tail );
    l.Line().Append( l2.CLine() );
    l.Line().Simplify();

    if( l.PointCount() == 0 || l2.PointCount() == 0 )
    {
        aNewHead = m_head;
        return false;
    }

    if( m_placingVia && viaOk )
    {
        VIA v1( makeVia( l.CPoint( -1 ) ) );
        VIA v2( makeVia( l2.CPoint( -1 ) ) );

        l.AppendVia( v1 );
        l2.AppendVia( v2 );
    }

    l.Line().Simplify();

    // in certain, uncommon cases there may be loops in the head+tail, In such case, we don't shove to avoid
    // screwing up the database.
    if( l.HasLoops() )
    {
        aNewHead = m_head;
        return false;
    }

    SHOVE::SHOVE_STATUS status = m_shove->ShoveLines( l );

    m_currentNode = m_shove->CurrentNode();

    if( status == SHOVE::SH_OK  || status == SHOVE::SH_HEAD_MODIFIED )
    {
        if( status == SHOVE::SH_HEAD_MODIFIED )
            l2 = m_shove->NewHead();

        optimizer.SetWorld( m_currentNode );

        int effortLevel = OPTIMIZER::MERGE_OBTUSE;

        if( Settings().SmartPads() )
            effortLevel = OPTIMIZER::SMART_PADS;

        optimizer.SetEffortLevel( effortLevel );

        optimizer.SetCollisionMask( ITEM::ANY_T );
        optimizer.Optimize( &l2 );

        aNewHead = l2;

        return true;
    }
    else
    {
        walkaround.SetWorld( m_currentNode );
        walkaround.SetSolidsOnly( false );
        walkaround.SetIterationLimit( 10 );
        walkaround.SetApproachCursor( true, aP );
        walkaround.Route( initTrack, l2 );
        aNewHead = l2.ClipToNearestObstacle( m_shove->CurrentNode() );

        return false;
    }

    return false;
}


bool LINE_PLACER::routeHead( const VECTOR2I& aP, LINE& aNewHead )
{
    switch( m_currentMode )
    {
    case RM_MarkObstacles:
        return rhMarkObstacles( aP, aNewHead );
    case RM_Walkaround:
        return rhWalkOnly( aP, aNewHead );
    case RM_Shove:
        return rhShoveOnly( aP, aNewHead );
    default:
        break;
    }

    return false;
}


bool LINE_PLACER::optimizeTailHeadTransition()
{
    LINE linetmp = Trace();

    if( OPTIMIZER::Optimize( &linetmp, OPTIMIZER::FANOUT_CLEANUP, m_currentNode ) )
    {
        if( linetmp.SegmentCount() < 1 )
            return false;

        m_head = linetmp;
        m_p_start = linetmp.CLine().CPoint( 0 );
        m_direction = DIRECTION_45( linetmp.CSegment( 0 ) );
        m_tail.Line().Clear();

        return true;
    }

    SHAPE_LINE_CHAIN& head = m_head.Line();
    SHAPE_LINE_CHAIN& tail = m_tail.Line();

    int tailLookbackSegments = 3;

    //if(m_currentMode() == RM_Walkaround)
    //    tailLookbackSegments = 10000;

    int threshold = std::min( tail.PointCount(), tailLookbackSegments + 1 );

    if( tail.SegmentCount() < 3 )
        return false;

    // assemble TailLookbackSegments tail segments with the current head
    SHAPE_LINE_CHAIN opt_line = tail.Slice( -threshold, -1 );

    int end = std::min(2, head.PointCount() - 1 );

    opt_line.Append( head.Slice( 0, end ) );

    LINE new_head( m_tail, opt_line );

    // and see if it could be made simpler by merging obtuse/collnear segments.
    // If so, replace the (threshold) last tail points and the head with
    // the optimized line

    if( OPTIMIZER::Optimize( &new_head, OPTIMIZER::MERGE_OBTUSE, m_currentNode ) )
    {
        LINE tmp( m_tail, opt_line );

        wxLogTrace( "PNS", "Placer: optimize tail-head [%d]", threshold );

        head.Clear();
        tail.Replace( -threshold, -1, new_head.CLine() );
        tail.Simplify();

        m_p_start = new_head.CLine().CPoint( -1 );
        m_direction = DIRECTION_45( new_head.CSegment( -1 ) );

        return true;
    }

    return false;
}


void LINE_PLACER::routeStep( const VECTOR2I& aP )
{
    bool fail = false;
    bool go_back = false;

    int i, n_iter = 1;

    LINE new_head;

    wxLogTrace( "PNS", "INIT-DIR: %s head: %d, tail: %d segs",
                m_initial_direction.Format().c_str(),
                m_head.SegmentCount(),
                m_tail.SegmentCount() );

    for( i = 0; i < n_iter; i++ )
    {
        if( !go_back && Settings().FollowMouse() )
            reduceTail( aP );

        go_back = false;

        if( !routeHead( aP, new_head ) )
            fail = true;

        if( !new_head.Is45Degree() )
            fail = true;

        if( !Settings().FollowMouse() )
            return;

        m_head = new_head;

        if( handleSelfIntersections() )
        {
            n_iter++;
            go_back = true;
        }

        if( !go_back && handlePullback() )
        {
            n_iter++;
            go_back = true;
        }

    }

    if( !fail )
    {
       if( optimizeTailHeadTransition() )
          return;

        mergeHead();
    }
}


bool LINE_PLACER::route( const VECTOR2I& aP )
{
    routeStep( aP );

    if (!m_head.PointCount() )
        return false;

    return m_head.CPoint(-1) == aP;
}


const LINE LINE_PLACER::Trace() const
{
    LINE tmp( m_head );

    tmp.SetShape( m_tail.CLine() );
    tmp.Line().Append( m_head.CLine() );
    tmp.Line().Simplify();
    return tmp;
}


const ITEM_SET LINE_PLACER::Traces()
{
    m_currentTrace = Trace();
    return ITEM_SET( &m_currentTrace );
}


void LINE_PLACER::FlipPosture()
{
    m_postureSolver.FlipPosture();
}


NODE* LINE_PLACER::CurrentNode( bool aLoopsRemoved ) const
{
    if( aLoopsRemoved && m_lastNode )
        return m_lastNode;

    return m_currentNode;
}


bool LINE_PLACER::SplitAdjacentSegments( NODE* aNode, ITEM* aSeg, const VECTOR2I& aP )
{
    if( !aSeg )
        return false;

    if( !aSeg->OfKind( ITEM::SEGMENT_T ) )
        return false;

    JOINT* jt = aNode->FindJoint( aP, aSeg );

    if( jt && jt->LinkCount() >= 1 )
        return false;

    SEGMENT* s_old = static_cast<SEGMENT*>( aSeg );

    std::unique_ptr<SEGMENT> s_new[2] = { Clone( *s_old ), Clone( *s_old ) };

    s_new[0]->SetEnds( s_old->Seg().A, aP );
    s_new[1]->SetEnds( aP, s_old->Seg().B );

    aNode->Remove( s_old );
    aNode->Add( std::move( s_new[0] ), true );
    aNode->Add( std::move( s_new[1] ), true );

    return true;
}


bool LINE_PLACER::SetLayer( int aLayer )
{
    if( m_idle )
    {
        m_currentLayer = aLayer;
        return true;
    }
    else if( m_chainedPlacement )
    {
        return false;
    }
    else if( !m_startItem
            || ( m_startItem->OfKind( ITEM::VIA_T ) && m_startItem->Layers().Overlaps( aLayer ) )
            || ( m_startItem->OfKind( ITEM::SOLID_T ) && m_startItem->Layers().Overlaps( aLayer ) ) )
    {
        m_currentLayer = aLayer;
        m_head.Line().Clear();
        m_tail.Line().Clear();
        m_head.SetLayer( m_currentLayer );
        m_tail.SetLayer( m_currentLayer );
        Move( m_currentEnd, NULL );
        return true;
    }

    return false;
}


bool LINE_PLACER::Start( const VECTOR2I& aP, ITEM* aStartItem )
{
    m_placementCorrect = false;
    m_currentStart = VECTOR2I( aP );
    m_currentEnd = VECTOR2I( aP );
    m_currentNet = std::max( 0, aStartItem ? aStartItem->Net() : 0 );
    m_startItem = aStartItem;
    m_placingVia = false;
    m_chainedPlacement = false;
    m_fixedTail.Clear();

    setInitialDirection( Settings().InitialDirection() );

    initPlacement();

    m_postureSolver.Clear();
    m_postureSolver.AddTrailPoint( aP );
    m_postureSolver.SetTolerance( m_head.Width() );
    m_postureSolver.SetDefaultDirections( m_initial_direction, DIRECTION_45::UNDEFINED );

    NODE *n;

    if ( m_shove )
        n = m_shove->CurrentNode();
    else
        n = m_currentNode;

    m_fixedTail.AddStage( m_currentStart, m_currentLayer, m_placingVia, m_direction, n );

    return true;
}


void LINE_PLACER::initPlacement()
{
    m_idle = false;

    m_head.Line().Clear();
    m_tail.Line().Clear();
    m_head.SetNet( m_currentNet );
    m_tail.SetNet( m_currentNet );
    m_head.SetLayer( m_currentLayer );
    m_tail.SetLayer( m_currentLayer );
    m_head.SetWidth( m_sizes.TrackWidth() );
    m_tail.SetWidth( m_sizes.TrackWidth() );
    m_head.RemoveVia();
    m_tail.RemoveVia();

    m_p_start = m_currentStart;
    m_direction = m_initial_direction;

    NODE* world = Router()->GetWorld();

    world->KillChildren();
    NODE* rootNode = world->Branch();

    SplitAdjacentSegments( rootNode, m_startItem, m_currentStart );

    setWorld( rootNode );

    wxLogTrace( "PNS", "world %p, intitial-direction %s layer %d",
                m_world,
                m_direction.Format().c_str(),
                m_currentLayer );

    m_lastNode = NULL;
    m_currentNode = m_world;
    m_currentMode = Settings().Mode();

    m_shove.reset();

    if( m_currentMode == RM_Shove || m_currentMode == RM_Smart )
        m_shove = std::make_unique<SHOVE>( m_world->Branch(), Router() );
}


bool LINE_PLACER::Move( const VECTOR2I& aP, ITEM* aEndItem )
{
    LINE current;
    VECTOR2I p = aP;
    int eiDepth = -1;

    if( aEndItem && aEndItem->Owner() )
        eiDepth = static_cast<NODE*>( aEndItem->Owner() )->Depth();

    if( m_lastNode )
    {
        delete m_lastNode;
        m_lastNode = NULL;
    }

    bool reachesEnd = route( p );

    current = Trace();

    if( !current.PointCount() )
        m_currentEnd = m_p_start;
    else
        m_currentEnd = current.CLine().CPoint( -1 );

    NODE* latestNode = m_currentNode;
    m_lastNode = latestNode->Branch();

    if( reachesEnd
            && eiDepth >= 0
            && aEndItem && latestNode->Depth() > eiDepth
            && current.SegmentCount() )
    {
        SplitAdjacentSegments( m_lastNode, aEndItem, current.CPoint( -1 ) );

        if( Settings().RemoveLoops() )
            removeLoops( m_lastNode, current );
    }

    updateLeadingRatLine();
    m_postureSolver.AddTrailPoint( aP );
    return true;
}


bool LINE_PLACER::FixRoute( const VECTOR2I& aP, ITEM* aEndItem, bool aForceFinish )
{
    bool realEnd = false;
    int lastV;

    LINE pl = Trace();

    if( m_currentMode == RM_MarkObstacles )
    {
        // Mark Obstacles is sort of a half-manual, half-automated mode in which the
        // user has more responsibility and authority.

        if( aEndItem )
        {
            // The user has indicated a connection should be made.  If either the trace or
            // endItem is net-less, then allow the connection by adopting the net of the other.
            if( m_currentNet <= 0 )
            {
                m_currentNet = aEndItem->Net();
                pl.SetNet( m_currentNet );
            }
            else if (aEndItem->Net() <= 0 )
            {
                aEndItem->SetNet( m_currentNet );
            }
        }

        // Collisions still prevent fixing unless "Allow DRC violations" is checked
        if( !Settings().CanViolateDRC() && m_world->CheckColliding( &pl ) )
            return false;
    }

    const SHAPE_LINE_CHAIN& l = pl.CLine();

    if( !l.SegmentCount() )
    {
        // Nothing to commit if we have an empty line
        if( !pl.EndsWithVia() )
            return false;

        m_lastNode->Add( Clone( pl.Via() ) );
        m_currentNode = NULL;

        m_idle = true;
        m_placementCorrect = true;
        return true;
    }

    VECTOR2I p_pre_last = l.CPoint( -1 );
    const VECTOR2I p_last = l.CPoint( -1 );
    DIRECTION_45 d_last( l.CSegment( -1 ) );

    if( l.PointCount() > 2 )
        p_pre_last = l.CPoint( -2 );

    if( aEndItem && m_currentNet >= 0 && m_currentNet == aEndItem->Net() )
        realEnd = true;

    if( aForceFinish )
        realEnd = true;

    if( realEnd || m_placingVia )
        lastV = l.SegmentCount();
    else
        lastV = std::max( 1, l.SegmentCount() - 1 );

    SEGMENT* lastSeg = nullptr;
    int      lastArc = -1;

    for( int i = 0; i < lastV; i++ )
    {
        ssize_t arcIndex = l.ArcIndex( i );

        if( arcIndex < 0 )
        {
            const SEG& s = pl.CSegment( i );
            auto       seg = std::make_unique<SEGMENT>( s, m_currentNet );
            seg->SetWidth( pl.Width() );
            seg->SetLayer( m_currentLayer );

            if( !m_lastNode->Add( std::move( seg ) ) )
                lastSeg = nullptr;
        }
        else
        {
            if( arcIndex == lastArc )
                continue;

            auto arc = std::make_unique<ARC>( l.Arc( arcIndex ), m_currentNet );
            arc->SetWidth( pl.Width() );
            arc->SetLayer( m_currentLayer );
            m_lastNode->Add( std::move( arc ) );
            lastSeg = nullptr;
            lastArc = arcIndex;
        }
    }

    if( pl.EndsWithVia() )
        m_lastNode->Add( Clone( pl.Via() ) );

    if( realEnd && lastSeg )
        simplifyNewLine( m_lastNode, lastSeg );

    if( !realEnd )
    {
        setInitialDirection( d_last );
        m_currentStart = m_placingVia ? p_last : p_pre_last;

        m_fixedTail.AddStage( m_p_start, m_currentLayer, m_placingVia, m_direction, m_currentNode );

        m_startItem = NULL;
        m_placingVia = false;
        m_chainedPlacement = !pl.EndsWithVia();

        m_p_start = m_currentStart;
        m_direction = m_initial_direction;

        m_head.Line().Clear();
        m_tail.Line().Clear();
        m_head.RemoveVia();
        m_tail.RemoveVia();
        m_currentNode = m_lastNode;
        m_lastNode = m_lastNode->Branch();

        if ( m_shove )
            m_shove->AddLockedSpringbackNode( m_currentNode );

        m_postureSolver.Clear();
        m_postureSolver.SetTolerance( m_head.Width() );
        m_postureSolver.AddTrailPoint( m_currentStart );
        m_postureSolver.SetDefaultDirections( m_initial_direction, d_last );

        m_placementCorrect = true;
    }
    else
    {
        m_placementCorrect = true;
        m_idle = true;
    }

    return realEnd;
}


bool LINE_PLACER::UnfixRoute()
{
    FIXED_TAIL::STAGE st;

    if ( !m_fixedTail.PopStage( st ) )
        return false;

    m_head.Line().Clear();
    m_tail.Line().Clear();
    m_startItem = NULL;
    m_p_start = st.pts[0].p;
    m_direction = st.pts[0].direction;
    m_placingVia = st.pts[0].placingVias;
    m_currentNode = st.commit;
    m_currentLayer = st.pts[0].layer;
    m_head.SetLayer( m_currentLayer );
    m_tail.SetLayer( m_currentLayer );
    m_head.RemoveVia();
    m_tail.RemoveVia();

    if (m_shove)
    {
        m_shove->RewindSpringbackTo( m_currentNode );
        m_shove->UnlockSpringbackNode( m_currentNode );
        m_currentNode = m_shove->CurrentNode();
        m_currentNode->KillChildren();
    }

    m_lastNode = m_currentNode->Branch();

    return true;
}


bool LINE_PLACER::HasPlacedAnything() const
{
     return m_placementCorrect || m_fixedTail.StageCount() > 1;
}


bool LINE_PLACER::CommitPlacement()
{
    if( m_lastNode )
        Router()->CommitRouting( m_lastNode );

    m_lastNode = NULL;
    m_currentNode = NULL;
    return true;
}


void LINE_PLACER::removeLoops( NODE* aNode, LINE& aLatest )
{
    if( !aLatest.SegmentCount() )
        return;

    if( aLatest.CLine().CPoint( 0 ) == aLatest.CLine().CPoint( -1 ) )
        return;

    std::set<LINKED_ITEM *> toErase;
    aNode->Add( aLatest, true );

    for( int s = 0; s < aLatest.LinkCount(); s++ )
    {
        LINKED_ITEM* seg = aLatest.GetLink(s);
        LINE ourLine = aNode->AssembleLine( seg );
        JOINT a, b;
        std::vector<LINE> lines;

        aNode->FindLineEnds( ourLine, a, b );

        if( a == b )
            aNode->FindLineEnds( aLatest, a, b );

        aNode->FindLinesBetweenJoints( a, b, lines );

        int removedCount = 0;
        int total = 0;

        for( LINE& line : lines )
        {
            total++;

            if( !( line.ContainsLink( seg ) ) && line.SegmentCount() )
            {
                for( LINKED_ITEM* ss : line.Links() )
                    toErase.insert( ss );

                removedCount++;
            }
        }

        wxLogTrace( "PNS", "total segs removed: %d/%d", removedCount, total );
    }

    for( LINKED_ITEM* s : toErase )
        aNode->Remove( s );

    aNode->Remove( aLatest );
}


void LINE_PLACER::simplifyNewLine( NODE* aNode, SEGMENT* aLatest )
{
    LINE l = aNode->AssembleLine( aLatest );
    SHAPE_LINE_CHAIN simplified( l.CLine() );

    simplified.Simplify();

    if( simplified.PointCount() != l.PointCount() )
    {
        aNode->Remove( l );
        l.SetShape( simplified );
        aNode->Add( l );
    }
}


void LINE_PLACER::UpdateSizes( const SIZES_SETTINGS& aSizes )
{
    // initPlacement will kill the tail, don't do that unless the track size has changed
    if( !m_idle && aSizes.TrackWidth() != m_sizes.TrackWidth() )
    {
        m_sizes = aSizes;
        initPlacement();
    }

    m_sizes = aSizes;
}


void LINE_PLACER::updateLeadingRatLine()
{
    LINE current = Trace();
    SHAPE_LINE_CHAIN ratLine;
    TOPOLOGY topo( m_lastNode );

    if( topo.LeadingRatLine( &current, ratLine ) )
        m_router->GetInterface()->DisplayRatline( ratLine, 5 );
}


void LINE_PLACER::SetOrthoMode( bool aOrthoMode )
{
    m_orthoMode = aOrthoMode;
}


bool LINE_PLACER::buildInitialLine( const VECTOR2I& aP, LINE& aHead )
{
    SHAPE_LINE_CHAIN l;
    int initial_radius = 0;

    DIRECTION_45 guessedDir = m_postureSolver.GetPosture( aP );

    if( m_p_start == aP )
    {
        l.Clear();
    }
    else
    {
        if( Settings().GetFreeAngleMode() && Settings().Mode() == RM_MarkObstacles )
        {
            l = SHAPE_LINE_CHAIN( { m_p_start, aP } );
        }
        else
        {
            // Rounded corners don't make sense when routing orthogonally (single track at a time)
            if( Settings().GetRounded()  && !m_orthoMode )
                initial_radius = Settings().GetMaxRadius();


            if( !m_tail.PointCount() )
                l = guessedDir.BuildInitialTrace( m_p_start, aP, false, initial_radius );
            else
                l = m_direction.BuildInitialTrace( m_p_start, aP, false, initial_radius );
        }

        if( l.SegmentCount() > 1 && m_orthoMode )
        {
            VECTOR2I newLast = l.CSegment( 0 ).LineProject( l.CPoint( -1 ) );

            l.Remove( -1, -1 );
            l.SetPoint( 1, newLast );
        }
    }

    aHead.SetLayer( m_currentLayer );
    aHead.SetShape( l );

    if( !m_placingVia )
        return true;

    VIA v( makeVia( aP ) );
    v.SetNet( aHead.Net() );

    if( m_currentMode == RM_MarkObstacles )
    {
        aHead.AppendVia( v );
        return true;
    }

    VECTOR2I force;
    VECTOR2I lead = aP - m_p_start;

    bool solidsOnly = ( m_currentMode != RM_Walkaround );

    if( v.PushoutForce( m_currentNode, lead, force, solidsOnly, 40 ) )
    {
        SHAPE_LINE_CHAIN line = m_direction.BuildInitialTrace( m_p_start, aP + force, initial_radius );
        aHead = LINE( aHead, line );

        v.SetPos( v.Pos() + force );
        return true;
    }

    return false; // via placement unsuccessful
}


void LINE_PLACER::GetModifiedNets( std::vector<int>& aNets ) const
{
    aNets.push_back( m_currentNet );
}


bool LINE_PLACER::AbortPlacement()
{
    m_world->KillChildren();
    return true;
}

FIXED_TAIL::FIXED_TAIL( int aLineCount )
{

}

FIXED_TAIL::~FIXED_TAIL()
{

}

void FIXED_TAIL::Clear()
{
    m_stages.clear();
}

void FIXED_TAIL::AddStage( VECTOR2I aStart, int aLayer, bool placingVias, DIRECTION_45 direction,
                           NODE *aNode )
{
    STAGE st;
    FIX_POINT pt;

    pt.p = aStart;
    pt.layer = aLayer;
    pt.direction = direction;
    pt.placingVias = placingVias;

    st.pts.push_back(pt);
    st.commit = aNode;

    m_stages.push_back( st );
}

bool FIXED_TAIL::PopStage( FIXED_TAIL::STAGE& aStage )
{
    if( !m_stages.size() )
        return false;

    aStage = m_stages.back();

    if( m_stages.size() > 1 )
        m_stages.pop_back();

    return true;
}


int FIXED_TAIL::StageCount() const
{
    return m_stages.size();
}


POSTURE_SOLVER::POSTURE_SOLVER()
{
    m_forced = false;
    m_tolerance = 0;
}


POSTURE_SOLVER::~POSTURE_SOLVER() {}


void POSTURE_SOLVER::Clear()
{
    m_forced = false;
    m_trail.Clear();
}


void POSTURE_SOLVER::AddTrailPoint( const VECTOR2I& aP )
{
    if( m_trail.SegmentCount() == 0 )
    {
        m_trail.Append( aP );
    }
    else
    {
        SEG s_new( m_trail.CPoint( -1 ), aP );

        for( int i = 0; i < m_trail.SegmentCount() - 1; i++ )
        {
            const auto& s_trail = m_trail.CSegment( i );

            if( s_trail.Distance( s_new ) <= m_tolerance )
            {
                m_trail = m_trail.Slice( 0, i );
                break;
            }
        }

        m_trail.Append( aP );
    }

    m_trail.Simplify();

    auto dbg = ROUTER::GetInstance()->GetInterface()->GetDebugDecorator();

    dbg->AddLine( m_trail, 5, 100000 );
}


DIRECTION_45 POSTURE_SOLVER::GetPosture( const VECTOR2I& aP )
{
    // Adjusts how far away from p0 we get before whatever posture we solved is locked in
    const int lockDistanceFactor = 40;

    // Adjusts how close to p0 we unlock the posture again if one was locked already
    const int unlockDistanceFactor = 4;

    if( m_trail.PointCount() < 2 )
        return m_direction;

    auto dbg = ROUTER::GetInstance()->GetInterface()->GetDebugDecorator();

    auto p0 = m_trail.CPoint( 0 );

    double refLength = SEG( p0, aP ).Length();

    SHAPE_LINE_CHAIN straight( DIRECTION_45().BuildInitialTrace( p0, aP, false ) );
    straight.SetClosed( true );
    straight.Append( m_trail.Reverse() );
    dbg->AddLine( straight, m_forced ? 3 : 2, 100000 );

    double areaS = straight.Area();

    SHAPE_LINE_CHAIN diag( DIRECTION_45().BuildInitialTrace( p0, aP, true ) );
    diag.Append( m_trail.Reverse() );
    diag.SetClosed( true );
    dbg->AddLine( diag, 1, 100000 );

    double areaDiag = diag.Area();
    double ratio    = abs( areaS ) / ( fabs( areaDiag ) + 1.0 );

    // heuristic to detect that the user dragged back the cursor to the beginning of the trace
    // in this case, we cancel any forced posture and restart the trail
    if( m_forced && refLength < unlockDistanceFactor * m_tolerance )
    {
        m_forced = false;
        VECTOR2I start = p0;
        m_trail.Clear();
        m_trail.Append( start );
    }

    // If we get far away from the initial point, lock in the current solution to prevent flutter
    if( !m_forced && refLength > lockDistanceFactor * m_tolerance )
        m_forced = true;

    if( m_forced )
        return m_direction;
    else if( ratio > areaRatioThreshold + areaRatioEpsilon )
        m_direction = DIRECTION_45::NE;
    else if( ratio < ( 1.0 / areaRatioThreshold ) - areaRatioEpsilon )
        m_direction = DIRECTION_45::N;
    else if( m_lastSegDirection != DIRECTION_45::UNDEFINED )
        m_direction = m_lastSegDirection;

    return m_direction;
}


void POSTURE_SOLVER::FlipPosture()
{
    m_direction = m_direction.Right();
    m_forced = true;
}


}