#include "sdLaMa091.h"

//using namespace bgslibrary::algorithms::sigmadelta;
namespace bgslibrary
{
  namespace algorithms
  {
    namespace sigmadelta
    {
      const int DEFAULT_N  = 1;
      const int DEFAULT_VMIN = 2;
      const int DEFAULT_VMAX = 255;

      const char* LIB = "sdLaMa091 - ";
      //const int RED = 0;
      //const int GREEN = 1;
      const int BLUE = 2;
      const int CHANNELS = 3;

      typedef enum {
        UNKNOWN,
        C1R,
        C3R
      } image_t;

      struct sdLaMa091 {
        image_t  imageType;

        uint32_t width;
        uint32_t rgbWidth;
        uint32_t height;
        uint32_t stride;
        uint32_t numBytes;
        uint32_t unusedBytes;
        uint32_t rgbUnusedBytes;

        uint32_t N;
        uint32_t Vmin;
        uint32_t Vmax;

        uint8_t* Mt;
        uint8_t* Ot;
        uint8_t* Vt;
      };

      #if defined(DEFENSIVE_ALLOC) || defined(DEFENSIVE_POINTER) || \
        defined(DEFENSIVE_PARAM)
      static inline void outputError(char* error);
      #endif

      static inline uint8_t absVal(int8_t num);
      static inline uint8_t min(uint8_t a, uint8_t b);
      static inline uint8_t max(uint8_t a, uint8_t b);

      #if defined(DEFENSIVE_ALLOC) || defined(DEFENSIVE_POINTER) || \
        defined(DEFENSIVE_PARAM)

      static inline void outputError(char* error) {
        fprintf(stderr, "%s%s\n", LIB, error);
      }
      #endif


      static inline uint8_t absVal(int8_t num) {
        return (num < 0) ? (uint8_t)-num : (uint8_t)num;
      }

      static inline uint8_t min(uint8_t a, uint8_t b) {
        return (a < b) ? a : b;
      }

      static inline uint8_t max(uint8_t a, uint8_t b) {
        return (a > b) ? a : b;
      }

      sdLaMa091_t* sdLaMa091New(void) {
        sdLaMa091_t* sdLaMa091 = (sdLaMa091_t*)malloc(sizeof(*sdLaMa091));

      #ifdef DEFENSIVE_ALLOC
        if (sdLaMa091 == NULL) {
          outputError("Cannot allocate sdLaMa091 structure");
          return NULL;
        }
      #endif

        sdLaMa091->imageType = UNKNOWN;

        sdLaMa091->width = 0;
        sdLaMa091->rgbWidth = 0;
        sdLaMa091->height = 0;
        sdLaMa091->stride = 0;
        sdLaMa091->numBytes = 0;
        sdLaMa091->unusedBytes = 0;
        sdLaMa091->rgbUnusedBytes = 0;

        sdLaMa091->N = DEFAULT_N;
        sdLaMa091->Vmin = DEFAULT_VMIN;
        sdLaMa091->Vmax = DEFAULT_VMAX;

        sdLaMa091->Mt = NULL;
        sdLaMa091->Ot = NULL;
        sdLaMa091->Vt = NULL;

        return sdLaMa091;
      }

      int32_t sdLaMa091AllocInit_8u_C1R(sdLaMa091_t* sdLaMa091,
        const uint8_t* image_data,
        const uint32_t width,
        const uint32_t height,
        const uint32_t stride) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot initialize a NULL structure");
          return EXIT_FAILURE;
        }

        if (image_data == NULL) {
          outputError("Cannot allocate ressources for a NULL image");
          return EXIT_FAILURE;
        }
      #endif

      #ifdef DEFENSIVE_PARAM
        if (width == 0 || height == 0 || stride == 0) {
          outputError("Cannot allocate ressources for zero values");
          return EXIT_FAILURE;
        }

        if (stride < width) {
          outputError("Cannot allocate ressources for a stride lower than the width");
          return EXIT_FAILURE;
        }
      #endif

        sdLaMa091->imageType = C1R;

        sdLaMa091->width = width;
        sdLaMa091->height = height;
        sdLaMa091->stride = stride;
        sdLaMa091->numBytes = stride * height;
        sdLaMa091->unusedBytes = stride - sdLaMa091->width;

