#include "CbmRichRonchiAna.h"

#include <boost/gil/gil_all.hpp>
//#include <boost/gil/extension/io/tiff.hpp>
#include <boost/gil/extension/io/tiff_dynamic_io.hpp>


#include <iostream>
#include "TH2D.h"
#include "TH3D.h"
#include "TCanvas.h"
#include "CbmDrawHist.h"
#include "TEllipse.h"
#include "TVector3.h"
#include "TLine.h"
#include <math.h>
#include <set>
#include "TGeoManager.h"
#include "TGeoArb8.h"

using namespace boost::gil;
using namespace std;

// For Ubuntu one needs dev version of libtiff
// sudo apt-get install libtiff-dev

CbmRichRonchiAna::CbmRichRonchiAna() : // constructor

// constant values
fPi(3.141592654),
fRadiusMirror(3000000), // in microns   !!! MIGHT HAVE TO BE CHANGED TO DISTANCE_MIRROR-POINTSOURCE IF IT IS NOT IN THE CENTER OF CURVATURE
fEdgeLengthCCD(13300),  // in microns
fCcdPixelSize(13),      // in microns
fPitchGrid(200),        // in microns
fImageWidth(1024),      // in pixels

// values to be measured first
fDistRulingCCD(24500),                // in microns
fDistMirrorRuling(3100000),           // in microns
fOffsetCCDOptAxisX(0),                // in microns
fOffsetCCDOptAxisY(-42000),           // in microns
fOffsetLEDOpticalAxisX(0),            // in microns
fOffsetLEDOpticalAxisY(37000),        // in microns
fEffectiveEdgeLengthGridPlate(10000), // in microns
fEffectiveEdgeLengthMirror(380000),   // in microns
fEffectiveEdgeLengthCCD(38 * 100),    // in microns
fCenterCcdX(0),                       // in pixels
fCenterCcdY(0)                        // in pixels
{

}

CbmRichRonchiAna::~CbmRichRonchiAna()
{

}

