#ifndef MODEL_HC
#define MODEL_HC

#include "ModelTemplate"
#include "Quaternion"
#include "GrFast"

#define MODEL_DIRTY_NORMS 0x1
#define MODEL_DIRTY_MAT  0x2
#define MODEL_DIRTY_POS 0x4

public class TriData {
  CD3I32 tri[3];
  CColorROPU32 color;
  U16 dProbability;
};

public class Model {
  U8 flags;
  CD3 pos;
  Quat rot;
  F64 mtrans[16];
  F64 mscale[16];
  F64 mrot[16];
  F64 mmodel[16];
  F64 mvp[16];
  TriData *buffer;
  CD3 *normBuf;
  /* Original Data */
  CD3I32 *tris;
  U8 *colors;
  I64 triCnt;
  I64 normCnt;
  I64 bufCnt;
  CD3 *normals;
  U16 *ni; // normal index
};

public U0 InitModel(Model *model, ModelTemplate *template) {
  model->flags = 0;
  model->rot.x = 0;
  model->rot.y = 0;
  model->rot.z = 0;
  model->rot.w = 1;

  /* Set Model Data */
  model->triCnt = template->triCnt;
  model->normCnt = template->normCnt;
  model->colors = template->colors;
  model->tris = template->tris;
  model->colors = template->colors;
  model->normals = template->normals;
  model->ni = template->normIdx;

  /* Setup Buffer */
  model->bufCnt = 0;
  model->buffer = MAlloc(sizeof(TriData)*template->triCnt);
  model->normBuf = MAlloc(sizeof(CD3)*template->normCnt);
}

public U0 FreeModel(Model *model) {
  Free(model->buffer);
  Free(model->normBuf);
}

public U0 ModelScale(Model *model, F64 scale) {
  model->flags |= MODEL_DIRTY_MAT;
  model->mscale[0] = scale;
  model->mscale[1] = 0.0;
  model->mscale[2] = 0.0;
  model->mscale[3] = 0.0;

  model->mscale[4] = 0.0;
  model->mscale[5] = scale;
  model->mscale[6] = 0.0;
  model->mscale[7] = 0.0;

  model->mscale[8] = 0.0;
  model->mscale[9] = 0.0;
  model->mscale[10] = scale;
  model->mscale[11] = 0.0;

  model->mscale[12] = 0.0;
  model->mscale[13] = 0.0;
  model->mscale[14] = 0.0;
  model->mscale[15] = 1.0;
}

// ModelRot adds a rotation to an existing rotation
public U0 ModelRot(Model* model, Quat *rot) {
  model->flags |= MODEL_DIRTY_MAT|MODEL_DIRTY_NORMS;
  QuatMul(&model->rot, rot, &model->rot);
  Quat2Mat(model->rot.x, model->rot.y, model->rot.z, model->rot.w,
           model->mrot);
  CalcNormals(model->normals, model->normCnt, model->mrot, model->normBuf);
}

// ModelRotSet sets the rotation, overwriting existing rotation
public U0 ModelRotSet(Model* model, Quat *rot) {
  model->flags |= MODEL_DIRTY_MAT|MODEL_DIRTY_NORMS;
  MemCpy(&model->rot, rot, sizeof(Quat));
  Quat2Mat(model->rot.x, model->rot.y, model->rot.z, model->rot.w,
           model->mrot);
  CalcNormals(model->normals, model->normCnt, model->mrot, model->normBuf);
}

public U0 Lighting(CD3 *pos, CD3 *normal, CD3 *lightPos,
                   F64 shine, U16 *dither_probability_u16) {
  /* Diffuse */
  CD3 lightDir;
  D3Sub(&lightDir, pos, lightPos);
  CD3Normalize(&lightDir);

  /* instead of the traditional Max(diffuse, 0.0) move the result from
     -1.0 - 1.0 to 1.0 - 2.0 and then divide by 2.0 in order to preserve
     the negative range needed for proper dither-lighting */
  F64 diffuse = (D3Dot(normal, &lightDir) + 1.0) / 2.0;

  /* Possible Attenuation Implementation */
//  F64 linear = 0.00000001;
//  F64 quadratic = 0.000002;
//  F64 attenuation = 1.0/(1.0 + linear*dist + quadratic*(dist*dist));

  F64 attenuation = D3Dist(lightPos, pos) / 5000.0;
  diffuse += attenuation; // darken based off distance
  if (diffuse > 1.0) diffuse = 1.0; // limit to 1.0 to avoid 16bit overflow

  /* Specular */
/*
  CD3 viewDir, halfwayDir;
  D3Sub(&viewDir, pos, view);
  CD3Normalize(&viewDir);

  D3Add(&halfwayDir, &lightDir, &viewDir);
  CD3Normalize(&halfwayDir);
*/
  //F64 spec = Pow(Max(D3Dot(normal, &viewDir), 0.0), shine);
  //"%.1f\n", spec;
//  F64 spec = Pow(Max(D3Dot(normal, &halfwayDir), 0.0), shine);
//  diffuse += spec;
  //if (spec > 1.0) spec = 1.0;

/*
  // reflect(): I - 2.0 * dot(N, I) * N
  lightDir.x = -lightDir.x;
  lightDir.y = -lightDir.y;
  lightDir.z = -lightDir.z;
  CD3 subIncident;
  F64 f1 = 2.0 * D3Dot(normal, &lightDir);
  CD3 v1;
  D3Mul(&v1, f1, normal);
  CD3 reflectDir;
  D3Sub(&reflectDir, &lightDir, &v1);
  D3Sub(&reflectDir, &v1, &lightDir);
  F64 spec = Pow(Max(D3Dot(&viewDir, &reflectDir), 0.0), 32);
  F64 specular = shine * spec;
  //"%.1f\n", specular;
  //diffuse /= 2.0;
  diffuse -= specular/4;
*/
  *dither_probability_u16=diffuse*65535;
}

