void VilDraw::draw_cross(vil_image_view<vxl_byte> &image,
                         const vcl_vector< vgl_point_2d<double>> &pts,
                         int crossWidth,
                         const vcl_vector< vxl_byte >& colour, int lineWidth)
{
    assert(image.nplanes() == 3 || image.nplanes() == 1);
    assert(colour.size() == 3);
    
    vil_image_view<vxl_byte> rgb_image = image;
    if (image.nplanes() == 1) {
        image = VilUtil::gray_2_rgb(image);
    }
    
    for (unsigned int i = 0; i<pts.size(); i++)
    {
        //center point
        double px = pts[i].x();
        double py = pts[i].y();
        
        vgl_point_2d<double> p1, p2, p3, p4;
        
        double h_l = crossWidth;
        p1.set(px - h_l, py);
        p2.set(px + h_l, py);
        p3.set(px, py - h_l);
        p4.set(px, py + h_l);
        
        VilAlgoPlus::fill_line(image, p1, p2, colour);
        VilAlgoPlus::fill_line(image, p3, p4, colour);
    }
}

bool VilGMM::train(const vcl_vector<vnl_vector<double> > & data)
{
    vpdfl_gaussian_builder g_builder;
    vpdfl_mixture_builder builder;
    
    builder.init(g_builder,comp_n_);
    builder.set_weights_fixed(false);    
    
    if (data.size() <= comp_n_ * 20) {
        return false;
    }
    
    if (gmm_) {
        delete gmm_;
        gmm_ = NULL;
    }
    gmm_ = builder.new_model();
    
    mbl_data_array_wrapper<vnl_vector<double> > data_array(data);
    builder.build(*gmm_, data_array);
    if (verbose_) {
        vcl_cout<<"training sample number is "<<data.size()<<vcl_endl;
        vcl_cout<<"Probability distribution function is "<<gmm_<<vcl_endl;
        vcl_cout<<"Mean: "<<gmm_->mean()<<vcl_endl;
        vcl_cout<<"Var:  "<<gmm_->variance()<<vcl_endl;
    }
    return true;
}

bool VglPlus::intersectionFromLineSet(const vcl_vector<vgl_line_2d<double> > & lines, vgl_point_2d<double> & orthocenter)
{
    assert(lines.size() >= 2);
    
    int num = 0;
    double px = 0.0;
    double py = 0.0;
    for (int i = 0; i<lines.size(); i++) {
        for (int j = i+1; j<lines.size(); j++) {
            vgl_point_2d<double> p;
            bool isIntersect = vgl_intersection(lines[i], lines[j], p);
            if (isIntersect) {
                px += p.x();
                py += p.y();
                num++;
            }
        }
    }
    
    if (num >= 1) {
        orthocenter = vgl_point_2d<double>(px/num, py/num);
        return true;
    }
    else
    {
        return false;
    }
}

bool rgrsn_ldp::transition_matrix(const vcl_vector<int> & fns,
                                  const vcl_vector<double> & values,
                                  vnl_matrix<double> & transition,
                                  const double resolution)
{
    assert(fns.size() == values.size());
    
    double min_v = *vcl_min_element(values.begin(), values.end());
    double max_v = *vcl_max_element(values.begin(), values.end());
    
    unsigned num_bin = (max_v - min_v)/resolution;
    transition = vnl_matrix<double>(num_bin, num_bin, 0.0);
    vnl_vector<double> column(num_bin, 0.0);
    for (int i = 0; i<fns.size()-1; i++) {
        if (fns[i] + 1 == fns[i+1]) {
            double cur_v = values[i];
            double next_v = values[i+1];
            unsigned cur_bin = value_to_bin_number(min_v, resolution, cur_v, num_bin);
            unsigned next_bin = value_to_bin_number(min_v, resolution, next_v, num_bin);
            transition[next_bin][cur_bin] += 1.0;
            column[cur_bin] += 1.0;
        }
    }
    
    // normalize each column
    for (int r = 0; r < transition.rows(); r++) {
        for (int c = 0; c < transition.cols(); c++) {
            transition[r][c] /= column[c];
        }
    }
    return true;
}

