TSegment.cxx

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// $Id: TSegment.cxx,v 1.16 2010/03/31 09:58:59 liucy Exp $
//-----------------------------------------------------------------------------
// Filename : TSegment.cc
// Section  : Tracking
// Owner    : Yoshi Iwasaki
// Email    : [email protected]
//-----------------------------------------------------------------------------
// Description : A class to manage a group of TMLink's.
//               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
//-----------------------------------------------------------------------------
 
#include "TrkReco/TTrack.h"
#include "TrkReco/TSegment.h"
#include "TrkReco/TMDCUtil.h"
#include "TrkReco/TMLink.h"
#include "TrkReco/TMDC.h"
#include "TrkReco/TMDCWire.h"
#include "TrkReco/TMDCWireHit.h"
//#include "cdc/Range.h"
#include "TrkReco/Range.h"
 
#include "CLHEP/Vector/ThreeVector.h"
#include <math.h>
#include "CLHEP/Matrix/Vector.h"
 
TMDC *
TSegment::_cdc = 0;
 
TSegment::TSegment()
: TTrackBase(),
  _innerWidth(0),
  _outerWidth(0),
  _nLayer(0),
  _clusterType(0),
  _duality(0.),
  _nDual(0),
  _angle(0.),
  _state(0),
  _lineTsf(0.,0.,0.) {
//    _times = 4.0;
//    if (_links[0]->wire()->stereo()) _times = 7.0;
    _times[0] = 7;
    _times[1] = 7;
    _times[2] = 4;
    _times[3] = 4;
    _times[4] = 4;
    _times[5] = 5;
    _times[6] = 5;
    _times[7] = 5;
    _times[8] = 5;
    _times[9] = 4;
    _times[10] = 4;
    _fitted = false;
}
 
TSegment::TSegment(const AList<TMLink> & a)
: TTrackBase(a),
  _innerWidth(0),
  _outerWidth(0),
  _nLayer(0),
  _clusterType(0),
  _duality(0.),
  _nDual(0),
  _angle(0.),
  _state(0),
  _lineTsf(0.,0.,0.) {
    _links.sort(SortByWireId);
//    _times = 4.0;
//    if (_links[0]->wire()->stereo()) _times = 7.0;
    _times[0] = 7;
    _times[1] = 7;
    _times[2] = 4;
    _times[3] = 4;
    _times[4] = 4;
    _times[5] = 5;
    _times[6] = 5;
    _times[7] = 5;
    _times[8] = 5;
    _times[9] = 4;
    _times[10] = 4;
    _fitted = false;
}  //the above two innitial function are temporary for tsf!!!
 
/*TSegment::TSegment()
: TTrackBase(),
  _innerWidth(0),
  _outerWidth(0),
  _nLayer(0),
  _clusterType(0),
  _duality(0.),
  _nDual(0),
  _angle(0.),
  _state(0) {
    _fitted = false;
}
 
TSegment::TSegment(const AList<TMLink> & a)
: TTrackBase(a),
  _innerWidth(0),
  _outerWidth(0),
  _nLayer(0),
  _clusterType(0),
  _duality(0.),
  _nDual(0),
  _angle(0.),
  _state(0) {
    _links.sort(SortByWireId);
    _fitted = false;
}
*/
TSegment::~TSegment() {
}
 
void
TSegment::dump(const std::string & msg, const std::string & pre) const {
    if (! _fitted) update();
    bool def = false;
    if (msg == "") def = true;
 
    if (def || msg.find("cluster") != std::string::npos || msg.find("detail") != std::string::npos) {
    std::cout << pre;
    std::cout << "#links=" << _links.length();
    std::cout << "(" << _innerWidth << "," << _outerWidth << ":";
    std::cout << clusterType() << ")," << _nDual << "," << _duality << ",";
    std::cout << _angle << std::endl;
    }
    if (def || msg.find("vector") != std::string::npos || msg.find("detail") != std::string::npos) {
    std::cout << pre;
    std::cout << "pos" << _position << "," << "dir" << _direction;
    std::cout << std::endl;
    }
    if (def || msg.find("state") != std::string::npos || msg.find("detail") != std::string::npos) {
    std::cout << pre;
    std::cout << "state=" << _state << std::cout << std::endl;
    }
    if (def || msg.find("link") != std::string::npos || msg.find("detail") != std::string::npos) {
    std::cout << pre;
    std::cout << "unique links=" << NUniqueLinks(* this);
    std::cout << ",major links=" << NMajorLinks(* this);
    std::cout << ",branches=" << NLinkBranches(* this);
    std::cout << std::endl;
    }
    if (! def) TTrackBase::dump(msg, pre);
}
 
void
TSegment::update(void) const {
    _clusterType = 0;
    _position = ORIGIN;
    _direction = ORIGIN;
    _outerPosition = ORIGIN;
    _inners.removeAll();
    _outers.removeAll();
 
    if (_links.length() == 0) return; //tmp for tsf
//    if (_links.length() < 3) return;
 
    _innerMostLayer = _links[0]->wire()->layerId();
    _outerMostLayer = _innerMostLayer;
    for (unsigned i = 1; i < _links.length(); i++) {
    unsigned id = _links[i]->wire()->layerId();
    if (id < _innerMostLayer) _innerMostLayer = id;
    else if (id > _outerMostLayer) _outerMostLayer = id;
    }
    _nLayer = _outerMostLayer - _innerMostLayer + 1;
 
    double centerX = _links[0]->position().x();
    HepPoint3D inner = ORIGIN;
    unsigned nInner = 0;
    unsigned nOuter = 0;
    unsigned n = _links.length();
    for (unsigned i = 0; i < n; i++) {
    const HepPoint3D & tmp = _links[i]->position();
/*//--------------------------------tsf---------------------------------//
    const HepPoint3D & p = _links[i]->positionD();
    double d = _links[i]->cDrift();
    HepPoint3D posOnLine = closestPoint(p, _lineTsf);
    HepPoint3D v = (posOnLine - p).unit();
    const HepPoint3D tmp = p + d*v;
*///--------------------------------tsf---------------------------------//
    _position += tmp;
    unsigned id = _links[i]->wire()->layerId();
    if (id == _innerMostLayer) {
        inner += tmp;
        ++nInner;
        _inners.append(_links[i]);
    }
    if (id == _outerMostLayer) {
        _outerPosition += tmp;
        ++nOuter;
        _outers.append(_links[i]);
    }
    }
//      TTrackBase::dump("hits");
//      std::cout << "0:nin,nout=" << nInner << "," << nOuter << std::endl;
//      std::cout << "0:in,out=" << inner << ", " << _outerPosition << std::endl;
//      std::cout << "0:npos=" << n << std::endl;
//      std::cout << "0:pos=" << _position << std::endl;
 
    _innerWidth = Width(_inners);
    _outerWidth = Width(_outers);
    _position *= (1. / float(n));   //tmp for tsf
 
    inner *= (1. / (float) nInner);
    _outerPosition *= (1. / (float) nOuter);
    _direction = (inner - _outerPosition).unit();   //tmp for tsf
 
/*//----------------------liuqg add for tsf-----------------------//
    if (_links.length() > 3) {
    _position = ORIGIN;
    _direction = ORIGIN;
    HepPoint3D nearLine;
    nearLine = _lineTsf;
//  nearLine = leastSquaresFitting1(_links, _lineTsf);
//  _lineTsf = nearLine;
    if (nearLine.z() != 0) {
        HepPoint3D lineVector;  //exchange line to vector.
        lineVector.set(-nearLine.y()/nearLine.z(), nearLine.x()/nearLine.z(), 0.);
        _direction = (lineVector).unit();
    }
    else cout<<"nearLine.z == 0"<<endl;
    _position = positionTsf(_links, nearLine);
    //cout<<"pos of seg .. = "<<_position<<endl;
    //cout<<"dir of seg .. = "<<_direction<<endl;
    }
*///----------------------liuqg add for tsf-----------------------//
 
//      std::cout << "1:in,out=" << inner << ", " << _outerPosition << std::endl;
//      std::cout << "1:dir=" << _direction << std::endl;
//      std::cout << "1:pos=" << _position << std::endl;
 
    _fitted = true;
}
 
