//*********************************************************************************
//
//
//
//             3D - graphics - Demo for PIC 18F4525 Mikrocontroller
//                      (C) 20.03.2005 by Daniel Porzig
//                              www.base32.de.vu
//
//
//           The original version was created for a 128x64 pixels - graphics-LCD
//           (Crystalfontz CFAG12864B-TM-IV) with 2 KS0108 displaycontrollers.
//           I slightly modified the CCS driverfile to support 2 controllers.
//           This example will work for any other display type, just replace
//           the init-/putpixel functions with those for your display type.
//
//           Display data should be cached (1k RAM needed for 128x64 pixels) to
//           improve performance.
//
//           This example may compile for a 18FX58 if trigotables are not used
//           (tables need 1440 bytes but heavily increase framerate)
//
//           The CCS File "GRAPHICS.C" and a display driver file are needed
//           to compile this example!!!
//
//                                  Have fun!
//
//*********************************************************************************


#include <18F4525.h>
#device adc=8
#use delay(clock=40000000)       // a 10 MHz Crystal + 4xPLL should be used to get a smooth animation
#fuses NOWDT,WDT128,H4, NOPROTECT, NOIESO, BROWNOUT, BORV28, PUT, NOCPD, STVREN
#fuses NODEBUG, NOLVP, NOWRT, NOWRTD, NOEBTR, NOCPB, NOEBTRB, NOWRTC, NOWRTB,
#fuses FCMEN, XINST, NOPBADEN, NOLPT1OSC, MCLR


#include "ks0108.c"      // the display device driver comes with the compiler
                         // change this if you're using a different displaycontroller

#define glcd_pixel(a,b,c) KS0108_pixel(a,b,c)   // the display-specific putpixel procedure

#include "graphics.c"    /* comes with the CCS C compiler
                            This example uses the following functions:
                              glcd_line(...)
                              glcd_circle(...)
                              glcd_text57(...)
                         */

#include "3dmath.c"     // vector & matrix functions


void CreateRotateMat(int8 rx,int8 ry,int8 rz);
void TransformVec(Vector3 res, Vector3 v, Matrix4x4 m);

void SetFov(float newFov);
void RecalcDists(void);
void Project(Vector3 coords);

void RenderFrame(void);
void InitTrigotable(void);

void CreateBox();
void Oscillate();


// clamps the given angle to the range between 0 and 180 degrees
#define CLAMP_ANGLE(x)							\
{												\
	if(x >=180) x = 0;			\
}

#define NUM_POINTS            14
#define HALF_DISPLAY_WIDTH  64.0    // half width of the used display in pixels
#define HALF_DISPLAY_HEIGHT 32.0    // half height of the used display in pixels


// defines whether FPS should be displayed or not
#define SHOW_FPS

// string used for text output
char     str[20];

float		fov,							   // FieldOfView angle
			distX,							// Focal distance to X projection plane
			distY;							// Focal distance to Y projection plane

// rotation angles of the 3D-object
int8		rotX = 0,
			rotY = 0,
			rotZ = 0;

// position of the 3D-object
float    posX = 0,
         posY = 0,
         posZ = 10.0;


Vector3  points[NUM_POINTS];
byte     indices[24];

float    _SIN[180];  // variables for the precalculated sin &
float    _COS[180];  // cos values


Matrix4x4	temp,matWorld;			// Transformation matrices
float	      scrX,scrY;
Vector3	   coordsWorld;


int16 counter=0;
int32 frametime_i;
float fps;



// timer1 overflow ISR used for fps-calculation
#int_timer1
void t1_isr()
{
   counter++;
}



