/* LucidTronix color compass 
 * Rainbow colored wearable navigator 
 * For instructions, details and schematic, See:
 * http://www.lucidtronix.com/tutorials/30
 * Uses the HMC5883L.h library from bildr
 */
#include <color.h>
#include <ST7735.h>
#include <SPI.h>

#define cs 10   // for MEGAs you probably want this to be pin 53
#define dc 9
#define rst 8  // you can also connect this to the Arduino reset

ST7735 tft = ST7735(cs, dc, rst);

// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>

// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

int btnPin0 = 0;
int btnPin1 = 1;

int mode = 0;
int num_modes = 4;

unsigned int last_press = 0;

int background_color = 0xFF;
int color = 0xFF;

int backlight_pwm_pin = 3;
char* stringy = "nothinghere";
void setup(){
  pinMode(btnPin1,INPUT);
  pinMode(btnPin0,INPUT);
  tft.initR();               // initialize a ST7735R chip
  tft.writecommand(ST7735_DISPON); 
  delay(1000);
  tft.fillScreen(BLACK);
  tft.drawString(4, 4, "Starting up...", CYAN,1);
  Wire.begin(); // Start the I2C interface.
  compass = HMC5883L(); // Construct a new HMC5883 compass.
  float scalar = 4.0;
  //not checking errors you should if you have issues.
  error = compass.SetScale(scalar); // Set the scale of the compass.
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  delay(500);
}
void loop(){
  if ( digitalRead(btnPin0) == HIGH)tft.fillScreen(WHITE);
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  // Values are accessed like so:
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(scaled.YAxis, scaled.XAxis);
  float declinationAngle = 0.0457;
  heading += declinationAngle;
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
  float headingDegrees = heading * 180/M_PI; 
  draw_ray(64,80, headingDegrees, 40);
  draw_orbiter(64,80, headingDegrees, 22,6);
  draw_orbiter(64,80, headingDegrees, 32,3);
}

void draw_ray(int ax, int ay, int angle, int radius ){
  float ray_x = cos(((float)angle*PI)/180.0f)*(float)radius;
  float ray_y = sin(((float)angle*PI)/180.0f)*(float)radius;
  int c = get_color_from_angle(angle);
  tft.drawLine(ax,ay,ax+ray_x, ay+ray_y, c);
}

void draw_orbiter(int ax, int ay, int angle, int radius, int a_size){
  float ray_x = cos(((float)angle*PI)/180.0f)*(float)radius;
  float ray_y = sin(((float)angle*PI)/180.0f)*(float)radius;
  int c = get_color_from_angle(angle);
  tft.fillCircle(ax+ray_x, ay+ray_y, a_size, c);
}

int get_color_from_angle(int angle){
  Color cur_color=Color(0,0,0);
  cur_color.convert_hcl_to_rgb((float)angle/360,0.95,0.7);
  return cur_color.get_color_16bit(); 
}