/*
 * I'm not going to worry about inconsistent sector times. The scope says that every sector is triggered.
 * The turntime does seem to be consistent, so the speed estimation willbe accurate.
*/
#include <stdlib.h>
#include <U8g2lib.h>
#include <stdio.h> //The standard C library
//#include "pico/stdlib.h" //Standard library for Pico
#include "hardware/gpio.h" //The hardware GPIO library
#include "hardware/flash.h" // for the flash erasing and writing
#include "hardware/sync.h" // for the interrupts
#include <Button2.h>

#define FIRST_TIME
//#define DEBUG
#ifdef DEBUG
#define DPRINT(...)    Serial1.print(__VA_ARGS__)
//OR, #define DPRINT(args...)    Serial.print(args)
#define DPRINTLN(...)  Serial1.println(__VA_ARGS__)
#define DRINTF(...)    Serial1.print(F(__VA_ARGS__))
#define DPRINTLNF(...) Serial1.println(F(__VA_ARGS__)) //printing text using the F macro
#define DBEGIN(...)    Serial1.begin(__VA_ARGS__)

#else
#define DPRINT(...)     //blank line
#define DPRINTLN(...)   //blank line
#define DPRINTF(...)    //blank line
#define DPRINTLNF(...)  //blank line
#define DBEGIN(...)     //blank line

#endif

// this is where the globalslive in Flash - after the program
//#define FLASH_TARGET_OFFSET (512 * 1024) // choosing to start at 512K
#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES - FLASH_SECTOR_SIZE)
// the states
#define STOP 0
#define PLAY 1
#define FF 2
#define REW 3
#define BRAKE 4
const char * stateStr[] = {"STOP" ,"PLAY", "FF", "REW", "BRAKE"};

static const uint SENSOR_PIN = 10;

// these are globals changed during tape movement
uint lastTurns = 0;
uint lastSectors = 0;
volatile uint sectors = 6600;
volatile uint pulseCount = 0;
ulong idleTime = 1500;
double outsideDiam = 0;
double tapeTime = 12.8;
double tapeTimeLeft = 17.2;
uint diamInches, diamTenths;

// the button
const int SHORT_PRESS_DURATION = 500; // 500 milliseconds
const int LONG_PRESS_DURATION = 750;
bool pressStarted = false;
ulong pressedTime = 0;
ulong pressDuration;
ulong lastDuration;


//the globals to be saved in flash and restoe on reboot
struct GlobalData {
  uint VERSION = 0;
  const int NSECTORS = 6;
  const double HUB_DIAM = 3.0f; // inches fixed
  const double TAPE_THICKNESS = 0.00279f; // inches fixed
  const double TOTAL_TAPE_LENGTH = 30000.0f; // inches fixed
  const double REEL_SIZE = 10.5f; // inches 10.5 or 7 or whatever

  double TAPE_SPEED = 15.0f; // 15 or 7.5
  double INIT_DIAM = 3.0f; // inches of right-hand reel
  ulong INIT_SECTORS = 1100;  // initTurns on right-hand reel
  double INIT_TIME = 32.3f; // of tape on right-hand reel
};

GlobalData GLOBAL_DATA;


// display connections for SPI from micropython version of sh1107 driver
//#define OLED_DC       21
//#define OLED_CS       13
//#define OLED_RST      20

//#define OLED_MOSI     15
//#define OLED_CLK      14

#define OLED_SCL 5
#define OLED_SDA 4

U8G2_SH1107_128X128_F_HW_I2C u8g2(U8G2_R0, OLED_SCL, OLED_SDA);

// the state variables
uint state = PLAY; // so that the first stop is registereed
uint lastState = STOP;
ulong brakingAt = 0;
static const uint stopPin = 14;
static const uint rewPin = 13;
static const uint ffPin = 12;
static const uint playPin = 11;
static const uint sensorPin = 26;
static const uint buttonPin = 2;
Button2 diamButton;
// the diam button
//Button2 diamButton;

//void irq_interrupt(uint gpio, uint32_t events) {
void sensor_interrupt() {
  if (state == REW) {
    sectors = max(sectors - 1, 1);
  } else {
    sectors += 1;
  }
  //Serial.println(digitalRead(SENSOR_PIN));
}

// just increase initial diameter by twice the tape
// thickness times the number of turns
void calcDiamFromTurns() {
  outsideDiam = 2.0 * GLOBAL_DATA.TAPE_THICKNESS * (double)(sectors / GLOBAL_DATA.NSECTORS) + GLOBAL_DATA.HUB_DIAM;
}

// 
void calcSectorsFromDiam(double diam) {
  sectors = (diam - GLOBAL_DATA.HUB_DIAM) * GLOBAL_DATA.NSECTORS / 2.0 / GLOBAL_DATA.TAPE_THICKNESS;
}