// define the 3D-object
void CreateBox()
{
   // 8 corners of the box
   V3_Set(points[0],-1.0,-1.0,-1.0);
   V3_Set(points[1],1.0,-1.0,-1.0);
   V3_Set(points[2],-1.0, 1.0,-1.0);
   V3_Set(points[3],1.0, 1.0,-1.0);
	V3_Set(points[4],-1.0,-1.0, 1.0);
   V3_Set(points[5], 1.0,-1.0, 1.0);
   V3_Set(points[6],-1.0, 1.0, 1.0);
   V3_Set(points[7],1.0, 1.0, 1.0);

   // coordinates of the oscillating spheres in
   // the center of the box
	V3_Set(points[8],0.0,0.0, 0.35);
   V3_Set(points[9], 0.0,0.0, -0.35);
   V3_Set(points[10],0.0, 0.35, 0.0);
   V3_Set(points[11],0.0, -0.35, 0.0);
   V3_Set(points[12],-0.35, 0.0, 0.0);
   V3_Set(points[13],0.35, 0.0, 0.0);

   // define indices for correct connections between the box-vertices
	indices[0]=0;
   indices[1]=1;
	indices[2]=2;
   indices[3]=3;
	indices[4]=4;
   indices[5]=5;
	indices[6]=6;
   indices[7]=7;
	indices[8]=0;
   indices[9]=4;
	indices[10]=1;
   indices[11]=5;
	indices[12]=2;
   indices[13]=6;
	indices[14]=3;
   indices[15]=7;
	indices[16]=0;
   indices[17]=2;
   indices[18]=1;
   indices[19]=3;
	indices[20]=4;
   indices[21]=6;
	indices[22]=5;
   indices[23]=7;
}



// animate the spheres in the box
void Oscillate()
{
   static int osc=0;
   osc+=2;
   CLAMP_ANGLE(osc);

	V3_Set(points[8],0.0,0.0, 1.5*_sin[osc]);
   V3_Set(points[9], 0.0,0.0, -1.5*_sin[osc]);
   V3_Set(points[10],0.0, 1.5*_sin[osc], 0.0);
   V3_Set(points[11],0.0, -1.5*_sin[osc], 0.0);
   V3_Set(points[12],-1.5*_sin[osc], 0.0, 0.0);
   V3_Set(points[13],1.5*_sin[osc], 0.0, 0.0);
}



// calculate sin/cos-table
void InitTrigotable(void)
{
   int8 i;
   float a;
   for(i=0; i<180; i++)
   {
      a=(float)i*pi/90.0;
      _SIN[i]=sin(a);
      _COS[i]=cos(a);
   }
}






// calculate a rotation matrix
void CreateRotateMat(int8 rx,int8 ry,int8 rz)
{
   Matrix4x4   mrot;

   // multiply the world-matrix with the rotation matrices
	M4X4_Copy(temp,MatWorld);

   M4X4_Identity(mrot);
	mrot[1][1] = _cos[rx];
	mrot[1][2] = _sin[rx];
	mrot[2][1] = -mrot[1][2];
	mrot[2][2] = mrot[1][1];

	M4X4_Mul(MatWorld,mrot,temp);

	M4X4_Identity(mrot);
	mrot[0][0] = _cos[ry];
	mrot[0][2] = -_sin[ry];
	mrot[2][0] = -mrot[0][2];
	mrot[2][2] = mrot[0][0];

	M4X4_Mul(temp,mrot,MatWorld);

	M4X4_Identity(mrot);
	mrot[0][0] = _cos[rz];
	mrot[0][1] = _sin[rz];
	mrot[1][0] = -mrot[0][1];
	mrot[1][1] = mrot[0][0];

	M4X4_Mul(MatWorld,mrot,temp);

}





// Sets a field-of-view angle
void SetFov(float newFov)
{
	fov = newFov;

	// Recalc focal distances
	RecalcDists();
}



// (Re)calculates the distances to the X/Y projection planes
void RecalcDists(void)
{
	float cotFOV;
   cotFOV = cos(fov/2.0) / sin(fov/2.0);

	distX = cotFOV * HALF_DISPLAY_WIDTH;
	distY = cotFOV * HALF_DISPLAY_HEIGHT;
}