void CbmRichRonchiAna::Run()
{
    // Initialization
    vector<vector<double> > dataV;
    vector<vector<double> > dataH;
        
    if ( fTiffFileNameV == "" || fTiffFileNameH == "" ) {
        Fatal("CbmRichRonchiAna::Run:", "No FileNameV or FileNameH!");
    } else {
        cout << "FileNameV: " << fTiffFileNameV << endl << "FileNameH: " << fTiffFileNameH << endl;
        dataV = ReadTiffFile(fTiffFileNameV);
        dataH = ReadTiffFile(fTiffFileNameH);
    }

    int width = dataV.size();
    int height = dataV[0].size();
    

    SetDefaultDrawStyle();
    // initialisierung der histogramme: name, groesse usw
    TH2D* hInitH = new TH2D("hInitH", "hInitH;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hMeanIntensityH = new TH2D("hMeanIntensityH", "hMeanIntensityH;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hPeakH = new TH2D("hPeakH", "hPeakH;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hSmoothLinesH = new TH2D("hSmoothLinesH", "hSmoothLinesH;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hLineSearchH = new TH2D("hLineSearchH", "hLineSearchH;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);

    TH2D* hInitV = new TH2D("hInitV", "hInitV;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hMeanIntensityV = new TH2D("hMeanIntensityV", "hMeanIntensityV;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hPeakV = new TH2D("hPeakV", "hPeakV;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hSmoothLinesV = new TH2D("hSmoothLinesV", "hSmoothLinesV;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    TH2D* hLineSearchV = new TH2D("hLineSearchV", "hLineSearchV;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);

    TH2D* hSuperpose = new TH2D("hSuperpose", "hSuperpose;X [pixel];Y [pixel];Intensity", width, -.5, width - 0.5, height, -0.5, height - 0.5);
    
    // vertical image
    DoRotation(dataV);
    FillH2WithVector(hInitV, dataV);
    DoMeanIntensityY(dataV);
    FillH2WithVector(hMeanIntensityV, dataV);
    DoPeakFinderY(dataV);
    FillH2WithVector(hPeakV, dataV);
    DoSmoothLines(dataV);
    FillH2WithVector(hSmoothLinesV, dataV);
    DoLineSearch(dataV);
    FillH2WithVector(hLineSearchV, dataV);
    DoRotation(dataV);


    // horizontal image
    FillH2WithVector(hInitH, dataH);
    DoMeanIntensityY(dataH);
    FillH2WithVector(hMeanIntensityH, dataH);
    DoPeakFinderY(dataH);
    FillH2WithVector(hPeakH, dataH);
    DoSmoothLines(dataH);
    FillH2WithVector(hSmoothLinesH, dataH);
    DoLineSearch(dataH);
    FillH2WithVector(hLineSearchH, dataH);
    
    // finding intersections
    vector<CbmRichRonchiIntersectionData> intersections = DoIntersection(dataH, dataV);
    vector<vector<double> > dataSup = DoSuperpose(dataH, dataV);
    FillH2WithVector(hSuperpose, dataSup);

    DoOrderLines(intersections, "x");
    DoOrderLines(intersections, "y");

    DoLocalNormal(intersections);
    DoSphere(intersections);
    
    bool vis = true;
    //rootgeom(vis);
    
    DoDeviation(intersections);
    
    //DoHeight(intersections);

    //DoSphere(intersections);
    //DoIntegrate(intersections);
    
    /*{
        TCanvas* c = new TCanvas("ronchi_2d_horizontal", "ronchi_2d_horizontal", 1500, 1000);
        c->Divide(3,2);
        c->cd(1);
        DrawH2(hInitH);
        c->cd(2);
        DrawH2(hMeanIntensityH);
        c->cd(3);
        DrawH2(hPeakH);
        c->cd(4);
        DrawH2(hSmoothLinesH);
        c->cd(5);
        DrawH2(hLineSearchH);
    }

    {
        TCanvas* c = new TCanvas("ronchi_2d_vertical", "ronchi_2d_vertical", 1500, 1000);
        c->Divide(3,2);
        c->cd(1);
        DrawH2(hInitV);
        c->cd(2);
        DrawH2(hMeanIntensityV);
        c->cd(3);
        DrawH2(hPeakV);
        c->cd(4);
        DrawH2(hSmoothLinesV);
        c->cd(5);
        DrawH2(hLineSearchV);
    }

    {
        TCanvas* c2 = new TCanvas("ronchi_1d_slices_horizontal", "ronchi_1d_slices_horizontal", 1200, 600);
        c2->Divide(2,1);

        TH1D* h1 = hInitH->ProjectionY("_py2", 250, 250);
        TH1D* h2 = hInitH->ProjectionY("_py3", 300, 300);
        TH1D* hM1 = hMeanIntensityH->ProjectionY("_pyM1", 250, 250);
        TH1D* hM2 = hMeanIntensityH->ProjectionY("_pyM2", 300, 300);
        TH1D* hP1 = hPeakH->ProjectionY("_pyP1", 250, 250);
        TH1D* hP2 = hPeakH->ProjectionY("_pyP2", 300, 300);
        c2->cd(1);
        DrawH1({h1,hM1,hP1}, {"Init", "Mean", "Peak"});
        c2->cd(2);
        DrawH1({h2,hM2,hP2}, {"Init", "Mean", "Peak"});
    }


    {
        TCanvas* c = new TCanvas("ronchi_2d_intersection", "ronchi_2d_intersection", 1000, 1000);
        DrawH2(hSuperpose);
        for (int i = 0; i < intersections.size(); i++) {
            TEllipse* center = new TEllipse(intersections[i].fPixelX, intersections[i].fPixelY, 5);
            center->Draw();
        }
    }

    vector<int> colors = {kBlack, kGreen, kBlue, kRed, kYellow, kOrange, kCyan, kGray, kMagenta, kYellow + 2, kRed + 3};
    {
        TCanvas* c = new TCanvas("ronchi_2d_intersection_x", "ronchi_2d_intersection_x", 1000, 1000);
        DrawH2(hSuperpose);
        for (int i = 0; i < intersections.size(); i++) {
            TEllipse* center = new TEllipse(intersections[i].fPixelX, intersections[i].fPixelY, 5);
            center->SetFillColor(colors[ intersections[i].fOrderedLineX % colors.size() ]);
            center->Draw();
        }
    }

    {
        TCanvas* c = new TCanvas("ronchi_2d_intersection_y", "ronchi_2d_intersection_y", 1000, 1000);
        DrawH2(hSuperpose);
        for (int i = 0; i < intersections.size(); i++) {
            TEllipse* center = new TEllipse(intersections[i].fPixelX, intersections[i].fPixelY, 5);
            center->SetFillColor(colors[ intersections[i].fOrderedLineY % colors.size() ]);
            center->Draw();
        }
    }*/


}

vector<vector<double> > CbmRichRonchiAna::ReadTiffFile(const string& fileName)
{
    cout << "ReadTiffFile:" << fileName << endl;
    vector<vector<double> > data;
    rgba8_image_t img;
    //read_and_convert_image(fileName, img, boost::gil::tiff_tag());
    tiff_read_and_convert_image(fileName, img);
    int height = img.height();
    int width = img.width();

    data.resize(width);

    auto view = const_view(img);
    for (int x = 0; x < width; ++x) 
    {
        auto it = view.col_begin(x);
        data[x].resize(height);
        for (int y = 0; y < height; ++y) 
        {
            int r = boost::gil::at_c<0>(it[y]);
            //int g = boost::gil::at_c<1>(it[y]);
            //int b = boost::gil::at_c<2>(it[y]);
            //int a = boost::gil::at_c<3>(it[y]);
            data[x][y] = r;
        }
    }
    return data;
}

// rotating the image (actually flipping diagonally arranged corners)
void CbmRichRonchiAna::DoRotation(vector<vector<double> >& data)
{
    int nX = data.size();
    int nY = data[0].size();
    for (int x = 0; x < nX; x++) {
        for (int y = x+1; y < nY; y++) {
            swap(data[x][y], data[y][x]);
        }
    }
}

// drawing histogram
void CbmRichRonchiAna::FillH2WithVector(TH2* hist, const vector<vector<double> >& data)
{
    int nX = data.size();
    int nY = data[0].size();

    for (int x = 0; x < nX; x++) {
        for (int y = 0; y < nY; y++){
            if (data[x][y] != 0) {
                hist->SetBinContent(x, y, data[x][y]);
            }
        }
    }
}

// averaging intensity with neighboured values
void CbmRichRonchiAna::DoMeanIntensityY(vector<vector<double> >& data)
{
    int nX = data.size();
    int nY = data[0].size();

    int halfAvWindow = 5;   // number of neighbour pixels to each side to average over
    double threshold = 25;     // threshold for average intensity to delete areas that are too dark (and so don't belong to mirror image)
    vector<double> weightsX = {1., 0.75, 0.4, 0.2, 0.08, 0.04}; // weightings in dependence of distance of current pixel
    vector<double> weightsY = {1., 0.75, 0.4, 0.2, 0.08, 0.04};

    vector<vector<double> > dataNew(nX, std::vector<double>(nY, 0.));

    for (int x = 0; x < nX; x++) {
        for (int y = 0; y < nY; y++) {
            double total = 0.;
            double weightSum = 0.;
            for (int xW = -halfAvWindow; xW <= halfAvWindow; xW++) {    // scanning window around current pixel to calculate average intensity
                for (int yW = -halfAvWindow; yW <= halfAvWindow; yW++) {

                    int xWAbs = std::abs(xW);
                    int yWAbs = std::abs(yW);

                    double weightX = (xWAbs < weightsX.size() )? weightsX[xWAbs] : weightsX[weightsX.size() - 1];   // if corresponding pixel is farther away than number of weights, take smallest ...
                    double weightY = (yWAbs < weightsY.size() )? weightsY[yWAbs] : weightsY[weightsY.size() - 1];   // ... weight value

                    double weight = weightX * weightY;
                    weightSum += weight;
                    int indX = x + xW;
                    if (indX < 0) indX = 0;     // preventing counting not existing pixels (beyond image)
                    if (indX >= nX) indX = nX - 1;
                    int indY = y + yW;
                    if (indY < 0) indY = 0;
                    if (indY >= nY) indY = nY - 1;

                    total += data[indX][indY] * weight;     
                }
            }

            dataNew[x][y] = total / weightSum; // averaged intensity
            if (dataNew[x][y] <= threshold) dataNew[x][y] = 0.;    // deleting pixels that are too dark; to yield only data for mirror and not background
        }
    }
    data = dataNew;
}

// getting a one dimensional line
void CbmRichRonchiAna::DoPeakFinderY(vector<vector<double> >& data)
{
    int nX = data.size();
    int nY = data[0].size();
    int halfWindow = 5;
    int samePeakCounterCut = 5;

    vector<vector<double> > dataNew(nX, std::vector<double>(nY, 0.));

    for (int x = 0; x < nX; x++) {
        for (int y = halfWindow; y < nY - halfWindow; y++){
            bool isPeak = (data[x][y] > 0. && data[x][y] >= data[x][y - 1]) && (data[x][y] >= data[x][y + 1]); // comparing current pixel with direct neighbours in y direction
            if (!isPeak) continue;

            // check if it is a plateau
            int samePeakCounter = 0;
            for (int yS = y; yS < nY; yS++) {
                if (data[x][y] == data[x][yS]) {
                    samePeakCounter++;
                } else {
                    break;
                }
            }
            if ( samePeakCounter >= samePeakCounterCut) {   // if plateau, then jump beyond it
                y = y + samePeakCounter + 1;
                continue;
            }

            bool isBiggest = true;
            for (int iW = -halfWindow; iW <= halfWindow; iW++) {    // comparing current pixel with 'halfWindow' next neighbours
                if (iW == 0) continue;
                if (data[x][y + iW] > data[x][y]) {
                    isBiggest = false;
                    break;
                }
            }
            bool hasNeighbourPeak = (dataNew[x][y - 2] > 0. || dataNew[x][y - 1] > 0. || dataNew[x][y + 1] > 0. || dataNew[x][y + 2] > 0.);

            if (isBiggest && isPeak && !hasNeighbourPeak) dataNew[x][y] = data[x][y];   // set value if is the biggest of 'halfWindow' neighbours and has no neighboured peak
        }
    }
    data = dataNew;
}

// smoothing lines by calculating mean position in y
void CbmRichRonchiAna::DoSmoothLines(vector<vector<double> >& data)
{
    int meanHalfLength = 8;
    int meanHalfHeight = 3;
    int nX = data.size();
    int nY = data[0].size();

    vector<vector<double> > dataNew(nX, std::vector<double>(nY, 0.));

    for (int x = meanHalfLength; x < nX-meanHalfLength; x++) {  // scanning whole image (except small border)
        for (int y = meanHalfHeight; y < nY-meanHalfHeight; y++) {
            if (data[x][y] == 0.) continue;
            double sumY = 0.;
            int counter = 0;
            for (int x2 = -meanHalfLength; x2 <= meanHalfLength; x2++) {    // scanning small window around current pixel and summing up positions in y of current and neighboured pixels
                for (int y2 = -meanHalfHeight; y2 <= meanHalfHeight; y2++) {
                    if (data[x+x2][y+y2] > 0) {
                        sumY += y + y2;
                        counter++;
                    }
                }
            }
            int newY = (int) sumY / counter; // average y-value of pixel
            dataNew[x][newY] = data[x][y];
        }
    }

    data = dataNew;
}

void CbmRichRonchiAna::DoLineSearch(vector<vector<double> >& data)
{
    int nX = data.size();
    int nY = data[0].size();

    int missingCounterCut = 15;
    int missingCounterTotalCut = 25;
    int halfWindowY = 2;

    vector<vector<double> > dataNew(nX, std::vector<double>(nY, 0.));
    double curIndex = 0.;   // why double?

    for (int x = 0; x < nX; x++) {  // scanning whole image
        for (int y = 0; y < nY; y++) {
            if (data[x][y] > 0. && dataNew[x][y] == 0.) {   // if this line pixel is not filled yet in 'dataNew'
                curIndex++;     // line index
                dataNew[x][y] = curIndex;   // found line is indexed 
                int missingCounterTotal = 0.;
                int missingCounter = 0.;
                int curY = y;
                for (int x1 = x + 1; x1 < nX; x1++) {   // drawing line in x; values are not intensity but line index
                    bool isFound = false;
                    for (int y1 = -halfWindowY; y1 <= halfWindowY; y1++) {  
                        if (data[x1][curY + y1] > 0. && dataNew[x1][curY + y1] == 0.){  // if pixel of line in next column is found ...
                            dataNew[x1][curY + y1] = curIndex;                          // ... fill current pixel with line index
                            curY += y1;
                            isFound = true;
                            break;
                        }
                    }
                    if (!isFound){  // to prevent indexing lines with wrong index (e.g. if gap is too big and next found line would not be continuation of current line)
                        missingCounterTotal++;
                        missingCounter++;
                    } 
                    else {
                        missingCounter = 0;
                    }
                    if (missingCounterTotal >= missingCounterTotalCut || missingCounter >= missingCounterCut) break;
                }
            }
        }
    }
    cout << "curIndex:" << curIndex << endl;
    data = dataNew;
}


// finding intersections
vector<CbmRichRonchiIntersectionData> CbmRichRonchiAna::DoIntersection(vector<vector<double> >& dataH, const vector<vector<double> >& dataV)
{
    int nX = dataV.size();
    int nY = dataV[0].size();

    vector<CbmRichRonchiIntersectionData> intersections;

    for (int x = 0; x < nX; x++) {
        for (int y = 0; y < nY; y++) {
            if (dataH[x][y] > 0. && dataV[x][y] > 0.) { // filling vector with data if both vertical and horizontal image have data greater ZERO here
                CbmRichRonchiIntersectionData data;
                data.fPixelX = x;       
                data.fPixelY = y;
                data.fLineY = dataH[x][y];  // 'data[x][y]' contains the line index now, not intensity (see prev. function 'DoLineSearch')
                data.fLineX = dataV[x][y];
                intersections.push_back(data);
            }
        }
    }
    cout << "intersections.size(): " << intersections.size() << endl;

    // remove close intersections
    set<int> removeSet;
    for (int i1 = 0; i1 < intersections.size(); i1++) {
        for (int i2 = i1 + 1; i2 < intersections.size(); i2++) {   
            double dX = intersections[i1].fPixelX - intersections[i2].fPixelX;  // distances in x and y of current observed intersections
            double dY = intersections[i1].fPixelY - intersections[i2].fPixelY;
            double dist = std::sqrt( dX * dX + dY * dY );
            if ( dist <= 10.) {     // if their distance is below a certain threshold, insert content to 'removeSet'
//                cout << i1 << " " << i2 << " " << intersections[i1].fPixelX <<  " " << intersections[i1].fPixelY << " "
//                        << intersections[i2].fPixelX <<  " " << intersections[i2].fPixelY << " " << dist << endl;
                removeSet.insert(i2);           // set of values from one of both close intersections is stored in 'removeSet'
            }
        }
    }

    set<int>::iterator it;
    for (it = removeSet.begin(); it != removeSet.end(); ++it) {
        swap(intersections[*it], intersections[intersections.size() - 1]);   // move one of both double counted intersections to the end of vector and remove these afterwards
        intersections.resize(intersections.size() - 1);
    }
    cout << "removeSet.size():" << removeSet.size() << " intersections.size(): " << intersections.size() << endl;

    return intersections;
}

// superposing horizontal and vertical image
vector<vector<double>> CbmRichRonchiAna::DoSuperpose(const vector<vector<double> >& dataH, const vector<vector<double> >& dataV)
{
    int nX = dataV.size();
    int nY = dataV[0].size();

    vector<vector<double> > dataSup(nX, std::vector<double>(nY, 0.));
    for (int x = 0; x < nX; x++) {
        for (int y = 0; y < nY; y++) {
            dataSup[x][y] = dataH[x][y] + dataV[x][y];  // data of horizontal and vertical image are being summed
        }
    }
    return dataSup;
}

void CbmRichRonchiAna::DoOrderLines(vector<CbmRichRonchiIntersectionData>& intersections, const string& option)
{
    map<int, CbmRichRonchiLineData> linesMap; // lineIndex to lineData
    vector<CbmRichRonchiLineData> lines;

    // indexing intersection points with either 'x' or 'y' and assign values from class 'Cbm...LineData' and storing in map
    for (auto const &curIntr : intersections){   
        int lineInd = (option == "x") ? curIntr.fLineX : curIntr.fLineY;    // fLineX/Y is the line index, set in 'DoLineSearch'
        linesMap[lineInd].fMeanPrimary += (option == "x") ? curIntr.fPixelX : curIntr.fPixelY;      // fPixelX/Y are the x/y-values, set in 'DoIntersection')
        linesMap[lineInd].fMeanSecondary += (option == "x") ? curIntr.fPixelY : curIntr.fPixelX;   
        linesMap[lineInd].fNofPoints++;         //fNofPoints is never set back?
        linesMap[lineInd].fLineInd = lineInd;   
    }

    for (auto &kv : linesMap) {     
        kv.second.fMeanPrimary = (double)kv.second.fMeanPrimary / kv.second.fNofPoints;     // calculating mean values of x and y positions to enable sorting
        kv.second.fMeanSecondary = (double)kv.second.fMeanSecondary / kv.second.fNofPoints;
        lines.push_back(kv.second);
        //cout << kv.first << " mean:" << kv.second.fMean << " nPoints:" << kv.second.fNofPoints << " lineInd:" << kv.second.fLineInd<< endl;
    }

    sort(lines.begin(), lines.end(), [](const CbmRichRonchiLineData &lhs, const CbmRichRonchiLineData &rhs) {
        return lhs.fMeanPrimary < rhs.fMeanPrimary;
    });

    // find first line with many points
    int lineIndMany = 0;
    for (int i = 0 ; i < lines.size() - 1; i++) {
        if (lines[i].fNofPoints >= 35) {
            lineIndMany = i; //dataSup
            break;
        }
    }

    // for the left edges we need to go other direction
    for (int i = lineIndMany - 1; i >= 1; i--) {
         if ( AreTwoSegmentsSameLine(&lines[i], &lines[i - 1]) ) {
             UpdateIntersectionLineInd(intersections, &lines[i], &lines[i - 1], option);
             i--; // skip next line
        }
    }

    for (int i = lineIndMany; i < lines.size() - 1; i++) {
        if (AreTwoSegmentsSameLine(&lines[i], &lines[i + 1]) ) {
            UpdateIntersectionLineInd(intersections, &lines[i], &lines[i + 1], option);
            i++; // skip next line
        }
    }

    // now we need to assign numbers in correct order
    int newLineIndex = 1;
    set<int> duplicateLines; // duplicates can occure for line segments
    for (auto &curLine : lines) {
        if (duplicateLines.find(curLine.fLineInd) != duplicateLines.end()) continue;
        duplicateLines.insert(curLine.fLineInd);
        for (auto &curIntr : intersections) {
            if (option == "x" && curIntr.fLineX == curLine.fLineInd) curIntr.fOrderedLineX = newLineIndex;
            if (option == "y" && curIntr.fLineY == curLine.fLineInd) curIntr.fOrderedLineY = newLineIndex;
        }
        newLineIndex++;
    }
    cout << "newLineIndex:" << newLineIndex << endl;
}

bool CbmRichRonchiAna::AreTwoSegmentsSameLine(const CbmRichRonchiLineData *line1, const CbmRichRonchiLineData *line2)
{
    int nofPointsMergeCut = 25;
    double pixelDistMergeCut = 200.;
    double pixelDiffMergeCut = 15.;

    return (line1->fNofPoints < nofPointsMergeCut && line2->fNofPoints < nofPointsMergeCut &&
            abs(line1->fMeanSecondary - line2->fMeanSecondary) > pixelDistMergeCut &&
            abs(line1->fMeanPrimary - line2->fMeanPrimary) <= pixelDiffMergeCut);
}

void CbmRichRonchiAna::UpdateIntersectionLineInd(vector<CbmRichRonchiIntersectionData> &intersections, CbmRichRonchiLineData *line1, 
                                      CbmRichRonchiLineData *line2, const string& option)
{
    for (auto &curIntr : intersections) {
        if (option == "x" && curIntr.fLineX == line2->fLineInd) curIntr.fLineX = line1->fLineInd;
        if (option == "y" && curIntr.fLineY == line2->fLineInd) curIntr.fLineY = line1->fLineInd;
    }
    line2->fLineInd = line1->fLineInd;
}

void CbmRichRonchiAna::DoLocalNormal(vector<CbmRichRonchiIntersectionData> &data)
{
    int xMin = 1000;
    int xMax = -1000;
    int yMin = 1000;
    int yMax = -1000;

    // extracting minimal and maximum x and y values
    for (int i = 0; i < data.size(); i++) {
        if (data[i].fOrderedLineX < xMin) xMin = data[i].fOrderedLineX;
        if (data[i].fOrderedLineX > xMax) xMax = data[i].fOrderedLineX;
        if (data[i].fOrderedLineY < yMin) yMin = data[i].fOrderedLineY;
        if (data[i].fOrderedLineY > yMax) yMax = data[i].fOrderedLineY;
    }
    
    int centerLineX = (xMax - xMin) / 2;    // horizontal and vertical 
    int centerLineY = (yMax - yMin) / 2;

    // defining reference point to calculate slopes
    for (int i = 0; i < data.size(); i++) {
        if (data[i].fOrderedLineX == centerLineX && data[i].fOrderedLineY == centerLineY) {
            fCenterCcdX = data[i].fPixelX - 20; // correction parameter in X
            fCenterCcdY = data[i].fPixelY - 34; // correction parameter in Y
        }
    }

    cout << "xMin:" << xMin << " xMax:" << xMax << " yMin:" << yMin << " yMax:" << yMax << endl;
    cout << "centerLineX:" << centerLineX << " centerLineY:" << centerLineY << endl;
    cout << "fCenterCcdX:" << fCenterCcdX << " fCenterCcdY:" << fCenterCcdY << endl;
    
    for (int i = 0; i < data.size(); i++) {
        // X and Y positions on CCD in microns
        data[i].fCcdV.SetX((data[i].fPixelX - fCenterCcdX) * fCcdPixelSize);
        data[i].fCcdV.SetY((data[i].fPixelY - fCenterCcdY) * fCcdPixelSize);
        data[i].fCcdV.SetZ(0.);

        // XYZ positions on ronchi ruling in microns
        data[i].fRulingV.SetX((data[i].fOrderedLineX - centerLineX) * fPitchGrid);
        data[i].fRulingV.SetY((data[i].fOrderedLineY - centerLineY) * fPitchGrid);
        data[i].fRulingV.SetZ(fDistRulingCCD);

        // slopes in X and Y direction of reflected beam
        double sRefX = -(data[i].fRulingV.X() - data[i].fCcdV.X()) / fDistRulingCCD; // 'minus' because Ruling is behind CoC
        double sRefY = -(data[i].fRulingV.Y() - data[i].fCcdV.Y()) / fDistRulingCCD;

        // extrapolating X and Y positions on mirror in microns
        data[i].fMirrorV.SetX((sRefX * fDistMirrorRuling) + fOffsetCCDOptAxisX);
        data[i].fMirrorV.SetY((sRefY * fDistMirrorRuling) + fOffsetCCDOptAxisY);
        data[i].fMirrorV.SetZ(fDistMirrorRuling + fDistRulingCCD);
        
        //cout << "fMirrorV_xyz = [" << data[i].fMirrorV.X() << ", " << data[i].fMirrorV.Y() << ", " << data[i].fMirrorV.Z() << "]" << endl;

        // calculating angles between optical axis of mirror and incident resp. reflected beam
        double mirrorCenterDist = sqrt(pow(data[i].fMirrorV.X(), 2) + pow(data[i].fMirrorV.Y(), 2));   // distance from mirrors center in microns
        double sagitta = fRadiusMirror - sqrt(pow(fRadiusMirror, 2) - pow(mirrorCenterDist, 2));                 // to calculate cathetus length
        double cathetus = fRadiusMirror - sagitta;   // if point source is in the center of curvature !!
        
        double angleIncX = atan(data[i].fMirrorV.X() / cathetus);  // if point source is in the center of curvature, otherwise add offset in Z direction !!
        double angleIncY = atan((data[i].fMirrorV.Y() - fOffsetLEDOpticalAxisY) / cathetus);
        double angleRefX = atan(sRefX);
        double angleRefY = atan(sRefY);
        data[i].fNormalRadX = ((angleIncX + angleRefX) / 2.0);   
        data[i].fNormalRadY = ((angleIncY + angleRefY) / 2.0);   
        
        // data[i].fNormalX = tan(angleNormalX);
        // data[i].fNormalY = tan(angleNormalY);

        double segmentSize = 5000; // half segment size; for the moment just assume that all segments are the same
        data[i].fCornerTL.SetXYZ( data[i].fMirrorV.X() + segmentSize, data[i].fMirrorV.Y() - segmentSize, data[i].fMirrorV.Z() );
        RotatePointImpl(&data[i].fCornerTL, &data[i].fCornerTLRot, data[i].fNormalRadX, data[i].fNormalRadY, &data[i].fMirrorV);

        data[i].fCornerTR.SetXYZ( data[i].fMirrorV.X() + segmentSize, data[i].fMirrorV.Y() + segmentSize, data[i].fMirrorV.Z() );
        RotatePointImpl(&data[i].fCornerTR, &data[i].fCornerTRRot, data[i].fNormalRadX, data[i].fNormalRadY, &data[i].fMirrorV);

        data[i].fCornerBL.SetXYZ( data[i].fMirrorV.X() - segmentSize, data[i].fMirrorV.Y() - segmentSize, data[i].fMirrorV.Z() );
        RotatePointImpl(&data[i].fCornerBL, &data[i].fCornerBLRot, data[i].fNormalRadX, data[i].fNormalRadY, &data[i].fMirrorV);

        data[i].fCornerBR.SetXYZ( data[i].fMirrorV.X() - segmentSize, data[i].fMirrorV.Y() + segmentSize, data[i].fMirrorV.Z() );
        RotatePointImpl(&data[i].fCornerBR, &data[i].fCornerBRRot, data[i].fNormalRadX, data[i].fNormalRadY, &data[i].fMirrorV);
    }
    
    /*// correction value to ZERO reference point at center of mirror
    for (int i = 0; i < data.size(); i++) {
        if (data[i].fOrderedLineX == centerLineX && data[i].fOrderedLineY == centerLineY) {
            double radius = sqrt(pow(data[i].fCornerTL.X(),2) + pow(data[i].fCornerTL.Y(),2));
            double sagitta2 = fRadiusMirror - sqrt(pow(fRadiusMirror,2) - pow(radius,2));
            fCorrection = (double)(data[i].fCornerTL.Z() - sagitta2 - fRadiusMirror);
            cout << "fCornerTL.Z_Sagitta = " << fRadiusMirror - data[i].fCornerTL.Z() << ", sagitta2 = " << sagitta2 << endl;
            cout << "CORRECTION = " << (double)fCorrection << endl << endl;
        }
    }*/
    
    {
        DrawXYMum(data);
    }
    
    {
        TCanvas *c = new TCanvas("ronchi_xz_mum_lineY20", "ronchi_xz_mum_lineY20", 1800, 900);
        c->Divide(2,1);
        c->cd(1);
        DrawXZProjection(data, 20, 1.);
        c->cd(2);
        DrawXZProjection(data, 20, 0.025);
    }

    {
        DrawMirrorSegments(data, 20, 20);
    }
}

// constructing spherical surface
void CbmRichRonchiAna::DoSphere(vector<CbmRichRonchiIntersectionData> &data)
{
    int nI = data.size();
    
    int iMin = 1000;
    int iMax = -1000;
    int jMin = 1000;
    int jMax = -1000;
    
    int iFirst, jFirst, lFirst; // dates of intersection that is closest to the center (i: column, j: row, l: l_th argument of vector)
    int currentI, currentJ;
    
    int    radiusIndex = 0;  // to determine n_th neighbour of center segment
    double radiusSag;        // distance of first found intersection to the center to calculate sagitta (NEEDED??)
    
    // extracting minimal and maximal I and J values
    for (int i = 0; i < nI; i++) {
        iMin = (data[i].fLineX < iMin) ? data[i].fOrderedLineX : iMin;
        iMax = (data[i].fLineX > iMax) ? data[i].fOrderedLineX : iMax;
        jMin = (data[i].fLineY < jMin) ? data[i].fOrderedLineY : jMin;
        jMax = (data[i].fLineY > jMax) ? data[i].fOrderedLineY : jMax;
    }
    
    // finding centered intersection and setting values
    for (int i = 0; i < nI; i++) {
        if (data[i].fOrderedLineX == (int)((iMin+iMax)/2) && data[i].fOrderedLineY == (int)((jMin+jMax)/2)) {
            data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
            data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
            data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
            data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
            data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
            data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y());
            data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
            data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());            
            data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z());
            data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z());
            data[i].fCornerBLSphere.SetZ(data[i].fCornerBLRot.Z());
            data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z());
            iFirst = data[i].fOrderedLineX;
            jFirst = data[i].fOrderedLineY;
            lFirst = i;
            currentI = data[i].fOrderedLineX;
            currentJ = data[i].fOrderedLineY;
            break;
        }
    }
    
    cout << "DoSphere:: currentI|J = " << currentI << "|" << currentJ << endl;
    
    // sphere constructing loop: shifting segments parallel to Z axis
    while (radiusIndex <= jMax/2) {    
        radiusIndex++;
        cout << endl << "DoSphere:: radiusIndex = " << radiusIndex << endl;
       
        // scanning left edge
        currentI = iFirst - radiusIndex;
        currentJ = jFirst - radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ && currentJ == (jFirst - radiusIndex)) {   // segment in bl corner
                for (int j = 0; j < nI; j++) {
                    if (data[j].fOrderedLineX == (currentI + 1) && data[j].fOrderedLineY == (jFirst - radiusIndex + 1)) {
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y());
                        data[i].fCornerTRSphere.SetZ(data[j].fCornerBLSphere.Z());
                        int offsetZ = data[j].fCornerBLSphere.Z() - data[i].fCornerTRRot.Z();
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y()) ;
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[i].fCornerBLRot.Z() + offsetZ);
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z() + offsetZ);
                        //cout << "bl: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ && currentJ == (jFirst + radiusIndex)) {   // segment in tl corner
                for (int j = 0; j < nI; j++) {
                    if (data[j].fOrderedLineX == (currentI + 1) && data[j].fOrderedLineY == (jFirst + radiusIndex - 1)) {
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[j].fCornerTLSphere.Z());
                        int offsetZ = data[j].fCornerTLSphere.Z() - data[i].fCornerBRRot.Z();
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[i].fCornerBLRot.Z() + offsetZ);
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y());
                        data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z() + offsetZ);
                        //cout << "tl: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ && currentJ < (jFirst + radiusIndex) && currentJ > (jFirst - radiusIndex)) { // remaining segments on left edge 
                for (int j = 0; j < nI; j++) {                                                                                                                           
                    if (data[j].fOrderedLineX == (currentI + 1) && data[j].fOrderedLineY == currentJ) {
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[j].fCornerBLSphere.Z());
                        int offsetZ = data[j].fCornerBLSphere.Z() - data[i].fCornerBRRot.Z();
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[i].fCornerBLRot.Z() + offsetZ);
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        //cout << "left edge: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
        }
        
        // scanning right edge      !! why are the offsetZ values so high??
        currentI = iFirst + radiusIndex;
        currentJ = jFirst - radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ && currentJ == (jFirst - radiusIndex)) {   // segment in br corner
                for (int j = 0; j < nI; j++) {
                    if (data[j].fOrderedLineX == (currentI - 1) && data[j].fOrderedLineY == (jFirst - radiusIndex + 1)) {
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[j].fCornerBRSphere.Z());
                        int offsetZ = data[j].fCornerBRSphere.Z() - data[i].fCornerTLRot.Z();
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y()) ;
                        data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z() + offsetZ);
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[i].fCornerBLRot.Z() + offsetZ);
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z() + offsetZ);
                        //cout << "br: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ && currentJ == (jFirst + radiusIndex)) {   // segment in tr corner
                for (int j = 0; j < nI; j++) {
                    if (data[j].fOrderedLineX == (currentI + 1) && data[j].fOrderedLineY == (jFirst + radiusIndex - 1)) {
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[j].fCornerTRSphere.Z());
                        int offsetZ = data[j].fCornerTRSphere.Z() - data[i].fCornerBLRot.Z();
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z() + offsetZ);
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y());
                        data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z() + offsetZ);
                        //cout << "tr: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ && currentJ < (jFirst + radiusIndex) && currentJ > (jFirst - radiusIndex)) { // remaining segments on right edge 
                for (int j = 0; j < nI; j++) {                                                                                                                           
                    if (data[j].fOrderedLineX == (currentI - 1) && data[j].fOrderedLineY == currentJ) {
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[j].fCornerBRSphere.Z());
                        int offsetZ = data[j].fCornerBRSphere.Z() - data[i].fCornerBLRot.Z();
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z() + offsetZ);
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y());
                        data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z() + offsetZ);
                        //cout << "right edge: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
        }
        
        // scanning upper edge
        currentI = iFirst - radiusIndex + 1;
        currentJ = jFirst + radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ) {   
                for (int j = 0; j < nI; j++) {
                    if (data[j].fOrderedLineX == (currentI) && data[j].fOrderedLineY == currentJ) {
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[j].fCornerTLSphere.Z());
                        int offsetZ = data[j].fCornerTLSphere.Z() - data[i].fCornerBLRot.Z();
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y()) ;
                        data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z() + offsetZ);
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z() + offsetZ);
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[i].fCornerTLRot.Z() + offsetZ);
                        //cout << "upper: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentI++;
                if (currentI >= iFirst + radiusIndex or currentI > iMax) break;
            }            
        }
        
        // scanning lower edge
        currentI = iFirst - radiusIndex + 1;
        currentJ = jFirst - radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (data[i].fOrderedLineX == currentI && data[i].fOrderedLineY == currentJ) {   
                for (int j = 0; j < nI; j++) {
                    if (data[j].fOrderedLineX == (currentI) && data[j].fOrderedLineY == currentJ) {
                        data[i].fCornerTLSphere.SetX(data[i].fCornerTLRot.X());
                        data[i].fCornerTLSphere.SetY(data[i].fCornerTLRot.Y());
                        data[i].fCornerTLSphere.SetZ(data[j].fCornerBLSphere.Z());
                        int offsetZ = data[j].fCornerTLSphere.Z() - data[i].fCornerBLRot.Z();
                        data[i].fCornerTRSphere.SetX(data[i].fCornerTRRot.X());
                        data[i].fCornerTRSphere.SetY(data[i].fCornerTRRot.Y()) ;
                        data[i].fCornerTRSphere.SetZ(data[i].fCornerTRRot.Z() + offsetZ);
                        data[i].fCornerBRSphere.SetX(data[i].fCornerBRRot.X());
                        data[i].fCornerBRSphere.SetY(data[i].fCornerBRRot.Y());
                        data[i].fCornerBRSphere.SetZ(data[i].fCornerBRRot.Z() + offsetZ);
                        data[i].fCornerBLSphere.SetX(data[i].fCornerBLRot.X());
                        data[i].fCornerBLSphere.SetY(data[i].fCornerBLRot.Y());
                        data[i].fCornerBLSphere.SetZ(data[i].fCornerBLRot.Z() + offsetZ);
                        cout << "lower: offsetZ = " << offsetZ << endl;
                        break;
                    }
                }
                i = -1;
                currentI++;
                if (currentI >= iFirst + radiusIndex or currentI > iMax) break;
            }            
        }
    }    
}