// this is the basic tape roll formula based on the number of 
// turns and the tape thickness
double calcLengthFromTurns() {
  uint tturns = sectors / GLOBAL_DATA.NSECTORS;
  double tapeLength =
    PI * (double)tturns * (GLOBAL_DATA.HUB_DIAM + GLOBAL_DATA.TAPE_THICKNESS * (double)(tturns - 1));
  Serial1.println(tapeLength);
  double addon = (PI * outsideDiam * (double)(sectors % GLOBAL_DATA.NSECTORS) / 
                                                    (double)GLOBAL_DATA.NSECTORS);
  Serial1.println(addon);
  return tapeLength + addon;
}

// just divide the length by the tape speed in ips
double calcTapeTimeFromLength(double tapeLength) {
  return tapeLength / GLOBAL_DATA.TAPE_SPEED;
}

double calcTapeLengthFromTime() {
  return tapeTime * GLOBAL_DATA.TAPE_SPEED;
}

// this is the quadratic equation solution from the basic roll formula
double calcTurnsFromLength(double tapeLength) {
  double s = sqrt(pow(GLOBAL_DATA.HUB_DIAM - GLOBAL_DATA. TAPE_THICKNESS, 2) +
          (4 * GLOBAL_DATA. TAPE_THICKNESS * tapeLength) / PI);
  return (GLOBAL_DATA. TAPE_THICKNESS - GLOBAL_DATA.HUB_DIAM + s) / (2 * GLOBAL_DATA. TAPE_THICKNESS); 
}

// convert a diameter into number of inches and tenths
void convertDiam(double diam) {
  diamInches = (int)diam;
  diamTenths = (int)(round((diam - (double)diamInches) * 10.0));
}

double convertDiamBack() {
  return (double)diamInches + (double)diamTenths / 10.0f;
}

// update everything when sectors has changed due to tape movement
// i.e. outsideDiam, tapeTime, tapeTimeLeft, diamInches, diamTenths
void updateGlobals() {
  double tapeLength = calcTapeLengthFromTime();
  calcDiamFromTurns();  // sets outsideDiam
  convertDiam(outsideDiam); // sets diamINches and diamTenths
  GLOBAL_DATA.INIT_DIAM = outsideDiam;
  GLOBAL_DATA.INIT_SECTORS = sectors;
  GLOBAL_DATA.INIT_TIME = tapeTime;
  Serial1.println(String(GLOBAL_DATA.VERSION) + "/" + String(GLOBAL_DATA.NSECTORS) + "/" +
                  String(outsideDiam) + "/" + 
                  String(sectors) + "/" + String(tapeTime) + "/" + 
                  String(diamInches) + "/" + String(diamTenths));
}

// when the outside diameter has changed by using the button, update the other globals
// i.e. ousideDiam, sectors, tapeTime, tapeTimeLeft
void updateDiameter() {
  outsideDiam = convertDiamBack();
  calcSectorsFromDiam(outsideDiam); // sets sectors
  double tapeLength  = calcLengthFromTurns();
  tapeTime = calcTapeTimeFromLength(tapeLength);
  double tapeLengthLeft = GLOBAL_DATA.TOTAL_TAPE_LENGTH - tapeLength;
  tapeTimeLeft = calcTapeTimeFromLength(tapeLengthLeft);
}


void extractGlobals() {
  tapeTime = GLOBAL_DATA.INIT_TIME;
  tapeTimeLeft = GLOBAL_DATA.TOTAL_TAPE_LENGTH / GLOBAL_DATA.TAPE_SPEED - tapeTime;
  outsideDiam = GLOBAL_DATA.INIT_DIAM;
  sectors = GLOBAL_DATA.INIT_SECTORS;
  convertDiam(outsideDiam);
  Serial1.println(String(GLOBAL_DATA.VERSION) + "/" +String(tapeTime) + "/" + String(outsideDiam) + "/" + 
                  String(sectors) + "/" + String(tapeTime) + "/" + 
                  String(diamInches) + "/" + String(diamTenths));

}

void saveGlobals() {
  GLOBAL_DATA.VERSION += 1;
  updateGlobals();
  uint8_t* globalDataAsBytes = (uint8_t*) &GLOBAL_DATA;
  int globalDataSize = sizeof(GlobalData);
  
  //int writeSize = (globalDataSize / FLASH_PAGE_SIZE) + 1; // how many flash pages we're gonna need to write
  //int sectorCount = ((writeSize * FLASH_PAGE_SIZE) / FLASH_SECTOR_SIZE) + 1; // how many flash sectors we're gonna need to erase
      
  Serial1.println("Programming flash target region..." + String(globalDataSize));

  uint32_t interrupts = save_and_disable_interrupts();
  flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE);
  //flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE * sectorCount);
  flash_range_program(FLASH_TARGET_OFFSET, globalDataAsBytes, FLASH_PAGE_SIZE);
  //flash_range_program(FLASH_TARGET_OFFSET, globalDataAsBytes, FLASH_PAGE_SIZE * writeSize);
  restore_interrupts(interrupts);

  Serial1.println("Globals Saved");
}