// Converts 3D coordinates to 2D screen coords
void Project(Vector3 coords)
{
   float invZ;

	// Skip if z = 0
	if(coords[2] == 0)
		return;

	invZ = 1.0f / coords[2];

	// Calculate the projected values using the formulas
	scrX = coords[0] * distX * invZ + HALF_DISPLAY_WIDTH;
	scrY = -coords[1] * distY * invZ + HALF_DISPLAY_HEIGHT;
}


// render the cube
void RenderFrame(void)
{
	int8 i,t;
	int8	x1,y1,x2,y2;

	// clamp rotation angles to range [0...180] degrees
	CLAMP_ANGLE(rotX);
	CLAMP_ANGLE(rotY);
	CLAMP_ANGLE(rotZ);

   // create translation matrix
	M4X4_Identity(matWorld);
	M4X4_Identity(temp);
	temp[3][0] = posx;
	temp[3][1] = posy;
	temp[3][2] = posz;
	M4X4_MulSelf(matWorld,temp);

   // create rotation matrix
	CreateRotateMat(rotX,rotY,rotZ);


	// Loop through the lines
	for(i = 0;i <12;i++)
	{
		// Transform and project first point
      t=indices[i*2];
		MatrixVectorMul(coordsWorld,points[t],matWorld);
		Project(coordsWorld);

		x1 = (int8)scrX;
		y1 = (int8)scrY;

		// Transform and project second point
      t=indices[i*2+1];
		MatrixVectorMul(coordsWorld,points[t],matWorld);
		Project(coordsWorld);

		x2 = (int8)scrX;
		y2 = (int8)scrY;


		// Draw the line

		glcd_line(x1,y1,x2,y2,1);

	}


   // draw the spheres
	for(i = 0;i <14;i++)
	{
 		MatrixVectorMul(coordsWorld,points[i],matWorld);
		Project(coordsWorld);

		x2 = (int8)scrX;
		y2 = (int8)scrY;

      glcd_circle(x2,y2,(int8)(20.0/coordsWorld[2]), 1,1);
   }

}






void main()
{
   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_psp(PSP_DISABLED);
   setup_wdt(WDT_OFF);
   setup_comparator(NC_NC_NC_NC);

   setup_timer_0(T1_DISABLED);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_4);   // setup timer for FPS-calculation
   setup_timer_2(T2_DISABLED,0,1);
   setup_timer_3(T3_INTERNAL);

   enable_interrupts(INT_TIMER1);
   enable_interrupts(GLOBAL);

   // init LCD
   KS0108_init(true);
   Ks0108_update();

   CreateBox();            // create the 3d-object
   InitTrigoTable();       // precalc sin/cos-values

	SetFov(45.0);



   while(1)
   {
      Ks0108_fill(0x00);   // clear the background
      Oscillate();         // animate spheres
      RenderFrame();       // render the image

#ifdef SHOW_FPS
      // calculate Frames per Second
      frametime_i=counter*0xffff;
      counter = 0;
      frametime_i*=4;
      frametime_i += (get_timer1()*4);
      fps=10000000.0;
      fps /= (float) frametime_i;
      set_timer1(0);
      // display text on LCD
      sprintf(str,"FPS: %3.2f",fps);
      glcd_text57(1,56,str,1,1);
#endif

      Ks0108_update();     /* send contents of Display memory to Display
                              not needed if your putpixel function sends data
                              to display directly
                           */

      // rotate cube
      rotX++;
      rotY++;
      rotZ++;


      // get Key Commands
      if(input(PIN_A3))
      {
         if(posY<5.0)posY+=.1;
      }

      if(input(PIN_A0))
      {
         if(posY>-5.0)posY-=.1;
      }

      if(input(PIN_A1))
      {
         if(posx<5.0)posx+=.1;
      }

      if(input(PIN_A2))
      {
         if(posx>-5.0)posx-=.1;
      }

      if(input(PIN_A5))
      {
         if(posz<15.0)posz+=.1;
      }

      if(input(PIN_A4))
      {
         if(posz>-15.0)posz-=.1;
      }


   }

}



