// simple project with rotating 3D cube using Arduino UNO and Transparent 128x64 OLED Display, 
// created by upir, 2022
// youtube channel: https://www.youtube.com/upir_upir
// full tutoral is here: https://youtu.be/kBAcaA7NAlA

// Turbo pressure gauge tutorial: https://youtu.be/JXmw1xOlBdk
// Transparent OLED tutorial: https://youtu.be/hIFDcksXgBk
// Knob + OLED tutorial: https://youtu.be/NPfaLKKsf_Q

// useful links:
// u8g documentation: https://github.com/olikraus/u8glib/wiki/userreference
// Wokwi starting project: https://wokwi.com/arduino/projects/300867986768527882
// Arduino UNO: http://store.arduino.cc/products/arduino-uno-rev3
// Arduino UNO MINI: https://store.arduino.cc/products/uno-mini-le
// Multidimensional arrays: https://www.tutorialspoint.com/arduino/arduino_multi_dimensional_arrays.htm
// 2D Rotation: https://en.wikipedia.org/wiki/Rotation_(mathematics)
// Normal OLED Display: https://www.aliexpress.com/item/4001051535838.html
// Transparent OLED Display: https://a.aliexpress.com/_mKGmhKg
// Big OLED Display: https://www.aliexpress.com/item/1005003091769556.html
// Arduino breadboard prototyping shield: https://www.adafruit.com/product/2077

#include <Arduino.h>
#include "U8glib.h" // u8g library, note there is a newer version u8g2, please use the older one
/*
const uint8_t upir_logo[] U8G_PROGMEM = {        // another simple way how to define pictures for u8g library
B00010101, B11010111,     //  ░░░█░█░███░█░███
B00010101, B01000101,     //  ░░░█░█░█░█░░░█░█
B00010101, B10010110,     //  ░░░█░█░██░░█░██░
B00011001, B00010101      //  ░░░██░░█░░░█░█░█
};
*/

// uncomment the correct connection - fast I2C, slow I2C, SPI
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI
//U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK); // slow I2C / TWI     -- I had to use "slow I2C" in my case
//U8GLIB_SSD1306_128X64 u8g(13, 11, 8, 9, 10); // SPI connection  - SCL = 13, SDA = 11, RES = 10, DC = 9, CS = 8

#define MAX_VERTICES_PER_OBJECT 64


typedef float matrix_4x4[4][4];
typedef float matrix_1x4[4];

typedef struct point_3d_def {
  float x,y,z,w;
} point_3d, *point_3d_ptr;

