//
// vga_test.c - create first VGA frame
//
#include <xc.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <pic16f1718.h>

// CONFIG1
#pragma config FOSC = INTOSC
#pragma config WDTE = OFF
#pragma config PWRTE = ON
#pragma config MCLRE = ON
#pragma config CP = OFF
#pragma config BOREN = ON
#pragma config CLKOUTEN = OFF
#pragma config FCMEN = ON

// CONFIG2
#pragma config WRT = ALL
#pragma config PPS1WAY = OFF
#pragma config ZCDDIS = ON
#pragma config PLLEN = ON
#pragma config STVREN = OFF
#pragma config BORV = LO
#pragma config LPBOR = OFF
#pragma config LVP = ON

//
// h/w interface definition
//
#define REG_OE_bar 0b00010000
#define WE_bar     0b00000001
#define OE_bar     0b00000010
#define CP_en      0b00001000
#define MR_en      0b00100000
#define CP_bar     0b00000100
#define MR_bar     0b00010000

#define VSYNC 0b10000000
#define HSYNC 0b01000000
#define RGB(r, g, b) (((r) << 4) | ((g) << 2) | (b))

void SetupPeripherals() {
  // intosc 32 MHz
  OSCCON = 0b11110000;

  // select digital I/O
  ANSELA = 0;
  ANSELB = 0;
  ANSELC = 0;

  // set TRIS bits: all outputs
  LATA = 0x00;
  TRISA = 0x00;
  LATB = 0x00;
  TRISB = 0x00;
  LATC = 0x00;
  TRISC = 0x00;
}

//
// set control lines for free-running VGA signal generation
//
void RunMode()
{
  TRISC = 0xff; // data lines all inputs
  // reset address counter, then let it rip
  LATB = WE_bar | OE_bar&0 | CP_en&0 | CP_bar&0 | MR_en   | MR_bar   ;
  LATB = WE_bar | OE_bar&0 | CP_en&0 | CP_bar&0 | MR_en&0 | MR_bar&0 ;
  LATA = REG_OE_bar&0;
}

//
// set control lines for bitbanging waveforms into SRAM, and
//   reset SRAM address counter to 0
//
void LoadMode()
{
  LATA = REG_OE_bar;
  // toggle CP with MR low to reset address counter
  LATB = WE_bar | OE_bar | CP_en | CP_bar   | MR_en | MR_bar   ;
  LATB = WE_bar | OE_bar | CP_en | CP_bar&0 | MR_en | MR_bar   ;
  LATB = WE_bar | OE_bar | CP_en | CP_bar   | MR_en | MR_bar   ;
  // bring out of reset
  LATB = WE_bar | OE_bar | CP_en | CP_bar   | MR_en | MR_bar&0 ;
  TRISC = 0x00; // data lines all outputs
}

//
// bitbang a number of identical bytes into sequential SRAM addresses
//
void write_SRAM_bytes(uint8_t value, uint8_t count)
{
  PORTC = value;
  LATB = WE_bar | OE_bar | CP_en | CP_bar | MR_en | MR_bar&0 ;
  do {
    // toggle WE to write data
    LATB = WE_bar&0 | OE_bar | CP_en | CP_bar   | MR_en | MR_bar&0 ;
    LATB = WE_bar   | OE_bar | CP_en | CP_bar   | MR_en | MR_bar&0 ;
    // toggle CP to advance address
    LATB = WE_bar   | OE_bar | CP_en | CP_bar&0 | MR_en | MR_bar&0 ;
    LATB = WE_bar   | OE_bar | CP_en | CP_bar   | MR_en | MR_bar&0 ;
  } while (--count);
}

void GenerateLine(uint8_t vsync, uint8_t rgb, uint8_t count)
{
  do {
    write_SRAM_bytes( vsync | HSYNC   | rgb&0 , 16);  // front porch
    write_SRAM_bytes( vsync | HSYNC&0 | rgb&0 , 96);  // sync pulse
    write_SRAM_bytes( vsync | HSYNC   | rgb&0 , 48);  // back porch
    write_SRAM_bytes( vsync | HSYNC   | rgb   , 200); // video
    write_SRAM_bytes( vsync | HSYNC   | rgb   , 200); // video
    write_SRAM_bytes( vsync | HSYNC   | rgb   , 240); // video
  } while (--count);
}


typedef struct
{
  float x, y, z;
} V3;

typedef struct
{
  V3 p;
  V3 v;
} Ray;

typedef struct
{
  V3 n;
  float d;
} Plane;

typedef struct
{
  V3 c;
  float r;
} Sphere;

float intersect_plane(Ray *r, Plane *p)
{
  float numer = -( p->d
                   + p->n.x * r->p.x
                   + p->n.y * r->p.y
                   + p->n.z * r->p.z);
  float denom = ( p->n.x * r->v.x
                  + p->n.y * r->v.y
                  + p->n.z * r->v.z );
  return numer / denom;
}

float intersect_sphere(Ray *r, Sphere *s)
{
  float px = r->p.x - s->c.x;
  float py = r->p.y - s->c.y;
  float pz = r->p.z - s->c.z;
  float a = r->v.x*r->v.x + r->v.y*r->v.y + r->v.z*r->v.z;
  float b = 2.f*(px*r->v.x + py*r->v.y + pz*r->v.z);
  float c = px*px + py*py + pz*pz - s->r*s->r;
  float det = b*b-4.f*a*c;
  if (det < 0.){
    return -100000.f;
  }
  det = sqrt(det);
  float d1 = (-b + det) / (2.f*a);
  float d2 = (-b - det) / (2.f*a);  
  float d = -100000.f;
  if (d1 > 0.f){
    d = d1;
  }
  if (d2 > 0.f && d2 < d){
    d = d2;
  }
  return d;
}