bool VxlMvlPlus::fundamental_RANSAC(vcl_vector< vgl_point_2d< double > > const & positions1,
                                    vcl_vector< vgl_point_2d< double > > const & positions2,
                                    vcl_vector< vcl_pair<int, int> > const & initialMatchedIndices,  // matched index first --> second
                                    double error_threshold,
                                    // output
                                    vcl_vector< vcl_pair<int, int> > & finalMatchedIndices,
                                    vpgl_fundamental_matrix<double> & F)
{
    bool isFind = false;
    
    // extract point pair from initial matches
    vcl_vector<vgl_point_2d<double> > pts1_matched;
    vcl_vector<vgl_point_2d<double> > pts2_matched;
    for (int i = 0; i<initialMatchedIndices.size(); i++) {
        pts1_matched.push_back(positions1[initialMatchedIndices[i].first]);
        pts2_matched.push_back(positions2[initialMatchedIndices[i].second]);
    }
    
    vcl_vector<bool> inliers;
    isFind = VxlMvlPlus::fundamental_RANSAC(pts1_matched, pts2_matched, inliers, error_threshold, F);
    assert(inliers.size() == initialMatchedIndices.size());
    
    if (isFind) {
        for (int i = 0; i<inliers.size(); i++) {
            if (inliers[i]) {
                finalMatchedIndices.push_back(initialMatchedIndices[i]);
            }
        }
    }
    
    printf("fundamental matrix RANSAC find %lu from %lu initial matchings\n", finalMatchedIndices.size(), initialMatchedIndices.size());
    return  isFind;
}

bool VilBaplSIFT::writeSIFT(const char *file, const vcl_vector< bapl_keypoint_sptr > & keypoints)
{
    FILE *pf = fopen(file, "w");
    if (!pf) {
        printf("Error: canot open %s\n", file);
        return false;
    }
    
    int n = (int)keypoints.size();
    int len = 128;
    fprintf(pf, "%d\t %d\n", n, len);
    for (int i = 0; i<keypoints.size(); i++) {
        bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoints[i].as_pointer());
        double loc_x = sift->location_i();
        double loc_y = sift->location_j();
        double scale = sift->scale();
        double orientation = sift->orientation();
        fprintf(pf, "%lf\t %lf\t %lf\t %lf\n", loc_x, loc_y, scale, orientation);
        
        vnl_vector_fixed<double,128> feat = sift->descriptor();
        for (int j = 0; j<feat.size(); j++) {
            fprintf(pf, "%lf  ", feat[j]);
        }
        fprintf(pf, "\n");
    }
    fclose(pf);
    
    printf("save sift to %s\n", file);
    return true;
}

void VglPlus::loadPointVector(const char *fileName, vcl_string &imageName, vcl_vector<vgl_point_2d<double>> &wldPts, vcl_vector< vgl_point_2d<double> > &imgPts)
{
    assert(wldPts.size() == 0);
    assert(imgPts.size() == 0);
    
    vsl_b_ifstream bfs_in(fileName);
    assert(bfs_in.is().good() == true);
    
    char strName[1024] = {NULL};
    vsl_b_read(bfs_in, strName);
    imageName = vcl_string(strName);
    
    int num = 0;
    vsl_b_read(bfs_in, num);
    wldPts.resize(num);
    imgPts.resize(num);
    
    for (int i = 0; i<num; i++) {
        vsl_b_read(bfs_in, wldPts[i]);
    }
    for (int i = 0; i<num; i++) {
        vsl_b_read(bfs_in, imgPts[i]);
    }
    bfs_in.close();
}

void VilBaplSIFT::getSiftPositions(const vil_image_view<vxl_byte> &image, vcl_vector<vgl_point_2d<double> > &points, double curve_ratio)
{
    assert(image.nplanes() == 1 || image.nplanes() == 3);
    assert(points.size() == 0);
    
    vil_image_view<vxl_byte> grey_img;
    if (image.nplanes() == 3) {
        vil_convert_planes_to_grey(image, grey_img);
    }
    else
    {
        grey_img.deep_copy(image);
    }
    
    vcl_vector<bapl_keypoint_sptr> sift_keypoints;
    vil_image_resource_sptr image_sptr = vil_new_image_resource_of_view(grey_img);
    
    //  float curve_ratio = 2.0f;
    bapl_keypoint_extractor(image_sptr, sift_keypoints, curve_ratio);
    
    
    vcl_vector<bapl_keypoint_sptr>::iterator keypoint_itr, keypoint_end;
    keypoint_end = sift_keypoints.end();
    
    
    for (keypoint_itr = sift_keypoints.begin(); keypoint_itr != keypoint_end; ++keypoint_itr)
    {
        bapl_lowe_keypoint_sptr sift_lowe_keypoint = dynamic_cast<bapl_lowe_keypoint*>((*keypoint_itr).as_pointer());
        double x = sift_lowe_keypoint->location_i();
        double y = sift_lowe_keypoint->location_j();
        
        points.push_back(vgl_point_2d<double>(x, y));
    }
}