// Alternative CalcNormals I forgot why I wrote this
public U0 CalcNormals2(CD3 *norms, I64 cnt, F64 *mat, CD3 *dest) {
  /* Rotate Normals */
  I64 i;
  F64 px, py, pz;
  CD3 *nPtr = norms;
  CD3 *dPtr = dest;
  for (i=0; i<cnt; i++, nPtr++, dPtr++) {
    px=nPtr->x; py=nPtr->y; pz=nPtr->z;
    /* Inline MatMulXYZ */
    dPtr->x = mat[0*4+0]*px+mat[1*4+0]*py+mat[2*4+0]*pz+mat[3*4+0];
    dPtr->y = mat[0*4+1]*px+mat[1*4+1]*py+mat[2*4+1]*pz+mat[3*4+1];
    dPtr->z = mat[0*4+2]*px+mat[1*4+2]*py+mat[2*4+2]*pz+mat[3*4+2];
  }
}

public U0 ModelTranslate(Model *model, F64 x, F64 y, F64 z) {
  model->flags |= MODEL_DIRTY_MAT | MODEL_DIRTY_POS;
  model->pos.x = x;
  model->pos.y = y;
  model->pos.z = z;
}

// CalcTri assumes you've already rotated your normals with CalcNormals
public U0 CalcTris(CD3I32 *tri, I64 cnt,           // Triangle Data
                   CD3 *norms, U16 *ni,            // Normal Data
                   CD3 *lightPos,                  // Light Data
                   F64 *mvp, F64 *mmodel,          // Matrix
                   U8 *colors, F64 shine,          // Color Data
                   TriData *dest, I64 *finalCnt) { // Destination
  I64 i;
  F64 pw;
  CD3 t1, t2, t3, w1, w2, w3; // t is screen-coord, w is world-coord
  CD3I32 *p=tri;
  TriData *dPtr = dest;
  CD3 fragPos;

  /* Transform Vertices */
  I64 _finalCnt = 0;
  for (i=0; i<cnt; i++, p++) {
    t1.x = p->x; t1.y = p->y; t1.z = p->z;
    w1.x = t1.x; w1.y = t1.y; w1.z = t1.z;

    //MatMulXYZ(mvp, &t1.x, &t1.y, &t1.z, &pw);
    MatMulXYZ(mmodel, &t1.x, &t1.y, &t1.z, &pw);
    t1.x /= pw;
    //t1.x += SCX;
    t1.y /= pw;
    //t1.y += SCY;
    //t1.z /= pw; // depth testing is integer-based don't normalize
    //t1.z <<= 32;
/*    if ( (t1.z >= 0.0 && t2.z >= 0.0 && t3.z >= 0.0) ||
         (t1.z < -10000.0 && t2.z < -10000.0 && t3.z < -10000.0) ) {
      goto next_vert;
    }*/

/*
    if (t1.x   >  1000.0 || t1.x   < -1000.0  ||
        t1.y   >  1000.0 || t1.y   < -1000.0  ){
//        t1.z   >= 0.0    || t1.z   < -10000.0) {
      p += 2;
      goto next_vert;
    }
*/
    p++;
    t2.x = p->x; t2.y = p->y; t2.z = p->z;
    w2.x = t2.x; w2.y = t2.y; w2.z = t2.z;

    //MatMulXYZ(mvp, &t2.x, &t2.y, &t2.z, &pw);
    MatMulXYZ(mmodel, &t2.x, &t2.y, &t2.z, &pw);
    t2.x /= pw;
    //t2.x += SCX;
    t2.y /= pw;
    //t2.y += SCY;
//    t2.z /= pw; // depth testing is integer-based don't normalize
    p++;
/*
    if (t2.x   >  1000.0 || t2.x   < -1000.0  ||
        t2.y   >  1000.0 || t2.y   < -1000.0  ){
//        t2.z   >= 0.0    || t2.z   < -10000.0) {
      goto next_vert;
    }
*/
    t3.x = p->x; t3.y = p->y; t3.z = p->z;
    w3.x = t3.x; w3.y = t3.y; w3.z = t3.z;

    //MatMulXYZ(mvp, &t3.x, &t3.y, &t3.z, &pw);
    MatMulXYZ(mmodel, &t3.x, &t3.y, &t3.z, &pw);
    t3.x /= pw;
    //t3.x += SCX;
    t3.y /= pw;
    //t3.y += SCY;
//    t3.z /= pw; // depth testing is integer-based don't normalize
/*
    if (t3.x   >  1000.0 || t3.x   < -1000.0  ||
        t3.y   >  1000.0 || t3.y   < -1000.0  ) {
//        t3.z   >= 0.0    || t3.z   < -10000.0) {
      goto next_vert;
    }
*/
    /* Save Triangle */
    dPtr->tri[0].x = t1.x;
    dPtr->tri[0].y = t1.y;
    dPtr->tri[0].z = t1.z;
    dPtr->tri[1].x = t2.x;
    dPtr->tri[1].y = t2.y;
    dPtr->tri[1].z = t2.z;
    dPtr->tri[2].x = t3.x;
    dPtr->tri[2].y = t3.y;
    dPtr->tri[2].z = t3.z;

    /* Calculate Color */
    MatMulXYZ(mmodel, &w1.x, &w1.y, &w1.z, &pw);
//    w1.x /= pw;
//    w1.y /= pw;
//    w1.z /= pw;
    MatMulXYZ(mmodel, &w2.x, &w2.y, &w2.z, &pw);
//    w2.x /= pw;
//    w2.y /= pw;
//    w2.z /= pw;
    MatMulXYZ(mmodel, &w3.x, &w3.y, &w3.z, &pw);
//    w3.x /= pw;
//    w3.y /= pw;
//    w3.z /= pw;

    fragPos.x = (w1.x + w2.x + w3.x) / 3;
    fragPos.y = (w1.y + w2.y + w3.y) / 3;
    fragPos.z = (w1.z + w2.z + w3.z) / 3;
    //dPtr->color = colors[i] | ROPF_PROBABILITY_DITHER;
    dPtr->color = colors[i];
    Lighting(&fragPos, &norms[ni[i]], lightPos, shine, &dPtr->dProbability);
    dPtr++;
    _finalCnt++;
next_vert:
  }
  *finalCnt = _finalCnt;
}