void restoreGlobals() {
    const uint8_t* flash_target_contents = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET);
    memcpy(&GLOBAL_DATA, flash_target_contents, sizeof(GlobalData));
    //memcpy(&GLOBAL_DATA, flash_target_contents + FLASH_PAGE_SIZE, sizeof(GlobalData));
    extractGlobals();
    Serial1.println("Globals Restored");
}

// just add twice the tape thickness times the number of turns tohub diameter
double calcOutsideDiamFromTurns() {
  uint tturns = sectors / GLOBAL_DATA.NSECTORS;
  return GLOBAL_DATA.HUB_DIAM + tturns * 2 * GLOBAL_DATA.TAPE_THICKNESS;
}

// this is formula based on cross section area
double calcLengthFromOutsideDiam() {
  return PI * (pow(outsideDiam, 2) - pow(GLOBAL_DATA.HUB_DIAM, 2)) / (4.0 * GLOBAL_DATA.TAPE_THICKNESS);
}

void displayDiam() {
  u8g2.clearBuffer(); // clear display
  u8g2.setFont(u8g2_font_t0_18_tr);
  u8g2.setCursor(0, 20);        // position to display
  u8g2.print("D=");
  u8g2.setCursor(30, 20);
  u8g2.print(String(diamInches) + "." + String(diamTenths));
  u8g2.sendBuffer();
 }

// calculate time from tape length and speed and display it on the OLED
// currently with the standard arduino, it is not fast wenouh to do this display 
// for every loop iteration, so on;y do this while stopped 
void displayTimes() {
  //Serial.print("8-");Serial.print(turns);Serial.print("/");Serial.println(sector);
   // update global time
  int mins = int(tapeTime / 60.0);
  double secs = tapeTime - 60.0 * mins;
  //Serial.print("4-");Serial.print(mins);Serial.print(":");Serial.println(secs);
  u8g2.drawFrame(0, 64, 128,64);
  u8g2.setCursor(6, 88);
  u8g2.setFont(u8g2_font_t0_16_tr);
  u8g2.print("Time: " + String(mins) + ":" + String(secs, 1));
  u8g2.setFont(u8g2_font_t0_12_tr );
  int minsLeft = int(tapeTimeLeft / 60.0);
  double secsLeft = tapeTimeLeft - 60.0 * minsLeft;
  u8g2.setCursor(10, 112);
  u8g2.print(String("Time Left: ") + String(minsLeft) + String(":") + String(secsLeft, 1));

  u8g2.setCursor(10, 124);
  u8g2.print(F("Diameter: "));
  u8g2.print(outsideDiam, 1);
}

void displayState(int turns, int sector) {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_t0_12_tr); 
  u8g2.setCursor(6, 18);
  u8g2.print(stateStr[state]);
  u8g2.setCursor(36, 18);
  u8g2.print("Speed: " + String(GLOBAL_DATA.TAPE_SPEED, 1));
  u8g2.setCursor(10, 52);
  u8g2.setFont(u8g2_font_inr21_mr );
  if (state == STOP || state == PLAY) {
    u8g2.print(String(turns) + "/" + String(sector));
  } else {
    u8g2.print(turns);
  }
  u8g2.sendBuffer();
}

void doDisplay() {
  //unsigned long now = millis();
  uint tturns = sectors / GLOBAL_DATA.NSECTORS;
  uint tsector = sectors % GLOBAL_DATA. NSECTORS + 1;
  displayState(tturns, tsector);
  u8g2.drawFrame(0, 0, 128, 64);
  //updateGlobals(turns, sector);
  displayTimes();
  u8g2.sendBuffer();   
  //Serial.print(v(String("display took")) + v(String(millis() - now), "\n"));
}



// arduino's initialization function
void setup() {
  Serial1.begin(115200);//enable serial monitor
  delay(1000);

  //u8g2.setBusClock(8000000);
  u8g2.begin(); 
 
  delay(1000);         // wait for initializing

  doDisplay();

  //gpio_init(SENSOR_PIN);
  //gpio_set_dir(SENSOR_PIN, GPIO_IN);
  //pinMode(SENSOR_PIN, INPUT);
  //gpio_set_irq_enabled_with_callback(SENSOR_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &irq_interrupt);
  attachInterrupt(digitalPinToInterrupt(sensorPin), sensor_interrupt, RISING);

  pinMode(stopPin, INPUT);
  pinMode(ffPin, INPUT);
  pinMode(rewPin, INPUT);
  pinMode(playPin, INPUT);

#ifdef FIRST_TIME
    saveGlobals(); // this saes all the default values, including version 0
#endif
  restoreGlobals();

  diamButton.begin(buttonPin);
  diamButton.setLongClickDetectedHandler(buttonLongClick);
  diamButton.setLongClickHandler(buttonLongReleased);
  diamButton.setLongClickTime(500);
  diamButton.setPressedHandler(buttonPressed);
  //diamButton.setReleasedHandler(buttonReleased);
  diamButton.setLongClickDetectedRetriggerable(true);

  Serial1.println("setup finished");
 
}

