#include <Arduino.h>

#include "SPI.h"
#include <Arduino_GFX_Library.h>
#include <U8g2lib.h>

#include <LittleFS.h>
#include <SD.h>

#include "JpegFunc.h"

extern "C" {
#include "hsv2rgb.h"
#include "sin1.h"
}
#include <Adafruit_Sensor.h>
#include <Adafruit_MMC56x3.h>
Adafruit_MMC5603 mag = Adafruit_MMC5603(12345);

float MagMinX, MagMaxX;
float MagMinY, MagMaxY;
float MagMinZ, MagMaxZ;

long lastDisplayTime;

#include <RDV_GY512.h>
GY_512 gy512 = GY_512();

int16_t accel_x, accel_y;
int16_t gyro_x, gyro_y;
// Decay factor
const float decay = 0.7;
const float decay_mag = 0.2;
int16_t raw_accel_x, raw_accel_y;
int16_t raw_gyro_x, raw_gyro_y;


#define TIMER_INTERRUPT_DEBUG         1
#define _TIMERINTERRUPT_LOGLEVEL_     4

#define OLD_ADDRESS 0x30
#define NEW_ADDRESS 0x42


// Can be included as many times as necessary, without `Multiple Definitions` Linker Error
#include "RPi_Pico_TimerInterrupt.h"

#ifndef LED_BUILTIN
  #define LED_BUILTIN       25
#endif

#define PIN_D1              1         // Pin D1 mapped to pin GPIO1 of RPI_PICO
bool TimerHandler0(struct repeating_timer *t)
{
    // Flash the LED on and off to prove timers are working
    (void) t;

    static bool toggle0 = false;
    /* #if (TIMER_INTERRUPT_DEBUG > 0)
        Serial.print("ITimer0: millis() = "); Serial.println(millis());
    #endif */

    //timer interrupt toggles pin LED_BUILTIN
    digitalWrite(LED_BUILTIN, toggle0);
    toggle0 = !toggle0;
    
    return true;
}

bool TimerHandler1(struct repeating_timer *t)
{
    // Flash the LED on and off to prove timers are working

    (void) t;

    static bool toggle1 = false;

    /* #if (TIMER_INTERRUPT_DEBUG > 0)
        Serial.print("ITimer1: millis() = "); Serial.println(millis());
    #endif */

    //timer interrupt toggles outputPin
    digitalWrite(PIN_D1, toggle1);
    toggle1 = !toggle1;

    return true;
}
bool TimerHandler2(struct repeating_timer *t)
{
    // Check the gyro/accel sensor in the background

    (void) t;
    // #if (TIMER_INTERRUPT_DEBUG > 0)
    //     Serial.print("ITimer2: millis() = "); Serial.println(millis());
    // #endif
    // Get the current acceleration values
    raw_accel_x = gy512.get_raw_accel_x();
    raw_accel_y = gy512.get_raw_accel_y();

    // Normalize the acceleration values over 100ms
    accel_x = (accel_x * decay) + ((1.0 - decay) * (raw_accel_x / 100.0));
    accel_y = (accel_y * decay) + ((1.0 - decay) * (raw_accel_y / 100.0));

    raw_gyro_x = gy512.get_raw_gyro_x();
    raw_gyro_y = gy512.get_raw_gyro_y();

    // Normalize the change in values values over 100ms
    gyro_x = (gyro_x * decay) + ((1.0 - decay) * (raw_gyro_x / 100.0));
    gyro_y = (gyro_y * decay) + ((1.0 - decay) * (raw_gyro_y / 100.0));
    //Serial.printf("- ax: %i; ay: %i; az: %i\r\n",     gy512.get_raw_accel_x(), gy512.get_raw_accel_y(), gy512.get_raw_accel_z());
    //Serial.printf("- gx: %i; gy: %i; gz: %i\r\n\r\n", gy512.get_raw_gyro_x(),  gy512.get_raw_gyro_y(),  gy512.get_raw_gyro_z());
    return true;
}

float heading;
float raw_heading;
float Pi = 3.14159;

