/****************************************************************************
 *
 * MODULE:       r.viewshed
 *
 * AUTHOR(S):    Laura Toma, Bowdoin College - ltoma@bowdoin.edu
 *               Yi Zhuang - yzhuang@bowdoin.edu

 *               Ported to GRASS by William Richard -
 *               wkrichar@bowdoin.edu or willster3021@gmail.com
 *               Markus Metz: surface interpolation
 *
 * Date:         april 2011
 *
 * PURPOSE: To calculate the viewshed (the visible cells in the
 * raster) for the given viewpoint (observer) location.  The
 * visibility model is the following: Two points in the raster are
 * considered visible to each other if the cells where they belong are
 * visible to each other.  Two cells are visible to each other if the
 * line-of-sight that connects their centers does not intersect the
 * terrain. The terrain is NOT viewed as a tessellation of flat cells,
 * i.e. if the line-of-sight does not pass through the cell center,
 * elevation is determined using bilinear interpolation.
 * The viewshed algorithm is efficient both in
 * terms of CPU operations and I/O operations. It has worst-case
 * complexity O(n lg n) in the RAM model and O(sort(n)) in the
 * I/O-model.  For the algorithm and all the other details see the
 * paper: "Computing Visibility on * Terrains in External Memory" by
 * Herman Haverkort, Laura Toma and Yi Zhuang.
 *
 * COPYRIGHT: (C) 2008 by the GRASS Development Team
 *
 * This program is free software under the GNU General Public License
 * (>=v2). Read the file COPYING that comes with GRASS for details.
 *
 *****************************************************************************/

/*

   A R/B BST. Always call initNILnode() before using the tree.
   Version 0.0.0

   Version 0.0.1
   Rewrote BST Deletion to improve efficiency

   Version 0.0.2
   Bug fixed in deletion.
   CLRS pseudocode forgot to make sure that x is not NIL before
   calling rbDeleteFixup(root,x).

   Version 0.0.3
   Some Cleanup. Separated the public portion and the
   private porthion of the interface in the header


   =================================
   This is based on BST 1.0.4
   BST change log
   <---------------->
   find max is implemented in this version.
   Version 1.0.2

   Version 1.0.4
   Major bug fix in deletion (when the node has two children,
   one of them has a wrong parent pointer after the rotation in the deletion.)
   <----------------->
 */

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include "rbbst.h"

extern "C" {
#include <grass/gis.h>
#include <grass/glocale.h>
}

TreeNode *NIL;

#define EPSILON 0.0000001

/*public:--------------------------------- */
RBTree *create_tree(const TreeValue &tv)
{
    init_nil_node();
    RBTree *rbt = (RBTree *)G_malloc(sizeof(RBTree));
    TreeNode *root = (TreeNode *)G_malloc(sizeof(TreeNode));

    rbt->root = root;
    rbt->root->value = tv;
    rbt->root->left = NIL;
    rbt->root->right = NIL;
    rbt->root->parent = NIL;
    rbt->root->color = RB_BLACK;

    return rbt;
}

/*LT: not sure if this is correct */
int is_empty(RBTree *t)
{
    assert(t);
    return (t->root == NIL);
}

void delete_tree(RBTree *t)
{
    destroy_sub_tree(t->root);
    return;
}

void destroy_sub_tree(TreeNode *node)
{
    if (node == NIL)
        return;

    destroy_sub_tree(node->left);
    destroy_sub_tree(node->right);
    G_free(node);
    return;
}

void insert_into(RBTree *rbt, const TreeValue &value)
{
    insert_into_tree(&(rbt->root), value);
    return;
}

void delete_from(RBTree *rbt, double key)
{
    delete_from_tree(&(rbt->root), key);
    return;
}

TreeNode *search_for_node_with_key(RBTree *rbt, double key)
{
    return search_for_node(rbt->root, key);
}

