#include "emu.h"
#include "const.h"

extern "C" void shld(void *);
extern "C" void shrd(void *);

extern void normalize(reg& r);

void r_uadd(reg& a, reg& b, reg& s) // signs ignored
{
  reg t;
  int dif = a.exp - b.exp;
  if (!dif) dif = a.sigh - b.sigh;
  if (!dif) dif = a.sigl - b.sigl;
  if (dif > 0)
  {
    r_mov(a, s);
    r_mov(b, t);
  }
  else
  {
    r_mov(b, s);
    r_mov(a, t);
  }
  if (s.exp - t.exp > 64)
    return;
  while (t.exp < s.exp)
  {
    t.exp ++;
    shrd(&t.sigl);
  }
  unsigned short *ss, *ts;
  unsigned long tmp;
  ss = (unsigned short *)&s.sigl;
  ts = (unsigned short *)&t.sigl;
  tmp = 0;
  for (int i=4; i>0; i--)
  {
    tmp += (unsigned long)*ss + (unsigned long)*ts;
    *ss = tmp;
    ss++;
    ts++;
    tmp >>= 16;
  }
  if (tmp)
  {
    shrd(&s.sigl);
    s.exp++;
    s.sigh |= 0x80000000;
  }
  if (!(s.sigh | s.sigl))
  {
    s.exp = 0;
    s.tag = TW_Z;
  }
  else
  {
    while (!(s.sigh & 0x80000000))
    {
      if (s.exp == 0)
        return;
      shld(&s.sigl);
      s.exp--;
    }
  }
}

void r_usub(reg& a, reg& b, reg& d) // a > b
{
  reg t;
  r_mov(a, d);
  r_mov(b, t);

  if (d.exp - t.exp > 64)
    return;
  while (t.exp < d.exp)
  {
    t.exp ++;
    shrd(&t.sigl);
  }
  unsigned short *ss, *ts;
  long tmp;
  ss = (unsigned short *)&d.sigl;
  ts = (unsigned short *)&t.sigl;
  tmp = 0;
  for (int i=4; i>0; i--)
  {
    tmp += (long)*ss - (long)*ts;
    *ss = tmp;
    ss++;
    ts++;
    tmp /= 65536;
  }
  if (!(d.sigh | d.sigl))
  {
    d.exp = 0;
    d.tag = TW_Z;
  }
  else
  {
    while (!(d.sigh & 0x80000000))
    {
      if (d.exp == 0)
        return;
      shld(&d.sigl);
      d.exp--;
    }
  }
}

void r_add(reg& a, reg& b, reg& s)
{
  if (a.tag == TW_Z)
    return r_mov(b, s);
  if (b.tag == TW_Z)
    return r_mov(a, s);
  if (a.tag == TW_S)
    return r_mov(a, s);
  if (b.tag == TW_S)
    return r_mov(b, s);

  switch (a.sign*2 + b.sign)
  {
    case 0: // P + P
    case 3: // N + N
      r_uadd(a, b, s);
      s.sign = a.sign;
      break;
    case 1: // P + N
      b.sign ^= SIGN_POS^SIGN_NEG;
      r_sub(a, b, s);
      break;
    case 2: // N + P
      a.sign ^= SIGN_POS^SIGN_NEG;
      r_sub(b, a, s);
      break;
  }
}

void r_sub(reg& a, reg& b, reg& d)
{
  if (b.tag == TW_Z)
    return r_mov(a, d);
  if (a.tag == TW_Z)
  {
    r_mov(b, d);
    d.sign ^= SIGN_POS^SIGN_NEG;
    return;
  }
  if (a.tag == TW_S)
    return r_mov(a, d);
  if (b.tag == TW_S)
  {
    r_mov(b, d);
    d.sign ^= SIGN_POS^SIGN_NEG;
    return;
  }

  int mdif;
  mdif = a.exp - b.exp;
  if (!mdif)
    mdif = a.sigh - b.sigh;
  if (!mdif)
    mdif = a.sigl - b.sigl;

  switch (a.sign*2 + b.sign)
  {
    case 0: // P - P
    case 3: // N - N
      if (mdif > 0)
      {
        r_usub(a, b, d);
        d.sign = a.sign;
      }
      else
      {
        r_usub(b, a, d);
        d.sign = a.sign ^= SIGN_POS^SIGN_NEG;
      }
      break;
    case 1: // P - N
      r_uadd(a, b, d);
      d.sign = SIGN_POS;
      break;
    case 2: // N - P
      r_uadd(a, b, d);
      d.sign = SIGN_NEG;
      break;
  }
}

