// 2 Operator FM Synth firmware for Sitka Instruments WS-1.0
// by Oleksiy Hrachov
// 
// Code is partly based on Knob_LightLevel_x2_FMSynth example from Mozzi Library
// 
// Although the code designed to work on Sitka Instruments WS-1.0 synth, it should  
// be pretty easy to adapt to run on other arduino/mozzi-based setups.
// Edit config.h to match your setup.
//
// This code is licenced under GPL v3 or later

//ToDo:
//optimize MIDI input (maybe neohwserial and this parser might help https://github.com/eclab/grains/tree/main/midi)
//add ramp LFO
//add uneven rations (like 3:2, 3:5)
//rework LFO/Intensity so when LFO rate is 0, intensity doesn't depend on its phase

#include <Mozzi.h>
#include <MIDI.h>
#include <Oscil.h>
#include <tables/sin2048_int8.h>
#include <tables/square_no_alias_2048_int8.h>
#include <tables/saw2048_int8.h>
#include <tables/whitenoise8192_int8.h>
#include <Smooth.h>
#include <ADSR.h>
#include <mozzi_midi.h>
#include <IntMap.h>
#include <OverSample.h>
#include <EEPROM.h>
#include "config.h"

IntMap kMapCarrierNote(  0, 4095, 24 * pitchSubSteps,  84 * pitchSubSteps );
IntMap kMapIntensity(    0, 1023, 10,  350   );
IntMap kMapHarmonics(    0, 1023, 1,   40    );
IntMap kMapLFOSpeed(     0, 1023, 1,   10000 );
IntMap kMapAttack(       0, 1023, 0,   80    );
IntMap kMapDecayRelease( 0, 1023, 8,   160   );
IntMap kMapSustain(      0, 1023, 0,   255   );

Oscil<SIN2048_NUM_CELLS, MOZZI_AUDIO_RATE>          aCarrier(SIN2048_DATA);
Oscil<SIN2048_NUM_CELLS, MOZZI_AUDIO_RATE>          aModulator(SIN2048_DATA);
Oscil<SIN2048_NUM_CELLS, MOZZI_CONTROL_RATE>        kSineLFO(SIN2048_DATA);
Oscil<SQUARE_NO_ALIAS_2048_NUM_CELLS, MOZZI_CONTROL_RATE> kSquareLFO(SQUARE_NO_ALIAS_2048_DATA);
Oscil<SAW2048_NUM_CELLS, MOZZI_CONTROL_RATE>        kSawLFO(SAW2048_DATA);
Oscil<WHITENOISE8192_NUM_CELLS, MOZZI_CONTROL_RATE> kNoiseLFO(WHITENOISE8192_DATA);
ADSR <MOZZI_CONTROL_RATE, MOZZI_CONTROL_RATE> envelope;
Smooth <long> aSmoothIntensity(0.95f);
OverSample <unsigned int, 2> kOverSamplePitch;

//Global variables
byte gain;
bool MIDINotePlaying;
bool gateIsHigh = false;
float carrierFreq;
long FMIntensity; // carries control info from updateControl to updateAudio
byte MIDIChannel;
byte memCode = "a";

void MIDINoteOn(byte channel, byte note, byte velocity) {
  carrierFreq = mtof((int) note);
  envelope.noteOn();
  MIDINotePlaying = true;
  digitalWrite(LED, LOW);
}

void MIDINoteOff(byte channel, byte note, byte velocity) {
  envelope.noteOff();
  digitalWrite(LED, HIGH);
}

long softClip(long input) {
  int threshold = 2048;
  if (input < -threshold) {
    return -threshold + (input + threshold) / 4;
  } else if (input > threshold) {
    return threshold + (input - threshold) / 4;
  } else {
    return input;
  }
}