void CbmRichRonchiAna::rootgeom(bool &vis)
{
   // gStyle->SetCanvasPreferGL(true);
   TGeoManager *geom = new TGeoManager("simple1", "Simple geometry");
   //--- define some materials
   TGeoMaterial *matVacuum = new TGeoMaterial("Vacuum", 0,0,0);
   TGeoMaterial *matAl = new TGeoMaterial("Al", 26.98,13,2.7);
//   //--- define some media
   TGeoMedium *Vacuum = new TGeoMedium("Vacuum",1, matVacuum);
   TGeoMedium *Al = new TGeoMedium("Root Material",2, matAl);
   //--- define the transformations
   TGeoTranslation *tr1 = new TGeoTranslation(20., 0, 0.);
   TGeoTranslation *tr2 = new TGeoTranslation(10., 0., 0.);
   TGeoTranslation *tr3 = new TGeoTranslation(10., 20., 0.);
   TGeoTranslation *tr4 = new TGeoTranslation(5., 10., 0.);
   TGeoTranslation *tr5 = new TGeoTranslation(20., 0., 0.);
   TGeoTranslation *tr6 = new TGeoTranslation(-6., 0., 0.);
   TGeoTranslation *tr7 = new TGeoTranslation(7.5, 7.5, 0.);
   TGeoRotation   *rot1 = new TGeoRotation("rot1", 90., 0., 90., 270., 0., 0.);
   TGeoCombiTrans *combi1 = new TGeoCombiTrans(7.5, -7.5, 0., rot1);
   TGeoTranslation *tr8 = new TGeoTranslation(7.5, -5., 0.);
   TGeoTranslation *tr9 = new TGeoTranslation(7.5, 20., 0.);
   TGeoTranslation *tr10 = new TGeoTranslation(85., 0., 0.);
   TGeoTranslation *tr11 = new TGeoTranslation(35., 0., 0.);
   TGeoTranslation *tr12 = new TGeoTranslation(-15., 0., 0.);
   TGeoTranslation *tr13 = new TGeoTranslation(-65., 0., 0.);
   TGeoTranslation  *tr14 = new TGeoTranslation(0,0,-100);
   TGeoCombiTrans *combi2 = new TGeoCombiTrans(0,0,100,
                                   new TGeoRotation("rot2",90,180,90,90,180,0));
   TGeoCombiTrans *combi3 = new TGeoCombiTrans(100,0,0,
                                   new TGeoRotation("rot3",90,90,180,0,270,0));
   TGeoCombiTrans *combi4 = new TGeoCombiTrans(-100,0,0,
                                   new TGeoRotation("rot4",90,90,0,0,90,0));
   TGeoCombiTrans *combi5 = new TGeoCombiTrans(0,100,0,
                                   new TGeoRotation("rot5",0,0,90,180,90,270));
   TGeoCombiTrans *combi6 = new TGeoCombiTrans(0,-100,0,
                                   new TGeoRotation("rot6",180,0,90,180,90,90));
   //--- make the top container volume
   Double_t worldx = 110.;
   Double_t worldy = 50.;
   Double_t worldz = 5.;
   TGeoVolume *top = geom->MakeBox("TOP", Vacuum, 270., 270., 120.);
   geom->SetTopVolume(top);
   TGeoVolume *replica = geom->MakeBox("REPLICA", Vacuum,120,120,120);
   replica->SetVisibility(kFALSE);
   TGeoVolume *rootbox = geom->MakeBox("ROOT", Vacuum, 110., 50., 5.);
   rootbox->SetVisibility(kFALSE);
   
   /* //--- make letter 'R'
   TGeoVolume *R = geom->MakeBox("R", Vacuum, 25., 25., 5.);
   R->SetVisibility(kFALSE);
   TGeoVolume *bar1 = geom->MakeBox("bar1", Al, 5., 25, 5.);
   bar1->SetLineColor(kRed);
   R->AddNode(bar1, 1, tr1);
   TGeoVolume *bar2 = geom->MakeBox("bar2", Al, 5., 5., 5.);
   bar2->SetLineColor(kRed);
   R->AddNode(bar2, 1, tr2);
   R->AddNode(bar2, 2, tr3);
   TGeoVolume *tub1 = geom->MakeTubs("tub1", Al, 5., 15., 5., 90., 270.);
   tub1->SetLineColor(kRed);
   R->AddNode(tub1, 1, tr4);
   TGeoVolume *bar3 = geom->MakeArb8("bar3", Al, 5.);
   bar3->SetLineColor(kRed);
   TGeoArb8 *arb = (TGeoArb8*)bar3->GetShape();
   arb->SetVertex(0, 15., -5.);
   arb->SetVertex(1, 0., -25.);
   arb->SetVertex(2, -10., -25.);
   arb->SetVertex(3, 5., -5.);
   arb->SetVertex(4, 15., -5.);
   arb->SetVertex(5, 0., -25.);
   arb->SetVertex(6, -10., -25.);
   arb->SetVertex(7, 5., -5.);
   R->AddNode(bar3, 1, gGeoIdentity); 
   
   //--- make letter 'O'
   TGeoVolume *O = geom->MakeBox("O", Vacuum, 25., 25., 5.);
   O->SetVisibility(kTRUE);
   TGeoVolume *bar4 = geom->MakeBox("bar4", Al, 5., 7.5, 5.);
   bar4->SetLineColor(kGreen);
   O->AddNode(bar4, 1, tr7);
   O->AddNode(bar4, 1, tr6);
   TGeoVolume *tub2 = geom->MakeTubs("tub1", Al, 7.5, 17.5, 5., 0., 180.);
   tub2->SetLineColor(kBlue);
   O->AddNode(tub2, 1, tr9);
   O->AddNode(tub2, 2, combi1); */
   
   
   //--- make letter 'T'
   TGeoVolume *T = geom->MakeBox("T", Vacuum, 250., 250., 50.);
   T->SetVisibility(kFALSE);
   TGeoVolume *bar5 = geom->MakeBox("bar5", Al, 5., 20., 5.);
   bar5->SetLineColor(kBlue);
   T->AddNode(bar5, 1, tr8);
   TGeoVolume *bar6 = geom->MakeBox("bar6", Al, 17.5, 5., 5.);
   bar6->SetLineColor(kBlue);
   T->AddNode(bar6, 1, tr9); 
   
   //rootbox->AddNode(R, 1, tr10);
   //rootbox->AddNode(O, 1, tr11);
   //rootbox->AddNode(O, 2, tr12);
   rootbox->AddNode(T, 1, tr13);
   replica->AddNode(rootbox, 1, tr1);
   //replica->AddNode(rootbox, 2, combi2);
   replica->AddNode(rootbox, 3, combi3);
   /*replica->AddNode(rootbox, 4, combi4);
   replica->AddNode(rootbox, 5, combi5);
   replica->AddNode(rootbox, 6, combi6);*/
   top->AddNode(replica, 1, new TGeoTranslation(-150, -150, 0));
/*   top->AddNode(replica, 2, new TGeoTranslation(150, -150, 0));
   top->AddNode(replica, 3, new TGeoTranslation(150, 150, 0));
   top->AddNode(replica, 4, new TGeoTranslation(-150, 150, 0)); */
   
   //--- close the geometry
   geom->CloseGeometry();
   //--- draw the ROOT box.
   // by default the picture will appear in the standard ROOT TPad.
   //if you have activated the following line in system.rootrc,
   //it will appear in the GL viewer
   //#Viewer3D.DefaultDrawOption:   ogl
   geom->SetVisLevel(4);
   if (vis) top->Draw("ogle");
}