typedef struct object_def {
  int id;
  int num_vertices;
  float size;
  //point_3d vertices_local[MAX_VERTICES_PER_OBJECT];
  point_3d_ptr vertices_local;
  point_3d world_pos;
  
  //int state;

} object, *object_ptr;
/*
const float cos_look[] PROGMEM = {  1,0.999847695,0.999390827,0.998629535,0.99756405,0.996194698,0.994521895,0.992546152,0.990268069,0.987688341,
                                    0.984807753,0.981627183,0.978147601,0.974370065,0.970295726,0.965925826,0.961261696,0.956304756,0.951056516,
                                    0.945518576,0.939692621,0.933580426,0.927183855,0.920504853,0.913545458,0.906307787,0.898794046,0.891006524,
                                    0.882947593,0.874619707,0.866025404,0.857167301,0.848048096,0.838670568,0.829037573,0.819152044,0.809016994,
                                    0.79863551,0.788010754,0.777145961,0.766044443,0.75470958,0.743144825,0.731353702,0.7193398,0.707106781,
                                    0.69465837,0.68199836,0.669130606,0.656059029,0.64278761,0.629320391,0.615661475,0.601815023,0.587785252,
                                    0.573576436,0.559192903,0.544639035,0.529919264,0.515038075,0.5,0.48480962,0.469471563,0.4539905,0.438371147,
                                    0.422618262,0.406736643,0.390731128,0.374606593,0.35836795,0.342020143,0.325568154,0.309016994,0.292371705,
                                    0.275637356,0.258819045,0.241921896,0.224951054,0.207911691,0.190808995,0.173648178,0.156434465,0.139173101,
                                    0.121869343,0.104528463,0.087155743,0.069756474,0.052335956,0.034899497,0.017452406,0,-0.017452406,-0.034899497,
                                    -0.052335956,-0.069756474,-0.087155743,-0.104528463,-0.121869343,-0.139173101,-0.156434465,-0.173648178,-0.190808995,
                                    -0.207911691,-0.224951054,-0.241921896,-0.258819045,-0.275637356,-0.292371705,-0.309016994,-0.325568154,-0.342020143,
                                    -0.35836795,-0.374606593,-0.390731128,-0.406736643,-0.422618262,-0.438371147,-0.4539905,-0.469471563,-0.48480962,
                                    -0.5,-0.515038075,-0.529919264,-0.544639035,-0.559192903,-0.573576436,-0.587785252,-0.601815023,-0.615661475,
                                    -0.629320391,-0.64278761,-0.656059029,-0.669130606,-0.68199836,-0.69465837,-0.707106781,-0.7193398,-0.731353702,
                                    -0.743144825,-0.75470958,-0.766044443,-0.777145961,-0.788010754,-0.79863551,-0.809016994,-0.819152044,-0.829037573,
                                    -0.838670568,-0.848048096,-0.857167301,-0.866025404,-0.874619707,-0.882947593,-0.891006524,-0.898794046,-0.906307787,
                                    -0.913545458,-0.920504853,-0.927183855,-0.933580426,-0.939692621,-0.945518576,-0.951056516,-0.956304756,-0.961261696,
                                    -0.965925826,-0.970295726,-0.974370065,-0.978147601,-0.981627183,-0.984807753,-0.987688341,-0.990268069,-0.992546152,
                                    -0.994521895,-0.996194698,-0.99756405,-0.998629535,-0.999390827,-0.999847695,-1,-0.999847695,-0.999390827,-0.998629535,
                                    -0.99756405,-0.996194698,-0.994521895,-0.992546152,-0.990268069,-0.987688341,-0.984807753,-0.981627183,-0.978147601,
                                    -0.974370065,-0.970295726,-0.965925826,-0.961261696,-0.956304756,-0.951056516,-0.945518576,-0.939692621,-0.933580426,
                                    -0.927183855,-0.920504853,-0.913545458,-0.906307787,-0.898794046,-0.891006524,-0.882947593,-0.874619707,-0.866025404,
                                    -0.857167301,-0.848048096,-0.838670568,-0.829037573,-0.819152044,-0.809016994,-0.79863551,-0.788010754,-0.777145961,
                                    -0.766044443,-0.75470958,-0.743144825,-0.731353702,-0.7193398,-0.707106781,-0.69465837,-0.68199836,-0.669130606,
                                    -0.656059029,-0.64278761,-0.629320391,-0.615661475,-0.601815023,-0.587785252,-0.573576436,-0.559192903,-0.544639035,
                                    -0.529919264,-0.515038075,-0.5,-0.48480962,-0.469471563,-0.4539905,-0.438371147,-0.422618262,-0.406736643,-0.390731128,
                                    -0.374606593,-0.35836795,-0.342020143,-0.325568154,-0.309016994,-0.292371705,-0.275637356,-0.258819045,-0.241921896,
                                    -0.224951054,-0.207911691,-0.190808995,-0.173648178,-0.156434465,-0.139173101,-0.121869343,-0.104528463,-0.087155743,
                                    -0.069756474,-0.052335956,-0.034899497,-0.017452406,0,0.017452406,0.034899497,0.052335956,0.069756474,0.087155743,
                                    0.104528463,0.121869343,0.139173101,0.156434465,0.173648178,0.190808995,0.207911691,0.224951054,0.241921896,0.258819045,
                                    0.275637356,0.292371705,0.309016994,0.325568154,0.342020143,0.35836795,0.374606593,0.390731128,0.406736643,0.422618262,
                                    0.438371147,0.4539905,0.469471563,0.48480962,0.5,0.515038075,0.529919264,0.544639035,0.559192903,0.573576436,0.587785252,
                                    0.601815023,0.615661475,0.629320391,0.64278761,0.656059029,0.669130606,0.68199836,0.69465837,0.707106781,0.7193398,
                                    0.731353702,0.743144825,0.75470958,0.766044443,0.777145961,0.788010754,0.79863551,0.809016994,0.819152044,
                                    0.829037573,0.838670568,0.848048096,0.857167301,0.866025404,0.874619707,0.882947593,0.891006524,0.898794046,
                                    0.906307787,0.913545458,0.920504853,0.927183855,0.933580426,0.939692621,0.945518576,0.951056516,0.956304756,
                                    0.961261696,0.965925826,0.970295726,0.974370065,0.978147601,0.981627183,0.984807753,0.987688341,0.990268069,0.992546152,
                                    0.994521895,0.996194698,0.99756405,0.998629535,0.999390827,0.999847695,1};

const float sin_look[] PROGMEM  = { 0.000000000,0.017452406,0.034899497,0.052335956,0.069756474,0.087155743,0.104528463,0.121869343,0.139173101,0.156434465,
                                    0.173648178,0.190808995,0.207911691,0.224951054,0.241921896,0.258819045,0.275637356,0.292371705,0.309016994,0.325568154,
                                    0.342020143,0.35836795,0.374606593,0.390731128,0.406736643,0.422618262,0.438371147,0.4539905,0.469471563,0.48480962,0.5,
                                    0.515038075,0.529919264,0.544639035,0.559192903,0.573576436,0.587785252,0.601815023,0.615661475,0.629320391,0.64278761,
                                    0.656059029,0.669130606,0.68199836,0.69465837,0.707106781,0.7193398,0.731353702,0.743144825,0.75470958,0.766044443,0.777145961,
                                    0.788010754,0.79863551,0.809016994,0.819152044,0.829037573,0.838670568,0.848048096,0.857167301,0.866025404,0.874619707,
                                    0.882947593,0.891006524,0.898794046,0.906307787,0.913545458,0.920504853,0.927183855,0.933580426,0.939692621,0.945518576,
                                    0.951056516,0.956304756,0.961261696,0.965925826,0.970295726,0.974370065,0.978147601,0.981627183,0.984807753,0.987688341,
                                    0.990268069,0.992546152,0.994521895,0.996194698,0.99756405,0.998629535,0.999390827,0.999847695,1,0.999847695,0.999390827,
                                    0.998629535,0.99756405,0.996194698,0.994521895,0.992546152,0.990268069,0.987688341,0.984807753,0.981627183,0.978147601,
                                    0.974370065,0.970295726,0.965925826,0.961261696,0.956304756,0.951056516,0.945518576,0.939692621,0.933580426,0.927183855,
                                    0.920504853,0.913545458,0.906307787,0.898794046,0.891006524,0.882947593,0.874619707,0.866025404,0.857167301,0.848048096,
                                    0.838670568,0.829037573,0.819152044,0.809016994,0.79863551,0.788010754,0.777145961,0.766044443,0.75470958,0.743144825,
                                    0.731353702,0.7193398,0.707106781,0.69465837,0.68199836,0.669130606,0.656059029,0.64278761,0.629320391,0.615661475,0.601815023,
                                    0.587785252,0.573576436,0.559192903,0.544639035,0.529919264,0.515038075,0.5,0.48480962,0.469471563,0.4539905,0.438371147,
                                    0.422618262,0.406736643,0.390731128,0.374606593,0.35836795,0.342020143,0.325568154,0.309016994,0.292371705,0.275637356,
                                    0.258819045,0.241921896,0.224951054,0.207911691,0.190808995,0.173648178,0.156434465,0.139173101,0.121869343,0.104528463,
                                    0.087155743,0.069756474,0.052335956,0.034899497,0.017452406,0.000000000,-0.017452406,-0.034899497,-0.052335956,-0.069756474,
                                    -0.087155743,-0.104528463,-0.121869343,-0.139173101,-0.156434465,-0.173648178,-0.190808995,-0.207911691,-0.224951054,
                                    -0.241921896,-0.258819045,-0.275637356,-0.292371705,-0.309016994,-0.325568154,-0.342020143,-0.35836795,-0.374606593,
                                    -0.390731128,-0.406736643,-0.422618262,-0.438371147,-0.4539905,-0.469471563,-0.48480962,-0.5,-0.515038075,-0.529919264,
                                    -0.544639035,-0.559192903,-0.573576436,-0.587785252,-0.601815023,-0.615661475,-0.629320391,-0.64278761,-0.656059029,
                                    -0.669130606,-0.68199836,-0.69465837,-0.707106781,-0.7193398,-0.731353702,-0.743144825,-0.75470958,-0.766044443,-0.777145961,
                                    -0.788010754,-0.79863551,-0.809016994,-0.819152044,-0.829037573,-0.838670568,-0.848048096,-0.857167301,-0.866025404,
                                    -0.874619707,-0.882947593,-0.891006524,-0.898794046,-0.906307787,-0.913545458,-0.920504853,-0.927183855,-0.933580426,
                                    -0.939692621,-0.945518576,-0.951056516,-0.956304756,-0.961261696,-0.965925826,-0.970295726,-0.974370065,-0.978147601,
                                    -0.981627183,-0.984807753,-0.987688341,-0.990268069,-0.992546152,-0.994521895,-0.996194698,-0.99756405,-0.998629535,
                                    -0.999390827,-0.999847695,-1,-0.999847695,-0.999390827,-0.998629535,-0.99756405,-0.996194698,-0.994521895,-0.992546152,
                                    -0.990268069,-0.987688341,-0.984807753,-0.981627183,-0.978147601,-0.974370065,-0.970295726,-0.965925826,-0.961261696,
                                    -0.956304756,-0.951056516,-0.945518576,-0.939692621,-0.933580426,-0.927183855,-0.920504853,-0.913545458,-0.906307787,
                                    -0.898794046,-0.891006524,-0.882947593,-0.874619707,-0.866025404,-0.857167301,-0.848048096,-0.838670568,-0.829037573,
                                    -0.819152044,-0.809016994,-0.79863551,-0.788010754,-0.777145961,-0.766044443,-0.75470958,-0.743144825,-0.731353702,
                                    -0.7193398,-0.707106781,-0.69465837,-0.68199836,-0.669130606,-0.656059029,-0.64278761,-0.629320391,-0.615661475,
                                    -0.601815023,-0.587785252,-0.573576436,-0.559192903,-0.544639035,-0.529919264,-0.515038075,-0.5,-0.48480962,-0.469471563,
                                    -0.4539905,-0.438371147,-0.422618262,-0.406736643,-0.390731128,-0.374606593,-0.35836795,-0.342020143,-0.325568154,-0.309016994,
                                    -0.292371705,-0.275637356,-0.258819045,-0.241921896,-0.224951054,-0.207911691,-0.190808995,-0.173648178,-0.156434465,
                                    -0.139173101,-0.121869343,-0.104528463,-0.087155743,-0.069756474,-0.052335956,-0.034899497,-0.017452406,-0.000000000};
*/