/*------------The following is designed for viewshed's algorithm-------*/
double find_max_gradient_within_key(RBTree *rbt, double key, double angle,
                                    double gradient)
{
    return find_max_value_within_key(rbt->root, key, angle, gradient);
}

/*<--------------------------------->
   //Private below this line */
void init_nil_node()
{
    NIL = (TreeNode *)G_malloc(sizeof(TreeNode));
    NIL->color = RB_BLACK;
    NIL->value.angle[0] = 0;
    NIL->value.angle[1] = 0;
    NIL->value.angle[2] = 0;
    NIL->value.gradient[0] = SMALLEST_GRADIENT;
    NIL->value.gradient[1] = SMALLEST_GRADIENT;
    NIL->value.gradient[2] = SMALLEST_GRADIENT;
    NIL->value.maxGradient = SMALLEST_GRADIENT;
    NIL->value.key = 0;

    NIL->parent = NULL;
    NIL->left = NULL;
    NIL->right = NULL;
    return;
}

/*you can write change this compare function, depending on your TreeValue struct
   //compare function used by findMaxValue
   //-1: v1 < v2
   //0:  v1 = v2
   //2:  v1 > v2 */
char compare_values(TreeValue *v1, TreeValue *v2)
{
    if (v1->gradient[1] > v2->gradient[1])
        return 1;
    if (v1->gradient[1] < v2->gradient[1])
        return -1;

    return 0;
}

/*a function used to compare two doubles */
/* a < b : -1
 * a > b : 1
 * a == b : 0 */
char compare_double(double a, double b)
{
    return (a < b ? -1 : (a > b));
}

/*create a tree node */
TreeNode *create_tree_node(const TreeValue &value)
{
    TreeNode *ret;

    ret = (TreeNode *)G_malloc(sizeof(TreeNode));

    ret->color = RB_RED;

    ret->left = NIL;
    ret->right = NIL;
    ret->parent = NIL;

    ret->value = value;
    ret->value.maxGradient = SMALLEST_GRADIENT;
    return ret;
}

/*create node with its value set to the value given
   //and insert the node into the tree
   //rbInsertFixup may change the root pointer, so TreeNode** is passed in */
void insert_into_tree(TreeNode **root, TreeValue value)
{
    TreeNode *curNode;
    TreeNode *nextNode;

    curNode = *root;

    if (compare_double(value.key, curNode->value.key) == -1) {
        nextNode = curNode->left;
    }
    else {
        nextNode = curNode->right;
    }

    while (nextNode != NIL) {
        curNode = nextNode;

        if (compare_double(value.key, curNode->value.key) == -1) {
            nextNode = curNode->left;
        }
        else {
            nextNode = curNode->right;
        }
    }

    /*create a new node
       //and place it at the right place
       //created node is RED by default */
    nextNode = create_tree_node(value);

    nextNode->parent = curNode;

    if (compare_double(value.key, curNode->value.key) == -1) {
        curNode->left = nextNode;
    }
    else {
        curNode->right = nextNode;
    }

    TreeNode *inserted = nextNode;

    /*update augmented maxGradient */
    nextNode->value.maxGradient = find_value_min_value(&nextNode->value);
    while (nextNode->parent != NIL) {
        if (nextNode->parent->value.maxGradient < nextNode->value.maxGradient)
            nextNode->parent->value.maxGradient = nextNode->value.maxGradient;

        if (nextNode->parent->value.maxGradient > nextNode->value.maxGradient)
            break;
        nextNode = nextNode->parent;
    }

    /*fix rb tree after insertion */
    rb_insert_fixup(root, inserted);

    return;
}