double
TSegment::distance(const TSegment & c) const {
    HepVector3D dir = c.position() - _position;
    if (dir.x() > M_PI) dir.setX(dir.x() - 2. * M_PI);
    else if (dir.x() < - M_PI) dir.setX(2. * M_PI + dir.x());
 
    float radial = fabs(_direction.dot(dir));
    float radial2 = radial * radial;
 
    return (dir.mag2() - radial2) > 0.0 ? sqrt(dir.mag2() - radial2) : 0.;
}
 
double
TSegment::distance(const HepPoint3D & p, const HepVector3D & v) const {
    HepVector3D dir = _position - p;
    if (dir.x() > M_PI) dir.setX(dir.x() - 2. * M_PI);
    else if (dir.x() < - M_PI) dir.setX(2. * M_PI + dir.x());
 
    float radial = fabs(v.unit().dot(dir));
    float radial2 = radial * radial;
 
    return (dir.mag2() - radial2) > 0.0 ? sqrt(dir.mag2() - radial2) : 0.;
}
 
Range
TSegment::rangeX(double min, double max) const {
#ifdef TRKRECO_DEBUG_DETAIL
    if (min > max) {
    std::cout << "TSegment::range !!! bad arguments:min,max=";
    std::cout << min << "," << max << std::endl;
    }
#endif
 
    unsigned n = _links.length();
    if (n == 0) return Range(0., 0.);
 
    //...Search for a center...
    bool found = false;
    double center;
    for (unsigned i = 0; i < n; i++) {
    double x = _links[i]->position().x();
    if (x < min || x > max) continue;
    center = x;
    found = true;
    break;
    }
    if (! found) return Range(0., 0.);
 
#ifdef TRKRECO_DEBUG_DETAIL
//     std::cout << "    center=" << center << std::endl;
#endif
 
    double distanceR = 0.;
    double distanceL = 0.;
    double distanceMax = max - min;
    for (unsigned i = 0; i < n; i++) {
    double x = _links[i]->position().x();
    if (x < min || x > max) continue;
 
    double distance0, distance1;
    if (x > center) {
        distance0 = x - center;
        distance1 = distanceMax - distance0;
    }
    else {
        distance1 = center - x;
        distance0 = distanceMax - distance1;
    }
 
    if (distance0 < distance1) {
        if (distance0 > distanceR) distanceR = distance0;
    }
    else {
        if (distance1 > distanceL) distanceL = distance1;
    }
 
#ifdef TRKRECO_DEBUG_DETAIL
//  std::cout << "    ";
//  std::cout << _links[i]->wire()->layerId() << "-";
//  std::cout << _links[i]->wire()->localId() << ",";
//  std::cout << _links[i]->position().x();
//  std::cout << ",0,1=" << distance0 << "," << distance1;
//  std::cout << ",l,r=" << distanceL << "," << distanceR;
//  std::cout << std::endl;
#endif
    }
 
    double right = center + distanceR;
    double left = center - distanceL;
 
    return Range(left, right);
}
 
void 
TSegment::updateType(void) const {
    if (! nLinks()) return;
    if (! _fitted) update();
 
    //...Parameter...
    unsigned fat = 3;
    unsigned tall = 3;
//    unsigned tall = 2;
 
    //...Calculate
    updateDuality();
 
    //...Long or short...
    if ((_innerWidth < fat) && (_outerWidth < fat)) {
    _clusterType = 1;
 
    //...Check length across a super layer...
    if (_nLayer > tall) _clusterType = 2;
    return;
    }
 
    //...A...
    else if (_outerWidth < fat) {
    _clusterType = 3;
    return;
    }
 
    //...V...
    else if (_innerWidth < fat) {
    _clusterType = 4;
    return;
    }
 
    //...X or parallel...
    else {
    bool space = true;
    for (unsigned i = _innerMostLayer; i <= _outerMostLayer; i++) {
        unsigned n = 0;
        AList<TMLink> tmp;
        for (unsigned j = 0; j < _links.length(); j++)
        if (_links[j]->wire()->layerId() == i) {
            ++n;
            tmp.append(_links[j]);
        }
 
        if (n == Width(tmp)) {
        space = false;
        break;
        }
    }
 
    _clusterType = 5;
    if (space) _clusterType = 6;
    return;
    }
}
 
AList<TSegment>
TSegment::split(void) const {
    AList<TSegment> list;
 
    //...Do not split if cluster type is 1, 2, or 7...
    unsigned t = clusterType();
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "    ... splitting : type=" << t << std::endl;
#endif 
    if (t == 0) return list;
    else if (t == 2) {
    // beta 5
    //if (_nDual > 2 && _duality > 0.7 && _angle > 0.7) return splitDual();
 
    // 1.67g
    // if (_nDual > 2 && _duality > 0.7) return splitDual();
 
    if (_nDual > 2 && _duality > 0.7 && _angle > 0.7) return splitDual();
    return list;
    }
    else if (t == 1) return list;
    else if (t == 7) return list;
 
    //...Parallel...
    else if (t == 6) return splitParallel();
    // else if (t == 6) return list;
 
    //...Avoid splitting of X or parallel...(future implementation)...
    else if (t > 4) return splitComplicated();
 
    //...A or V...
    return splitAV();
}
 
AList<TSegment>
TSegment::splitAV(void) const {
    unsigned t = clusterType();
    AList<TMLink> seeds[2];
 
    //...Calculate corner of V or A...
    const AList<TMLink> * corners;
    if (t == 3) corners = & _outers;
    else        corners = & _inners;
    HepPoint3D corner;
    for (unsigned i = 0; i < corners->length(); i++)
    corner += (* corners)[i]->wire()->xyPosition();
    corner *= 1. / (float) corners->length();
    seeds[0].append(* corners);
    seeds[1].append(* corners);
 
    //...Calculdate edges...
    const AList<TMLink> * links;
    if (t == 3) links = & _inners;
    else        links = & _outers;
    AList<TMLink> edge = Edges(* links);
    HepPoint3D edgePos[2];
    HepVector3D v[2];
    for (unsigned i = 0; i < 2; i++) {
    edgePos[i] = edge[i]->wire()->xyPosition();
    v[i] = (edgePos[i] - corner).unit();
    }
    seeds[0].append(edge[0]);
    seeds[1].append(edge[1]);
 
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "    corner:" << corner << std::endl;
    std::cout << "    edge:" << edgePos[0] << "(";
    std::cout << edge[0]->wire()->layerId() << "-";
    std::cout << edge[0]->wire()->localId() << ")";
    std::cout << v[0] << std::endl;
    std::cout << "         ";
    std::cout << edgePos[1] << "(";
    std::cout << edge[1]->wire()->layerId() << "-";
    std::cout << edge[1]->wire()->localId() << ")";
    std::cout << v[1] << std::endl;
#endif
 
    //...Examine each hits...
    unsigned n = _links.length();
    for (unsigned i = 0; i < n; i++) {
    TMLink * l = _links[i];
    if (edge.member(l)) continue;
    if (corners->member(l)) continue;
 
    HepVector3D p = l->wire()->xyPosition() - corner;
    double p2 = p.mag2();
 
    double dist[2];
    for (unsigned j = 0; j < 2; j++) {
        dist[j] = v[j].dot(p);
        double d2 = dist[j] * dist[j];
        dist[j] = (p2 - d2) > 0. ? sqrt(p2 - d2) : 0.;
    }
    if (dist[0] < dist[1]) seeds[0].append(l);
    else                   seeds[1].append(l);
 
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "        p:" << p << std::endl;
    std::cout << "    " << l->wire()->layerId() << "-";
    std::cout << l->wire()->localId() << ":" << dist[0];
    std::cout << "," << dist[1] << std::endl;
#endif
    }
 
    AList<TSegment> list;
    for (unsigned i = 0; i < 2; i++) {
    if (seeds[i].length()) {
        TSegment * nc = new TSegment(seeds[i]);
        AList<TSegment> ncx = nc->split();
        if (ncx.length() == 0) {
        nc->solveDualHits();
        list.append(nc);
        }
        else {
        list.append(ncx);
        delete nc;
        }
    }
    }
    return list;
}
 