bool rgrsn_ldp::local_dynamic_programming(const vnl_matrix<double> & probMap, int nNeighborBin,
                                          vcl_vector<int> & optimalBins)
{
    const int N    = probMap.rows();
    const int nBin = probMap.cols();
    
    // dynamic programming
    vnl_matrix<double> accumulatedProbMap = vnl_matrix<double>(N, nBin);
    accumulatedProbMap.fill(0.0);
    vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
    lookbackTable.fill(0);
    // copy first row
    for (int c = 0; c<probMap.cols(); c++) {
        accumulatedProbMap[0][c] = probMap[0][c];
        lookbackTable[0][c] = c;
    }
    
    for (int r = 1; r <N; r++) {
        for (int c = 0; c<probMap.cols(); c++) {
            // lookup all possible place in the window
            double max_val = -1;
            int max_index  = -1;
            for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
                if (c + w <0 || c + w >= probMap.cols()) {
                    continue;
                }
                double val = probMap[r][c] + accumulatedProbMap[r-1][c+w];
                if (val > max_val) {
                    max_val = val;
                    max_index = c + w; // most probable path from the [r-1] row, in column c + w
                }
            }
            assert(max_index != -1);
            accumulatedProbMap[r][c] = max_val;
            lookbackTable[r][c]      = max_index;
        }
    }
    
    // lookback the table
    double max_prob    = -1.0;
    int max_prob_index = -1;
    for (int c = 0; c<accumulatedProbMap.cols(); c++) {
        if (accumulatedProbMap[N-1][c] > max_prob) {
            max_prob = accumulatedProbMap[N-1][c];
            max_prob_index = c;
        }
    }
    
    // back track
    optimalBins.push_back(max_prob_index);
    for (int r = N-1; r > 0; r--) {
        int bin = lookbackTable[r][optimalBins.back()];
        optimalBins.push_back(bin);
    }
    assert(optimalBins.size() == N);
    
  //  vcl_reverse(optimalBins.begin(), optimalBins.end());
    return true;
}

void VilBaplSIFT::getSIFTLocations(const vcl_vector<bapl_keypoint_sptr> & keypoints, vcl_vector<vgl_point_2d<double> > & pts)
{
    for (int i = 0; i<keypoints.size(); i++) {
        bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoints[i].as_pointer());
        vgl_point_2d<double> p(sift->location_i(), sift->location_j());
        pts.push_back(p);
    }
}

void VglPlus::saveVector(const char *matName, const vcl_vector<double> & data)
{
    assert(data.size() > 0);
    vnl_matlab_filewrite writer(matName);
    vnl_vector<double> dataVec(&data[0], (int)data.size());
    writer.write(dataVec, "data");
    printf("save to file: %s\n", matName);
}

bool VglPlus::lineEllipseTangencyByProjection(int imageW, int imageH,
                                              const vgl_line_2d<double> & line,
                                              const vgl_ellipse_2d<double> & ellipseInImage,
                                              vgl_point_2d<double> & pt, int sampleNum)
{
    vil_image_view<vxl_byte> maskImage(imageW, imageH, 3);
    maskImage.fill(0);
    
    VilPlus::draw_line(maskImage, line, VilPlus::white(), 2);
    
    vgl_polygon<double> poly = ellipseInImage.polygon(sampleNum);
    assert(poly.num_sheets() == 1);
    
    const vcl_vector< vgl_point_2d< double > > sheet = poly[0];
    assert( sheet.size() > 1 );
    
    vcl_vector<vgl_point_2d<double>> pts;
    for (int i = 0; i<sheet.size(); i++) {
        int x = vnl_math::rnd_halfinttoeven(sheet[i].x());
        int y = vnl_math::rnd_halfinttoeven(sheet[i].y());
        if (x >= 0 && x < imageW &&
            y >= 0 && y < imageH) {
            if (maskImage(x, y) == 255) {
                pts.push_back(sheet[i]);
            }
        }
    }
    
    if (pts.size() == 0) {
        return false;
    }
    
    // caluclate average position
    double avgX = 0.0;
    double avgY = 0.0;
    for (int i =0; i<pts.size(); i++) {
        avgX += pts[i].x();
        avgY += pts[i].y();
    }
    avgX /= pts.size();
    avgY /= pts.size();
    
    double stdX = 0.0;
    double stdY = 0.0;
    for (int i = 0; i<pts.size(); i++) {
        stdX += (avgX - pts[i].x()) * (avgX - pts[i].x());
        stdY += (avgY - pts[i].y()) * (avgY - pts[i].y());
    }
    
    stdX = sqrt(stdX/pts.size());
    stdY = sqrt(stdY/pts.size());
    printf("std x, y is %f %f\n", stdX, stdY);
    
    pt = vgl_point_2d<double>(avgX, avgY);
    return true;
}