void rb_insert_fixup(TreeNode **root, TreeNode *z)
{
    /*see pseudocode on page 281 in CLRS */
    TreeNode *y;

    while (z->parent->color == RB_RED) {
        if (z->parent == z->parent->parent->left) {
            y = z->parent->parent->right;
            if (y->color == RB_RED) { /*case 1 */
                z->parent->color = RB_BLACK;
                y->color = RB_BLACK;
                z->parent->parent->color = RB_RED;
                z = z->parent->parent;
            }
            else {
                if (z == z->parent->right) { /*case 2 */
                    z = z->parent;
                    left_rotate(root, z); /*convert case 2 to case 3 */
                }
                z->parent->color = RB_BLACK; /*case 3 */
                z->parent->parent->color = RB_RED;
                right_rotate(root, z->parent->parent);
            }
        }
        else { /*(z->parent == z->parent->parent->right) */
            y = z->parent->parent->left;
            if (y->color == RB_RED) { /*case 1 */
                z->parent->color = RB_BLACK;
                y->color = RB_BLACK;
                z->parent->parent->color = RB_RED;
                z = z->parent->parent;
            }
            else {
                if (z == z->parent->left) { /*case 2 */
                    z = z->parent;
                    right_rotate(root, z); /*convert case 2 to case 3 */
                }
                z->parent->color = RB_BLACK; /*case 3 */
                z->parent->parent->color = RB_RED;
                left_rotate(root, z->parent->parent);
            }
        }
    }
    (*root)->color = RB_BLACK;

    return;
}

/*search for a node with the given key */
TreeNode *search_for_node(TreeNode *root, double key)
{
    TreeNode *curNode = root;

    while (curNode != NIL && compare_double(key, curNode->value.key) != 0) {

        if (compare_double(key, curNode->value.key) == -1) {
            curNode = curNode->left;
        }
        else {
            curNode = curNode->right;
        }
    }

    return curNode;
}

/*function used by treeSuccessor */
TreeNode *tree_minimum(TreeNode *x)
{
    while (x->left != NIL)
        x = x->left;

    return x;
}

/*function used by deletion */
TreeNode *tree_successor(TreeNode *x)
{
    if (x->right != NIL)
        return tree_minimum(x->right);
    TreeNode *y = x->parent;

    while (y != NIL && x == y->right) {
        x = y;
        if (y->parent == NIL)
            return y;
        y = y->parent;
    }
    return y;
}