void CbmRichRonchiAna::DoDeviation(vector<CbmRichRonchiIntersectionData> &data)
{
    double radius, sagitta;
    
    for (int i = 0; i < data.size(); i++) {
        radius = sqrt(pow(data[i].fCornerTL.X(),2) + pow(data[i].fCornerTL.Y(),2));
        sagitta = fRadiusMirror - sqrt(pow(fRadiusMirror,2) - pow(radius,2));
        data[i].fDeviation = fRadiusMirror - data[i].fCornerTLRot.Z() - fCorrection; // - sagitta;
        //cout << "deviation = " << data[i].fDeviation << endl;
    }
    {
        DrawHeightMum(data);    
    }
}


void CbmRichRonchiAna::DrawHeightMum(vector<CbmRichRonchiIntersectionData> &data)
{
    int nBins = 1000;
    TH2D* hMirrorHeight = new TH2D("hMirrorHeight", "hMirrorHeight; Mirror_X [mum]; Mirror_Y [mum];", nBins, -250000, 250000, nBins, -250000, 250000);
    TCanvas* c = new TCanvas("mirror_height", "mirror_height", 1000, 1000);
    int radius = 800;
    DrawH2(hMirrorHeight);
    for (int i = 0; i < data.size(); i++) {
        //TEllipse *center = new TEllipse((int)data[i].fMirrorV.X(), (int)data[i].fMirrorV.Y(), 1000, 1000);
        //center->Draw();
        for (int x = (int)(data[i].fCornerTLRot.X()-radius); x <= (int)(data[i].fCornerTLRot.X()+radius); x++) {
            for (int y = data[i].fCornerTLRot.Y()-radius; y <= data[i].fCornerTLRot.Y()+radius; y++) {
                hMirrorHeight->SetBinContent((int)(x/500)+490, (int)(y/500)+530, data[i].fDeviation+124500);
                hMirrorHeight->GetXaxis()->FindBin(y);
            }
        }
    }
}