AList<TSegment>
TSegment::splitComplicated(void) const {
 
    //...Select best hits...
    AList<TSegment> newClusters;
    AList<TMLink> goodHits;
    unsigned n = _links.length();
    for (unsigned i = 0; i < n; i++) {
    const TMDCWireHit * h = _links[i]->hit();
    unsigned state = h->state();
    if (! (state & WireHitContinuous)) continue;
    if (! (state & WireHitIsolated)) continue;
    if ((! (state & WireHitPatternLeft)) &&
        (! (state & WireHitPatternRight))) continue;
    goodHits.append(_links[i]);
    }
    if (goodHits.length() == 0) return newClusters;
 
    //...Main loop...
    goodHits.sort(SortByWireId);
    AList<TMLink> original(_links);
    while (goodHits.length()) {
 
    //...Select an edge hit...
    TMLink * seed = goodHits.last();
    const TMDCWire * wire = seed->wire();
    unsigned localId = wire->localId();
    AList<TMLink> used;
    unsigned nn = _links.length();
    for (unsigned i = 0; i < nn; i++) {
        TMLink * t = _links[i];
        const TMDCWire * w = t->wire();
 
        //...Straight?...
        if (abs((int) w->localId() - (int) localId) < 2) used.append(t);
    }
 
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "        seed is " << seed->wire()->name() << std::endl;
    std::cout << "            ";
    for (unsigned i = 0; i < used.length(); i++) {
        std::cout << used[i]->wire()->name() << ",";
    }
    std::cout << std::endl;
#endif
 
    //...Create new cluster...
    if (used.length() == 0) continue;
    if (used.length() == nLinks()) return newClusters;
    TSegment * c = new TSegment(used);
    AList<TSegment> cx = c->split();
    if (cx.length() == 0) {
        c->solveDualHits();
        newClusters.append(c);
    }
    else {
        newClusters.append(cx);
        delete c;
    }
    goodHits.remove(used);
    original.remove(used);
    }
 
    //...Remainings...
    if ((original.length()) && (original.length() < nLinks())) {
    TSegment * c = new TSegment(original);
    AList<TSegment> cx = c->split();
    if (cx.length() == 0) {
        c->solveDualHits();
        newClusters.append(c);
    }
    else {
        newClusters.append(cx);
        delete c;
    }
    }
 
    return newClusters;
}
 
AList<TSegment>
TSegment::splitParallel(void) const {
    AList<TMLink> seeds[2];
    AList<TSegment> newClusters;
    for (unsigned i = _innerMostLayer; i <= _outerMostLayer; i++) {
    AList<TMLink> list = SameLayer(_links, i);
    AList<TMLink> outerList = Edges(list);
 
#ifdef TRKRECO_DEBUG_DETAIL
    if (outerList.length() != 2) {
        std::cout << "TSegment::splitParallel !!! ";
        std::cout << "This is not a parallel cluster" << std::endl;
    }
#endif
 
    seeds[0].append(outerList[0]);
    seeds[1].append(outerList[1]);
    if (list.length() == 2) continue;
 
    const TMDCWire & wire0 = * outerList[0]->wire();
    const TMDCWire & wire1 = * outerList[1]->wire();
    for (unsigned k = 0; k < list.length(); k++) {
        TMLink * t = list[k];
        if (t == outerList[0]) continue;
        if (t == outerList[1]) continue;
 
        if (abs(wire0.localIdDifference(* t->wire())) <
        abs(wire1.localIdDifference(* t->wire())))
        seeds[0].append(t);
        else
        seeds[1].append(t);
    }
    }
    
    if ((seeds[0].length()) && (seeds[0].length() < nLinks())) {
    TSegment * c0 = new TSegment(seeds[0]);
    AList<TSegment> c0x = c0->split();
    if (c0x.length()) {
        newClusters.append(c0x);
        delete c0;
    }
    else {
        c0->solveDualHits();
        newClusters.append(c0);
    }
    }
 
    if ((seeds[1].length()) && (seeds[1].length() < nLinks())) {
    TSegment * c1 = new TSegment(seeds[1]);
    AList<TSegment> c1x = c1->split();
    if (c1x.length()) {
        newClusters.append(c1x);
        delete c1;
    }
    else {
        c1->solveDualHits();
        newClusters.append(c1);
    }
    }
 
    return newClusters;
}
 
void
TSegment::updateDuality(void) const {
    _duality = 0.;
    _nDual = 0;
    HepPoint3D x[2];
    for (unsigned i = _innerMostLayer; i <= _outerMostLayer; i++) {
    AList<TMLink> list = SameLayer(_links, i);
    if (i == _innerMostLayer) {
        for (unsigned j = 0; j < list.length(); j++)
        x[0] += list[j]->hit()->xyPosition();
        x[0] *= 1. / float(list.length());
    }
    else if (i == _outerMostLayer) {
        for (unsigned j = 0; j < list.length(); j++)
        x[1] += list[j]->hit()->xyPosition();
        x[1] *= 1. / float(list.length());
    }
 
    if (list.length() == 2) {
        if (Width(list) != 2) continue;
        const TMDCWireHit * h0 = list[0]->hit();
        const TMDCWireHit * h1 = list[1]->hit();
 
        double distance = (h0->xyPosition() - h1->xyPosition()).mag();
        distance = fabs(distance - list[0]->drift() - list[1]->drift());
        _duality += distance;
        ++_nDual;
    }
    }
    if (_nDual > 0) _duality /= float(_nDual);
    _angle = (x[1] - x[0]).unit().dot(x[0].unit());
}
 
AList<TSegment>
TSegment::splitDual(void) const {
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "    TSegment::splitDual called" << std::endl;
#endif
    AList<TMLink> seeds[2];
    AList<TMLink> unknown;
    for (unsigned i = _innerMostLayer; i <= _outerMostLayer; i++) {
    AList<TMLink> list = SameLayer(_links, i);
 
    if (list.length() == 2) {
        if (Width(list) == 2) {
        seeds[0].append(list[0]);
        seeds[1].append(list[1]);
        continue;
        }
    }
    unknown.append(list);
    }
 
    if (unknown.length() > 0) {
#ifdef TRKRECO_DEBUG_DETAIL
    if (seeds[0].length() == 0)
        std::cout << "TSegment::splitDual !!! no TMLink for seed 0" << std::endl;
    if (seeds[1].length() == 0)
        std::cout << "TSegment::splitDual !!! no TMLink for seed 1" << std::endl;
#endif  
    const HepPoint3D & p0 = seeds[0][0]->xyPosition();
    const HepPoint3D & p1 = seeds[1][0]->xyPosition();
    HepVector3D v0 = (seeds[0].last()->xyPosition() - p0).unit();
    HepVector3D v1 = (seeds[1].last()->xyPosition() - p1).unit();
 
    for (unsigned i = 0; i < unknown.length(); i++) {
        TMLink * t = unknown[i];
        HepVector3D x0 = t->xyPosition() - p0;
        double d0 = (x0 - (x0.dot(v0) * v0)).mag();
        HepVector3D x1 = t->xyPosition() - p1;
        double d1 = (x1 - (x1.dot(v0) * v0)).mag();
 
        if (d0 < d1) seeds[0].append(t);
        else         seeds[1].append(t);
    }
    }
 
    AList<TSegment> newClusters;
    newClusters.append(new TSegment(seeds[0]));
    newClusters.append(new TSegment(seeds[1]));
    return newClusters;
}
 