/*delete the node out of the tree */
void delete_from_tree(TreeNode **root, double key)
{
    double tmpMax;
    TreeNode *z;
    TreeNode *x;
    TreeNode *y;
    TreeNode *toFix;

    z = search_for_node(*root, key);

    if (z == NIL) {
        /*node to delete is not found */
        G_fatal_error(_("Attempt to delete node with key=%f failed"), key);
    }

    /*1-3 */
    if (z->left == NIL || z->right == NIL)
        y = z;
    else
        y = tree_successor(z);

    if (y == NIL) {
        G_fatal_error(_("Successor node not found. Deletion fails."));
    }

    /*4-6 */
    if (y->left != NIL)
        x = y->left;
    else
        x = y->right;

    /*7 */
    x->parent = y->parent;

    /*8-12 */
    if (y->parent == NIL) {
        *root = x;

        toFix = *root; /*augmentation to be fixed */
    }
    else {
        if (y == y->parent->left)
            y->parent->left = x;
        else
            y->parent->right = x;

        toFix = y->parent; /*augmentation to be fixed */
    }

    /*fix augmentation for removing y */
    TreeNode *curNode = y;
    double left, right;

    while (curNode->parent != NIL) {
        if (curNode->parent->value.maxGradient ==
            find_value_min_value(&y->value)) {
            left = find_max_value(curNode->parent->left);
            right = find_max_value(curNode->parent->right);

            if (left > right)
                curNode->parent->value.maxGradient = left;
            else
                curNode->parent->value.maxGradient = right;

            if (find_value_min_value(&curNode->parent->value) >
                curNode->parent->value.maxGradient)
                curNode->parent->value.maxGradient =
                    find_value_min_value(&curNode->parent->value);
        }
        else {
            break;
        }
        curNode = curNode->parent;
    }

    /*fix augmentation for x */
    tmpMax = toFix->left->value.maxGradient > toFix->right->value.maxGradient
                 ? toFix->left->value.maxGradient
                 : toFix->right->value.maxGradient;
    if (tmpMax > find_value_min_value(&toFix->value))
        toFix->value.maxGradient = tmpMax;
    else
        toFix->value.maxGradient = find_value_min_value(&toFix->value);

    /*13-15 */
    if (y != NIL && y != z) {
        double zGradient = find_value_min_value(&z->value);

        z->value.key = y->value.key;
        z->value.gradient[0] = y->value.gradient[0];
        z->value.gradient[1] = y->value.gradient[1];
        z->value.gradient[2] = y->value.gradient[2];
        z->value.angle[0] = y->value.angle[0];
        z->value.angle[1] = y->value.angle[1];
        z->value.angle[2] = y->value.angle[2];

        toFix = z;
        /*fix augmentation */
        tmpMax =
            toFix->left->value.maxGradient > toFix->right->value.maxGradient
                ? toFix->left->value.maxGradient
                : toFix->right->value.maxGradient;
        if (tmpMax > find_value_min_value(&toFix->value))
            toFix->value.maxGradient = tmpMax;
        else
            toFix->value.maxGradient = find_value_min_value(&toFix->value);

        while (z->parent != NIL) {
            if (z->parent->value.maxGradient == zGradient) {
                if (find_value_min_value(&z->parent->value) != zGradient &&
                    (!(z->parent->left->value.maxGradient == zGradient &&
                       z->parent->right->value.maxGradient == zGradient))) {

                    left = find_max_value(z->parent->left);
                    right = find_max_value(z->parent->right);

                    if (left > right)
                        z->parent->value.maxGradient = left;
                    else
                        z->parent->value.maxGradient = right;

                    if (find_value_min_value(&z->parent->value) >
                        z->parent->value.maxGradient)
                        z->parent->value.maxGradient =
                            find_value_min_value(&z->parent->value);
                }
            }
            else {
                if (z->value.maxGradient > z->parent->value.maxGradient)
                    z->parent->value.maxGradient = z->value.maxGradient;
            }
            z = z->parent;
        }
    }

    /*16-17 */
    if (y->color == RB_BLACK && x != NIL)
        rb_delete_fixup(root, x);

    /*18 */
    G_free(y);

    return;
}

/*fix the rb tree after deletion */
void rb_delete_fixup(TreeNode **root, TreeNode *x)
{
    TreeNode *w;

    while (x != *root && x->color == RB_BLACK) {
        if (x == x->parent->left) {
            w = x->parent->right;
            if (w->color == RB_RED) {
                w->color = RB_BLACK;
                x->parent->color = RB_RED;
                left_rotate(root, x->parent);
                w = x->parent->right;
            }

            if (w == NIL) {
                x = x->parent;
                continue;
            }

            if (w->left->color == RB_BLACK && w->right->color == RB_BLACK) {
                w->color = RB_RED;
                x = x->parent;
            }
            else {
                if (w->right->color == RB_BLACK) {
                    w->left->color = RB_BLACK;
                    w->color = RB_RED;
                    right_rotate(root, w);
                    w = x->parent->right;
                }

                w->color = x->parent->color;
                x->parent->color = RB_BLACK;
                w->right->color = RB_BLACK;
                left_rotate(root, x->parent);
                x = *root;
            }
        }
        else { /*(x==x->parent->right) */
            w = x->parent->left;
            if (w->color == RB_RED) {
                w->color = RB_BLACK;
                x->parent->color = RB_RED;
                right_rotate(root, x->parent);
                w = x->parent->left;
            }

            if (w == NIL) {
                x = x->parent;
                continue;
            }

            if (w->right->color == RB_BLACK && w->left->color == RB_BLACK) {
                w->color = RB_RED;
                x = x->parent;
            }
            else {
                if (w->left->color == RB_BLACK) {
                    w->right->color = RB_BLACK;
                    w->color = RB_RED;
                    left_rotate(root, w);
                    w = x->parent->left;
                }

                w->color = x->parent->color;
                x->parent->color = RB_BLACK;
                w->left->color = RB_BLACK;
                right_rotate(root, x->parent);
                x = *root;
            }
        }
    }
    x->color = RB_BLACK;

    return;
}