void CbmRichRonchiAna::DrawXYMum(vector<CbmRichRonchiIntersectionData> &data)
{
    vector<int> colors = {kBlack, kGreen, kBlue, kRed, kYellow, kOrange, kCyan, kGray, kMagenta, kYellow + 2, kRed + 3};
    TH2D *hCcdXY = new TH2D("hCcdXY", "hCcdXY;CCD_X [mum];CCD_Y [mum];", 1, -7000, 7000, 1, -7000, 7000);
    TH2D *hRulingXY = new TH2D("hRulingXY", "hRulingXY;Ruling_X [mum];Ruling_Y [mum];", 1, -7000, 7000, 1, -7000, 7000);
    TH2D *hMirrorXY = new TH2D("hMirrorXY", "hMirrorXY;Mirror_X [mum];Mirror_Y [mum];", 1, -250000, 250000, 1, -250000, 250000);
    TCanvas *c = new TCanvas("ronchi_xy_mum", "ronchi_xy_mum", 1800, 600);
    c->Divide(3,1);
    c->cd(1);
    DrawH2(hCcdXY);
    gPad->SetRightMargin(0.10);
    for (int i = 0; i < data.size(); i++){
        TEllipse *center = new TEllipse(data[i].fCcdV.X(), data[i].fCcdV.Y(), 50);
        center->SetFillColor(colors[data[i].fOrderedLineX % colors.size()]);
        center->Draw();
    }

    c->cd(2);
    DrawH2(hRulingXY);
    gPad->SetRightMargin(0.10);
    for (int i = 0; i < data.size(); i++){
        TEllipse *center = new TEllipse(data[i].fRulingV.X(), data[i].fRulingV.Y(), 50);
        center->SetFillColor(colors[data[i].fOrderedLineX % colors.size()]);
        center->Draw();
    }

    c->cd(3);
    DrawH2(hMirrorXY);
    gPad->SetRightMargin(0.10);
    for (int i = 0; i < data.size(); i++) {
        TEllipse *center = new TEllipse(data[i].fMirrorV.X(), data[i].fMirrorV.Y(), 2500);
        center->SetFillColor(colors[data[i].fOrderedLineX % colors.size()]);
        center->Draw();
    }
}

void CbmRichRonchiAna::DrawXZProjection(vector<CbmRichRonchiIntersectionData> &data, int orderedLineY, double scale)
{
    string histName = "hZX_lineY" + to_string(orderedLineY) + "_scale" + to_string(scale);

    TH2D *hZX = new TH2D(histName.c_str(), (histName+";Z [mum];X [mum];").c_str(), 100, -0.02* scale * fDistMirrorRuling, 1.05 * scale * fDistMirrorRuling, 100, 
            scale * -250000, scale * 250000);
    DrawH2(hZX);
    gPad->SetRightMargin(0.10);
    for (int i = 0; i < data.size(); i++){
        if (data[i].fOrderedLineY != orderedLineY) continue;

        TEllipse *ccdEllipse = new TEllipse(data[i].fCcdV.Z(), data[i].fCcdV.X(), scale * 20000, scale * 2000);
        ccdEllipse->SetFillColor(kRed);
        ccdEllipse->Draw();

        TEllipse *rulingEllipse = new TEllipse(data[i].fRulingV.Z(), data[i].fRulingV.X(), scale * 20000, scale * 2000);
        rulingEllipse->SetFillColor(kBlue);
        rulingEllipse->Draw();

        TEllipse *mirrorEllipse = new TEllipse(data[i].fMirrorV.Z(), data[i].fMirrorV.X(), scale * 20000, scale * 2000);
        mirrorEllipse->SetFillColor(kGreen);
        mirrorEllipse->Draw();

        TLine *rulingLine = new TLine(data[i].fCcdV.Z(), data[i].fCcdV.X(), data[i].fRulingV.Z(), data[i].fRulingV.X());
        rulingLine->Draw();

        TLine *mirrorLine = new TLine(data[i].fRulingV.Z(), data[i].fRulingV.X(), data[i].fMirrorV.Z(), data[i].fMirrorV.X());
        mirrorLine->Draw();

        double xNorm = data[i].fMirrorV.X() - fDistMirrorRuling * tan(data[i].fNormalRadX); 
        TLine *mirrorLineNorm = new TLine(fDistRulingCCD, xNorm, data[i].fMirrorV.Z(), data[i].fMirrorV.X());
        mirrorLineNorm->SetLineColor(kBlue);
        mirrorLineNorm->Draw();
    }
}