int
TSegment::solveDualHits(void) {
    updateType();
    if (_clusterType == 0) return 0;
    if (_nDual == 0) return 0;
    update();
    return 0;
 
    updateType();
    if (_clusterType == 0) return 0;
    if (_nDual == 0) return 0;
 
    AList<TMLink> seeds;
    AList<TMLink> duals;
    for (unsigned i = _innerMostLayer; i <= _outerMostLayer; i++) {
    AList<TMLink> list = SameLayer(_links, i);
 
    if (list.length() == 1) {
        seeds.append(list[0]);
    }
    else if (list.length() == 2) {
        if (Width(list) > 1) {
        const TMDCWireHit * h0 = list[0]->hit();
        const TMDCWireHit * h1 = list[1]->hit();
        double distance = (h0->xyPosition() - h1->xyPosition()).mag();
        distance = fabs(distance - 
                list[0]->drift() - 
                list[1]->drift());
        if (distance > 0.5) duals.append(list);
#ifdef TRKRECO_DEBUG_DETAIL
        h0->dump("", "        ");
        h1->dump("", "        ");
        std::cout << "        lyr=" << i;
        std::cout << ", duality distance = " << distance << std::endl;
#endif
        }
    }
    else if (list.length() == 0) {
        continue;
    }
#ifdef TRKRECO_DEBUG_DETAIL
    else {
        std::cout << "TSegment::solveDualHits !!! this is not expected 2";
        std::cout << std::endl;
        this->dump("cluster hits mc", "    ");
    }
#endif
    }
 
    //...Solve them...
    if (seeds.length() < 2) return -1;
    AList<TMLink> outers = InOut(seeds);
    const HepPoint3D & p = outers[0]->xyPosition();
    HepVector3D v = (outers[1]->xyPosition() - p).unit();
    unsigned n = duals.length() / 2;
    for (unsigned i = 0; i < n; i++) {
    TMLink * t0 = duals[i * 2 + 0];
    TMLink * t1 = duals[i * 2 + 1];
    HepVector3D x0 = t0->xyPosition() - p;
    HepVector3D x1 = t1->xyPosition() - p;
    double d0 = (x0 - (x0.dot(v) * v)).mag();
    double d1 = (x1 - (x1.dot(v) * v)).mag();
    if (d0 < d1) _links.remove(t1);
    else         _links.remove(t0);
    }
    update();
    return n;
}
 
/*
//#include "tuple/BelleTupleManager.h"
#include "BesKernel/BesModule.h"
#include "BesKernel/BesHistogramService.h"
#include "BesKernel/BesHObject.h"
 
void
CheckSegments(const CAList<TSegment> & list) {
    static bool first = true;
//    static BelleHistogram * hError;
//    static BelleHistogram * hNHeps[11];
//    static BelleHistogram * hNHits[3];
    BesHistogramService * svc = Framework()->HistogramSvc();
    static BesHObject * hError;
    static BesHObject * hNHeps[11];  
    static BesHObject * hNHits[3];  
    
    if (first) {
    first = false;
//  extern BelleTupleManager * BASF_Histogram;
//  BelleTupleManager * m = BASF_Histogram;
 
//  hError = m->histogram("segment errors", 10, 0, 10, 20000);
//
//  hNHeps[0] = m->histogram("segment nheps sl 0", 10, 0., 10.);
//  hNHeps[1] = m->histogram("segment nheps sl 1", 10, 0., 10.);
//  hNHeps[2] = m->histogram("segment nheps sl 2", 10, 0., 10.);
//  hNHeps[3] = m->histogram("segment nheps sl 3", 10, 0., 10.);
//  hNHeps[4] = m->histogram("segment nheps sl 4", 10, 0., 10.);
//  hNHeps[5] = m->histogram("segment nheps sl 5", 10, 0., 10.);
//  hNHeps[6] = m->histogram("segment nheps sl 6", 10, 0., 10.);
//  hNHeps[7] = m->histogram("segment nheps sl 7", 10, 0., 10.);
//  hNHeps[8] = m->histogram("segment nheps sl 8", 10, 0., 10.);
//  hNHeps[9] = m->histogram("segment nheps sl 9", 10, 0., 10.);
//  hNHeps[10] = m->histogram("segment nheps sl 10", 10, 0., 10.);
//
//  hNHits[0] = m->histogram("segment nhits, nheps = 1", 20, 0., 20.);
//  hNHits[1] = m->histogram("segment nhits, nheps = 2", 20, 0., 20.);
//  hNHits[2] = m->histogram("segment nhits, nheps >= 3", 20, 0., 20.);
//
    hError = svc -> Book(1,"segment errors", 10, 0, 10, 20000);
    
    hNHeps[0] = svc -> Book(1,"segment nheps sl 0", 10, 0., 10.); 
    hNHeps[1] = svc -> Book(1,"segment nheps sl 1", 10, 0., 10.);    
    hNHeps[2] = svc -> Book(1,"segment nheps sl 2", 10, 0., 10.);    
    hNHeps[3] = svc -> Book(1,"segment nheps sl 3", 10, 0., 10.);    
    hNHeps[4] = svc -> Book(1,"segment nheps sl 4", 10, 0., 10.);    
    hNHeps[5] = svc -> Book(1,"segment nheps sl 5", 10, 0., 10.);    
    hNHeps[6] = svc -> Book(1,"segment nheps sl 6", 10, 0., 10.);    
    hNHeps[7] = svc -> Book(1,"segment nheps sl 7", 10, 0., 10.);    
    hNHeps[8] = svc -> Book(1,"segment nheps sl 8", 10, 0., 10.);    
    hNHeps[9] = svc -> Book(1,"segment nheps sl 9", 10, 0., 10.);    
    hNHeps[10] = svc -> Book(1,"segment nheps sl 10", 10, 0., 10.);    
    
    hNHits[0] = svc -> Book(1,"segment nhits, nheps = 1", 20, 0., 20.);  
    hNHits[1] = svc -> Book(1,"segment nhits, nheps = 2", 20, 0., 20.);     
    hNHits[2] = svc -> Book(1,"segment nhits, nheps >= 3", 20, 0., 20.);     
    
    std::cout << "CheckSegments ... initialized" << std::endl;
    return;
    }
 
    unsigned n = list.length();
    for (unsigned i = 0; i < n; i++) {
    const TSegment & s = * list[i];
    const AList<TMLink> & links = s.links();
    unsigned nLinks = links.length();
    if (nLinks == 0) {
//      hError->accumulate(0.5);
        hError->Fill(0.5);
        continue;
    }
 
    unsigned sl = links[0]->wire()->superLayerId();
    unsigned nHeps = s.nHeps();
//  hNHeps[sl]->accumulate((float) nHeps + .5);
    hNHeps[sl]->Fill((float) nHeps + .5);  
//  if (nHeps == 1)      hNHits[0]->accumulate((float) nLinks + .5);
//  else if (nHeps == 2) hNHits[1]->accumulate((float) nLinks + .5);
//  else                 hNHits[2]->accumulate((float) nLinks + .5);
        if (nHeps == 1)      hNHits[0]->Fill((float) nLinks + .5);
    else if (nHeps == 2) hNHits[1]->Fill((float) nLinks + .5);
    else                 hNHits[2]->Fill((float) nLinks + .5);
            
    }
}
 
void
CheckSegmentLink(const TSegment & base,
         const TSegment & next,
         float distance,
         float dirAngle) {
    
    static bool first = true;
    static BelleHistogram * hAngle[2];
    static BelleHistogram * hAngle1[2];
    static BelleHistogram * hDistance[2];
    static BelleHistogram * hDirAngle[2];
    static BelleHistogram * hDirAngle1[2];
 
    if (first) {
    first = false;
    extern BelleTupleManager * BASF_Histogram;
    BelleTupleManager * m = BASF_Histogram;
 
    hAngle[0] = m->histogram("segment correct link, angle",
                 50, -1., 1., 21000);
    hAngle[1] = m->histogram("segment wrong link, angle", 50, -1., 1.);
    hAngle1[0] = m->histogram("segment correct link, angle, wide",
                  50, .8, 1.);
    hAngle1[1] = m->histogram("segment wrong link, angle, wide",
                  50, .8, 1.);
    hDistance[0] = m->histogram("segment correct link, dist", 50, 0., 1.);
    hDistance[1] = m->histogram("segment wrong link, dist", 50, 0., 1.);
    hDirAngle[0] = m->histogram("segment correct link, dir angle",
                    50, -1, 1.);
    hDirAngle[1] = m->histogram("segment wrong link, dir angle",
                    50, -1, 1.);
    hDirAngle1[0] = m->histogram("segment correct link, dir angle, wide",
                    50, .8, 1.);
    hDirAngle1[1] = m->histogram("segment wrong link, dir angle, wide",
                    50, .8, 1.);
 
    std::cout << "CheckSegmentLink ... initialized" << std::endl;
    return;
    }
 
    const TTrackHEP * const hep0 = base.hep();
    const TTrackHEP * const hep1 = next.hep();
 
    float angle = base.direction().dot(next.direction());
 
    unsigned id = 0;
    if (hep0 != hep1) id = 1;
    hAngle[id]->accumulate(angle);
    hAngle1[id]->accumulate(angle);
    hDistance[id]->accumulate(distance);
    hDirAngle[id]->accumulate(dirAngle);
    hDirAngle1[id]->accumulate(dirAngle);
}
*/
 