/*find the min value in the given tree value */
double find_value_min_value(TreeValue *v)
{
    if (v->gradient[0] < v->gradient[1]) {
        if (v->gradient[0] < v->gradient[2])
            return v->gradient[0];
        else
            return v->gradient[2];
    }
    else {
        if (v->gradient[1] < v->gradient[2])
            return v->gradient[1];
        else
            return v->gradient[2];
    }
    return v->gradient[0];
}

/*find the max value in the given tree
   //you need to provide a compare function to compare the nodes */
double find_max_value(TreeNode *root)
{
    if (!root)
        return SMALLEST_GRADIENT;
    assert(root);
    /*assert(root->value.maxGradient != SMALLEST_GRADIENT);
       //LT: this should be fixed
       //if (root->value.maxGradient != SMALLEST_GRADIENT) */
    return root->value.maxGradient;
}

/* find max within the max key */
double find_max_value_within_key(TreeNode *root, double maxKey, double angle,
                                 double gradient)
{
    TreeNode *keyNode = search_for_node(root, maxKey);

    if (keyNode == NIL) {
        /*fprintf(stderr, "key node not found. error occurred!\n");
           //there is no point in the structure with key < maxKey */
        /*node not found */
        G_fatal_error(_("Attempt to find node with key=%f failed"), maxKey);
        return SMALLEST_GRADIENT;
    }

    TreeNode *currNode = keyNode;
    double max = SMALLEST_GRADIENT;
    double tmpMax;
    double curr_gradient;

    while (currNode->parent != NIL) {
        if (currNode ==
            currNode->parent->right) { /*its the right node of its parent; */
            tmpMax = find_max_value(currNode->parent->left);
            if (tmpMax > max)
                max = tmpMax;
            if (find_value_min_value(&currNode->parent->value) > max)
                max = find_value_min_value(&currNode->parent->value);
        }
        currNode = currNode->parent;
    }

    if (max > gradient)
        return max;

    /* traverse all nodes with smaller distance */
    max = SMALLEST_GRADIENT;
    currNode = keyNode;
    while (currNode != NIL) {
        int checkme = (currNode->value.angle[0] <= angle &&
                       currNode->value.angle[2] >= angle);

        if (!checkme && currNode->value.key > 0) {
            G_warning(_("Angles outside angle %.4f"), angle);
            G_warning(_("ENTER angle %.4f"), currNode->value.angle[0]);
            G_warning(_("CENTER angle %.4f"), currNode->value.angle[1]);
            G_warning(_("EXIT angle %.4f"), currNode->value.angle[2]);
            G_warning(_("ENTER gradient %.4f"), currNode->value.gradient[0]);
            G_warning(_("CENTER gradient %.4f"), currNode->value.gradient[1]);
            G_warning(_("EXIT gradient %.4f"), currNode->value.gradient[2]);
        }

        if (currNode->value.key > maxKey) {
            G_fatal_error(_("current dist too large %.4f > %.4f"),
                          currNode->value.key, maxKey);
        }

        if (checkme && currNode != keyNode) {
            if (angle < currNode->value.angle[1]) {
                curr_gradient =
                    currNode->value.gradient[1] +
                    (currNode->value.gradient[0] -
                     currNode->value.gradient[1]) *
                        (currNode->value.angle[1] - angle) /
                        (currNode->value.angle[1] - currNode->value.angle[0]);
            }
            else if (angle > currNode->value.angle[1]) {
                curr_gradient =
                    currNode->value.gradient[1] +
                    (currNode->value.gradient[2] -
                     currNode->value.gradient[1]) *
                        (angle - currNode->value.angle[1]) /
                        (currNode->value.angle[2] - currNode->value.angle[1]);
            }
            else /* angle == currNode->value.angle[1] */
                curr_gradient = currNode->value.gradient[1];

            if (curr_gradient > max)
                max = curr_gradient;

            if (max > gradient)
                return max;
        }
        /* get next smaller key */
        if (currNode->left != NIL) {
            currNode = currNode->left;
            while (currNode->right != NIL)
                currNode = currNode->right;
        }
        else {
            /* at smallest item in this branch, go back up */
            TreeNode *lastNode;

            do {
                lastNode = currNode;
                currNode = currNode->parent;
            } while (currNode != NIL && lastNode == currNode->left);
        }
    }
    return max;

    /* old code assuming flat cells */
    while (keyNode->parent != NIL) {
        if (keyNode ==
            keyNode->parent->right) { /*its the right node of its parent; */
            tmpMax = find_max_value(keyNode->parent->left);
            if (tmpMax > max)
                max = tmpMax;
            if (keyNode->parent->value.gradient[1] > max)
                max = keyNode->parent->value.gradient[1];
        }
        keyNode = keyNode->parent;
    }

    return max;
}