// for when view or lighting has changed
public U0 ModelUpdate(Model *models, I64 cnt, CD3 *light, F64 shine) {//, F64 *vp) {
  I64 i;
  F64 mtmp[16];
  Model *model = models;
  for (i=0; i<cnt; i++, model++) {
    if (model->flags & MODEL_DIRTY_POS) {
      model->mtrans[0] = 1.0;
      model->mtrans[1] = 0.0;
      model->mtrans[2] = 0.0;
      model->mtrans[3] = 0.0;

      model->mtrans[4] = 0.0;
      model->mtrans[5] = 1.0;
      model->mtrans[6] = 0.0;
      model->mtrans[7] = 0.0;

      model->mtrans[8] = 0.0;
      model->mtrans[9] = 0.0;
      model->mtrans[10] = 1.0;
      model->mtrans[11] = 0.0;

      model->mtrans[12] = model->pos.x;
      model->mtrans[13] = model->pos.y;
      model->mtrans[14] = model->pos.z;
      model->mtrans[15] = 1.0;
    }
    if (model->flags & MODEL_DIRTY_MAT) {
      MatMul(model->mtrans, model->mscale, mtmp);
      MatMul(mtmp, model->mrot, model->mmodel);
    }
    model->flags = 0;
  }

  /* Generate Triangles */
  model = models;
  for (i=0; i<cnt; i++, model++) { // TODO make this conditional for optimization
    //MatMul(vp, model->mmodel, model->mvp);
    CalcTris(model->tris, model->triCnt, model->normBuf, model->ni,
             light, NULL, model->mmodel, model->colors, shine, model->buffer,
             &model->bufCnt);
  }
}