unsigned
NCoreLinks(const CAList<TSegment> & list) {
    unsigned n = 0;
    unsigned nList = list.length();
    for (unsigned i = 0; i < nList; i++) {
    const AList<TMLink> & links = list[i]->links();
    for (unsigned j = 0; j < links.length(); j++) {
        unsigned state = links[j]->hit()->state();
        if ((! (state & WireHitPatternLeft)) &&
        (! (state & WireHitPatternRight)))
        ++n;
    }
    }
    return n;
}
 
AList<TMLink>
Links(const TSegment & s, const TTrack & t) {
    AList<TMLink> a;
 
    const AList<TMLink> & links = s.links();
    const AList<TMLink> & trackLinks = t.links();
    unsigned n = links.length();
    for (unsigned i = 0; i < n; i++) {
    if (trackLinks.hasMember(links[i]))
        a.append(links[i]);
    }
 
    return a;
}
 
unsigned
NUniqueLinks(const TSegment & a) {
    unsigned n = 0;
    const TSegment * s = & a;
    while (s) {
    unsigned nLinks = s->innerLinks().length();
    if (nLinks != 1) return n;
    ++n;
    s = s->innerLinks()[0];
    }
    return n;
}
 
AList<TSegment>
UniqueLinks(const TSegment & a) {
    AList<TSegment> links;
    const TSegment * s = & a;
    while (s) {
    unsigned nLinks = s->innerLinks().length();
    if (nLinks != 1) return links;
    const TSegment * t = s->innerLinks()[0];
    links.append((TSegment *) t);
    s = t;
    }
    return links;
}
 
unsigned
NMajorLinks(const TSegment & a) {
    unsigned n = 0;
    const TSegment * s = & a;
    while (s) {
    unsigned nLinks = s->innerLinks().length();
    if (nLinks == 0) return n;
    ++n;
    int tmp = 0, ii = 0;
    for (int i = 0; i < nLinks; ++i) {
        if(s->innerLinks()[i]->links().length() > tmp) {
        tmp = s->innerLinks()[i]->links().length();
        ii = i;
        }
    }
    s = s->innerLinks()[ii];
//  s = s->innerLinks()[0];    //tmp for tsf
    }
    return n;
}
 
AList<TSegment>
MajorLinks(const TSegment & a) {
    AList<TSegment> links;
    const TSegment * s = & a;
    while (s) {
    unsigned nLinks = s->innerLinks().length();
    if (nLinks == 0) return links;
    int tmp = 0, ii = 0;
    for (int i = 0; i < nLinks; ++i) {
        if(s->innerLinks()[i]->links().length() > tmp) {
        tmp = s->innerLinks()[i]->links().length();
        ii = i;
        }
    }
    const TSegment * t = s->innerLinks()[ii];
//  const TSegment * t = s->innerLinks()[0];   //tmp for tsf
    links.append((TSegment *) t);
    s = t;
    }
    return links;
}
 
unsigned
NLinkBranches(const TSegment & a) {
    unsigned n = 0;
    const TSegment * s = & a;
    while (s) {
    unsigned nLinks = s->innerLinks().length();
    if (nLinks == 0) return n;
    if (nLinks > 1) ++n;
    s = s->innerLinks()[0];
    }
    return n;
}
 
void
SeparateCrowded(const AList<TSegment> & in,
        AList<TSegment> & isolated,
        AList<TSegment> & crowded) {
    unsigned n = in.length();
    if (n == 0) return;
 
    for (unsigned i = 0; i < n; i++) {
    TSegment & s = * in[i];
    if (s.state() & TSegmentCrowd) crowded.append(s);
    else                           isolated.append(s);
    }
}
 
unsigned
SuperLayer(const AList<TSegment> & list) {
    unsigned sl = 0;
    unsigned n = list.length();
    for (unsigned i = 0; i < n; i++)
    sl |= (1 << (list[i]->superLayerId()));
    return sl;
}
 
TSegment *
OuterMostUniqueLink(const TSegment & a) {
    const TSegment * o = & a;
    while (o->outerLinks().length() == 1)
    o = o->outerLinks()[0];
    return (TSegment *) o;
}
 
AList<TMLink>
Links(const AList<TSegment> & list) {
    AList<TMLink> links;
    unsigned n = list.length();
    for (unsigned i = 0; i < n; i++)
    links.append(list[i]->links());
    return links;
}
 
unsigned
TSegment::width(void) const {
    unsigned maxWidth = 0;
    for (unsigned i = innerMostLayer(); i <= outerMostLayer(); i++) {
    AList<TMLink> tmp = SameLayer(links(), i);
    unsigned w = Width(tmp);
    if (w > maxWidth) maxWidth = w;
    }
    return maxWidth;
}
 
//liuqg add for tsf...................//
double
TSegment::maxdDistance(TMLink *l) const{
    unsigned sl = l->wire()->superLayerId();
    unsigned lId = l->wire()->layerId();
    double _averR[11] = {9.7, 14.5, 22.14, 28.62, 35.1, 42.39, 48.87, 55.35, 61.83, 69.12, 74.79};
    double _averR2[43] = { 7.885, 9.07, 10.29, 11.525,
            12.72, 13.875, 15.01, 16.16,
            19.7, 21.3, 22.955, 24.555,
            26.215, 27.82, 29.465, 31.06,
            32.69, 34.265, 35.875, 37.455,
            39.975, 41.52, 43.12, 44.76,
            46.415, 48.02, 49.685, 51.37,
            53.035, 54.595, 56.215, 57.855,
            59.475, 60.995, 62.565, 64.165,
            66.68, 68.285, 69.915, 71.57,
            73.21, 74.76, 76.345};
    double radius = _averR2[lId];
    const double singleSigma = l->dDrift();
    return 2 * singleSigma / (radius * radius);
}
 