void r_mul(reg& a, reg& b, reg& s)
{
  if (a.tag == TW_Z)
  {
    r_mov(CONST_Z, s);
  }
  else if (b.tag == TW_Z)
  {
    r_mov(CONST_Z, s);
  }
  else if (a.tag == TW_S)
  {
    r_mov(a, s);
  }
  else if (b.tag == TW_S)
  {
    r_mov(b, s);
  }
  else
  {
    unsigned short sl[9], carry[10];
    unsigned short *as = (unsigned short *)(&a.sigl);
    unsigned short *bs = (unsigned short *)(&b.sigl);
    unsigned long l, sum;
    int ai, bi;
    for (ai=0; ai<8; ai++)
      sl[ai] = carry[ai] = 0;
    for (ai = 0; ai < 4; ai++)
      for (bi = 0; bi < 4; bi++)
      {
        l = as[ai] * bs[bi];

        sum = sl[ai+bi] + (l & 0xffff);
        sl[ai+bi] = sum & 0xffff;

        sum = sl[ai+bi+1] + (l>>16) + (sum>>16);
        sl[ai+bi+1] = sum & 0xffff;

        carry[ai+bi+2] += sum>>16;
      }
    for (ai=0; ai<8; ai++)
    {
      if (carry[ai])
      {
        sum = sl[ai] + carry[ai];
        sl[ai] = sum & 0xffff;
        carry[ai+1] += sum>>16;
      }
    }
    s.sigl = *(long *)(sl+4);
    s.sigh = *(long *)(sl+6);
    s.exp = a.exp + b.exp - EXP_BIAS + 1;
    s.tag = TW_V;
  }
  if (a.sign == b.sign)
    s.sign = SIGN_POS;
  else
    s.sign = SIGN_NEG;
  normalize(s);
}

void r_div(reg& a, reg& b, reg& q)
{
  if (a.tag == TW_S)
  {
    if (val_same(a, CONST_PINF))
      r_mov(a, q);
    else if (val_same(a, CONST_NINF))
      r_mov(a, q);
  }
  else if (b.tag == TW_S)
  {
    if (val_same(b, CONST_PINF))
      r_mov(CONST_Z, q);
    else if (val_same(b, CONST_NINF))
      r_mov(CONST_Z, q);
  }
  else if (a.tag == TW_Z)
  {
    r_mov(a, q);
  }
  else if (b.tag == TW_Z)
  {
    exception(EX_Z);
  }
  else
  {
    q.exp = a.exp - b.exp + EXP_BIAS;
    if (q.exp > EXP_MAX)
      r_mov(CONST_PINF, q);
    else if (q.exp <= 0)
      r_mov(CONST_Z, q);
    else
    {
      unsigned long long al, bl, ql, f;
      int i;
      al = *(unsigned long long *)(&a.sigl);
      bl = *(unsigned long long *)(&b.sigl);
      ql = 0;
      f = (unsigned long long)1 << 63;
      for (i=0; i<64; i++)
      {
        if (al >= bl)
        {
          al -= bl;
          ql += f;
        }
        bl >>= 1;
        f >>= 1;
      }
      *(unsigned long long *)(&q.sigl) = ql;
      q.tag = TW_V;
    }
  }
  if (a.sign == b.sign)
    q.sign = SIGN_POS;
  else
    q.sign = SIGN_NEG;
  normalize(q);
}