const point_3d sphere_points[] PROGMEM = { // VERTICES_LOCAL
                        {-0.80136517,0.582540191,0.135870491,0},
                        {-0.553520394,0.827494962,0.094166138,0},
                        {-0.713354232,-0.661677376,-0.230886964,0},
                        {-0.835666787,-0.154218295,-0.527141099,0},
                        {-0.563024286,-0.624559483,-0.541229254,0},
                        {0.327762969,-0.874259711,0.358108076,0},
                        {-0.468924739,0.530995407,0.705799877,0},
                        {0.269304658,-0.179168083,-0.946241935,0},
                        {0.099460024,0.864771646,0.49221713,0},
                        {-0.785579204,0.590415541,0.185134551,0},
                        {0.402747645,0.388599718,-0.828724679,0},
                        {-0.755503229,0.451847097,0.474393372,0},
                        {0.90718773,0.370656217,-0.19905876,0},
                        {-0.749009019,0.445672261,0.490266994,0},
                        {-0.096426665,0.216022417,-0.97161526,0},
                        {-0.88924459,0.447116411,-0.096596965,0},
                        {0.488215723,0.528590985,0.694432847,0},
                        {0.242529573,-0.105681744,-0.964370663,0},
                        {-0.931458449,-0.360071029,0.052287774,0},
                        {-0.378132239,0.915975585,0.134181732,0},
                        {-0.827717577,-0.412055505,0.380911898,0},
                        {-0.011347787,-0.064602308,-0.997846566,0},
                        {-0.834891097,0.127421433,-0.535463009,0},
                        {0.459724252,0.091806562,-0.883303554,0},
                        {-0.31704545,0.649710535,-0.690911284,0},
                        {-0.352747929,-0.842162069,0.407838139,0},
                        {0.912974496,0.000257543,-0.408016548,0},
                        {-0.994912418,0.076295468,-0.065789688,0},
                        {0.360232054,0.274648934,0.891516029,0},
                        {-0.810596026,0.563332659,-0.159969989,0},
                        {0.166013906,0.676282473,-0.717691717,0},
                        {0.78587603,0.291441741,-0.545399466,0},
                        {-0.175906546,0.838574167,0.515606685,0},
                        {-0.09676423,-0.39393243,0.914031687,0},
                        {0.240931853,-0.468446776,0.850005565,0},
                        {-0.48026995,-0.79724692,0.365702234,0},
                        {-0.38997826,-0.554595764,0.735078564,0},
                        {0.938242352,-0.137588794,-0.317443872,0},
                        {-0.721821201,-0.630020947,-0.286439803,0},
                        {0.444758397,0.384598276,-0.808872137,0},
                        {0.105117842,-0.775868415,-0.622075913,0},
                        {0.676525875,0.561769641,-0.476159229,0},
                        {-0.098772147,-0.346905117,-0.932684782,0},
                        {0.217987233,-0.762626406,0.609001257,0},
                        {0.60939375,-0.455121798,-0.649232937,0},
                        {-0.705430805,0.546312761,-0.451563669,0},
                        {-0.229519878,-0.003491752,0.973297711,0},
                        {-0.083780902,-0.994272747,-0.066351071,0},
                        {-0.45624127,-0.320262961,-0.830226197,0},
                        {-0.176084816,-0.939514923,0.293778567,0},
                        {-0.459892027,-0.238563258,-0.855328531,0},
                        {0.826036014,-0.404423778,0.392563259,0},
                        {-0.245538696,-0.963896783,-0.103023014,0},
                        {-0.922140394,0.266511964,-0.28040768,0},
                        {0.317154986,-0.369420363,-0.873465117,0},
                        {0.825606904,0.174765759,-0.53649806,0},
                        {0.084595086,-0.567511879,-0.819007899,0},
                        {-0.225313838,-0.09696049,-0.969449502,0},
                        {0.780297096,-0.091921371,0.618616928,0},
                        {-0.878646706,-0.058809924,-0.473836848,0},
                        {-0.097299161,0.943088839,-0.317987915,0},
                        {0.368910213,-0.665383513,-0.648976144,0},
                        {-0.345341452,0.906971482,0.241126546,0},
                        {-0.541678621,0.791623404,-0.282695345,0}


};