typedef struct
{
  int8_t r, g, b;
} RGB;

uint8_t rgb_sat(int8_t x)
{
  if (x < 0){
    return 0;
  } else {
    return x;
  }
}

uint8_t mod_sat(int8_t x)
{
  if (x > 2){
    return 2;
  } else {
    return x;
  }
}

#define MAX_BOUNCE 5

RGB trace_ray(Ray ray)
{
  // scene geometry definition
  Plane plane = {{0, -1, -0.2}, 496};
  Sphere sphere = {{-20, 100, 600}, 256};
  Sphere sphere2 = {{510, 150, 300}, 256};
  Sphere sphere3 = {{240, 380, 50}, 80};

  RGB color = {0, 0, 0};
  RGB color_mod = {0, 0, 0};

  int bounce = 0;
  do {

    float dist = 100000;
    int obj = 0;
  
    float d = intersect_plane(&ray, &plane);
    if (d > 0 && d < dist){
      dist = d;
      obj = 1;
    }
  
    d = intersect_sphere(&ray, &sphere);
    if (d > 0 && d < dist){
      dist = d;
      obj = 2;
    }
  
    d = intersect_sphere(&ray, &sphere2);
    if (d > 0 && d < dist){
      dist = d;
      obj = 3;
    }

    d = intersect_sphere(&ray, &sphere3);
    if (d > 0 && d < dist){
      dist = d;
      obj = 4;
    }  

    V3 p;
    p.x = ray.p.x + dist * ray.v.x;
    p.y = ray.p.y + dist * ray.v.y;
    p.z = ray.p.z + dist * ray.v.z;
  
    if (0 == obj){
      // classic blue sky
      color.r = 0;
      color.g = 0;
      color.b = 3;
      break;
    } else if (1 == obj){
      // obligatory checkerboard plane
      int i = (int)p.x;
      int j = (int)p.z;
      if ((i & 128) ^ (j & 128)){
        color.r = 1;
        color.g = 1;
        color.b = 1;
        break;
      } else {
        color.r = 3;
        color.g = 3;
        color.b = 3;
        break;
      }
    } else {
      
      // calculate normal without sqrt (use sphere radius)
      V3 n;  
      if (2 == obj) {
        n.x = (p.x- sphere.c.x)/sphere.r;
        n.y = (p.y - sphere.c.y)/sphere.r;
        n.z = (p.z - sphere.c.z)/sphere.r;
      } else if (3 == obj) {
        n.x = (p.x - sphere2.c.x)/sphere2.r;
        n.y = (p.y - sphere2.c.y)/sphere2.r;
        n.z = (p.z - sphere2.c.z)/sphere2.r;
      } else {
        n.x = (p.x - sphere3.c.x)/sphere3.r;
        n.y = (p.y - sphere3.c.y)/sphere3.r;
        n.z = (p.z - sphere3.c.z)/sphere3.r;
      }      

      // reflect from sphere
      float eps = 1.f;
      ray.p.x = p.x + eps * n.x;
      ray.p.y = p.y + eps * n.y;
      ray.p.z = p.z + eps * n.z;
      float m = n.x * ray.v.x + n.y * ray.v.y + n.z * ray.v.z;
      ray.v.x = ray.v.x - 2 * m * n.x;
      ray.v.y = ray.v.y - 2 * m * n.y;
      ray.v.z = ray.v.z - 2 * m * n.z;
      color_mod.r = mod_sat(color_mod.r + 1);
      color_mod.g = mod_sat(color_mod.g + 1);
      color_mod.b = mod_sat(color_mod.b + 1);
    }
  } while (++bounce < MAX_BOUNCE);

  color.r = rgb_sat(color.r - color_mod.r);
  color.g = rgb_sat(color.g - color_mod.g);
  color.b = rgb_sat(color.b - color_mod.b);

  return color;
}

void GenerateFrame()
{
  GenerateLine( VSYNC   , 0,  33);  // V back porch

  V3 viewpoint = {320, 240, -500};

  int16_t row = 480;
  do {
    write_SRAM_bytes( VSYNC | HSYNC   | 0 , 16);  // H front porch
    write_SRAM_bytes( VSYNC | HSYNC&0 | 0 , 96);  // H sync pulse
    write_SRAM_bytes( VSYNC | HSYNC   | 0 , 48);  // H back porch

    int16_t col = 640;
    do {

      V3 direction;
      direction.x = viewpoint.x - col;
      direction.y = viewpoint.y - row;
      direction.z = 0 - viewpoint.z;
      Ray ray;
      ray.p = viewpoint;
      ray.v = direction;
      RGB rgb;
      rgb = trace_ray(ray);
      write_SRAM_bytes( VSYNC | HSYNC   | RGB(rgb.r, rgb.g, rgb.b), 1);
    } while (--col);
  } while (--row);

  GenerateLine( VSYNC   , 0,  10);  // V front porch
  GenerateLine( VSYNC&0 , 0,  2);   // V sync pulse
  write_SRAM_bytes( VSYNC | HSYNC | 0, 2);     // end of vsync; resets counter
}

int main() {
  SetupPeripherals();
  LoadMode();
  GenerateFrame();
  RunMode();

  while(1){
    continue;
  }

  return 0;
}