void CbmRichRonchiAna::DrawMirrorSegments(vector<CbmRichRonchiIntersectionData> &data, int orderedLineX, int orderedLineY)
{
    string cName = "ronchi_mirror_segments_lineX" + to_string(orderedLineX) + "_lineY" + to_string(orderedLineY);
    TCanvas *c = new TCanvas(cName.c_str(), cName.c_str(), 1800, 900);
    c->Divide(2,1);
    
    string histNameXY = "hXY_mirror_segments_lineX" + to_string(orderedLineX) + "_lineY" + to_string(orderedLineY);
    TH2D *hXY = new TH2D(histNameXY.c_str(), (histNameXY+"hXY;X [mum];Y [mum];").c_str(), 1, -250000, 250000, 1, -250000, 250000);
    vector<int> colors = {kBlack, kGreen, kBlue, kRed, kYellow, kOrange, kCyan, kGray, kMagenta, kYellow + 2, kRed + 3};
    c->cd(1);
    DrawH2(hXY);    // boxes
    gPad->SetRightMargin(0.10);
    for (int i = 0; i < data.size(); i++){
        //if (data[i].fOrderedLineY != orderedLineY) continue;
        int color = colors[data[i].fOrderedLineY % colors.size()];
        TLine *line1 = new TLine(data[i].fCornerTLRot.X(), data[i].fCornerTLRot.Y(), data[i].fCornerTRRot.X(), data[i].fCornerTRRot.Y());
        line1->SetLineColor(color);
        line1->Draw();

        TLine *line2 = new TLine(data[i].fCornerTRRot.X(), data[i].fCornerTRRot.Y(), data[i].fCornerBRRot.X(), data[i].fCornerBRRot.Y());
        line2->SetLineColor(color);
        line2->Draw();

        TLine *line3 = new TLine(data[i].fCornerBRRot.X(), data[i].fCornerBRRot.Y(), data[i].fCornerBLRot.X(), data[i].fCornerBLRot.Y());
        line3->SetLineColor(color);
        line3->Draw();

        TLine *line4 = new TLine(data[i].fCornerBLRot.X(), data[i].fCornerBLRot.Y(), data[i].fCornerTLRot.X(), data[i].fCornerTLRot.Y());
        line4->SetLineColor(color);
        line4->Draw();
    }

    double hSize = 250000;
    string histNameZX = "hZX_mirror_segments_lineX" + to_string(orderedLineX) + "_lineY" + to_string(orderedLineY);
    TH2D *hZX = new TH2D(histNameZX.c_str(), (histNameZX+";Z [mum];X [mum];").c_str(), 100, fDistMirrorRuling - hSize, fDistMirrorRuling + hSize, 100, -1.*hSize, hSize);
    c->cd(2);
    DrawH2(hZX);
    gPad->SetRightMargin(0.10);
    for (int i = 0; i < data.size(); i++){
        if (data[i].fOrderedLineY != orderedLineY) continue;

        TEllipse *mirrorEllipse = new TEllipse(data[i].fMirrorV.Z(), data[i].fMirrorV.X(), 2000, 2000);
        mirrorEllipse->SetFillColor(kGreen);
        mirrorEllipse->Draw();

        TLine *rulingLine = new TLine(data[i].fCcdV.Z(), data[i].fCcdV.X(), data[i].fRulingV.Z(), data[i].fRulingV.X());
        rulingLine->Draw();

        TLine *mirrorLine = new TLine(data[i].fRulingV.Z(), data[i].fRulingV.X(), data[i].fMirrorV.Z(), data[i].fMirrorV.X());
        mirrorLine->Draw();

        double xNorm = data[i].fMirrorV.X() - fDistMirrorRuling * tan(data[i].fNormalRadX); 
        TLine *mirrorLineNorm = new TLine(fDistRulingCCD, xNorm, data[i].fMirrorV.Z(), data[i].fMirrorV.X());
        mirrorLineNorm->SetLineColor(kBlue);
        mirrorLineNorm->Draw();

        TLine *mirrorLineSeg = new TLine(data[i].fCornerBRRot.Z(), data[i].fCornerBRRot.X(), data[i].fCornerTRRot.Z(), data[i].fCornerTRRot.X());
        mirrorLineSeg->SetLineColor(kRed);
        mirrorLineSeg->Draw();
    }
}

