#ifndef _SPARSE_MATRIX_H
#define _SPARSE_MATRIX_H

#include <iostream>

namespace math_tools {

using namespace std;

template< class T >
class MatrixNode {
public:
   size_t y;
   size_t x;
   T data;
   MatrixNode * down;
   MatrixNode * right;
   MatrixNode() {
      x = y = 0;
      down = right = 0;
   }
   ~MatrixNode() {
      if(down) delete down;
      if(right) delete right;
   }
};


template< class T > 
class SparseMatrix {
   MatrixNode< T > * top_left;
public:
   SparseMatrix() {
      top_left = new MatrixNode< T >();
      top_left->y = 0;
      top_left->x = 0;
      top_left->down = 0;
      top_left->right = 0;
   }
   ~SparseMatrix() {
      delete top_left;
   }
   void clear() {
      delete top_left;
      top_left = new MatrixNode< T >();
      top_left->y = 0;
      top_left->x = 0;
      top_left->down = 0;
      top_left->right = 0;
   }
   bool get(size_t x, size_t y, T & data) {
      bool ret = false;
      MatrixNode< T > * p_node = top_left;
      while(p_node != 0) {
         if(p_node->y == y && p_node->x == x) {
            data = p_node->data;
            ret = true; 
            break;
         }
         else if(x == p_node->x) {
            p_node = p_node->down;
         }
         else if(y == p_node->y) {
            p_node = p_node->right;
         }
         else if(x > p_node->x) {
            p_node = p_node->right;
            if(p_node) {
               if(x != p_node->x && y > p_node->y && p_node->down) {
                  p_node = p_node->down; 
               }
            }
         }
         else if(y > p_node->y) {
            p_node = p_node->down;
            if(p_node) {
               if(y != p_node->y && x > p_node->x && p_node->right) {
                  p_node = p_node->right; 
               }
            }
         }
         else {
            break;
         }
      }
      return ret;
   }

   void put(size_t x, size_t y, const T & data) {
      MatrixNode< T > * p_node = top_left, * p_temp;
      bool flag = false;
      while(p_node != 0) {
         p_temp = p_node;
         if(p_node->y == y && p_node->x == x) {
            p_node->data = data;
            flag = true;
            break;
         }
         else if(x == p_node->x && p_node->down) {
            p_node = p_node->down;
         }
         else if(y == p_node->y && p_node->right) {
            p_node = p_node->right;
         }
         else if(x > p_node->x && p_node->right) {
            p_node = p_node->right;
            if(x < p_node->x) {
               p_node = p_temp;
               break;
            }
            else if(x != p_node->x && y > p_node->y && p_node->down) {
               p_node = p_node->down;
            }
         }
         else if(y > p_node->y && p_node->down) {
            p_node = p_node->down;
            if(y < p_node->y) {
               p_node = p_temp;
               break;
            }
            else if(y != p_node->y && x > p_node->x && p_node->right) {
               p_node = p_node->right;
            }
         }
         else {
            break;
         }
      }

      if(!flag && p_node) {
         if(x == p_node->x) {
            p_temp = new MatrixNode< T >();
            p_temp->data = data;
            p_temp->x = x;
            p_temp->y = y;
            p_node->down = p_temp; 
         }
         else if(y == p_node->y) { 
            p_temp = new MatrixNode< T >();
            p_temp->data = data;
            p_temp->x = x;
            p_temp->y = y;
            p_node->right = p_temp; 
         }
         else if(x > p_node->x) {
            p_temp = new MatrixNode< T >();
            p_temp->data = data;
            p_temp->x = x;
            p_temp->y = y;
            p_temp->right = p_node->right;
            p_node->right = p_temp; 
         }
         else if(y > p_node->y) {
            p_temp = new MatrixNode< T >();
            p_temp->data = data;
            p_temp->x = x;
            p_temp->y = y;
            p_temp->down = p_node->down;
            p_node->down = p_temp; 
         }
      }
   }
};

}

#endif /* _SPARSE_MATRIX_H */