// Full schematic and software for the controller
// of a missile launching vehicle
// by DedGzus
// Started on 1-17-2024

#include "ProcessManager.h"
#include "Process.h"
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>

// IO pin definitions
#define displaySCL 2
#define displaySDA 3
#define throttlePin A0
#define steeringPin A1
#define pitchPin A2
#define launchBtnPin 4
#define dirSwPin 5
#define radarBtnPin 6

// display definitions
#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_ADDR   0x3C

// created the OLED display pointer
Adafruit_SSD1306* pOLED;

// create the process manager pointer
ProcessManager* pManager;

// global variables for controls
volatile byte launchBtn(HIGH);    // launches when set LOW
volatile byte dirSw(LOW);         // LOW = fwd, HIGH = rev
volatile byte radarBtn(HIGH);     // runs radar when set HIGH
volatile int throttleRead(0);     // 0-1023 analog input for throttle
volatile int steeringRead(512);   // 0[right] 1023[left] 512[center]
volatile int pitchRead(512);      // 0[up] 1023[down] 512[center]

volatile bool radarRunning(false);

//--------------------------------------
// setup function
//--------------------------------------
void setup()
{
  // acquire the display
  pOLED = new Adafruit_SSD1306(OLED_WIDTH, OLED_HEIGHT);

  // initialize the display
  pOLED->begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);

  // acquire a process manager
  pManager = new ProcessManager();

  // start processes for reading input from controls
  pManager->attach(new digitalReadProcess(launchBtnPin, &launchBtn));
  pManager->attach(new digitalReadProcess(dirSwPin, &dirSw));
  pManager->attach(new digitalReadProcess(radarBtnPin, &radarBtn));
  pManager->attach(new analogReadProcess(throttlePin, &throttleRead));
  pManager->attach(new analogReadProcess(steeringPin, &steeringRead));
  pManager->attach(new analogReadProcess(pitchPin, &pitchRead));

  displaySetup();
}

//--------------------------------------
// main loop function
//--------------------------------------
void loop()
{
  // update processes
  pManager->onLoop();
  // update LCDs
  displayUpdate();
}


//--------------------------------------
// Setup the display
//--------------------------------------

void displaySetup()
{
  // clear the display
  pOLED->clearDisplay();

  // splash screen
  pOLED->setTextSize(2);
  pOLED->setTextColor(WHITE);
  pOLED->setCursor(24,14);
  pOLED->print("DedGzus");
  pOLED->setTextSize(1);
  pOLED->setTextColor(WHITE);
  pOLED->setCursor(2,40);
  pOLED->print("-[ c o n t r o l s ]-");

  pOLED->display();

  delay(2000);
}

void displayUpdate()
{
  static int oldT, oldS, oldP;
  static byte oldD, oldL;

  pOLED->clearDisplay();

  pOLED->setTextSize(1);
  pOLED->setTextColor(WHITE);
  pOLED->setCursor(0, 0);
  pOLED->print("Throttle: ");
  pOLED->print(mapPct(throttleRead));
  pOLED->println("%");
  pOLED->print("Steering: ");
  writeSteering(steeringRead);
  pOLED->print("Pitch:");
  writePitch(pitchRead);
  pOLED->print("Direction: ");
  pOLED->println(fwdRev(dirSw));
  pOLED->print("Missile: ");
  pOLED->println(armedLaunch(launchBtn));
  pOLED->print("Radar: ");
  pOLED->println(radarStatus(radarBtn));

  // if throttle changes update on display
/*  int newVal = throttleRead;
  if (newVal != oldT)
  {
    pOLED->setCursor(64, 0);
    pOLED->setTextColor(BLACK);
    pOLED->print("/xFF/xFF/xFF/xFF");
    pOLED->setCursor(64, 0);
    pOLED->setTextColor(WHITE);
    pOLED->print(mapPct(newVal));
    pOLED->print("%");
    oldT = newVal;
  }
/*
  // if steering changes update on display
  newVal = steeringRead;
  if (newVal != oldS)
  {
    pLcd->pos(3, 1);
    writeSteering(newVal);
    oldS = newVal;
  }

  // if pitch changes update on display
  newVal = pitchRead;
  if (newVal != oldP)
  {
    pLcd->pos(11, 1);
    writePitch(newVal);
    oldP = newVal;
  }

  // update lcd 1
  pLcd->set(1);

  // if launch button changes update on display
  newVal = launchBtn;
  if (newVal != oldL)
  {
    pLcd->pos(3, 0);
    pLcd->send(armedLaunch(newVal));
    oldL = newVal;
  }

  // if direction switch changes update on display
  newVal = dirSw;
  if (newVal != oldD)
  {
    pLcd->pos(3, 1);
    pLcd->send(fwdRev(newVal));
    oldD = newVal;
  }
*/
  pOLED->display();
}

char* fwdRev(byte sw)
{
  if(sw)
  {
    return "reverse";
  }

  return "forward";
}

char* armedLaunch(byte btn)
{
  if(btn)
  {
    return "[armed]";
  }

  return "LAUNCH!";
}

char* radarStatus(byte btn)
{
  if(btn)
  {
    return "stationary";
  }

  return "sweeping";
}

int mapPct(int val)
{
  return map(val,0,1023,0,100);
}

void writeSteering(int val)
{
  if(val==512)
  {
    pOLED->println("center");
  }
  if(val<512)
  {
    int i = map(val,511,0,1,100);
    pOLED->print(i);
    pOLED->println("% right");
  }
  if(val>512)
  {
    int i = map(val,513,1023,1,100);
    pOLED->print(i);
    pOLED->println("% left");
  }
}

void writePitch(int val)
{
  if(val==512)
  {
    pOLED->println("inactive");
  }
  if(val<512)
  {
    int i = map(val,511,0,1,100);
    pOLED->print(i);
    pOLED->println("% down");
  }
  if(val>512)
  {
    int i = map(val,513,1023,1,100);
    pOLED->print(i);
    pOLED->println("% up");
  }
}