/*
// calculating heights of single segments
void CbmRichRonchiAna::DoHeight(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int currentLineX, currentLineY;
        
    // determining absolute positions in X and Y of segments corners on mirror
    for (int i = 0; i < intersections.size(); i++) {
        currentLineX = intersections[i].fOrderedLineX;
        currentLineY = intersections[i].fOrderedLineY;
        for (int j = 0; j < intersections.size(); j++) {
            // position of top left corner
            if (intersections[j].fOrderedLineX == currentLineX - 1 && intersections[j].fOrderedLineY == currentLineY + 1) {
                intersections[i].fTLPositionX = (intersections[i].fMirrorV.X() + intersections[j].fMirrorV.X()) / 2.0;
                intersections[i].fTLPositionY = (intersections[i].fMirrorV.Y() + intersections[j].fMirrorV.Y()) / 2.0;
            }
            // position of top right corner
            if (intersections[j].fOrderedLineX == currentLineX + 1 && intersections[j].fOrderedLineY == currentLineY + 1) {
                intersections[i].fTRPositionX = (intersections[i].fMirrorV.X() + intersections[j].fMirrorV.X()) / 2.0;
                intersections[i].fTRPositionY = (intersections[i].fMirrorV.Y() + intersections[j].fMirrorV.Y()) / 2.0;
            }
            // position of bottom left corner
            if (intersections[j].fOrderedLineX == currentLineX - 1 && intersections[j].fOrderedLineY == currentLineY - 1) {
                intersections[i].fBLPositionX = (intersections[i].fMirrorV.X() + intersections[j].fMirrorV.X()) / 2.0;
                intersections[i].fBLPositionY = (intersections[i].fMirrorV.Y() + intersections[j].fMirrorV.Y()) / 2.0;
            }
            // position of bottom right corner
            if (intersections[j].fOrderedLineX == currentLineX + 1 && intersections[j].fOrderedLineY == currentLineY - 1) {
                intersections[i].fBRPositionX = (intersections[i].fMirrorV.X() + intersections[j].fMirrorV.X()) / 2.0;
                intersections[i].fBRPositionY = (intersections[i].fMirrorV.Y() + intersections[j].fMirrorV.Y()) / 2.0;
            }
        }
    }    
    
    // calculating heights of corners
    for (int i = 0; i < intersections.size(); i++) {
        // calculating length of X and Y parts of corner vectors
        double xTL, xTR, xBL, xBR;
        double yTL, yTR, yBL, yBR;
        double z = 0;
        xTL = (intersections[i].fTLPositionX - intersections[i].fMirrorV.X());
        yTL = (intersections[i].fTLPositionY - intersections[i].fMirrorV.Y());
        xTR = (intersections[i].fTRPositionX - intersections[i].fMirrorV.X());
        yTR = (intersections[i].fTRPositionY - intersections[i].fMirrorV.Y());
        xBL = (intersections[i].fBLPositionX - intersections[i].fMirrorV.X());
        yBL = (intersections[i].fBLPositionY - intersections[i].fMirrorV.Y());
        xBR = (intersections[i].fBRPositionX - intersections[i].fMirrorV.X());
        yBR = (intersections[i].fBRPositionY - intersections[i].fMirrorV.Y());
        intersections[i].fVecTL.push_back(xTL);     
        intersections[i].fVecTL.push_back(yTL);
        intersections[i].fVecTL.push_back(z);
        intersections[i].fVecTR.push_back(xTR);
        intersections[i].fVecTR.push_back(yTR);
        intersections[i].fVecTR.push_back(z);
        intersections[i].fVecBL.push_back(xBL);
        intersections[i].fVecBL.push_back(yBL);
        intersections[i].fVecBL.push_back(z);
        intersections[i].fVecBR.push_back(xBR);
        intersections[i].fVecBR.push_back(yBR);
        intersections[i].fVecBR.push_back(z);
        
        //cout << "At [" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "]:" << endl;
        //cout << "normalX/Y = " << intersections[i].fNormalX << "/" << intersections[i].fNormalY << endl;
        //cout << "AnglesNormalX/Y = " << intersections[i].fAngleNormalX << "/" << intersections[i].fAngleNormalY << endl;
        //cout << "vectorTL = [" << intersections[i].fVecTL[0] << "," << intersections[i].fVecTL[1] << "," << intersections[i].fVecTL[2] << "]" << endl;
        //cout << "vectorTR = [" << intersections[i].fVecTR[0] << "," << intersections[i].fVecTR[1] << "," << intersections[i].fVecTR[2] << "]" << endl;
        //cout << "vectorBL = [" << intersections[i].fVecBL[0] << "," << intersections[i].fVecBL[1] << "," << intersections[i].fVecBL[2] << "]" << endl;
        //cout << "vectorBR = [" << intersections[i].fVecBR[0] << "," << intersections[i].fVecBR[1] << "," << intersections[i].fVecBR[2] << "]" << endl;
        
        double sinX = sin(intersections[i].fNormalRadX);
        double cosX = cos(intersections[i].fNormalRadX);
        double sinY = sin(intersections[i].fNormalRadY);
        double cosY = cos(intersections[i].fNormalRadY);
        // calculating heights by rotating the corner vectors (V' = R_x * R_y * V)  (!= R_y * R_x * V !!!!!!!!!!!!)
        intersections[i].fTLHeight = yTL * sinX + cosX * (-xTL * sinY + z * cosY );
        intersections[i].fTRHeight = yTR * sinX + cosX * (-xTR * sinY + z * cosY );
        intersections[i].fTLHeight = yBL * sinX + cosX * (-xBL * sinY + z * cosY );
        intersections[i].fTLHeight = yBR * sinX + cosX * (-xBR * sinY + z * cosY );
    
    }
}


// constructing spherical surface
void CbmRichRonchiAna::DoSphere(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int nI = intersections.size();
    
    int xMin, xMax, yMin, yMax; // to find intersection that is closest to the center
    int iMin = 1000;
    int iMax = -1000;
    int jMin = 1000;
    int jMax = -1000;
    
    int iFirst, jFirst, lFirst; // dates of intersection that is closest to the center (i: column, j: row, l: l_th argument of vector)
    int currentI, currentJ;
    
    int    radiusCCD   = 1;  // distance in pixels from center on CCD
    int    radiusIndex = 0;  // to determine n_th neighbour of center segment
    double radiusSag;        // distance of first found intersection to the center to calculate sagitta (NEEDED??)
    
    // extracting minimal and maximal I and J values
    for (int i = 0; i < intersections.size(); i++) {
        iMin = (intersections[i].fLineX < iMin) ? intersections[i].fOrderedLineX : iMin;
        iMax = (intersections[i].fLineX > iMax) ? intersections[i].fOrderedLineX : iMax;
        jMin = (intersections[i].fLineY < jMin) ? intersections[i].fOrderedLineY : jMin;
        jMax = (intersections[i].fLineY > jMax) ? intersections[i].fOrderedLineY : jMax;
    }

    cout << "DoSphere:: i/j Min/Max = " << iMin << "," << iMax << "," << jMin << "," << jMax << endl;

    // finding intersection that is closest to the center and determine its center as ZERO
    for (int i = 0; i < intersections.size(); i++ ) {
        xMin = fCenterCcdX - radiusCCD;
        xMax = fCenterCcdX + radiusCCD;
        yMin = fCenterCcdY - radiusCCD;
        yMax = fCenterCcdY + radiusCCD;
        if (intersections[i].fPixelX >= xMin && intersections[i].fPixelX <= xMax && intersections[i].fPixelY >= yMin && intersections[i].fPixelY <= yMax) {
            iFirst = intersections[i].fOrderedLineX;
            jFirst = intersections[i].fOrderedLineY;
            lFirst = i;
            intersections[i].fTLSum = intersections[i].fTLHeight;
            intersections[i].fTRSum = intersections[i].fTRHeight;
            intersections[i].fBLSum = intersections[i].fBLHeight;
            intersections[i].fBRSum = intersections[i].fBRHeight;
            intersections[i].fHeightCenterSum = 0;
            //intersections[i].fIndexSphere  = true;
            //cout << "DoSphere:: Found center: intersections[" << i << "] = [" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "," << intersections[i].fOrderedLineX << "," << intersections[i].fOrderedLineY << "], radiusCCD = " << radiusCCD << endl;
            break;
        }
        if (i == intersections.size()-1) {
            radiusCCD++;
            i = -1;
        }     
    }
    
    // sphere constructing loop
    while (radiusIndex <= jMax/2) {    
        radiusIndex++;
        cout << "radiusIndex = " << radiusIndex << endl;
       
        // scanning left edge
        currentI = iFirst - radiusIndex;
        currentJ = jFirst - radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ && currentJ == (jFirst - radiusIndex)) {   // segment in bl corner
                for (int j = 0; j < nI; j++) {
                    if (intersections[j].fOrderedLineX == (currentI + 1) && intersections[j].fOrderedLineY == (jFirst - radiusIndex + 1)) {
                        intersections[i].fHeightCenterSum = intersections[j].fBLSum - intersections[i].fTRHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ && currentJ == (jFirst + radiusIndex)) {   // segment in tl corner
                for (int j = 0; j < nI; j++) {
                    if (intersections[j].fOrderedLineX == (currentI + 1) && intersections[j].fOrderedLineY == (jFirst + radiusIndex - 1)) {
                        intersections[i].fHeightCenterSum = intersections[j].fTLSum - intersections[i].fBRHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ && currentJ < (jFirst + radiusIndex) && currentJ > (jFirst - radiusIndex)) {  // remaining... 
                for (int j = 0; j < nI; j++) {                                                                                                                           // ... segments on left edge
                    if (intersections[j].fOrderedLineX == (currentI + 1) && intersections[j].fOrderedLineY == currentJ) {
                        intersections[i].fHeightCenterSum = intersections[j].fBLSum - intersections[i].fBRHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
        }
        
        // scanning right edge        
        currentI = iFirst + radiusIndex;
        currentJ = jFirst - radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ && currentJ == (jFirst - radiusIndex)) {   // segment in br corner
                for (int j = 0; j < nI; j++) {
                    if (intersections[j].fOrderedLineX == (currentI - 1) && intersections[j].fOrderedLineY == (jFirst - radiusIndex + 1)) {
                        intersections[i].fHeightCenterSum = intersections[j].fBRSum - intersections[i].fTLHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ && currentJ == (jFirst + radiusIndex)) {   // segment in tr corner
                for (int j = 0; j < nI; j++) {
                    if (intersections[j].fOrderedLineX == (currentI - 1) && intersections[j].fOrderedLineY == (jFirst + radiusIndex - 1)) {
                        intersections[i].fHeightCenterSum = intersections[j].fTRSum - intersections[i].fBLHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ && currentJ < (jFirst + radiusIndex) && currentJ > (jFirst - radiusIndex)) {  // remaining... 
                for (int j = 0; j < nI; j++) {                                                                                                                           // ... segments on right edge
                    if (intersections[j].fOrderedLineX == (currentI - 1) && intersections[j].fOrderedLineY == (currentJ)) {
                        intersections[i].fHeightCenterSum = intersections[j].fBRSum - intersections[i].fBLHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentJ++;
                if (currentJ > jFirst + radiusIndex or currentJ > jMax) break;
            }
        }
        
        // scanning upper edge        
        currentI = iFirst - radiusIndex + 1;
        currentJ = jFirst + radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ) {  // remaining... 
                for (int j = 0; j < nI; j++) {                                                                                                                           // ... segments on right edge
                    if (intersections[j].fOrderedLineX == currentI && intersections[j].fOrderedLineY == (currentJ - 1)) {
                        intersections[i].fHeightCenterSum = intersections[j].fTRSum - intersections[i].fBRHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentI++;
                if (currentI > iFirst + radiusIndex or currentI > iMax) break;
            }
        }
        
        // scanning lower edge        
        currentI = iFirst - radiusIndex + 1;
        currentJ = jFirst - radiusIndex;
        for (int i = 0; i < nI; i++) {
            if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ) {  // remaining... 
                for (int j = 0; j < nI; j++) {                                                                                                                           // ... segments on right edge
                    if (intersections[j].fOrderedLineX == currentI && intersections[j].fOrderedLineY == (currentJ + 1)) {
                        intersections[i].fHeightCenterSum = intersections[j].fBRSum - intersections[i].fTRHeight;
                        intersections[i].fIndexSphere = true;
                        DoHeightCorners(intersections);
                        break;
                    }
                }
                i = -1;
                currentI++;
                if (currentI > iFirst + radiusIndex or currentI > iMax) break;
            }
        }       
    }
}

// calculating heights of segments corners
void CbmRichRonchiAna::DoHeightCorners(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int nI = intersections.size();
    
    for (int i = 0; i < nI; i++) {
        if (intersections[i].fIndexSphere == true) {
            intersections[i].fIndexSphere = false;
            intersections[i].fTLSum = intersections[i].fHeightCenterSum + intersections[i].fTLHeight;
            intersections[i].fTRSum = intersections[i].fHeightCenterSum + intersections[i].fTRHeight;
            intersections[i].fBLSum = intersections[i].fHeightCenterSum + intersections[i].fBLHeight;
            intersections[i].fBRSum = intersections[i].fHeightCenterSum + intersections[i].fBRHeight;
            //cout << "fTLSum, fTRSum, fBLSum, fBRSum = " << intersections[i].fTLSum << ", " << intersections[i].fTRSum << ", " << intersections[i].fBLSum << ", " << intersections[i].fBRSum << endl;
            cout << "HeightSum = " << intersections[i].fHeightCenterSum << endl;
            break;
        }
    }
}


/*
// calculating remaining segments
void CbmRichRonchiAna::DoCalculateRemaining(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int nI = intersections.size();
    int currentI, currentJ, initI;
    int heightY;

    // finding intersection that is to be assigned
    for (int i = 0; i < nI; i++) {
        if (intersections[i].fIndexSum == 2) {
            currentI = intersections[i].fOrderedLineX;
            currentJ = intersections[i].fOrderedLineY;
            initI = i;
            intersections[i].fIndexSum = 0;
            break;
        }
    }
   
    // searching below
    for (int i = 0; i < nI; i++) {
        if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ - 1) { // if corresponding intersections is found, load and calculate data
            if (intersections[i].fOrderedLineY <= fImageCenterCCDY) {
                heightY = intersections[i].fHeightY - intersections[initI].fHeightCenter;
                //cout << "DoRemaining:: heightY = " << heightY << " at int[" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "]" << endl;
            }
            if (intersections[i].fOrderedLineY > fImageCenterCCDY) {
                heightY = intersections[i].fHeightY + intersections[initI].fHeightCenter;
                //cout << "DoRemaining:: heightY = " << heightY << " at int[" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "]" << endl;
            }
            intersections[initI].fHeightY      = heightY;
            intersections[initI].fTotHeight    = intersections[i].fTotHeight + intersections[initI].fHeightCenter;
            intersections[initI].fUpperEdgeSum = intersections[i].fUpperEdgeSum + 2*intersections[initI].fUpperEdge;
            intersections[initI].fLowerEdgeSum = intersections[i].fLowerEdgeSum + 2*intersections[initI].fLowerEdge;
            intersections[initI].fLeftEdgeSum  = intersections[i].fLeftEdgeSum  + 2*intersections[initI].fLeftEdge;
            intersections[initI].fRightEdgeSum = intersections[i].fRightEdgeSum + 2*intersections[initI].fRightEdge;
            intersections[initI].fIndexScan    = true;
            intersections[initI].fIndexSum     = 1;
            DoScanLineHeight(intersections);
            break;
        }
    }
    
    // searching above
    for (int i = 0; i < nI; i++) {
        if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ + 1) {
            if (intersections[i].fOrderedLineY <= fImageCenterCCDY) {
                heightY = intersections[i].fHeightY + intersections[initI].fHeightCenter;
                //cout << "DoRemaining:: heightY = " << heightY << " at int[" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "]" << endl;
            }
            if (intersections[i].fOrderedLineY > fImageCenterCCDY) {
                heightY = intersections[i].fHeightY - intersections[initI].fHeightCenter;
                //cout << "DoRemaining:: heightY = " << heightY << " at int[" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "]" << endl;
            }
            intersections[initI].fHeightY      = heightY;
            intersections[initI].fTotHeight    = intersections[i].fTotHeight    + intersections[initI].fHeightCenter;
            intersections[initI].fUpperEdgeSum = intersections[i].fUpperEdgeSum + 2*intersections[initI].fUpperEdge;
            intersections[initI].fLowerEdgeSum = intersections[i].fLowerEdgeSum + 2*intersections[initI].fLowerEdge;
            intersections[initI].fLeftEdgeSum  = intersections[i].fLeftEdgeSum  + 2*intersections[initI].fLeftEdge;
            intersections[initI].fRightEdgeSum = intersections[i].fRightEdgeSum + 2*intersections[initI].fRightEdge;
            intersections[initI].fIndexScan    = true;
            intersections[initI].fIndexSum     = 1;
            DoScanLineHeight(intersections);
            break;
        }
    }  
}

// scanning line in horizontal direction and add height values
void CbmRichRonchiAna::DoScanLineHeight(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int nI = intersections.size();
    int iInit, currentI, currentJ;
    int iMin = 1000;
    int iMax = -1000;
    double heightY, totHeight; // heights in x and y, measured at center of segments

    // finding maxima and minima of 'i' and 'j'
    for (int i = 0; i < nI; i++){
        iMin = (intersections[i].fOrderedLineX < iMin) ? intersections[i].fOrderedLineX : iMin;
        iMax = (intersections[i].fOrderedLineX > iMax) ? intersections[i].fOrderedLineX : iMax;
    }
    
    //cout << "DoScan::   iMin/iMax = " << iMin << " / " << iMax << endl;
    
    // loading data of considered intersection
    for (int i = 0; i < nI; i++) {
        if (intersections[i].fIndexScan == true) {
            heightY   = intersections[i].fHeightY;
            totHeight = intersections[i].fHeightY;
            iInit     = intersections[i].fOrderedLineX;
            currentI  = intersections[i].fOrderedLineX;
            currentJ  = intersections[i].fOrderedLineY;
            intersections[i].fIndexScan = false;
            break;
        }
    }

    // scanning line to the left
    for (int i = 0; i < nI; i++) {
        bool toggle = false;
        if (intersections[i].fOrderedLineX == currentI - 1 && intersections[i].fOrderedLineY == currentJ) {
            if (intersections[i].fIndexSum == 1) break;   // to prevent that this intersection will be assigned manifold
            totHeight                     += intersections[i].fHeightCenter;
            intersections[i].fHeightY      = heightY;
            intersections[i].fTotHeight    = totHeight;
            intersections[i].fUpperEdgeSum = totHeight + intersections[i].fUpperEdge;
            intersections[i].fLowerEdgeSum = totHeight + intersections[i].fLowerEdge;
            intersections[i].fLeftEdgeSum  = totHeight + intersections[i].fLeftEdge;
            intersections[i].fRightEdgeSum = totHeight + intersections[i].fRightEdge;
            intersections[i].fIndexSum     = 1;
            currentI--;
            toggle = true;
            if (currentI < iMin) {
                //cout << "DoScan:: BREAK-condition: currentI = " << currentI << endl;
                break;
            }
            if (i == nI-1 && toggle == false) {
                currentI--;
                //cout << "DoScan:: currentI jumped by ONE" << endl;
                if (currentI < iMin) {
                    //cout << "DoScan:: BREAK-condition: currentI = " << currentI << endl;
                }
            }
            i = -1;
        }
    }

    // reset variables
    totHeight = heightY;
    currentI = iInit;
    
    // scanning line to the right
    for (int i = 0; i < nI; i++) {
        bool toggle = false;
        if (intersections[i].fOrderedLineX == currentI + 1 && intersections[i].fOrderedLineY == currentJ) {
            if (intersections[i].fIndexSum == 1) break;   // to prevent that this intersection will be assigned manifold
            totHeight                     += intersections[i].fHeightCenter;
            intersections[i].fHeightY      = heightY;
            intersections[i].fTotHeight    = totHeight;
            intersections[i].fUpperEdgeSum = totHeight + intersections[i].fUpperEdge;
            intersections[i].fLowerEdgeSum = totHeight + intersections[i].fLowerEdge;
            intersections[i].fLeftEdgeSum  = totHeight + intersections[i].fLeftEdge;
            intersections[i].fRightEdgeSum = totHeight + intersections[i].fRightEdge;
            intersections[i].fIndexSum     = 1;
            currentI++;
            toggle = true;
            if (currentI > iMax) {
                //cout << "DoScan:: BREAK-condition: currentI = " << currentI << endl;
                break;
            }
            if (i == nI-1 && toggle == false) {
                currentI++;
                //cout << "DoScan:: currentI jumped by ONE" << endl;
                if (currentI > iMax) {
                    //cout << "DoScan:: BREAK-condition: currentI = " << currentI << endl;
                }
            }
            i = -1;
        }
    }
}

// sphere: [x,y,i,j,normalX,normalY,upEdge,lowEdge,lEdge,rEdge,hCenterSingle, heightY, totHeight,index1, upper, lower, left, right, averageDev, hCenterSummed, index2, index3]
//         [0 1 2 3   4        5      6       7      8     9        10          11         12       13     14     15    16     17       18            19         20      21  ]


void CbmRichRonchiAna::DoIntegrate(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int nI = intersections.size();
    int xMin, xMax, yMin, yMax;
    int currentI, currentJ;
    int iFirst;     // data of segment that is closest to the center
    int jFirst;
    int lFirst;
    double avDiff;
        
    int radius    = 1;
    int counter   = 0;
    int delta     = 0;       // integrated height differences of neighboured segments
    int threshold = 100;     // threshold for delta
    
    bool last = false;
    bool abort = false;
    
    
    
    

    // to find the intersection that is closest to the center
    for (int i = 0; i < nI; i++ ) {
        xMin = fImageCenterCCDX - radius;
        xMax = fImageCenterCCDX + radius;
        yMin = fImageCenterCCDY - radius;
        yMax = fImageCenterCCDY + radius;
        if (intersections[i].fPixelX >= xMin && intersections[i].fPixelX <= xMax && intersections[i].fPixelY >= yMin && intersections[i].fPixelY <= yMax) {
            iFirst = intersections[i].fOrderedLineX;
            jFirst = intersections[i].fOrderedLineY;
            lFirst = i;
            //cout << "DoSphere:: Found center: intersections[" << i << "] = [" << intersections[i].fPixelX << "," << intersections[i].fPixelY << "," << intersections[i].fOrderedLineX << "," << intersections[i].fOrderedLineY << "], radius = " << radius << endl;
            break;
        }
        if (i == nI-1) {
            radius++;
            i = -1;
        }     
    }
    
    
    intersections[lFirst].fUpperEdgeSum = intersections[lFirst].fUpperEdge;
    intersections[lFirst].fLowerEdgeSum = intersections[lFirst].fLowerEdge;
    intersections[lFirst].fLeftEdgeSum  = intersections[lFirst].fLeftEdge;
    intersections[lFirst].fRightEdgeSum = intersections[lFirst].fRightEdge;
    intersections[lFirst].fTotHeight    = intersections[lFirst].fHeightCenter;
    
    // finding maxima and minima of 'i' and 'j'
    int iMin = 1000;
    int iMax = -1000;
    int jMin = 1000;
    int jMax = -1000;
    
    for (int i = 0; i < nI; i++){
        iMin = (intersections[i].fOrderedLineX < iMin) ? intersections[i].fOrderedLineX : iMin;
        iMax = (intersections[i].fOrderedLineX > iMax) ? intersections[i].fOrderedLineX : iMax;
    }
    
    radius = 1;
    
    // main integrating function
    while (counter < 100) {
        counter++;
        while (abort == false) {
            // scanning left edge
            currentI = iFirst - radius;
            currentJ = jFirst - radius;
            //cout << "currentI/J = " << currentI << "," << currentJ << endl;
            for (int i = 0; i < nI; i++) {
                if ( intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ) {
                    intersections[i].fIndexSum = 3;
                    DoAverageSurroundings(intersections);
                    currentJ++;
                    i = -1;
                    if (currentJ > jFirst + radius or currentJ > jMax) break;
                }
            }
            // scanning right edge
            currentI = iFirst + radius;
            currentJ = jFirst - radius;
            for (int i = 0; i < nI; i++) {
                if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ) {
                    intersections[i].fIndexSum = 3;
                    DoAverageSurroundings(intersections);
                    currentJ++;
                    i = -1;
                    if (currentJ > jFirst + radius or currentJ > jMax) break;
                }
            }
            // scanning upper edge
            currentI = iFirst - (radius - 1);
            currentJ = jFirst + radius;
            for (int i = 0; i < nI; i++) {
                if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ) {
                    intersections[i].fIndexSum = 3;
                    DoAverageSurroundings(intersections);
                    currentI++;
                    i = -1;
                    if (currentI > iFirst + (radius - 1) or currentI > iMax) break;
                }
            }
            // scanning lower edge
            currentI = iFirst - (radius - 1);
            currentJ = jFirst - radius;
            for (int i = 0; i < nI; i++) {
                if (intersections[i].fOrderedLineX == currentI && intersections[i].fOrderedLineY == currentJ) {
                    intersections[i].fIndexSum = 3;
                    DoAverageSurroundings(intersections);
                    currentI++;
                    i = -1;
                    if (currentI > iFirst + (radius - 1) or currentI > iMax) break;
                }
            }
            radius++;
            if (radius > 1.2 * ((iMax - iMin) / 2)) { // breaking the loop if it is sure that all segments have been considered
                //cout << "value 'radius' at limit" << endl;
                break;
            }
        }
        for (int i = 0; i < nI; i++) { //summing up all average differences
            avDiff += intersections[iFirst].fAvDiff;
        }
        //cout << "-------------------------------------------------------avDiff = " << avDiff << " -----------------------------------------------------" << endl;
        if (abs(avDiff) <= threshold) {
            //cout << "---------------------------------------------------avDiff < Threshold !! -------------------------------------------------" << endl;
            break;
        }
        for (int i = 0; i < nI; i++) {
            intersections[i].fAvDiff = 0;
        }
        if (counter > 99) {
            cout << "------------------------------------------------------- COUNTER > 99 -----------------------------------------------------------" << endl;
        }
        radius = 1;
    }
    for (int i = 0; i < nI; i++) {
        //cout << "sphere[" << l << "] = " << "[" << sphere[l][0] << "," << sphere[l][1] << "," << sphere[l][2] << "," << sphere[l][3] << "," << sphere[l][4] << "," << sphere[l][5] << "," << sphere[l][6] << "," << sphere[l][7] << "," << sphere[l][8] << "," << sphere[l][9] << "," << sphere[l][10] << "," << sphere[l][11] << "," << sphere[l][12] << "," << sphere[l][13] << "," << sphere[l][14] << "," << sphere[l][15] << "," << sphere[l][16] << "," << sphere[l][17] << "," << sphere[l][18] << "," << sphere[l][19] << "," << sphere[l][20] << "]" << endl;
    }
    /*cout << endl
         << "sphere: [x,y,i,j,normalX,normalY,upEdge,lowEdge,lEdge,rEdge,hCenterSingle, heightY, totHeight,index1, upper, lower, left, right, averageDev, hCenterSummed, index2, index3]" << endl;
    cout << "        [0 1 2 3   4        5      6       7      8     9        10          11         12       13     14     15    16     17       18           19          20      21  ]" << endl
         << endl;
}

// calculating average height difference between the four edges of current segment with edges of neighboured segments
void CbmRichRonchiAna::DoAverageSurroundings(vector<CbmRichRonchiIntersectionData> &intersections)
{
    int nI = intersections.size();
    int initI, initJ, initL;
    int counter = 0;

    double diffTop = 0;
    double diffBot = 0;
    double diffLeft = 0;
    double diffRight = 0;
    double avDiff = 0;

    // to get dates of initial segment
    for (int i = 0; i < nI; i++) {
        if (intersections[i].fIndexSum == 3) {
            initI = intersections[i].fOrderedLineX;
            initJ = intersections[i].fOrderedLineY;
            initI = i;
            intersections[i].fIndexSum = 0;
            break;
        }
    }

    // calculating height differences of all four adjacent edges
    for (int i = 0; i < nI; i++) {  // doppelt belegte index-kombinationen lassen counter auf ueber 4 steigen !!
        if (intersections[i].fOrderedLineX == initI && intersections[i].fOrderedLineY == initJ + 1) {
            diffTop = intersections[i].fLowerEdgeSum - intersections[initI].fUpperEdgeSum;
            counter++;
        }
        if (intersections[i].fOrderedLineX == initI && intersections[i].fOrderedLineY == initJ - 1) {
            diffBot = intersections[i].fUpperEdgeSum - intersections[initI].fLowerEdgeSum;
            counter++;
        }
        if (intersections[i].fOrderedLineX == initI - 1 && intersections[i].fOrderedLineY == initJ) {
            diffLeft = intersections[i].fRightEdgeSum - intersections[initI].fLeftEdgeSum;
            counter++;
        }
        if (intersections[i].fOrderedLineX == initI + 1 && intersections[i].fOrderedLineY == initJ) {
            diffRight = intersections[i].fLeftEdgeSum - intersections[initI].fRightEdgeSum;
            counter++;
        }
    }
    
    double oldHeight;
    double newHeight;
    if (counter != 0)
    {
        avDiff = (diffTop + diffBot + diffLeft + diffRight) / counter;
        intersections[initI].fAvDiff = avDiff;
        oldHeight = intersections[initI].fTotHeight;
        intersections[initI].fUpperEdgeSum += avDiff;
        intersections[initI].fLowerEdgeSum += avDiff;
        intersections[initI].fLeftEdgeSum  += avDiff;
        intersections[initI].fRightEdgeSum += avDiff;
        intersections[initI].fTotHeight    += avDiff;
        newHeight = intersections[initI].fTotHeight;
        //cout << "At sphere[" << initL << "] " << "difference up/down/left/right/av | counter = " << diffTop << ", " << diffBot << ", " << diffLeft << ", " << diffRight << ", " << avDiff << " | " << counter << endl;
    }
    else
    {
        //cout << "!! NO SEGMENTS TO AVERAGE OVER !!" << endl;
    }
    //cout << "oldHeight = " << oldHeight << ", newHeight = " << newHeight << endl;
}*/

void CbmRichRonchiAna::RotatePointImpl(TVector3 *inPos, TVector3 *outPos, Double_t rotX, Double_t rotY, TVector3* cV)
{
    double x = inPos->X() - cV->X();
    double y = inPos->Y() - cV->Y();
    double z = inPos->Z() - cV->Z();

    double sinY = sin(rotY);
    double cosY = cos(rotY);
    double sinX = sin(rotX);
    double cosX = cos(rotX);
    
    double xNew = x * cosX - y * sinX * sinY + z * cosY * sinX + cV->X();
    double yNew = y * cosY + z * sinY + cV->Y();
    double zNew = -x * sinX - y * sinY * cosX + z * cosY * cosX + cV->Z();

    outPos->SetXYZ(xNew, yNew, zNew);
}

ClassImp(CbmRichRonchiAna)