void setup() {
  pinMode(LED, OUTPUT);
  pinMode(GateIn, INPUT_PULLUP);
  pinMode(EnvSwitch, INPUT_PULLUP);
  pinMode(DroneSwitch, INPUT_PULLUP);

  //MIDI Channel setup
  if(!digitalRead(GateIn)) { //&& !digitalRead(DroneSwitch) && !digitalRead(EnvSwitch)) { //MIDI setup mode, first knob you rotate = MIDI channel
    int knob1Orig = analogRead(Knob1);
    int knob2Orig = analogRead(Knob2);
    int knob3Orig = analogRead(Knob3);
    int knob4Orig = analogRead(Knob4);
    int knobAOrig = analogRead(KnobA);
    int knobDROrig = analogRead(KnobDR);
    int knobSOrig = analogRead(KnobS);
    int threshhold = 50; //To reduce noise from the input, needs testing and fine-tuning
    while(true) {
      if(abs(analogRead(Knob1) - knob1Orig) > threshhold) { 
        MIDIChannel = 1;
        break;
      } else if (abs(analogRead(Knob2) - knob2Orig) > threshhold) {
        MIDIChannel = 2;
        break;
      } else if (abs(analogRead(Knob3) - knob3Orig) > threshhold) {
        MIDIChannel = 3;
        break;
      } else if (abs(analogRead(Knob4) - knob4Orig) > threshhold) {
        MIDIChannel = 4;
        break;
      } else if (abs(analogRead(KnobA) - knobAOrig) > threshhold) {
        MIDIChannel = 5;
        break;
      } else if (abs(analogRead(KnobDR) - knobDROrig) > threshhold) {
        MIDIChannel = 6;
        break;
      } else if (abs(analogRead(KnobS) - knobSOrig) > threshhold) {
        MIDIChannel = 7;
        break;
      }
      digitalWrite(LED, HIGH); //slow blinking to indicate the setup mode
      delay(500);
      digitalWrite(LED, LOW);
      delay(500);
    }
    EEPROM.write(0, MIDIChannel);
    EEPROM.write(1023, memCode);
    digitalWrite(LED, LOW);
    delay(150);
    for(int i = 0; i < MIDIChannel; i ++) { //fast blink to indicate saved MIDI channel
      digitalWrite(LED, HIGH);
      delay(150);
      digitalWrite(LED, LOW);
      delay(150);
    }
    delay(500);
  } else if (EEPROM.read(1023) == memCode) { 
    MIDIChannel = EEPROM.read(0);
  } else { //Save default MIDI Channel to EEPROM if it wasn't set
    MIDIChannel = 1; //channel 1 by default
    EEPROM.write(0, MIDIChannel);
    EEPROM.write(1023, memCode);
  }

  disconnectDigitalIn(Knob1);
  disconnectDigitalIn(Knob2);
  disconnectDigitalIn(Knob3);
  disconnectDigitalIn(Knob4);
  disconnectDigitalIn(KnobA);
  disconnectDigitalIn(KnobDR);
  disconnectDigitalIn(KnobS);
  disconnectDigitalIn(CVIn);

  MIDI.setHandleNoteOn(MIDINoteOn);
  MIDI.setHandleNoteOff(MIDINoteOff);

  MIDI.setHandleClock(nullptr);
  MIDI.setHandleStart(nullptr);
  MIDI.setHandleStop(nullptr);
  MIDI.setHandleContinue(nullptr);
  MIDI.setHandleControlChange(nullptr);

  MIDI.begin(MIDIChannel);
  MIDI.turnThruOff();

  startMozzi();

  envelope.setAttackLevel(255);

  digitalWrite(LED, HIGH);
}