AList<TSegment>
TSegment::splitTsf(AList<TMLink> & seedNeighbors) {
    //get links in each layer.
    AList<TSegment> list;
    AList<TMLink> links[4];   //links in each local layer.
    AList<TMLink> seeds2;     //links in the outer layer.
    AList<TMLink> exTsf;      //links in other local layers.
    TMLink *seed;             //link in the mostinner layer.
 
    for (unsigned i = 0; i < _links.length(); ++i) {
    TMLink * l = _links[i];
    links[l->wire()->localLayerId()].append(l);
    }
 
    //prepare for segment finding.
    if (links[0].length() == 0) {  //find in 234.
    if (links[3].length() == 0) return list;
    if (links[1].length() != 1) {
        cout<<"wrong in tsf, TSegment::splitTsf ...1"<<endl;
        return list;
    }
    seed = links[1][0];
    seeds2.append(links[3]);
    exTsf.append(links[2]);
    }
    else if (links[0].length() == 1) { //find in 1234, 124, 134, 123.
    seed = links[0][0];
    if (links[3].length() > 0) {     //1..4
        seeds2.append(links[3]);
        exTsf.append(links[1]);
        exTsf.append(links[2]);
    }
    else if (links[2].length() > 0) { //1..3
        seeds2.append(links[2]);
        exTsf.append(links[1]);
    }
    else return list;
    }
    else cout<<"wrong in tsf, TSegment::splitTsf ...2"<<endl;
    exTsf.append(seeds2);  //add the outer layer...
 
    //find segments
    for (unsigned i = 0; i < seeds2.length(); ++i) {
    if (seed->tsfTag() > 0 && seeds2[i]->tsfTag() > 0) continue;
    AList<TSegment> segments;
    HepPoint3D line[4];
    fitLine(seed, seeds2[i], line);
    segments = appendByLine(seed, seeds2[i], seedNeighbors, exTsf, line);
    list.append(segments);
    segments.removeAll();
    }
    if (seed->wire()->localLayerId() == 0) {   //for 123
    exTsf.removeAll();
    seeds2.removeAll();
    exTsf.append(links[1]);
    exTsf.append(links[2]);   //add the outer layer...
    for (int k = 0; k < links[2].length(); ++k){
        if (links[2][k]->tsfTag() == 0) seeds2.append(links[2][k]);
    }
    for (unsigned i = 0; i < seeds2.length(); ++i) {
        if (seed->tsfTag() > 0 && seeds2[i]->tsfTag() > 0) continue;
        AList<TSegment> segments2;
        HepPoint3D line2[4];
        fitLine(seed, seeds2[i], line2);
        segments2 = appendByLine(seed, seeds2[i], seedNeighbors, exTsf, line2);
        list.append(segments2);
        segments2.removeAll();
    }
    }
 
    return list;
}
 
void
TSegment::fitLine(TMLink *seed, TMLink *outer, HepPoint3D line[4]) const {
    double ax = seed->positionD().x();
    double ay = seed->positionD().y();
    double ar = seed->cDrift();
    double bx = outer->positionD().x();
    double by = outer->positionD().y();
    double br = outer->cDrift();
    //calculate ...
    double dis = sqrt((ax - bx) * (ax - bx) + (ay - by) * (ay - by));
    double costheta = (bx - ax) / dis;
    double sintheta = (by - ay) / dis;
    double c1 = (ax * ax - bx * bx + ay * ay - by * by) / (2 * dis);
    double c2 = (ax * by - bx * ay) / dis;
//cout<<"dis of circles = "<<dis<<"   radius of circles N1 = "<<ar<<"  N2 = "<<br<<endl;
    line[0].setX((br - ar) * costheta + sqrt(dis * dis - (ar - br) * (ar - br)) * sintheta);
    line[0].setY((br - ar) * sintheta - sqrt(dis * dis - (ar - br) * (ar - br)) * costheta);
    line[0].setZ((br - ar) * c1 - sqrt(dis * dis - (ar - br) * (ar - br)) * c2 + 0.5 * dis * (ar + br));
    line[1].setX((br - ar) * costheta - sqrt(dis * dis - (ar - br) * (ar - br)) * sintheta);
    line[1].setY((br - ar) * sintheta + sqrt(dis * dis - (ar - br) * (ar - br)) * costheta);
    line[1].setZ((br - ar) * c1 + sqrt(dis * dis - (ar - br) * (ar - br)) * c2 + 0.5 * dis * (ar + br));
    line[2].setX((br + ar) * costheta + sqrt(dis * dis - (ar + br) * (ar + br)) * sintheta);
    line[2].setY((br + ar) * sintheta - sqrt(dis * dis - (ar + br) * (ar + br)) * costheta);
    line[2].setZ((br + ar) * c1 - sqrt(dis * dis - (ar + br) * (ar + br)) * c2 + 0.5 * dis * (br - ar));
    line[3].setX((br + ar) * costheta - sqrt(dis * dis - (ar + br) * (ar + br)) * sintheta);
    line[3].setY((br + ar) * sintheta + sqrt(dis * dis - (ar + br) * (ar + br)) * costheta);
    line[3].setZ((br + ar) * c1 + sqrt(dis * dis - (ar + br) * (ar + br)) * c2 + 0.5 * dis * (br - ar));
/*    for (int i = 0; i < 4; ++i) {
    double d1 = distance2(seed, line[i]);
    double d2 = distance2(outer, line[i]);
    cout<<"dis of line to seed = "<<d1<<"  dis to outer = "<<d2<<endl;
    cout<<"radius of seed      = "<<ar<<"  rad to outer = "<<br<<endl;
    }
*///check the calculation of line. right!!
    return;
}
 
AList<TSegment>
TSegment::appendByLine(TMLink * seed, TMLink * outer, AList<TMLink> & seedNeighbors, AList<TMLink> & restHits, const HepPoint3D line[4]) {
    AList<TSegment> list;
    list.removeAll();
 
    const unsigned slId = seed->wire()->superLayerId();
 
//cout<<"seed: lId:"<<seed->wire()->layerId()<<" loId:"<<seed->wire()->localId()<<endl;
//cout<<"outer: lId:"<<outer->wire()->layerId()<<" loId:"<<outer->wire()->localId()<<endl;
    for (int i = 0; i < 4; ++i) {
    AList<TMLink> seeds;
    for (int j = 0, size = restHits.length(); j < size; ++j) {
        TMLink * l = restHits[j];
        if (l->wire()->id() == outer->wire()->id()) continue;
//cout<<"restHits: lId:"<<l->wire()->layerId()<<" loId:"<<l->wire()->localId()<<endl;
        double maxdis = maxdDistance(l) * _times[slId];
        double dis = distance2(l, line[i]);
//cout<<"maxdis: "<<maxdis<<"  dis: "<<dis<<endl;
        if(seeds.hasMember(l)) continue;
        if (fabs(dis - l->cDrift()) < maxdis) seeds.append(l);
        else continue;
    }
    seeds.append(seed);
    seeds.append(outer);
    unsigned nHitsLayer[4] = {0, 0, 0, 0};
    unsigned nLayers = 0;
    //check...at least 3 layers.
    for (int j = 0; j < seeds.length(); ++j)
        ++nHitsLayer[seeds[j]->wire()->localLayerId()];
    for (int j = 0; j < 4; ++j)
        if (nHitsLayer[j] > 0) ++nLayers;
    if (nLayers < 3) continue;
 
    //check the neighbor hits in the first locallayer.
    for (int k = 0; k < seedNeighbors.length(); ++k) {
        TMLink * l =seedNeighbors[k];
//cout<<"seedNeighbors: lId:"<<l->wire()->layerId()<<" loId:"<<l->wire()->localId()<<endl;
        double maxdis = maxdDistance(l) * _times[slId];
        double dis = distance2(l, line[i]);
//cout<<"maxdis: "<<maxdis<<"  dis: "<<dis<<endl;
        if(seeds.hasMember(l)) continue;
        if (fabs(dis - l->cDrift()) < maxdis) seeds.append(l);
        else continue;
    }
 
    //update tag of each link.
    for (int k = 0; k < seeds.length(); ++k) {
        unsigned a = seeds[k]->tsfTag();
        ++a;
        seeds[k]->tsfTag(a);
    }
 
    //creat segment and update.
    TSegment * seg = new TSegment(seeds);
    seg->lineTsf(line[i]);
    seg->update();
 
    list.append(seg);
    }
 
    //salvage in new line...after update
//    AList<TMLink> all;
//    all.removeAll();
//    all.append(restHits);
//    all.append(seedNeighbors);
//    for (int j = 0; j < list.length(); ++j) 
//  list[j]->segSalvage(all);
 
    //expand 3layers segment
/*    unsigned sl = seed->wire()->superLayerId();
    if(sl != 10) {
    for( int i = 0; i < list.length(); ++i) {
        AList<TMLink> segLinks = list[i]->links();
        unsigned nHits[4] = {0,0,0,0};
        int emptyLayer = -1;
        for(int j = 0; j < segLinks.length(); ++j)
        ++nHits[segLinks[j]->wire()->localLayerId()];
        for (int j = 0; j < 4; ++j) {
        if(nHits[j] == 0) emptyLayer = j;
        if (emptyLayer != -1) break;
        }
        if(emptyLayer == -1) continue;
 
        for(int j = 0; j < all.length(); ++j){
        TMLink *l = all[j];
        if (l->wire()->localLayerId() == emptyLayer){
            double maxdis = maxdDistance(l) * _times;
            double dis = distance2(l, list[i]->lineTsf());
        cout<<"layer:  "<<l->wire()->layerId()<<"  local: "<<l->wire()->localId()<<endl;
        cout<<"maxdis: "<<maxdis<<"  dis in empty layer: "<<dis<<endl;
        }
        }
 
//      list[i]->expandSeg(emptyLayer, all);
    }
    }
*/
    return list;
}
 