// a long press means increment inches
void buttonLongClick(Button2& btn) {
  Serial1.println("long press");
  if (state != STOP) {
    return;
  }
  diamInches = diamInches + 1;
  if (convertDiamBack() > GLOBAL_DATA.REEL_SIZE) {
    diamInches = (int)GLOBAL_DATA.HUB_DIAM;
    diamTenths = (int)round((GLOBAL_DATA.HUB_DIAM - (double)diamInches) * 10.0);
  }
  updateDiameter();
  displayDiam(); 
}

// a short press means increment tenths
void buttonPressed(Button2& btn) {
  Serial1.println("pressed:" + String(diamInches) + "/" + String(diamTenths));
  if (state != STOP) {
    return;
  }
  pressedTime = millis();
  diamTenths = diamTenths + 1;
  if (convertDiamBack() > GLOBAL_DATA.REEL_SIZE) {
    diamInches = (int)GLOBAL_DATA.HUB_DIAM;
    diamTenths = (int)round((GLOBAL_DATA.HUB_DIAM - (double)diamInches) * 10.0);
  }
  updateDiameter();
  displayDiam();
}

void buttonLongReleased(Button2& btn) {
  outsideDiam = convertDiamBack();
  updateDiameter();
  doDisplay();
}

void buttonReleased(Button2& btn) {
  //pressedTime = 0;
}



void setState() {
  uint stop = digitalRead(stopPin);
  // if stop has gone high, then one of FF, RRW, PLAY has gone low
  // when stopped
  lastState = state;
  if (stop == HIGH && state == STOP) {
    uint ff = digitalRead(ffPin);
    uint rew = digitalRead(rewPin);
    uint play = digitalRead(playPin);
    if (ff == LOW) {
      state = FF;
    } else if (rew == LOW) {
      state = REW;
    } else if (play == LOW) {
      state = PLAY;
    } else {
      // should we wait a little bit?
      delay(5);
    }
    Serial1.println(stateStr[state]);
  } else if (stop == LOW && state != STOP) {
    // stop has gone low again, so it could be the end of FF/REW/PLAY
    if (state == PLAY) {
      state = STOP;
      Serial1.println(stateStr[state]);
    } else if (state == BRAKE) {
      ulong brakeDuration = millis() - brakingAt;
      if (sectors == lastSectors && brakeDuration > idleTime) {
          // we are done, so actually stop
          state = STOP;
          Serial.println("Finally stop");
      }
    } else { // rew or ff
      // brake for the slow down pulses
      state = BRAKE;
      Serial1.println(stateStr[state]);
      // we need to be in brake until pulses stop i.e. no pulses 
      // for 500 ms
      brakingAt = millis();
    }
  } // else do nothing since we are stopped or in play/ff/rew
    // and should stay there
}

bool stopped = false;
ulong playStart;

void loop() {
  diamButton.loop();

  setState();
  if (state != STOP && lastState == STOP) {
    // we just started ff/rew/play
    //Serial.println(String(stateStr[state]) + " " + String(sector) + " " + String(lastSector) + " " + String(stopped));
    //playStart = millis();
    lastSectors = sectors;
  } else if (state == STOP && lastState != STOP) {
      // we just stopped after ff/rew/play
      doDisplay(); // uses sector
      saveGlobals();
      Serial1.println("STOPPED " + String(pulseCount));
      lastSectors = sectors;
  } else if (state == STOP && lastState == STOP) {
      if (pressedTime > 0) {
        delay(3000);
        doDisplay();
        pressedTime = 0;
      }
      // while stopped we may move the tape by hand, but which way?
      if (sectors > lastSectors) {
        updateGlobals();
        doDisplay();
        Serial1.println("Moved tape while stopped");
        lastSectors = sectors;
      }
  } else if (state != STOP) { // only for ff/rew/play or brake
      int tturns = sectors / GLOBAL_DATA.NSECTORS;
      if ((state == REW && tturns < lastTurns) || tturns > lastTurns) {
        // only display turns, not sectors
        int tsector = sectors % GLOBAL_DATA.NSECTORS + 1;
        displayState(tturns, tsector);
        lastTurns = tturns;
        lastSectors = sectors;
      } // else don't display'
  } // else do nothing since we stopped
   
}