/**
 * V. Hunter Adams (vha3@cornell.edu)
 * 
 * This demonstration utilizes the MPU6050.
 * It gathers raw accelerometer/gyro measurements, scales
 * them, and plots them to the VGA display. The top plot
 * shows gyro measurements, bottom plot shows accelerometer
 * measurements.
 * 
 * HARDWARE CONNECTIONS
 *  - GPIO 16 ---> VGA Hsync
 *  - GPIO 17 ---> VGA Vsync
 *  - GPIO 18 ---> 470 ohm resistor ---> VGA Green
 *  - GPIO 19 ---> 330 ohm resistor ---> VGA Green
 *  - GPIO 20 ---> 330 ohm resistor ---> VGA Blue
 *  - GPIO 21 ---> 330 ohm resistor ---> VGA Red
 *  - RP2040 GND ---> VGA GND
 *  - GPIO 8 ---> MPU6050 SDA
 *  - GPIO 9 ---> MPU6050 SCL
 *  - 3.3v ---> MPU6050 VCC
 *  - RP2040 GND ---> MPU6050 GND
 */


// Include standard libraries
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
// Include PICO libraries
#include "pico/stdlib.h"
#include "pico/multicore.h"
// Include hardware libraries
#include "hardware/pwm.h"
#include "hardware/dma.h"
#include "hardware/irq.h"
#include "hardware/adc.h"
#include "hardware/pio.h"
#include "hardware/i2c.h"
#include "hardware/clocks.h"
#include "hardware/sync.h"
// Include custom libraries
#include "vga16_graphics_v3.h"
//#include "mpu6050.h"
#include "pt_cornell_rp2040_v1_4.h"

// We will start drawing at column 81
#define LEFT_EDGE 81 

// Rescale the measurements for display
#define OLD_RANGE 500.  // (+/- 250)
#define NEW_RANGE 150.  // (looks nice on VGA)
#define OLD_MIN -250. 
#define OLD_MAX 250. 
#define ACCEL_SCALE 120.0

// Place the plots
#define TOP_PLOT_BOTTOM 230
#define BOTTOM_PLOT_BOTTOM 430
#define PLOT_WIDTH 530

// Arrays in which raw measurements will be stored
fix15 acceleration[3], gyro[3];

// Arrays for circular display buffer
fix15 ax[PLOT_WIDTH] ;
fix15 ay[PLOT_WIDTH] ;
fix15 az[PLOT_WIDTH] ;
fix15 gx[PLOT_WIDTH] ;
fix15 gy[PLOT_WIDTH] ;
fix15 gz[PLOT_WIDTH] ;

// Pointer to head of buffer
volatile int head = 0 ;

// character array
char screentext[40];

// draw speed
volatile int threshold = 10 ;

// Some macros for max/min/abs
#define min(a,b) ((a<b) ? a:b)
#define max(a,b) ((a<b) ? b:a)
#define abs(a) ((a>0) ? a:-a)

// semaphore
static struct pt_sem vga_semaphore ;

// Some paramters for PWM
#define WRAPVAL 6000
#define CLKDIV  25.0
uint slice_num ;


void drawAxes() {
    // Draw the static aspects of the display
    setTextSize(1) ;
    setTextColor(WHITE);

    // Draw bottom plot
    drawHLine(75, 430, 5, CYAN) ;
    drawHLine(75, 355, 5, CYAN) ;
    drawHLine(75, 280, 5, CYAN) ;
    drawVLine(80, 280, 150, CYAN) ;
    sprintf(screentext, "0") ;
    setCursor(50, 350) ;
    writeString(screentext) ;
    sprintf(screentext, "+2") ;
    setCursor(50, 280) ;
    writeString(screentext) ;
    sprintf(screentext, "-2") ;
    setCursor(50, 425) ;
    writeString(screentext) ;

    // Draw top plot
    drawHLine(75, 230, 5, CYAN) ;
    drawHLine(75, 155, 5, CYAN) ;
    drawHLine(75, 80, 5, CYAN) ;
    drawVLine(80, 80, 150, CYAN) ;
    sprintf(screentext, "0") ;
    setCursor(50, 150) ;
    writeString(screentext) ;
    sprintf(screentext, "+250") ;
    setCursor(45, 75) ;
    writeString(screentext) ;
    sprintf(screentext, "-250") ;
    setCursor(45, 225) ;
    writeString(screentext) ;
}