double
TSegment::distance2(TMLink * hit, HepPoint3D line) const {
    double x = hit->positionD().x();
    double y = hit->positionD().y();
    double a = line.x();
    double b = line.y();
    double c = line.z();
    return fabs(a * x + b * y + c) / sqrt(a * a + b * b);
}
 
HepPoint3D
TSegment::positionTsf(const AList<TMLink> & links, const HepPoint3D line) const {
    HepPoint3D sumPos(0., 0., 0.);
    int n = 0;
    for(int i = 0; i < links.length(); ++i){
    const HepPoint3D & p = links[i]->positionD();
    double d = links[i]->cDrift();
    HepPoint3D posOnLine = closestPoint(p, _lineTsf);
    HepPoint3D v = (posOnLine - p).unit();
    const HepPoint3D p1 = p + d*v;
    sumPos += p1;
    ++n;
    }
    sumPos *= (1. / double(n));
    HepPoint3D posTsf = closestPoint(sumPos, line);
    return posTsf;
}
 
HepPoint3D
TSegment::leastSquaresFitting1(const AList<TMLink> & links, const HepPoint3D tsfLine) const {
    HepPoint3D line0;
    line0.setX(tsfLine.x()/sqrt(tsfLine.x()*tsfLine.x()+tsfLine.y()*tsfLine.y()));
    line0.setY(tsfLine.y()/sqrt(tsfLine.x()*tsfLine.x()+tsfLine.y()*tsfLine.y()));
    line0.setZ(tsfLine.z()/sqrt(tsfLine.x()*tsfLine.x()+tsfLine.y()*tsfLine.y()));
 
    HepPoint3D line;
    unsigned n = links.length();
    unsigned nTrial = 0;
    while(1){
    if (nTrial > 10) break;
    double a = 0., b = 0.;
    double sumXSig = 0., sumYSig = 0., sumX2Sig = 0., sumXYSig = 0., sumSig = 0.;
    for (unsigned i = 0; i < n; i++) {
    const HepPoint3D & p = links[i]->positionD();
    double d = links[i]->cDrift();
    HepPoint3D posOnLine = closestPoint(p, _lineTsf);
    HepPoint3D v = (posOnLine - p).unit();
    const HepPoint3D p1 = p + d*v;
    double Sig = maxdDistance(links[i]);
    Sig = 1./(Sig*Sig);
    double X = p1.x();
    double Y = p1.y();
    sumXSig  += X*Sig;
    sumYSig  += Y*Sig;
    sumX2Sig += X * X * Sig;
    sumXYSig += X * Y * Sig;
    sumSig += Sig;
    }
 
    b = (sumXYSig * sumSig - sumXSig * sumYSig) / (sumX2Sig * sumSig - sumXSig * sumXSig);
    a = (sumYSig - sumXSig * b) / sumSig;
 
    line.set(b/sqrt(1+b*b), -1/sqrt(1+b*b), a/sqrt(1+b*b));
//cout<<"n: "<<nTrial<<"  line = "<<line<<endl;
 
    double chi2 = 0.;
    for (unsigned i = 0; i < n; i++) {
    const HepPoint3D & p = links[i]->positionD();
    double d = links[i]->cDrift();
    double Sig = maxdDistance(links[i]);
    Sig = 1./(Sig*Sig);
    double dis = fabs(p.x()*line.x()+p.y()*line.y()+line.z()) - d;
    chi2 += dis * dis * Sig;
    }
    if (nTrial > 6) {
    cout<<"n = "<<n<<"  nTrial = "<<nTrial<<"  line0 = "<<line0<<endl;
    cout<<"chi2 in TSF = "<<chi2<<endl;
    }
 
    if (fabs(line.x()-line0.x()) < 0.0001) break;
 
    line0 = line;
    ++nTrial;
    }//while...
    return line;
}
 
HepPoint3D
TSegment::leastSquaresFitting(const AList<TMLink> & links, const HepPoint3D tsfLine) const {
    HepPoint3D line0;
    line0.setX(tsfLine.x()/sqrt(tsfLine.x()*tsfLine.x()+tsfLine.y()*tsfLine.y()));
    line0.setY(tsfLine.y()/sqrt(tsfLine.x()*tsfLine.x()+tsfLine.y()*tsfLine.y()));
    line0.setZ(tsfLine.z()/sqrt(tsfLine.x()*tsfLine.x()+tsfLine.y()*tsfLine.y()));
 
//    cout<<"tsfLine = "<<tsfLine<<endl;
    HepPoint3D line;
    int n = links.length();
    int nTrial = 0;
 
    while(1){
    if (nTrial>15) break;
    double a0 = line0.x();
    double b0 = line0.y();
    double c0 = line0.z();
//cout<<"nTrial: "<<nTrial<<"    line0"<<a0<<" "<<b0<<" "<<c0<<endl;
 
    if (b0 == 0) {
        cout<<"b0 == 0 TSegment::leastSquaresFitting!"<<endl;
        break;
    }
 
    double a = 0., b = 0., c = 0.;
    double sumSigX2 = 0., sumSigX = 0., sumSig = 0.;
    for (int i = 0; i < n; ++i) {
        const HepPoint3D & p = links[i]->positionD();
        double X = p.x();
        double Y = p.y();
        double Sig = maxdDistance(links[i]);
        Sig = 1./(Sig*Sig);
        sumSigX2 += Sig*(X-Y*a0/b0)*(X-Y*a0/b0);
        sumSigX += Sig*(X-Y*a0/b0);
        sumSig += Sig;
    }
    double detM = sumSigX2*sumSig - sumSigX*sumSigX;
    double M11 = sumSig/detM;
    double M12 = -sumSigX/detM;
    double M21 = M12;
    double M22 = sumSigX2/detM;
    for (int i = 0; i < n; ++i) {
        const HepPoint3D & p = links[i]->positionD();
        double X = p.x();
        double Y = p.y();
        double DRIFT = links[i]->cDrift();
        if (a0*p.x() + b0*p.y() + c0*p.z() < 0) DRIFT = (-1) * DRIFT;
        double Sig = maxdDistance(links[i]);
        Sig = 1./(Sig*Sig);
        double D = DRIFT-Y/b0;
        a += ((X-Y*a0/b0)*M11+M12)*Sig*D;
        c += ((X-Y*a0/b0)*M21+M22)*Sig*D;
    }
 
    b = (1 - a0*a)/b0;
 
    line.setX(a/sqrt(a*a+b*b));
    line.setY(b/sqrt(a*a+b*b));
    line.setZ(c/sqrt(a*a+b*b));
    a = line.x();
    b = line.y();
    c = line.z();
//  cout<<"line = "<<line<<endl;
    double chi2 = 0.;
    for (int i = 0; i< n; ++i){
        const HepPoint3D & p = links[i]->positionD();
        double dis = fabs(a*p.x()+b*p.y()+c);
        double DRIFT = links[i]->cDrift();
        double Sig = maxdDistance(links[i]);
//cout<<"dis: "<<dis<<"  DRIFT: "<<DRIFT<<"  Sig: "<<Sig<<endl;
        Sig = 1./(Sig*Sig);
        chi2 += (dis - DRIFT)*(dis - DRIFT)*Sig;
    }
//cout<<"chi2 = "<<chi2<<endl;
 
    //check...
    if(fabs(a-a0)<0.00001) break;
    //get line para...
    line0 = line;
    ++nTrial;
/*  if(nTrial>9){
        cout<<"length of links = "<<n<<endl;
        for(int i = 0; i<n; ++i){
        const HepPoint3D & p = links[i]->positionD();
        cout<<"layer: "<<links[i]->wire()->layerId()<<" localId:"<<links[i]->wire()->localId()<<endl;
        cout<<"drift: "<<links[i]->cDrift()<<" dis0: "<<a0*p.x() + b0*p.y() + c0*p.z()
            <<" dis1:"<<a*p.x() + b*p.y() + c*p.z()<<endl;
        }
    }*/
    }
    return line;
}
 