        sdLaMa091->Mt = (uint8_t*)malloc(sdLaMa091->numBytes);
      #ifdef DEFENSIVE_ALLOC
        if (sdLaMa091->Mt == NULL) {
          outputError("Cannot allocate sdLaMa091->Mt table");
          return EXIT_FAILURE;
        }
      #endif 
        memcpy(sdLaMa091->Mt, image_data, sdLaMa091->numBytes);

        sdLaMa091->Ot = (uint8_t*)malloc(sdLaMa091->numBytes);
      #ifdef DEFENSIVE_ALLOC
        if (sdLaMa091->Ot == NULL) {
          outputError("Cannot allocate sdLaMa091->Ot table");
          return EXIT_FAILURE;
        }
      #endif 
        uint8_t* workOt = sdLaMa091->Ot;

        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->width; ++j, ++workOt)
            *workOt = 0;


          if (sdLaMa091->unusedBytes > 0)
            workOt += sdLaMa091->unusedBytes;
        }

        sdLaMa091->Vt = (uint8_t*)malloc(sdLaMa091->numBytes);
      #ifdef DEFENSIVE_ALLOC
        if (sdLaMa091->Vt == NULL) {
          outputError("Cannot allocate sdLaMa091->Vt table");
          return EXIT_FAILURE;
        }
      #endif
        uint8_t* workVt = sdLaMa091->Vt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->width; ++j, ++workVt)
            *workVt = sdLaMa091->Vmin;


          if (sdLaMa091->unusedBytes > 0)
            workVt += sdLaMa091->unusedBytes;
        }

        return EXIT_SUCCESS;
      }

      int32_t sdLaMa091AllocInit_8u_C3R(sdLaMa091_t* sdLaMa091,
        const uint8_t* image_data,
        const uint32_t width,
        const uint32_t height,
        const uint32_t stride) {
        int32_t success = sdLaMa091AllocInit_8u_C1R(sdLaMa091, image_data, width,
          height, stride);

        if (success == EXIT_SUCCESS) {
          sdLaMa091->imageType = C3R;
          sdLaMa091->rgbWidth = sdLaMa091->width * CHANNELS;
          sdLaMa091->rgbUnusedBytes = stride - sdLaMa091->rgbWidth;
          sdLaMa091->width = 0;
          sdLaMa091->unusedBytes = 0;
        }

        return success;
      }

      int32_t sdLaMa091SetAmplificationFactor(sdLaMa091_t* sdLaMa091,
        const uint32_t amplificationFactor) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot set a parameter of a NULL structure");
          return EXIT_FAILURE;
        }
      #endif 

      #ifdef DEFENSIVE_PARAM
        if (amplificationFactor == 0) {
          outputError("Cannot set a parameter with a zero value");
          return EXIT_FAILURE;
        }
      #endif 

        sdLaMa091->N = amplificationFactor;

        return EXIT_SUCCESS;
      }

      uint32_t sdLaMa091GetAmplificationFactor(const sdLaMa091_t* sdLaMa091) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot get a parameter of a NULL structure");
          errno = ERROR_OCCURED;

          return EXIT_FAILURE;
        }
      #endif 

        return sdLaMa091->N;
      }

      int32_t sdLaMa091SetMaximalVariance(sdLaMa091_t* sdLaMa091,
        const uint32_t maximalVariance) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot set a parameter of a NULL structure");
          return EXIT_FAILURE;
        }
      #endif 

      #ifdef DEFENSIVE_PARAM
        if (maximalVariance == 0) {
          outputError("Cannot set a parameter with a zero value");
          return EXIT_FAILURE;
        }
      #endif 

        sdLaMa091->Vmax = maximalVariance;

        return EXIT_SUCCESS;
      }

      uint32_t sdLaMa091GetMaximalVariance(const sdLaMa091_t* sdLaMa091) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot get a parameter of a NULL structure");
          errno = ERROR_OCCURED;

          return EXIT_FAILURE;
        }
      #endif

        return sdLaMa091->Vmax;
      }

      int32_t sdLaMa091SetMinimalVariance(sdLaMa091_t* sdLaMa091,
        const uint32_t minimalVariance) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot set a parameter of a NULL structure");
          return EXIT_FAILURE;
        }
      #endif 

        sdLaMa091->Vmin = minimalVariance;

        return EXIT_SUCCESS;
      }

      uint32_t sdLaMa091GetMinimalVariance(const sdLaMa091_t* sdLaMa091) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot get a parameter of a NULL structure");
          errno = ERROR_OCCURED;

          return EXIT_FAILURE;
        }
      #endif 

        return sdLaMa091->Vmin;
      }


      int32_t sdLaMa091Update_8u_C1R(sdLaMa091_t* sdLaMa091,
        const uint8_t* image_data,
        uint8_t* segmentation_map) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot update a NULL structure");
          return EXIT_FAILURE;
        }

        if (image_data == NULL) {
          outputError("Cannot update a structure with a NULL image");
          return EXIT_FAILURE;
        }

        if (segmentation_map == NULL) {
          outputError("Cannot update a structure with a NULL segmentation map");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Mt == NULL) {
          outputError("Cannot update a structure with a NULL Mt table");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Ot == NULL) {
          outputError("Cannot update a structure with a NULL Ot table");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Vt == NULL) {
          outputError("Cannot update a structure with a NULL Vt table");
          return EXIT_FAILURE;
        }
      #endif 

      #ifdef DEFENSIVE_PARAM
        if (sdLaMa091->imageType != C1R) {
          outputError("Cannot update a structure which is not C1R");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->width == 0 || sdLaMa091->height == 0 ||
          sdLaMa091->stride == 0) {
          outputError("Cannot update a structure with zero values");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->stride < sdLaMa091->width) {
          outputError("Cannot update a structure with a stride lower than the width");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Vmax < sdLaMa091->Vmin) {
          outputError("Cannot update a structure with Vmax inferior to Vmin");
          return EXIT_FAILURE;
        }
      #endif 


        const uint8_t* workImage = image_data;
        uint8_t* workMt = sdLaMa091->Mt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->width; ++j, ++workImage, ++workMt) {
            if (*workMt < *workImage)
              ++(*workMt);
            else if (*workMt > *workImage)
              --(*workMt);
          }


          if (sdLaMa091->unusedBytes > 0) {
            workImage += sdLaMa091->unusedBytes;
            workMt += sdLaMa091->unusedBytes;
          }
        }

        workImage = image_data;
        workMt = sdLaMa091->Mt;
        uint8_t* workOt = sdLaMa091->Ot;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->width; ++j, ++workImage, ++workMt,
            ++workOt)
            *workOt = absVal(*workMt - *workImage);


          if (sdLaMa091->unusedBytes > 0) {
            workImage += sdLaMa091->unusedBytes;
            workMt += sdLaMa091->unusedBytes;
            workOt += sdLaMa091->unusedBytes;
          }
        }


        workOt = sdLaMa091->Ot;
        uint8_t* workVt = sdLaMa091->Vt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->width; ++j, ++workOt, ++workVt) {
            uint32_t ampOt = sdLaMa091->N * *workOt;

            if (*workVt < ampOt)
              ++(*workVt);
            else if (*workVt > ampOt)
              --(*workVt);

            *workVt = max(min(*workVt, sdLaMa091->Vmax), sdLaMa091->Vmin);
          }


          if (sdLaMa091->unusedBytes > 0) {
            workOt += sdLaMa091->unusedBytes;
            workVt += sdLaMa091->unusedBytes;
          }
        }


        workOt = sdLaMa091->Ot;
        workVt = sdLaMa091->Vt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->width; ++j, ++segmentation_map,
            ++workOt, ++workVt) {

            if (*workOt < *workVt)
              *segmentation_map = BACKGROUND;
            else
              *segmentation_map = FOREGROUND;
          }


          if (sdLaMa091->unusedBytes > 0) {
            segmentation_map += sdLaMa091->unusedBytes;
            workOt += sdLaMa091->unusedBytes;
            workVt += sdLaMa091->unusedBytes;
          }
        }

        return EXIT_SUCCESS;
      }

      int32_t sdLaMa091Update_8u_C3R(sdLaMa091_t* sdLaMa091,
        const uint8_t* image_data,
        uint8_t* segmentation_map) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot update a NULL structure");
          return EXIT_FAILURE;
        }

        if (image_data == NULL) {
          outputError("Cannot update a structure with a NULL image");
          return EXIT_FAILURE;
        }

        if (segmentation_map == NULL) {
          outputError("Cannot update a structure with a NULL segmentation map");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Mt == NULL) {
          outputError("Cannot update a structure with a NULL Mt table");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Ot == NULL) {
          outputError("Cannot update a structure with a NULL Ot table");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Vt == NULL) {
          outputError("Cannot update a structure with a NULL Vt table");
          return EXIT_FAILURE;
        }
      #endif

      #ifdef DEFENSIVE_PARAM
        if (sdLaMa091->imageType != C3R) {
          outputError("Cannot update a structure which is not C3R");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->rgbWidth == 0 || sdLaMa091->height == 0 ||
          sdLaMa091->stride == 0) {
          outputError("Cannot update a structure with zero values");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->stride < sdLaMa091->rgbWidth) {
          outputError("Cannot update a structure with a stride lower than the width");
          return EXIT_FAILURE;
        }

        if (sdLaMa091->Vmax < sdLaMa091->Vmin) {
          outputError("Cannot update a structure with Vmax inferior to Vmin");
          return EXIT_FAILURE;
        }
      #endif 


        const uint8_t* workImage = image_data;
        uint8_t* workMt = sdLaMa091->Mt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->rgbWidth; ++j, ++workImage, ++workMt) {
            if (*workMt < *workImage)
              ++(*workMt);
            else if (*workMt > *workImage)
              --(*workMt);
          }


          if (sdLaMa091->rgbUnusedBytes > 0) {
            workImage += sdLaMa091->rgbUnusedBytes;
            workMt += sdLaMa091->rgbUnusedBytes;
          }
        }


        workImage = image_data;
        workMt = sdLaMa091->Mt;
        uint8_t* workOt = sdLaMa091->Ot;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->rgbWidth; ++j, ++workImage, ++workMt,
            ++workOt)
            *workOt = absVal(*workMt - *workImage);


          if (sdLaMa091->rgbUnusedBytes > 0) {
            workImage += sdLaMa091->rgbUnusedBytes;
            workMt += sdLaMa091->rgbUnusedBytes;
            workOt += sdLaMa091->rgbUnusedBytes;
          }
        }

        workOt = sdLaMa091->Ot;
        uint8_t* workVt = sdLaMa091->Vt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          for (uint32_t j = 0; j < sdLaMa091->rgbWidth; ++j, ++workOt, ++workVt) {
            uint32_t ampOt = sdLaMa091->N * *workOt;

            if (*workVt < ampOt)
              ++(*workVt);
            else if (*workVt > ampOt)
              --(*workVt);

            *workVt = max(min(*workVt, sdLaMa091->Vmax), sdLaMa091->Vmin);
          }


          if (sdLaMa091->rgbUnusedBytes > 0) {
            workOt += sdLaMa091->rgbUnusedBytes;
            workVt += sdLaMa091->rgbUnusedBytes;
          }
        }

        workOt = sdLaMa091->Ot;
        workVt = sdLaMa091->Vt;


        for (uint32_t i = 0; i < sdLaMa091->numBytes; i += sdLaMa091->stride) {

          uint32_t numColor = 0;

          bool isForeground = false;


          for (uint32_t j = 0; j < sdLaMa091->rgbWidth; ++j, ++workOt, ++workVt) {
            if (*workOt >= *workVt)
              isForeground = true;


            if (numColor == BLUE) {
              if (isForeground) {
                *segmentation_map = FOREGROUND;
                *(++segmentation_map) = FOREGROUND;
                *(++segmentation_map) = FOREGROUND;
                ++segmentation_map;
              }
              else {
                *segmentation_map = BACKGROUND;
                *(++segmentation_map) = BACKGROUND;
                *(++segmentation_map) = BACKGROUND;
                ++segmentation_map;
              }

              isForeground = false;
            }

            numColor = (numColor + 1) % CHANNELS;
          }


          if (sdLaMa091->rgbUnusedBytes > 0) {
            segmentation_map += sdLaMa091->rgbUnusedBytes;
            workOt += sdLaMa091->rgbUnusedBytes;
            workVt += sdLaMa091->rgbUnusedBytes;
          }
        }

        return EXIT_SUCCESS;
      }

      int32_t sdLaMa091Free(sdLaMa091_t* sdLaMa091) {
      #ifdef DEFENSIVE_POINTER
        if (sdLaMa091 == NULL) {
          outputError("Cannot free a NULL strucutre");
          return EXIT_FAILURE;
        }
      #endif

        if (sdLaMa091->Mt != NULL)
          free(sdLaMa091->Mt);
        if (sdLaMa091->Ot != NULL)
          free(sdLaMa091->Ot);
        if (sdLaMa091->Vt != NULL)
          free(sdLaMa091->Vt);

        free(sdLaMa091);

        return EXIT_SUCCESS;
      }
    }
  }
}
