#include <stdio.h>
#include "typedfs.h"
#include "img_tool.h"

/***********  FOR MORPHOLOGY ***********/

/** R1 := R1 or R2 */
void OrR(void *R1, const void *R2, unsigned maxX)
{
  while(maxX>=4)
  {
    *(uint32_t*)R1 |= *(uint32_t*)R2;
    R1 = (uint32_t*)R1 + 1;
    R2 = (uint32_t*)R2 + 1;
    maxX -= 4;
  }
  while(maxX>=1)
  {
    *(int8_t*)R1 |= *(int8_t*)R2;
    R1 = (int8_t*)R1 + 1;
    R2 = (int8_t*)R2 + 1;
    maxX--;
  }
}


/** R1 := R1 and R2 */
void AndR(void *R1, const void *R2, unsigned maxX)
{
  while(maxX>=4)
  {
    *(uint32_t*)R1 &= *(uint32_t*)R2;
    R1 = (uint32_t*)R1 + 1;
    R2 = (uint32_t*)R2 + 1;
    maxX -= 4;
  }
  while(maxX>=1)
  {
    *(int8_t*)R1 &= *(int8_t*)R2;
    R1 = (int8_t*)R1 + 1;
    R2 = (int8_t*)R2 + 1;
    maxX--;
  }
}


/** In place right shift R1:=R1 shr 1
 * Please note that this is a continuous shift without quantisation U8, U16, U32... */
void ShrR(void *R, unsigned maxX)
{
uint8_t PrevVal, val;
  if(R==NULL || maxX==0) return;

  PrevVal = (*(uint8_t*)R & 0x80);
  while(maxX-->0)
  {
    val = *(uint8_t*)R;
    *(uint8_t*)R = PrevVal | (val>>1);
    R = (uint8_t*)R + 1;
    PrevVal = (val & 1) ? 0x80 : 0;
  }
}


/** In place left shift R1:=R1 shl 1
* Please note that this is a continuous shift without quantisation U8, U16, U32...  */
void ShlR(void *R, unsigned maxX, unsigned char LastBit)
{
uint8_t PrevVal, val;
  if(R==NULL || maxX==0) return;

  R = (int8_t*)R + maxX - 1;

  PrevVal = *(uint8_t*)R & LastBit;
  while(maxX-->0)
  {
    val = *(uint8_t*)R;
   *(int8_t*)R = PrevVal | (val<<1);
   R = (uint8_t*)R - 1;
   PrevVal = (val & 0x80) ? 1 : 0;
  }
}


/***********  FOR MASK FILTER ***********/


void AddLu32u8(unsigned Size, uint32_t *Accu, uint8_t *Add)
{
 while(Size--)
 {
   *Accu++ += *Add++;
 }
}


void SubLu32u8(unsigned Size, uint32_t *Accu, uint8_t *Sub)
{
 while(Size--)
 {
   *Accu++ -= *Sub++;
 }
}


/***********  FOR MEDIAN FILTER ***********/



/***********  FOR GAUSIAN ***********/

void AbsDiff_u32(uint32_t *Out, uint32_t *In, int SizeX)
{
  while(SizeX-- > 0)
  {
    if(*In > *Out)
      *Out = *In++ - *Out;
    else
      *Out = *Out - *In++;
    Out++;
  }
}


/** OUT = abs(IN1 - IN2) */
void AbsDiffCopy_u32(uint32_t *Out, uint32_t *In1, uint32_t *In2, int SizeX)
{
  while(SizeX-- > 0)
  {
    if(*In1 > *In2)
      *Out++ = *In1++ - *In2++;
    else
      *Out++ = *In2++ - *In1++;
  }
}