volatile int throttle = 10 ;

// Interrupt service routine
// void on_pwm_wrap() {

//     // Clear the interrupt flag that brought us here
//     pwm_clear_irq(pwm_gpio_to_slice_num(5));
//     gpio_put(14, !gpio_get(14)) ;

    // // Read the IMU
    // // NOTE! This is in 15.16 fixed point. Accel in g's, gyro in deg/s
    // // If you want these values in floating point, call fix2float15() on
    // // the raw measurements.
    // mpu6050_read_raw(acceleration, gyro);

    // // Increment drawspeed controller
    // throttle += 1 ;
    // // Maintain a circular buffer for display
    // if ((throttle >= threshold)) { 
    //     // Zero drawspeed controller
    //     throttle = 0 ;

    //     // Replace oldest data in accel arrays with new sample
    //     ax[head] = acceleration[0] ;
    //     ay[head] = acceleration[1] ;
    //     az[head] = acceleration[2] ;

    //     // Replace oldest data in gyro arrays with new sample
    //     gx[head] = gyro[0] ;
    //     gy[head] = gyro[1] ;
    //     gz[head] = gyro[2] ;

    //     // Move the pointer to the oldest data
    //     head = ((head+1) == PLOT_WIDTH) ? 0 : (head + 1) ;

    // }
// }

static PT_THREAD (protothread_draw(struct pt *pt))
{
    PT_BEGIN(pt) ;

    static int start ;
    static int i ;

    drawAxes() ;
    copy_buffer_to_other() ;

    while(1) {
        // Wait for a buffer swap
        PT_YIELD_UNTIL(pt, draw_start_signal()) ;
        // reset_start_signal() ;
        gpio_put(15, !gpio_get(15)) ;
        // clearLowFrame(0, BLACK) ;
        // drawAxes() ;

        // // Plot data oldest-->newest
        // start = head ;
        // for (i = 0; i < PLOT_WIDTH; i++) {
        //     drawPixel(LEFT_EDGE + i, (BOTTOM_PLOT_BOTTOM -
        //         (int)(NEW_RANGE*((float)((fix2float15(ax[start])*ACCEL_SCALE)-OLD_MIN)/OLD_RANGE))), WHITE) ;
        //     drawPixel(LEFT_EDGE + i, (BOTTOM_PLOT_BOTTOM -
        //         (int)(NEW_RANGE*((float)((fix2float15(ay[start])*ACCEL_SCALE)-OLD_MIN)/OLD_RANGE))), RED) ;
        //     drawPixel(LEFT_EDGE + i, (BOTTOM_PLOT_BOTTOM -
        //         (int)(NEW_RANGE*((float)((fix2float15(az[start])*ACCEL_SCALE)-OLD_MIN)/OLD_RANGE))), GREEN) ;
        //     drawPixel(LEFT_EDGE + i, (TOP_PLOT_BOTTOM -
        //         (int)(NEW_RANGE*((float)((fix2float15(gx[start]))-OLD_MIN)/OLD_RANGE))), WHITE) ;
        //     drawPixel(LEFT_EDGE + i, (TOP_PLOT_BOTTOM -
        //         (int)(NEW_RANGE*((float)((fix2float15(gy[start]))-OLD_MIN)/OLD_RANGE))), RED) ;
        //     drawPixel(LEFT_EDGE + i, (TOP_PLOT_BOTTOM -
        //         (int)(NEW_RANGE*((float)((fix2float15(gz[start]))-OLD_MIN)/OLD_RANGE))), GREEN) ;
        //     start = ((start+1)==PLOT_WIDTH) ? 0 : (start+1) ;
        // }
    }

    PT_END(pt) ;
}

