//
//  MeanOfAngles.cpp
//  
//
//  Created by Arne Pommerening on 2/12/13.
//  Copyright 2013 Philodendron International. All rights reserved.
//

#include <iostream>
#include <Rcpp.h>
#include <cmath>
using namespace Rcpp;
using namespace std;

struct point2d {
	double x;
	double y;
};

/**
 * Calculates Euclidean distance (with periodic boundary conditions).
 */
double getEuclideanDistance(double xmax, double ymax, double x1, double y1, double x2, double y2) { // Illian et al. (2008), p. 184
	double dx = fabs(x1 - x2);
	double dy = fabs(y1 - y2);
	dx = min(dx, xmax - dx);
	dy = min(dy, ymax - dy);
	return sqrt(dx * dx + dy * dy);
}

/**
 * Calculates Euclidean distance (without periodic boundary conditions).
 */
double getSimpleEuclideanDistance(double x1, double y1, double x2, double y2) {
    double dx = fabs(x2 - x1);
    double dy = fabs(y2 - y1);
    double dz = dx * dx + dy * dy;
    return sqrt(dz);
}

/**
 * Calculates the number of grid points.
 */
double calcGridPoints(double xmax, double ymax, double gridwidth) {
    return (xmax - gridwidth) * (ymax - gridwidth) * (1 / (gridwidth * gridwidth));
}

/**
 * Calculates the grid point coordinates. xrow -> x direction, ycolumn -> y 
 * direction, numbers start in the bottom left corner and progress in x direction.
 */
point2d calcGridPointCoordinates(double xmax, double gridwidth, int number, double offset) {
    offset /= 2;
    int numberPerRow, xnumber, xrow, ycolumn;
    numberPerRow = (int) floor((xmax / gridwidth) - gridwidth);
    xnumber = numberPerRow;
    xrow = 1;
    if (number > numberPerRow)
        do {
            xnumber += numberPerRow;
            xrow++;
        } while (number > xnumber);
    else
        xrow = 1;
    ycolumn = number - (numberPerRow * (xrow - 1));
    point2d xpoint;
	xpoint.x = offset + ycolumn * gridwidth;
    xpoint.y = offset + xrow * gridwidth;
    return xpoint;
}

/**
 * This function identifies the nerest tree neighbour of a grid point.
 */
int findNearestNeighbourOfXY(NumericVector treeX, NumericVector treeY, double x, double y, int ind, double xmax, double ymax) {
    double actualDistance;
    double minDistance = 1E20;
    int index = 0;
    int n = treeX.size();
    for (int i = 0; i < n; i++) {
        if (i != ind) {
            actualDistance = getEuclideanDistance(xmax, ymax, x, y, treeX[i], treeY[i]);
            if (actualDistance < minDistance) {
                minDistance = actualDistance;
                index = i;
            }
        }
    }
    return index;
}

/**
 * Calculates angle between one of the trees and the test location in relation to 
 * zero/the x axis. Includes periodic boundary conditions.
 */
double calcAngle(NumericVector treeX, NumericVector treeY, int index, double x, double y, double xmax, double ymax) {
    double dx, dy, angle, r, xx, yy, w;
    const double pi = 4.0 * atan(1.0);
    dx = treeX[index] - x;
    dy = treeY[index] - y;
    r = pow(dx, 2) + pow(dy, 2);
    xx = treeX[index];
    yy = treeY[index];
    if (dx > xmax * 0.5) {
        w = pow(dx - xmax, 2);
        if (w + pow(dy, 2) < r)
            xx = treeX[index] - xmax;
    }
    if (dx < -xmax * 0.5) {
        w = pow(dx + xmax, 2);
        if (w + pow(dy, 2) < r)
            xx = treeX[index] + xmax;
    }
    if (dy > ymax * 0.5) {
        w = pow(dy - ymax, 2);
        if (pow(dx, 2) + w < r)
            yy = treeY[index] - ymax;
    }
    
    if (dy < -ymax * 0.5) {
        w = pow(dy + ymax, 2);
        if (pow(dx, 2) + w < r)
            yy = treeX[index] + ymax;
    }
    dx = xx - x;
    dy = yy - y;
    int i = 0;
    if ((dx > 0) && (dy > 0))
        i = 0;
    if ((dx <= 0) && (dy <= 0)) 
        i = 2;
    if ((dx <= 0) && (dy > 0)) 
        i = 1;
    if ((dx > 0) && (dy <= 0))
        i = 3;
    if (fabs(dx) < 0.00000001)
        dx = 0.00000001;
    if (fabs(dy) < 0.00000001)
        dy = 0.00000001;
    if ((i == 0) || (i == 2))
        angle = atan(dy / dx);
    else
        angle = atan(-dx / dy);
    angle = angle + i * pi * 0.5;
    angle = angle * 180 / pi;
    return angle;
}

/**
 * Calculates the angle between two trees in relation to the test location.
 */
double calcAngleTwoPoints(double angle1, double angle2) {
    double finalAngle;
    if (angle1 > angle2) {
        finalAngle = angle1;
        angle1 = angle2;
        angle2 = finalAngle;
    }
    finalAngle = angle2 - angle1;
    if (finalAngle > 180) {
        if (angle1 > 180)
            finalAngle = 360 - angle1 + angle2;
        if (angle2 > 180)
            finalAngle = 360 - angle2 + angle1;
    }
    return finalAngle;
}

/**
 *  MeanOfAngles. Index calculation.
 */
// [[Rcpp::export]]
DataFrame calcMeanOfAngles(NumericVector x, NumericVector y, double xmax, double ymax, double gridwidth, double offset) {
    int numberOfPoints = (int) round(calcGridPoints(xmax - offset, ymax - offset, gridwidth));
    cout << "numberOfPoints: " << numberOfPoints << endl;
    vector<double> xx(numberOfPoints);
    vector<double> yy(numberOfPoints);
    vector<double> angle(numberOfPoints);
	for (int i = 0; i < numberOfPoints; i++) {
        point2d xpoint = calcGridPointCoordinates(xmax - offset, gridwidth, i, offset);
        int index1 = findNearestNeighbourOfXY(x, y, xpoint.x, xpoint.y, 0, xmax, ymax);
        int index2 = findNearestNeighbourOfXY(x, y, xpoint.x, xpoint.y, index1, xmax, ymax);
        double angle1 = calcAngle(x, y, index1, xpoint.x, xpoint.y, xmax, ymax);
        double angle2 = calcAngle(x, y, index2, xpoint.x, xpoint.y, xmax, ymax);
        angle[i] = calcAngleTwoPoints(angle1, angle2);
		xx[i] = xpoint.x;
		yy[i] = xpoint.y;
    }
	return DataFrame::create(Named("x") = xx, Named ("y") = yy, Named ("angle") = angle);
}