/*
public class ModelLight {
  CD3 pos;
  Quat q;
  CD3I32 *tris;
  U8 *colors;
  I64 triCnt;
  I64 normCnt;
  I64 bufCnt;
  CD3 *normals;
  U16 *ni; // normal index
};

U0 DrawModelLight(CDC *dc, ModelLight *model, CD3 *pos, CD3 *light, I64 *r, I32 *depthBuf) {
  I64 i, xx, yy, zz, rr[16];
  CD3I32 tri[3];

/*
  CalcNormals(model->normals, model->normCnt, model->mrot, model->normBuf);
  CalcTris(model->tris, model->triCnt, model->normBuf, model->ni,
           light, NULL, model->mmodel, model->colors, shine, model->buffer,
           &model->bufCnt);


U0 CalcNormals(CD3 *norms, I64 cnt, F64 *mat, CD3 *dest) {
  /* Rotate Normals */
  I64 i;
  F64 px, py, pz;
  CD3 *nPtr = norms;
  CD3 *dPtr = dest;
  for (i=0; i<cnt; i++, nPtr++, dPtr++) {
    px=nPtr->x; py=nPtr->y; pz=nPtr->z;
    /* Inline MatMulXYZ */
    dPtr->x = mat[0*4+0]*px+mat[1*4+0]*py+mat[2*4+0]*pz+mat[3*4+0];
    dPtr->y = mat[0*4+1]*px+mat[1*4+1]*py+mat[2*4+1]*pz+mat[3*4+1];
    dPtr->z = mat[0*4+2]*px+mat[1*4+2]*py+mat[2*4+2]*pz+mat[3*4+2];
  }
}
*/

//  CD3I32 tri[3];
//  CColorROPU32 color;
//  U16 dProbability;

  QuatToMatrix(&model->q, rr);
  CD3I32 *ptr = model->tris;
  for (i=0; i<model->triCnt; i++) {
    xx=ptr->x; yy=ptr->y; zz=ptr->z;
    tri[0].x=(r[0*4+0]*xx+r[0*4+1]*yy+r[0*4+2]*zz+r[0*4+3])>>32;
    tri[0].y=(r[1*4+0]*xx+r[1*4+1]*yy+r[1*4+2]*zz+r[1*4+3])>>32;
    tri[0].z=(r[2*4+0]*xx+r[2*4+1]*yy+r[2*4+2]*zz+r[2*4+3])>>32;

    ptr++; xx=ptr->x; yy=ptr->y; zz=ptr->z;
    tri[1].x=(r[0*4+0]*xx+r[0*4+1]*yy+r[0*4+2]*zz+r[0*4+3])>>32;
    tri[1].y=(r[1*4+0]*xx+r[1*4+1]*yy+r[1*4+2]*zz+r[1*4+3])>>32;
    tri[1].z=(r[2*4+0]*xx+r[2*4+1]*yy+r[2*4+2]*zz+r[2*4+3])>>32;

    ptr++; xx=ptr->x; yy=ptr->y; zz=ptr->z;
    tri[2].x=(r[0*4+0]*xx+r[0*4+1]*yy+r[0*4+2]*zz+r[0*4+3])>>32;
    tri[2].y=(r[1*4+0]*xx+r[1*4+1]*yy+r[1*4+2]*zz+r[1*4+3])>>32;
    tri[2].z=(r[2*4+0]*xx+r[2*4+1]*yy+r[2*4+2]*zz+r[2*4+3])>>32;

    dc->color = model->colors[i];

    // Calculate Normal
    CD3 normal;
    U64 ni = model->ni[i];
    xx=model->normals[ni].x; yy=model->normals[ni].y; zz=model->normals[ni].z;
    normal.x=(rr[0*4+0]*xx+rr[0*4+1]*yy+rr[0*4+2]*zz+rr[0*4+3])>>32;
    normal.y=(rr[1*4+0]*xx+rr[1*4+1]*yy+rr[1*4+2]*zz+rr[1*4+3])>>32;
    normal.z=(rr[2*4+0]*xx+rr[2*4+1]*yy+rr[2*4+2]*zz+rr[2*4+3])>>32;

    // Calculate Dither Color
    //Lighting(&fragPos, &norms[ni[i]], lightPos, shine, &dPtr->dProbability);
    CD3 lightDir;
    D3Sub(&lightDir, pos, light);
    CD3Normalize(&lightDir);
    F64 diffuse = (D3Dot(&normal, &lightDir) + 1.0) / 2.0;
    F64 attenuation = D3Dist(light, pos) / 5000.0;
    diffuse += attenuation; // darken based off distance
    if (diffuse > 1.0) diffuse = 1.0; // limit to 1.0 to avoid 16bit overflow
    dc->dither_probability_u16 = diffuse*65535;

    dc->color = ptr->color;
    dc->dither_probability_u16 = ptr->dProbability;
    FillTri(dc, &ptr->tri[0], &ptr->tri[1], &ptr->tri[2], depthBuf);
  }
}
*/

#endif