bool TimerHandler3(struct repeating_timer *t)
{
    // Get the  magnetoscope sensor information in the background
    sensors_event_t event;
    mag.getEvent(&event);

    /*/ Display the results (magnetic vector values are in micro-Tesla (uT))
    Serial.print("X: ");
    Serial.print(event.magnetic.x);
    Serial.print("  ");
    Serial.print("Y: ");
    Serial.print(event.magnetic.y);
    Serial.print("  ");
    Serial.print("Z: ");
    Serial.print(event.magnetic.z);
    Serial.print("  ");
    Serial.println("uT");*/

    // Calculate the angle of the vector y,x
    raw_heading = (atan2(event.magnetic.y,event.magnetic.z) * 180) / Pi;

    // Normalize to 0-360
    if (raw_heading < 0)
    {
        raw_heading = 360 + raw_heading;
    }
    heading = (raw_heading * decay_mag) + ((1.0 - decay_mag) * (raw_heading / 100.0)) - 70;

    Serial.print("Compass Heading: ");
    Serial.println(heading);
    return true;
}

#define TIMER0_INTERVAL_MS 1000

#define TIMER1_INTERVAL_MS 5000

RPI_PICO_Timer ITimer0(0);
RPI_PICO_Timer ITimer1(1);
RPI_PICO_Timer ITimer2(2);
RPI_PICO_Timer ITimer3(3);


const char name[] = "Your Name";

#define SPI0_SCK		2
#define SPI0_MISO		GFX_NOT_DEFINED
#define SPI0_MOSI		3

#define DISP_NSS		1
#define DISP_RESET  	4
#define DISP_DC     	5

#define TEXT_SIZE       5
#define BORDER_COLOR    DARKGREY
#define ROT_SIZE        70

#define LETTER_SPACING  1942    // user-configurable


Arduino_DataBus *bus = new Arduino_RPiPicoSPI(DISP_DC, DISP_NSS, SPI0_SCK, SPI0_MOSI, SPI0_MISO, spi0);
Arduino_GFX *disp = new Arduino_GC9A01(bus, DISP_RESET, 0 /* rotation */, true /* IPS */);

Arduino_Canvas *dout  = new Arduino_Canvas(240 /* width */, 240 /* height */, disp);
Arduino_Canvas *drot  = new Arduino_Canvas(ROT_SIZE, ROT_SIZE, disp);


color_hsv hsv = {0, 255, 255};
color_rgb rgb;


void rotate_letter(uint8_t idx, int16_t rot, int16_t out_x, int16_t out_y, uint16_t color)
{
    int16_t x, y;
    int16_t nx, ny;
    int16_t sx, sy;

    int16_t start_x, start_y;

    int16_t x_width;

    uint16_t *src, *dst;
    uint16_t src_pxl;
    uint16_t bgcolor = CYAN;

    uint8_t is_color;

    // clear the rotation buffer
    drot->fillScreen(CYAN);

    // print the letter to the center of the rotation buffer
    drot->setCursor(1, (TEXT_SIZE * 10));
    drot->setTextColor(color);
    drot->print(name[idx]);
    x_width = drot->getCursorX() - TEXT_SIZE + 1;

    // framebuffer pointers
    src = drot->getFramebuffer();
    dst = dout->getFramebuffer();

    // naive bordering
    for (y = 0; y < ROT_SIZE; y++) {
        is_color = 0;
        for (x = 0; x < x_width; x++) {
            if (is_color) {
                if (src[x + (y * ROT_SIZE)] != color) {
                    is_color = 0;
                    src[x + (y * ROT_SIZE)] = BORDER_COLOR;
                }
            } else {
                if (src[x + (y * ROT_SIZE)] == color) {
                    is_color = 1;
                    src[x + (y * ROT_SIZE) - 1] = BORDER_COLOR;
                }
            }
        }
    }

    for (x = 0; x < x_width; x++) {
        is_color = 0;
        for (y = 0; y < ROT_SIZE; y++) {
            if (is_color) {
                if (src[x + (y * ROT_SIZE)] != color) {
                    is_color = 0;
                    src[x + (y * ROT_SIZE)] = BORDER_COLOR;
                }
            } else {
                if (src[x + (y * ROT_SIZE)] == color) {
                    is_color = 1;
                    src[x + (y * ROT_SIZE) - ROT_SIZE] = BORDER_COLOR;
                }
            }
        }
    }

    // position
    start_x = out_x + 120;
    start_y = out_y + 120;

    // rotate+copy the buffer
    for (y = 0; y < ROT_SIZE; y++) {
		for (x = 0; x < x_width; x++) {
            src_pxl = src[x + (y * ROT_SIZE)];
            sx = x                + 1 - (x_width >> 1);
            sy = y - (ROT_SIZE/2) + 1;
			if (src_pxl != bgcolor) {
				nx = ((sx * cos1(rot)) - (sy * sin1(rot))) >> 15;
				ny = ((sx * sin1(rot)) + (sy * cos1(rot))) >> 15;

				nx += start_x;
                ny += start_y;
                
                if ((nx >= 0) && (nx < 240)) {
					if ((ny >= 0) && (ny < 240)) {
						dst[nx + (ny*240)] = src_pxl;
					}
				}
			}
		}
	}
}