void left_rotate(TreeNode **root, TreeNode *x)
{
    TreeNode *y;

    y = x->right;

    /*maintain augmentation */
    double tmpMax;

    /*fix x */
    tmpMax = x->left->value.maxGradient > y->left->value.maxGradient
                 ? x->left->value.maxGradient
                 : y->left->value.maxGradient;

    if (tmpMax > find_value_min_value(&x->value))
        x->value.maxGradient = tmpMax;
    else
        x->value.maxGradient = find_value_min_value(&x->value);

    /*fix y */
    tmpMax = x->value.maxGradient > y->right->value.maxGradient
                 ? x->value.maxGradient
                 : y->right->value.maxGradient;

    if (tmpMax > find_value_min_value(&y->value))
        y->value.maxGradient = tmpMax;
    else
        y->value.maxGradient = find_value_min_value(&y->value);

    /*left rotation
       //see pseudocode on page 278 in CLRS */

    x->right = y->left; /*turn y's left subtree into x's right subtree */
    y->left->parent = x;

    y->parent = x->parent; /*link x's parent to y */

    if (x->parent == NIL) {
        *root = y;
    }
    else {
        if (x == x->parent->left)
            x->parent->left = y;
        else
            x->parent->right = y;
    }

    y->left = x;
    x->parent = y;

    return;
}

void right_rotate(TreeNode **root, TreeNode *y)
{
    TreeNode *x;

    x = y->left;

    /*maintain augmentation
       //fix y */
    double tmpMax;

    tmpMax = x->right->value.maxGradient > y->right->value.maxGradient
                 ? x->right->value.maxGradient
                 : y->right->value.maxGradient;

    if (tmpMax > find_value_min_value(&y->value))
        y->value.maxGradient = tmpMax;
    else
        y->value.maxGradient = find_value_min_value(&y->value);

    /*fix x */
    tmpMax = x->left->value.maxGradient > y->value.maxGradient
                 ? x->left->value.maxGradient
                 : y->value.maxGradient;

    if (tmpMax > find_value_min_value(&x->value))
        x->value.maxGradient = tmpMax;
    else
        x->value.maxGradient = find_value_min_value(&x->value);

    /*ratation */
    y->left = x->right;
    x->right->parent = y;

    x->parent = y->parent;

    if (y->parent == NIL) {
        *root = x;
    }
    else {
        if (y->parent->left == y)
            y->parent->left = x;
        else
            y->parent->right = x;
    }

    x->right = y;
    y->parent = x;

    return;
}