object sphere_ball = {
                      1, // ID
                      64, // NUM_VERTICES (sizeof?)
                      90, // SIZE
                      sphere_points, // VERTICES
                      {0.0,0.0,0.0,0.0} // WORLD POS
                    };



float angle_deg = 0.0;           // rotation around the Y axis
float eye_screen_distance = 2.0;     
float screen_origin_distance = 6.0;
//float cube_size = 90.0;           // cube size (multiplier)
float time_frame;                 // ever increasing time value

void setup() {

//  Serial.begin(9600); // opens serial port, sets data rate to 9600 bps

  u8g.setColorIndex(1); // set color to white

  // Set our ball to center of screen
  Translate_Object(&sphere_ball,64,32,0);
  

  //Build_Lookup_Tables();
}

void loop() {

  time_frame++;                                // increase the time frame value by 1
  //cube_size = 50 + sin(time_frame * 0.2)*20;   // oscilate cube size between values 30 - 70

  //z_offset =  -2.0;    // 
  //cube_size = 18.0;    // uncomment those two lines for a "wide angle camera" -- bigger perspective distort

  // increase the angle by 3° increments
  if (angle_deg < (float) 360-1) {
    angle_deg = angle_deg + 4.5;
  } else {
    angle_deg = 0;
  }

  // calculate the points
  /*
  for (int i=0; i<64; i++) {
    // rotate 3d points around the Y axis (rotating X nad Z positions)
    rotated_3d_points [i][0] = orig_points [i][0] * cos(radians(angle_deg)); 
    rotated_3d_points [i][0] -= orig_points [i][2] * sin(radians(angle_deg));
    rotated_3d_points [i][1] = orig_points [i][1];
    rotated_3d_points [i][2] = orig_points [i][0] * sin(radians(angle_deg));
    rotated_3d_points [i][2] += orig_points [i][2] * cos(radians(angle_deg)) + z_offset;  
  }
  */
/*
  for (int i=0; i<64; i++) {
      // project 3d points into 2d space with perspective divide -- 2D x = x/z,   2D y = y/z
    points[i][0] = round(64 + rotated_3d_points [i][0] / rotated_3d_points [i][2] * cube_size);
    points[i][1] = round(32 + rotated_3d_points [i][1] / rotated_3d_points [i][2] * cube_size);
  }
*/
  //delay(100);

  u8g.firstPage();
  do {

  //Serial.println(cos_look[angle_deg]);
  Rotate_And_Draw_Object(&sphere_ball, (int) angle_deg,(int) angle_deg, (int) angle_deg);
  //Draw_Object(&sphere_ball);
  //delay(5);
  } while ( u8g.nextPage() );   // u8g library specific, has to be there

}