int16_t rot = 0;

#define HUE_MIN     0x2000-300
#define HUE_MAX     0x2000+300
#define HUE_STEP    3

int16_t huecycle = 0;
uint8_t huedir = 0;
int16_t satcycle = 0;
uint8_t satdir = 0;

void bg_huesatcycle()
{
    dout->fillScreen(BLACK);

    // hue between limits
    if (!huedir) huecycle += HUE_STEP; else huecycle -= HUE_STEP;
    if (huecycle > HUE_MAX) {
        huedir = 1; huecycle = HUE_MAX;
    }
    if (huecycle < HUE_MIN) {
        huedir = 0; huecycle = HUE_MIN;
    }

    // saturation
    if (!satdir) satcycle++; else satcycle--;
    if (satcycle > 240) {
        satdir = 1;
        satcycle = 240;
    }
    if (satcycle < 20) {
        satdir = 0;
        satcycle = 60;
    }

    hsv.h = huecycle >> 3;
    hsv.s = satcycle;

    // draw some circles
    for (int8_t i = 120; i > 0; i--) {
        hsv.h += 2;
        hsv.h %= 1536;
        hsv2rgb_8b(hsv.h, hsv.s, (i * 2), &rgb.r, &rgb.g, &rgb.b);

        dout->drawCircle(120, 120, i, dout->color565(rgb.r, rgb.g, rgb.b));
    }
}

void bg_rainbow_circles(uint8_t saturation)
{
    uint16_t hue;

    // speed
    hsv.h += 24;

    // background fill ("X" pattern)
    // hue = hsv.h + 512;
    // hsv.h %= 1536;
    // hsv2rgb_8b(hue, 255, 128, &rgb.r, &rgb.g, &rgb.b);
    // dout->fillScreen(dout->color565(rgb.r, rgb.g, rgb.b));
    dout->fillScreen(BLACK);

    // draw some circles
    hue = hsv.h;
    for (int8_t i = 120; i > 0; i--) {
        hue += 2;
        hue %= 1536;
        
        // "flat" mode, same brightness
        // hsv2rgb_8b(hue, saturation, 192, &rgb.r, &rgb.g, &rgb.b);
        
        // "tunnel" mode, darker center
        hsv2rgb_8b(hue, saturation, (i * 2), &rgb.r, &rgb.g, &rgb.b);

        dout->drawCircle(120, 120, i, dout->color565(rgb.r, rgb.g, rgb.b));
    }
}

void texthue_180degree()
{
    uint16_t hue;

    hue = hsv.h + 768;
    hue %= 1536;
    hsv2rgb_8b(hue, 224, 64, &rgb.r, &rgb.g, &rgb.b);
}

void texthue_huesatcycle_circles()
{
    uint16_t hue;
    
    hue = (huecycle - 0x2000);
    if (hue > (1536 << 3)) hue += (1536 << 3);
    hsv2rgb_8b(hue >> 3, 224, 64, &rgb.r, &rgb.g, &rgb.b);
}

// pixel drawing callback
static int jpeg_draw_cb(JPEGDRAW *pDraw)
{
    // Serial.printf("Draw pos = %d,%d. size = %d x %d\n", pDraw->x, pDraw->y, pDraw->iWidth, pDraw->iHeight);
    dout->draw16bitBeRGBBitmap(pDraw->x, pDraw->y, pDraw->pPixels, pDraw->iWidth, pDraw->iHeight);
    return 1;
}