bool VxlMvlPlus::three_view_six_points_calib(const vcl_vector< vcl_vector<vgl_point_2d<double> > > & pointsVec,
                                             vcl_vector< vnl_matrix_fixed<double, 3, 4> > & Pmatrix,
                                             vgl_homg_point_3d<double> & Q)
{
    assert(pointsVec.size() == 3);
    for (int i = 0; i<3; i++) {
        assert(pointsVec[i].size() == 6);
    }
    
    mvl_three_view_six_point_structure mvl36;
    
    // set 6 (x, y)
    for (int i = 0; i<3; i++) {
        for (int j = 0; j<6; j++) {
            mvl36.u(i, j) = pointsVec[i][j].x();
            mvl36.v(i, j) = pointsVec[i][j].y();
        }
    }
    
    bool isFind = mvl36.compute();
    if (!isFind) {
        return false;
    }
    
    int validNum = 0;
    mvl_three_view_six_point_structure::solution_t slu; // only one solution can be used
    for (int i = 0; i<3; i++) {
        validNum += (mvl36.solution[i].valid == true ? 1:0);
        if (mvl36.solution[i].valid) {
            slu = mvl36.solution[i];
        }
    }
    
    // no ambiguity
    if (validNum != 1) {
        printf("%d ambituity solutions\n", validNum);
        for (int i = 0; i<3; i++) {
            if (mvl36.solution[i].valid)
            {
                for (int j = 0; j<3; j++) {
                    vcl_cout<<"P is: \n"<<mvl36.solution[i].P[j]<<vcl_endl;
                }
            }
            
            vcl_cout<<"Q is "<<vgl_point_3d<double>(mvl36.solution[i].Q[0], mvl36.solution[i].Q[1], mvl36.solution[i].Q[2])<<vcl_endl;
        }
        return false;
    }
    for (int i = 0; i<3; i++) {
        vcl_cout<<"P is: \n"<<slu.P[i]<<vcl_endl;
        Pmatrix.push_back(slu.P[i]);
    }
    vcl_cout<<"Q is: \n"<<slu.Q<<vcl_endl;
    Q = vgl_homg_point_3d<double>(slu.Q[0], slu.Q[1], slu.Q[2], slu.Q[3]);    
    return true;
}

void VglPlus::savePointVector(const char *fileName, vcl_vector<vgl_point_2d<double>> &pts)
{
    vsl_b_ofstream bfs_out(fileName);
    vcl_cout<<"save point vector, size is "<<pts.size()<<vcl_endl;
    
    for (int i = 0; i<pts.size(); i++) {
        vsl_b_write(bfs_out, pts[i]);
    }
    bfs_out.close();
}

int VglPlus::sampleElliseInImage(const vgl_ellipse_2d<double> & ellipse, int sampleNum,
                                 int imageW, int imageH, vcl_vector<vgl_point_2d<double> > & pts)
{
    vgl_polygon<double> poly = ellipse.polygon(sampleNum);
    assert(poly.num_sheets() == 1);
    const vcl_vector< vgl_point_2d< double > > sheet = poly[0];
    for (int i = 0; i<sheet.size(); i++) {
        if (VglPlus::vgl_inside_image(sheet[i], imageW, imageH)) {
            pts.push_back(sheet[i]);
        }
    }
    return (int)pts.size();
}