// User input thread. User can change draw speed
static PT_THREAD (protothread_serial(struct pt *pt))
{
    PT_BEGIN(pt) ;
    static char classifier ;
    static int test_in ;
    static float float_in ;
    while(1) {
        sprintf(pt_serial_out_buffer, "input a command: ");
        serial_write ;
        // spawn a thread to do the non-blocking serial read
        serial_read ;
        // convert input string to number
        sscanf(pt_serial_in_buffer,"%c", &classifier) ;

        // num_independents = test_in ;
        if (classifier=='t') {
            sprintf(pt_serial_out_buffer, "timestep: ");
            serial_write ;
            serial_read ;
            // convert input string to number
            sscanf(pt_serial_in_buffer,"%d", &test_in) ;
            if (test_in > 0) {
                threshold = test_in ;
            }
        }
    }
    PT_END(pt) ;
}

// Entry point for core 1
// void core1_entry() {
//     gpio_init(15) ;
//     gpio_set_dir(15, GPIO_OUT) ;
//     gpio_put(15, 0) ;
//     // pt_add_thread(protothread_buffer) ;
//     pt_add_thread(protothread_draw) ;
//     pt_schedule_start ;
// }

int main() {

    // Overclock
    set_sys_clock_khz(150000, true) ;

    // Initialize stdio
    stdio_init_all();

    // Initialize VGA
    initVGA() ;

    ////////////////////////////////////////////////////////////////////////
    ///////////////////////// I2C CONFIGURATION ////////////////////////////
    // i2c_init(I2C_CHAN, I2C_BAUD_RATE) ;
    // gpio_set_function(SDA_PIN, GPIO_FUNC_I2C) ;
    // gpio_set_function(SCL_PIN, GPIO_FUNC_I2C) ;

    // // MPU6050 initialization
    // mpu6050_reset();
    // mpu6050_read_raw(acceleration, gyro);

    ////////////////////////////////////////////////////////////////////////
    ///////////////////////// PWM CONFIGURATION ////////////////////////////
    ////////////////////////////////////////////////////////////////////////
    // // Tell GPIO's 4,5 that they allocated to the PWM
    // gpio_set_function(5, GPIO_FUNC_PWM);
    // gpio_set_function(4, GPIO_FUNC_PWM);

    // // Find out which PWM slice is connected to GPIO 5 (it's slice 2, same for 4)
    // slice_num = pwm_gpio_to_slice_num(5);

    // // Mask our slice's IRQ output into the PWM block's single interrupt line,
    // // and register our interrupt handler
    // pwm_clear_irq(slice_num);
    // pwm_set_irq_enabled(slice_num, true);
    // irq_set_exclusive_handler(PWM_IRQ_WRAP, on_pwm_wrap);
    // irq_set_enabled(PWM_IRQ_WRAP, true);

    // // This section configures the period of the PWM signals
    // pwm_set_wrap(slice_num, WRAPVAL) ;
    // pwm_set_clkdiv(slice_num, CLKDIV) ;

    // // This sets duty cycle
    // pwm_set_chan_level(slice_num, PWM_CHAN_B, 0);
    // pwm_set_chan_level(slice_num, PWM_CHAN_A, 0);

    // // Start the channel
    // pwm_set_mask_enabled((1u << slice_num));


    ////////////////////////////////////////////////////////////////////////
    ///////////////////////////// ROCK AND ROLL ////////////////////////////
    ////////////////////////////////////////////////////////////////////////
    // start core 1 
    // multicore_reset_core1();
    // multicore_launch_core1(core1_entry);

    // start core 0
    gpio_init(15) ;
    gpio_set_dir(15, GPIO_OUT) ;
    gpio_init(14) ;
    gpio_set_dir(14, GPIO_OUT) ;
    gpio_put(14, 0) ;
    gpio_put(15, 0) ;
    // pt_add_thread(protothread_buffer) ;
    pt_add_thread(protothread_draw) ;
    // pt_add_thread(protothread_serial) ;
    pt_schedule_start ;

}
