#include <SD.h>
#include <adafruit_1947_Obj.h>
#include <idlers.h>
#include <screen.h>
#include <label.h>
#include <mapper.h>

//#include "threeDTest.h"
#include <triDBase.h>
#include <triDVector.h>
#include <arrayList.h>
#include <facetList.h>
#include <stlFile.h>
#include <stlList.h>
#include <triDRender.h>

label*      ourLabel;
colorObj    backColor;
mapper      rawToRadians(0,1023,0,2*PI);
mapper      radToDegree(-2*PI,2*PI,-360,360);
triDRotation new_angle;      // This sets the oreantation of the model's drwaing on the screen.
triDRotation my_angle;
triDPoint    location;   // This sets the location of the model's drawing on the screen.
stlList*     STLModel;   // A model is a list of triangles in 3 space "facets" with normal vectors pointing out.
triDRender*  renderMan;  // triDRender is the engine that reads the model, the prarmeters and makes the 2D drawing.
rect         blankRect;

const byte ANGLE_X_POT_PIN = A0;
const byte ANGLE_Y_POT_PIN = A1;
const byte ANGLE_Z_POT_PIN = A2;
  
void setupModel(void) {

  STLModel = new stlList("/Test3D.STL");       // Small text model. Like crude boat.
   //STLModel = new stlList("/teensyM.STL");    // Big model, maybe 20 sec. to render?
   //STLModel = new stlList("/hemi.STL");       // Hemisphere. Another big model. About 1 minute to render.
   //renderMan = new triDRender(20,100,180,180);  // Create the render engine with its screen location. (x,y,w,h)
   renderMan = new triDRender(0, 0, 180, 180);  // Create the render engine with its screen location. (x,y,w,h)
   renderMan->begin(STLModel);                  // Slot the model into the render enegine.
   renderMan->setObjScale(5);                   // Set the drawing scale.
   //angle.xRad = deg_2_rad(-150);               // The angle around x axis.
   new_angle.xRad = 0;                           // The angle around x axis.
   new_angle.yRad = 0;                           // The angle around y axis.
   new_angle.zRad = 0;                           // The angle around z axis.
   renderMan->setObjAngle(&new_angle);            // Set the orentation.
   my_angle = new_angle;
   location.x = 0;                              // x position offset.
   location.y = 0;                              // y position offset.
   location.z = 30;                             // z position offset.
   renderMan->setObjLoc(&location);             // Set the location offsets in..
   viewList.addObj(renderMan);                  // Add this render object into the screen list. (causing it to draw itself)
}

void setup() {
   
   Serial.begin(115200);                                          // Fire up serial for debugging.
   Serial.println("We begin..");
   if (!initScreen(ADAFRUIT_1947,ADA_1947_SHIELD_CS,PORTRAIT)) {  // Init screen.
      Serial.println("NO SCREEN!");                               // Screen init failed. Tell user.
      Serial.flush();                                             // Make sure the message gets out.
      while(true);                                                // Lock the process here.
   }
   if (!SD.begin(ADA_1947_SHIELD_SDCS)) {                         // With icons, we now MUST have an SD card.
      Serial.println("NO SD CARD!");                              // Tell user we have no SD card.
      Serial.flush();                                             // Make sure the message gets out.
      while(true);                                                // Lock the process here.
   }
   backColor.setColor(LC_LIGHT_BLUE);
   screen->fillScreen(&backColor);                                    // Lets set the screen to the back color.
   setupModel();                                                  // This fires up the 3D rendering stuff. BUT seeing
   Serial.println("The end of the beginning.");
}


// void PrintAngle(const char xyz, const uint16_t raw, const float deg, const float rad) {
//    Serial.print(xyz);
//    Serial.print(": ");
//    Serial.print(raw);
//    Serial.print("\t");
//    Serial.print(deg);
//    Serial.print("\t");
//    Serial.print(rad);
//    Serial.print("\t\t");
// }

bool angleChange(void) {

   if (new_angle.xRad!=my_angle.xRad) return true;
   if (new_angle.yRad!=my_angle.yRad) return true;
   if (new_angle.zRad!=my_angle.zRad) return true;
   return false;
}

void loop()
{
   idle();
   new_angle.xRad = rawToRadians.map(analogRead(ANGLE_X_POT_PIN));
   new_angle.yRad = rawToRadians.map(analogRead(ANGLE_Y_POT_PIN));
   new_angle.zRad = rawToRadians.map(analogRead(ANGLE_Z_POT_PIN));
   if (angleChange()) {
      renderMan->setObjAngle(&new_angle);
      blankRect = renderMan->getLastRect();
      screen->fillRect(renderMan,&backColor);
      my_angle = new_angle;
   }
}