void updateControl(){

  for(int i=0; i<10; i++) { // Hacky way to drain the buffer faster
    MIDI.read();
  }

  //Get Control Values
  int CVInVal = mozziAnalogRead(CVIn);
  int knob1Val = mozziAnalogRead(Knob1);
  int knob2Val = mozziAnalogRead(Knob2);
  int knob3Val = mozziAnalogRead(Knob3);
  int knob4Val = mozziAnalogRead(Knob4);
  int knobAVal = mozziAnalogRead(KnobA);
  int knobDRVal = mozziAnalogRead(KnobDR);
  int knobSVal = mozziAnalogRead(KnobS);
  bool droneSwitchVal = digitalRead(DroneSwitch);
  bool envSwitchVal = digitalRead(EnvSwitch);
  bool gateInVal = !digitalRead(GateIn);

  //Remap the values and assign to parameter
  int intensity = kMapIntensity(knob1Val);
  int harmonics = kMapHarmonics(knob2Val);
  int LFOSpeed = kMapLFOSpeed(knob3Val);
  float expLFOSpeed = (float) LFOSpeed * LFOSpeed / 400000;
  float modSpeed = expLFOSpeed;

  //Set pitch and play notes on trigger
  if (!MIDINotePlaying) {
    if (oversamplingEnabled) {
      int oversampledCVInVal = kOverSamplePitch.next(CVInVal);
      carrierFreq = mtof((float) kMapCarrierNote(oversampledCVInVal) / pitchSubSteps);
    } else {
      carrierFreq = mtof((float) kMapCarrierNote(CVInVal << 2) / pitchSubSteps);
    }
    digitalWrite(LED, !gateInVal);
    if (gateInVal && !gateIsHigh) {
      gateIsHigh = true;
      envelope.noteOn();
    } else if (!gateInVal && gateIsHigh) {
      gateIsHigh = false;
      envelope.noteOff();
    }
  } else if (MIDINotePlaying && !envelope.playing()) {
    MIDINotePlaying = false;
  }

  //Update Envelope Settings
  int attackTime = kMapAttack(knobAVal);
  int decayReleaseTime = kMapDecayRelease(knobDRVal);
  int sustainLevel = kMapSustain(knobSVal);
  envelope.setDecayLevel(sustainLevel);
  envelope.setTimes(attackTime, decayReleaseTime, 30000, decayReleaseTime); //30000 is so the note will sustain 30 seconds unless a noteOff comes

  //Calculate the modulation frequency
  int FMmodFreq = carrierFreq * harmonics;
  
  //Set oscillator frequencies
  aCarrier.setFreq(carrierFreq); 
  aModulator.setFreq(FMmodFreq);
  kSineLFO.setFreq(modSpeed);
  kSquareLFO.setFreq(modSpeed);
  kSawLFO.setFreq(modSpeed);
  kNoiseLFO.setFreq(modSpeed/4096);
  
  envelope.update();

  int sineLFOLevel;
  int squareLFOLevel;
  int sawLFOLevel;
  int noiseLFOLevel;
  if (knob4Val < 255) {
    sineLFOLevel = 255 - knob4Val;
    squareLFOLevel = knob4Val;
    sawLFOLevel = 0;
    noiseLFOLevel = 0;
  } else if (knob4Val < 511) {
    sineLFOLevel = 0;
    squareLFOLevel = 511 - knob4Val;
    sawLFOLevel = knob4Val - 255;
    noiseLFOLevel = 0;
  } else if (knob4Val < 767) {
    sineLFOLevel = 0;
    squareLFOLevel = 0;
    sawLFOLevel = 767 - knob4Val;
    noiseLFOLevel = knob4Val - 511;
  } else {
    sineLFOLevel = 0;
    squareLFOLevel = 0;
    sawLFOLevel = 0;
    noiseLFOLevel = 255;
  }

  int shapedLFO = (kSineLFO.next() * sineLFOLevel +
                  kSquareLFO.next() * squareLFOLevel +
                  kSawLFO.next() * sawLFOLevel +
                  kNoiseLFO.next() * noiseLFOLevel)>>8; 

  int env = envelope.next();
  int intensityEnv;
  if(envSwitchVal) {
    intensityEnv = env*3;
  } else {
    intensityEnv = 1;
  }
  FMIntensity = ((long)(intensity + intensityEnv) * (shapedLFO+128))>>8; //(shapedLFO+128))>>16; //(kSineLFO.next()+128))>>8;

  if(!droneSwitchVal) {
    gain = env;
  } else {
    gain = 255;
  }

}

AudioOutput updateAudio(){
  long modulation = aSmoothIntensity.next(FMIntensity) * aModulator.next();
  long signal = (aCarrier.phMod(modulation) * gain) >> 8; //envelope
  signal = softClip((signal * (127 + driveAmount)) >> 8); //overdrive
  return MonoOutput::from8Bit(signal);
}

void loop(){
  audioHook();
}