void setup()
{
    Serial.begin(115200);

    Serial.println("starting...");

    delay(50);

    Wire.beginTransmission(OLD_ADDRESS);
    Wire.write(NEW_ADDRESS);
    Wire.endTransmission();

    bus->begin(90000000);

    if (!disp->begin()) {
        Serial.println("failed to start disp");
    }

    dout->begin();
    dout->fillScreen(BLACK);
    dout->flush();

    drot->begin();
    drot->setFont(u8g2_font_cubic11_h_cjk);     // u8g2_font_spleen16x32_mr
    drot->setTextSize(TEXT_SIZE);

    // filesystem begin
    LittleFS.begin();
    pinMode(LED_BUILTIN, OUTPUT);
    pinMode(PIN_D1,      OUTPUT);


    Serial.print(F("\nStarting Argument_None on ")); Serial.println(BOARD_NAME);
    Serial.println(RPI_PICO_TIMER_INTERRUPT_VERSION);
    Serial.print(F("CPU Frequency = ")); Serial.print(F_CPU / 1000000); Serial.println(F(" MHz"));

    // Interval in microsecs
    if (ITimer0.attachInterruptInterval(TIMER0_INTERVAL_MS * 1000, TimerHandler0))
    {
        Serial.print(F("Starting ITimer0 OK, millis() = ")); Serial.println(millis());
    }
    else 
    {
        Serial.println(F("Can't set ITimer0. Select another Timer, freq. or timer"));
    }


    // Interval in microsecs
    if (ITimer1.attachInterruptInterval(TIMER1_INTERVAL_MS * 1000, TimerHandler1))
    {
        Serial.print(F("Starting ITimer1 OK, millis() = ")); Serial.println(millis());
    }
    else
    {
        Serial.println(F("Can't set ITimer1. Select another Timer, freq. or timer"));
    }
    // Interval in microsecs
    if (ITimer2.attachInterruptInterval(TIMER1_INTERVAL_MS * 10, TimerHandler2))
    {
        Serial.print(F("Starting ITimer2 GyroAccelTemp OK, millis() = ")); Serial.println(millis());
    }
    else
    {
        Serial.println(F("Can't set ITimer2 GyroAccelTemp. Select another Timer, freq. or timer"));
    }
    // Interval in microsecs
    if (ITimer3.attachInterruptInterval(TIMER1_INTERVAL_MS * 100, TimerHandler3))
    {
        Serial.print(F("Starting ITimer3 Mag OK, millis() = ")); Serial.println(millis());
    }
    else
    {
        Serial.println(F("Can't set ITimer2 GyroAccelTemp. Select another Timer, freq. or timer"));
    }
    Serial.println("Adafruit_MMC5603 Magnetometer Calibration");
    Serial.println("");

    /* Initialise the sensor */
    if (!mag.begin(MMC56X3_DEFAULT_ADDRESS, &Wire1)) {  // I2C mode
        /* There was a problem detecting the MMC5603 ... check your connections */
        Serial.println("Ooops, no MMC5603 detected ... Check your wiring!");
    }

    /* Display some basic information on this sensor */
    mag.printSensorDetails();

    mag.setDataRate(10); // in Hz, from 1-255 or 1000
    mag.setContinuousMode(true);

    lastDisplayTime = millis();
    MagMinX = MagMaxX = MagMinY = MagMaxY = MagMinZ = MagMaxZ = 0;
}

void loop()
{
    uint16_t hue;
    Serial.println("wire pins");
    Wire1.setSDA(26);
    Wire1.setSCL(27);
    
    Serial.println("after wire pins");
    Serial.println("before init");
    gy512.sensor_init(); // initializes device for communication
    Serial.println("after init"); 
    char fn[20];
    
    
    while (true) {
        for (int i = 0; i <= 99; i++) {
            sprintf(fn, "out00%03i.jpg", i);
            //Serial.println(fn);
            
            dout->fillScreen(BLACK);
            jpegDraw(fn, jpeg_draw_cb, true /* useBigEndian */,
                accel_x + heading * 2 /* x */, accel_y + heading * 2/* y */, dout->width() /* widthLimit */, dout->height() /* heightLimit */);
            dout->flush();
            /* Get a new sensor event */
            
            
        }
    }
    

    dout->flush();

}