void
TSegment::segSalvage(AList<TMLink> & links) {
    HepPoint3D line = _lineTsf;
    AList<TMLink> segLinks = _links;
    for (int i = 0; i < links.length(); ++i) {
    TMLink * l = links[i];
    if(segLinks.hasMember(l)) continue;
    const unsigned slId = l->wire()->superLayerId();
    double maxdis = maxdDistance(l) * (_times[slId]);
    double dis = distance2(l, line);
    if (fabs(dis - l->cDrift()) < maxdis) {
        _links.append(l);
        unsigned a = l->tsfTag();
        ++a;
        l->tsfTag(a);
    }
    else continue;
    }
    update();
}
 
void
TSegment::expandSeg(const int empty, AList<TMLink> & allLinks) {
    if(empty>=4 || empty<0) return;
    AList<TMLink> linksOnEmptyLayer;
    for (int i = 0; i < allLinks.length(); ++i)
    if (allLinks[i]->wire()->localLayerId() == empty) linksOnEmptyLayer.append(allLinks[i]);
 
    if (linksOnEmptyLayer.length()==0) return;
 
    if (_cdc == 0) _cdc = TMDC::getTMDC();
    float maxPhi[4] = {0.,0.,0.,0.};
    float minPhi[4] = {999.,999.,999.,999.};
    float max = 0., min = 999.;
    for (int i = 0; i < _links.length(); ++i) {
    TMLink *l = _links[i];
    unsigned lId = l->wire()->layerId();
    int local = l->wire()->localId();
    unsigned loId = l->wire()->localLayerId();
    float phi0 = _cdc->layer(lId)->offset();
    int nWir = (int) _cdc->layer(lId)->nWires();
    float phi = phi0 + local*2*pi/nWir;
 
    if (phi > maxPhi[loId]) maxPhi[loId] = phi;
    if (phi < minPhi[loId]) minPhi[loId] = phi;
    if (phi > max) max = phi;
    if (phi < min) min = phi;
    }
 
    AList<TMLink> tmpList;
    for (int i = 0; i < linksOnEmptyLayer.length(); ++i) {
    TMLink *l = linksOnEmptyLayer[i];
    unsigned lId = l->wire()->layerId();
    int local = l->wire()->localId();
    float phi0 = _cdc->layer(lId)->offset();
    int nWir = (int) _cdc->layer(lId)->nWires();
    float phi = phi0 + local*2*pi/nWir;
 
    if (empty != 0 || empty != 3) {
        if (phi <= maxPhi[empty + 1] && phi >= minPhi[empty - 1]) tmpList.append(l);
        else if (phi <= maxPhi[empty - 1] && phi >= minPhi[empty + 1]) tmpList.append(l);
        else if (tmpList.length() == 0 && phi <= max && phi >= min) tmpList.append(l);
    }
    if (empty == 0 || empty == 3) {
        if (phi <= maxPhi[1] && phi >= minPhi[2]) tmpList.append(l);
        else if (phi <= maxPhi[2] && phi >= minPhi[1]) tmpList.append(l);
        else if (tmpList.length() == 0 && phi <= max && phi >= min) tmpList.append(l);
    }
    }
 
    for(int i = 0; i < tmpList.length(); ++i) {
    TMLink * l = tmpList[i];
    _links.append(l);
    unsigned a = l->tsfTag();
    ++a;
    l->tsfTag(a);
    }
    update();
}
 
void
TSegment::solveLR(void) {
    unsigned n = _links.length();
    //initial...
    if (n <= 3) {
      solveThreeHits();
      return;
    }
    for (int i = 0; i < n; ++i) {
    TMLink *l = _links[i];
    if (l->hit()->state() & WireHitPatternRight){
        unsigned newState = l->hit()->state()&(~WireHitPatternRight);
        l->hit()->state(newState);
    }
    if (l->hit()->state() & WireHitPatternLeft){
        unsigned newState = l->hit()->state()&(~WireHitPatternLeft);
        l->hit()->state(newState);
    }
    }
 
    HepPoint3D originCon(0., 0., 0.);
    HepPoint3D dirZ(0., 0., 1.);
    HepPoint3D center = closestPoint(originCon, _lineTsf);
    HepPoint3D v1 = (center - originCon).unit();
    HepPoint3D v2 = dirZ.cross(v1);
    for (int i = 0; i < n; ++i) {
//  cout<<"layerId: "<<_links[i]->wire()->layerId()<<"  localId: "<<_links[i]->wire()->localId()<<endl;
    const HepPoint3D & p = _links[i]->positionD();
    unsigned state = _links[i]->hit()->state();
 
    double cosA = (p - center).unit().dot(v1.unit());
    double cosB = (p - center).unit().dot(v2.unit());
    if (cosA*cosB < 0) state |= WireHitPatternLeft;
    else state |= WireHitPatternRight;
 
    _links[i]->hit()->state(state);
    }
}
 
HepPoint3D
TSegment::closestPoint(const HepPoint3D & p, const HepPoint3D line) const {
    HepPoint3D Dir(-line.y(), line.x(), 0);
    Dir = Dir.unit();
    HepPoint3D PointOnLine(1, -(line.x()+line.z())/line.y(), 0);
    double dis = (p - PointOnLine).dot(Dir);
    HepPoint3D tmp(dis*Dir.x(), dis*Dir.y(), 0.);
    HepPoint3D closestPoint = PointOnLine + tmp;
 
    double disPLine = fabs(p.x()*line.x()+p.y()*line.y()+line.z())/sqrt(line.x()*line.x()+line.y()*line.y());
    double disP1P2 = sqrt((p.x()-closestPoint.x())*(p.x()-closestPoint.x())+(p.y()-closestPoint.y())*(p.y()-closestPoint.y()));
    if (fabs(disPLine-disP1P2)>0.00001) cout<<"TSegment::closestPoint: dis p -> line = "<<disPLine<<" dis p -> pos  = "<<disP1P2<<endl;
    return closestPoint;
}
 
void
TSegment::solveThreeHits(void) {
    unsigned n = _links.length();
 
    for (unsigned i = 0; i < n; i++) {
    const TMDCWireHit * h = _links[i]->hit();
    const TMDCWire * w = h->wire();
    unsigned state = h->state();
 
    //...Cache pointers to a neighbor...
    const TMDCWire * neighbor[6];
    for (unsigned j = 0; j < 6; j++) neighbor[j] = w->neighbor(j);
 
    //...Decide hit pattern...
    unsigned pattern = 0;
    for (unsigned j = 0; j < 6; j++) {
        if (neighbor[j])
        if (neighbor[j]->hit())
            pattern += (1 << j);
    }
    state |= (pattern << WireHitNeighborHit);
 
    //...Solve LR locally...
    if ((pattern == 34) || (pattern == 42) ||
        (pattern == 40) || (pattern == 10) ||
        (pattern == 35) || (pattern == 50))
        state |= WireHitPatternRight;
    else if ((pattern == 17) || (pattern == 21) ||
         (pattern == 20) || (pattern ==  5) ||
         (pattern == 19) || (pattern == 49))
        state |= WireHitPatternLeft;
 
    //...Store it...
    h->state(state);
    }
}
 
//............................tsf//