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

#include "LCDBank.h"
#include "ProcessManager.h"
#include "Process.h"

// global const defines
#define totalLCDs 2

// IO pin definitions
#define lcdDataPin 2
#define lcdShiftPin 3
#define lcdLatchPin 4
#define throttlePin A0
#define steeringPin A1
#define pitchPin A2
#define launchBtnPin 5
#define dirSwPin 6

// create the LCD Bank
LCDBank* pLcd;
// create the process manager
ProcessManager* pManager;

// global variables for controls
volatile byte launchBtn(HIGH);   // launches when set LOW
volatile byte dirSw(LOW);        // LOW = fwd, HIGH = rev
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]

//--------------------------------------
// setup function
//--------------------------------------
void setup()
{
  // acquire an LCD bank
  pLcd = new LCDBank(lcdDataPin, lcdShiftPin, lcdLatchPin);
  // initialize the LCDs
  pLcd->init(totalLCDs);

  // 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 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 all displays
  pLcd->all();
  pLcd->clear();

  // setup lcd 0
  pLcd->set(0);
  pLcd->pos(0, 0);
  pLcd->send("T: ");
  pLcd->sendI(mapPct(throttleRead));
  pLcd->send("%");
  pLcd->pos(0, 1);
  pLcd->send("S: ");
  writeSteering(steeringRead);
  pLcd->pos(8, 1);
  pLcd->send("P: ");
  writePitch(pitchRead);

  // setup lcd 1
  pLcd->set(1);
  pLcd->pos(0, 0);
  pLcd->send("L: ");
  pLcd->send(armedLaunch(launchBtn));
  pLcd->pos(0, 1);
  pLcd->send("D: ");
  pLcd->send(fwdRev(dirSw));
}

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

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

  // if throttle changes update on display
  int newVal = throttleRead;
  if (newVal != oldT)
  {
    pLcd->pos(3, 0);
    pLcd->send("    ");
    pLcd->pos(3, 0);
    pLcd->sendI(mapPct(newVal));
    pLcd->send("%");
    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;
  }
}

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

  return "Forward";
}

char* armedLaunch(byte btn)
{
  if(btn)
  {
    return "Armed  ";
  }

  return "Launch!";
}

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

void writeSteering(int val)
{
  if(val==512)
  {
    pLcd->send("0   ");
  }
  if(val<512)
  {
    int i = map(val,511,0,1,100);
    pLcd->sendI(i);
    pLcd->send("R");
  }
  if(val>512)
  {
    int i = map(val,513,1023,1,100);
    pLcd->sendI(i);
    pLcd->send("L");
  }
}

void writePitch(int val)
{
  if(val==512)
  {
    pLcd->send("0   ");
  }
  if(val<512)
  {
    int i = map(val,511,0,1,100);
    pLcd->sendI(i);
    pLcd->send("D");
  }
  if(val>512)
  {
    int i = map(val,513,1023,1,100);
    pLcd->sendI(i);
    pLcd->send("U");
  }
}