//---- NEW CODE MATRIX COMPUTATIONS ETC.
void Mat_Identity_4x4(matrix_4x4 a)
{
  // this function creates a 4x4 identity matrix
/*
  a[0][1]=a[0][2]=a[0][3] = 0;
  a[1][0]=a[1][2]=a[1][3] = 0;
  a[2][0]=a[2][1]=a[2][3] = 0;
  a[3][0]=a[3][1]=a[3][2] = 0;
*/
  // faster
  memset((void*)a,0,sizeof(float)*16);
  // set main diagonal to 1's
  a[0][0]=a[1][1]=a[2][2]=a[3][3] = 1;
}

void Mat_Zero_4x4(matrix_4x4 a)
{
  // this function zeros out a 4x4 identity matrix
  a[0][0]=a[0][1]=a[0][2]=a[0][3] = 0;
  a[1][0]=a[1][1]=a[1][2]=a[1][3] = 0;
  a[2][0]=a[2][1]=a[2][2]=a[2][3] = 0;
  a[3][0]=a[3][1]=a[3][2]=a[3][3] = 0;
}

void Mat_Mul_4x4_4x4(matrix_4x4 a, matrix_4x4 b, matrix_4x4 result)
{
  // this function multiplies 4x4 matrix a with 4x4 b and stores the result in a 4x4
  
  /* SLOW ONE AND THERE IS A BUG SOMEWHERE ROTATE Y ONLY TWISTS?
  
  int index_i, index_j, index_k;
  float sum; //temp used to hold sum of products

  //Loop through rows of a
  for(index_i=0;index_i<4;index_i++)
  {
    //loop through columns of b
    for(index_j=0;index_j<4;index_j++)
    {
      sum=0;
      // multiply ith row of a by jth column of b ans store the sum in position i,j of result
      for(index_k=0; index_k<4; index_k++)
      {
        sum+=a[index_i][index_k]*b[index_k][index_j];
      }
      //store result
      result[index_i][index_j] = sum;
    }
  }
  */

  // first row
  result[0][0]=a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];
  result[0][1]=a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];
  result[0][2]=a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];
  result[0][3]=0;

 // second row
  result[1][0]=a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];
  result[1][1]=a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];
  result[1][2]=a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];
  result[1][3]=0;

   // third row
  result[2][0]=a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];
  result[2][1]=a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];
  result[2][2]=a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];
  result[2][3]=0;

   // fourth row
  result[3][0]=a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+b[3][0];
  result[3][1]=a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+b[3][1];
  result[3][2]=a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+b[3][2];
  result[3][3]=1;
}