void VilBaplSIFT::getMatchingLocations(const vcl_vector<bapl_key_match> & matches, vcl_vector<vgl_point_2d<double> > & pts1, vcl_vector<vgl_point_2d<double> > & pts2)
{
    for (int i = 0; i<matches.size(); i++)
    {
        bapl_lowe_keypoint_sptr s1 = dynamic_cast<bapl_lowe_keypoint*>(matches[i].first.as_pointer());
        bapl_lowe_keypoint_sptr s2 = dynamic_cast<bapl_lowe_keypoint*>(matches[i].second.as_pointer());
        vgl_point_2d<double> p1(s1->location_i(), s1->location_j());
        vgl_point_2d<double> p2(s2->location_i(), s2->location_j());
        pts1.push_back(p1);
        pts2.push_back(p2);
    }
    
}

void VilBaplSIFT::getSiftDescription(const vil_image_view<vxl_byte> & image, const vcl_vector<vgl_point_2d<double> > & pts, vcl_vector<bapl_lowe_keypoint_sptr> & sifts)
{
    assert(image.nplanes() == 3);
    assert(sifts.size() == 0);
    
    vil_image_view<vxl_byte> grey_img;
    vil_convert_planes_to_grey(image, grey_img);
    
    bapl_lowe_pyramid_set_sptr pyramid_set = new bapl_lowe_pyramid_set(vil_new_image_resource_of_view(grey_img));
    
    for (unsigned int i = 0; i<pts.size(); i++) {
        //    bapl_lowe_keypoint keypoint(pyramid_set, pts[i].x(), pts[i].y());
        bapl_lowe_keypoint_sptr keypoint = new bapl_lowe_keypoint(pyramid_set, pts[i].x(), pts[i].y());
        sifts.push_back(keypoint);
    }
}

bool VilBaplSIFT::readSIFT_keypoint(const char *file, vcl_vector< bapl_keypoint_sptr > & keypoints)
{
    FILE *pf = fopen(file, "r");
    if (!pf) {
        printf("Error: canot open %s\n", file);
        return false;
    }
    int n = 0;
    int ret = fscanf(pf, "%d ", &n);
    assert(ret == 1);
    
    for (int i = 0; i < n; i++) {
        double loc_x = 0;
        double loc_y = 0;
        double scale = 0;
        double orientation = 0;
        ret = fscanf(pf, "%lf %lf %lf %lf ", &loc_x, &loc_y, &scale, &orientation);
        assert(ret == 4);
        
        vnl_vector_fixed<double, 128> desc;
        desc.fill(0);
        
        bapl_lowe_pyramid_set_sptr py;
        bapl_lowe_keypoint_sptr kp = new bapl_lowe_keypoint(py, loc_x, loc_y, scale, orientation, desc);
        keypoints.push_back(kp);
    }
    fclose(pf);
    printf("read %d keypoints.\n", n);
    return true;
}

 // unknow 4, f, p_3x1
 // constraint, 5: up triangle in 3x3 matrix, may have meaning less constraint such as  1 - 1 = 0
 // but at least 2 meaningful constraits
 calibIdenticalKEqu195_Residual(const vcl_vector< vnl_matrix_fixed<double, 3, 4> > & projections,
                                const vgl_point_2d<double> & pp):
 vnl_least_squares_function(4, 5 * (int)projections.size(), no_gradient),
 projections_(projections),
 pp_(pp)
 {
 }

bool VxlSelfCalib::calibIdenticalK(const vcl_vector< vnl_matrix_fixed<double, 3, 4> > & projections,
                                   const vnl_matrix_fixed<double, 3, 3> & initK,
                                   const vnl_vector_fixed<double, 3> & init_p,
                                   vnl_matrix_fixed<double, 3, 3> & finalK,
                                   vnl_vector_fixed<double, 3> & final_p)
{
    assert(projections.size() >= 3);
    
    vgl_point_2d<double> pp(initK[0][2], initK[1][2]);  
    calibIdenticalKEqu195_Residual residual(projections, pp);
    
    vnl_vector<double> x(4, 0.0);
    x[0] = initK[0][0];
    x[1] = init_p[0];
    x[2] = init_p[1];
    x[3] = init_p[2];
    
    vnl_levenberg_marquardt lmq(residual);
    
    bool isMinized = lmq.minimize(x);
    if (!isMinized) {
        vcl_cerr<<"Error: minimization failed.\n";
        vcl_cerr<<"x = "<<x<<vcl_endl;
        lmq.diagnose_outcome();
        return false;
    }
    lmq.diagnose_outcome();
    
    vcl_cerr<<"x = "<<x<<vcl_endl;
    
    
    
    return true;
}