void Translate_Object(object_ptr the_object, int x_trans, int y_trans, int z_trans)
{
  the_object->world_pos.x += x_trans;
  the_object->world_pos.y += y_trans;
  the_object->world_pos.z += z_trans;
}

/*
void Build_Lookup_Tables(void)
{
  int angle;
  float rad;

  for(angle=0;angle<=360;angle++)
  {
    rad = (float)(PI*(float)angle/(float)180.0);

    Serial.print((float)cos(rad));
    Serial.print(",");
    delay(50);
  }
}
*/

void Rotate_And_Draw_Object(object_ptr the_object, int angle_x, int angle_y, int angle_z)
{
/*  Serial.print("cos_look:[");
  Serial.print(angle_x);
  Serial.print("]=");
  Serial.print((float)pgm_read_float(&cos_look[angle_x]));
  Serial.print("sin:[");
  Serial.print(angle_x);
  Serial.print("]=");
  Serial.print((float)pgm_read_float(&sin_look[angle_x]));
  */
  //Serial.print((float)pgm_read_float(&cos_look[angle_x]));

  matrix_4x4 rotate_x, rotate_y, rotate_z, rotate, temp;
  
  int index;
  float temp_x,temp_y,temp_z,temp_x2,temp_y2;

  // test if we need to rotate at all
 // if(angle_x==0 && angle_y==0 && angle_z==0)
 //   return;

  // create identity matrices
  //Mat_Identity_4x4(rotate);
  Mat_Identity_4x4(rotate_x);

  

  // X

// NOTE!!!!!!!!!!!!!!!!!! The commented code below looses decimals when reading from flash (wrong function?) which occurs defects around Y-axis


 /*
  rotate_x[1][1] = (pgm_read_float_near(&cos_look[angle_x]));
  rotate_x[1][2] = (pgm_read_float_near(&sin_look[angle_x]));
  rotate_x[2][1] = (-pgm_read_float_near(&sin_look[angle_x]));
  rotate_x[2][2] = (pgm_read_float_near(cos_look[angle_x]));
  */


  rotate_x[1][1] = cos(radians(angle_x));
  rotate_x[1][2] = sin(radians(angle_x));
  rotate_x[2][1] = (-sin(radians(angle_x)));
  rotate_x[2][2] = cos(radians(angle_x));

 // Serial.print("[");
 // Serial.print((float)cos_look[angle_x]);
 // Serial.print("]");
  Mat_Identity_4x4(rotate_y);
  // Y
/*
  rotate_y[0][0] = (pgm_read_float_near(&cos_look[angle_y]));
  rotate_y[0][2] = (-pgm_read_float_near(&sin_look[angle_y]));
  rotate_y[2][0] = (pgm_read_float_near(&sin_look[angle_y]));
  rotate_y[2][2] = (pgm_read_float_near(&cos_look[angle_y]));
*/
  
 
  rotate_y[0][0] = cos(radians(angle_y));
  rotate_y[0][2] = (-sin(radians(angle_y)));
  rotate_y[2][0] = sin(radians(angle_y));
  rotate_y[2][2] = cos(radians(angle_y));

  

  //Multiply matrixes
  Mat_Mul_4x4_4x4(rotate_x,rotate_y,temp);

  Mat_Identity_4x4(rotate_z);
// Z
/*
  rotate_z[0][0] = (pgm_read_float_near(&cos_look[angle_z]));
  rotate_z[0][1] = (pgm_read_float_near(&sin_look[angle_z]));
  rotate_z[1][0] = (-pgm_read_float_near(&sin_look[angle_z]));
  rotate_z[1][1] = (pgm_read_float_near(&cos_look[angle_z]));
*/

  rotate_z[0][0] = cos(radians(angle_z));
  rotate_z[0][1] = sin(radians(angle_z));
  rotate_z[1][0] = (-sin(radians(angle_z)));
  rotate_z[1][1] = cos(radians(angle_z));


  Mat_Mul_4x4_4x4(temp,rotate_z,rotate);

  for(index=0;index<the_object->num_vertices;index++)
  {
    // x component
    temp_x =  the_object->vertices_local[index].x * rotate[0][0] +
              the_object->vertices_local[index].y * rotate[1][0] +
              the_object->vertices_local[index].z * rotate[2][0];
    
    // y component
    temp_y =  the_object->vertices_local[index].x * rotate[0][1] +
              the_object->vertices_local[index].y * rotate[1][1] +
              the_object->vertices_local[index].z * rotate[2][1];
    
    // x component
    temp_z =  the_object->vertices_local[index].x * rotate[0][2] +
              the_object->vertices_local[index].y * rotate[1][2] +
              the_object->vertices_local[index].z * rotate[2][2];

    // does drift over time due to small sinus rounding error
    /*
    the_object->vertices_local[index].x = temp_x;
    the_object->vertices_local[index].y = temp_y;
    the_object->vertices_local[index].z = temp_z;
    */
    temp_x2 = (float)(temp_x * the_object->size*eye_screen_distance) / (float)(temp_z + eye_screen_distance + screen_origin_distance);
    temp_y2 = (float)(temp_y * the_object->size*eye_screen_distance) / (float)(temp_z + eye_screen_distance + screen_origin_distance);
    

    // x component
    temp_x2 +=  the_object->world_pos.x;
    temp_y2 +=  the_object->world_pos.y;

/*
    Serial.print("[X:");
    Serial.print(temp_x);
    Serial.print(" Y:");
    Serial.print(temp_y);
    Serial.print(" Z:");
    Serial.print(temp_z);
    Serial.print("]");
*/
    if(temp_z > 0)
      u8g.drawPixel(round(temp_x2), round(temp_y2));



  /*  
    Serial.print(" X:");
    Serial.print(temp_x);
    Serial.print(" Y:");
    Serial.print(temp_y);
    Serial.print(" Z:");
    Serial.print(temp_z);
   */ 



  /*  
    Serial.print("ROT X:");
    Serial.print(temp_x);
    Serial.print(" Y:");
    Serial.print(temp_y);
    Serial.print(" Z:");
    Serial.print(temp_z);
*/

    //u8g.drawPixel(get2dx(temp_x, temp_z),
    //          get2dy(temp_y, temp_z));
  }

  

}

void Draw_Object(object_ptr the_object)
{
  
  int index;
  float temp_x,temp_y,temp_z;
  
  for(index=0;index<the_object->num_vertices;index++)
  {
    temp_x =  the_object->size*the_object->vertices_local[index].x * eye_screen_distance / (the_object->vertices_local[index].z + eye_screen_distance + screen_origin_distance);
    temp_y =  the_object->size*the_object->vertices_local[index].y * eye_screen_distance / (the_object->vertices_local[index].z + eye_screen_distance + screen_origin_distance);
    
    // x component
    temp_x +=  the_object->vertices_local[index].x + the_object->world_pos.x;
    temp_y +=  the_object->vertices_local[index].y + the_object->world_pos.y;
/*
    Serial.print("[X:");
    Serial.print(round(temp_x));
    Serial.print(" Y:");
    Serial.print(round(temp_y));
   // Serial.print(" Z:");
   // Serial.print(temp_z);
    Serial.print("]");
*/
    //if(the_object->vertices_local[index].z > 0)
      u8g.drawPixel(round(temp_x), round(temp_y));
  }

  

}