/*
 * Celestial Fusion
 * Software for digitized engine data acquisition and processing
 *
 * Author: James McCrae
 */

#include <allegro.h>
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <dos.h>
#include <conio.h>

//port address
#define outport 0x3BC
#define inport 0x3BD
#define controlport 0x3BE

//sensor information
#define NUMINPUTSENSORS 2

#define	TACHBUSADDR	0
#define SPEEDBUSADDR	1

//gui constants
#define GAUGERESOLUTIONX 480
#define GAUGERESOLUTIONY 480

//Vehicle specific constants
#define	MAXTACHOMETERRANGE	8000.0
#define TACHOMETERREDLINE	5500.0
#define MAXSPEEDOMETERRANGE	240.0
#define SPEEDPULSESPERKM	637.0

//Global variables (for screen size, etc)
//modifyable by command line arguments

int NUMINPUTSAMPLES=20;

int RESOLUTIONX=640;
int RESOLUTIONY=480;

float PULSETIMECONSIDERED=1.0;

float POLLDELAYSEC=0.05;

float UPDATESPERSEC=10.0;

/* Class declarations */

class Poll {

  public:
  Poll();
  Poll(double timestamp, unsigned char reading);
  void setTimestamp(double timestamp);
  void setReading(unsigned char reading);
  double getTimestamp();
  unsigned char getReading();

  private:
  double timestamp;
  unsigned char reading;

};

class ParallelPortIO {

  public:
  ParallelPortIO();
  void setSimulated(bool simulated);
  bool getSimulated();
  Poll *obtainPolls(int busaddress);
  void addPoll(int whichsensor, double timestamp, unsigned char reading);
  int pollSensor(int sensoraddress);

  private:
  Poll *inputsequence[NUMINPUTSENSORS];
  int beginindex[NUMINPUTSENSORS];
  bool simulated;

};

class Simulator {

  public:
  Simulator();
  void update(ParallelPortIO *ppio);
 
  private:
  int state;
  float rpm;
  float speed;
  int gear;
  double eventtimestamp;
  double tachpulsemissedlastpoll;
  double speedpulsemissedlastpoll;

};

class GUI {
  public:
  GUI();
  void update(ParallelPortIO *ppio);
  void incrementPoll();
  
  private:
  void loadGraphics();
  void prepareTachometer();
  void prepareSpeedometer();
 
  void drawTachometer(ParallelPortIO *ppio);
  void drawSpeedometer(ParallelPortIO *ppio);

  double lastupdate;
  int currentscreen;
  int lastpolls;
  int numpolls;
  double lastsecond;

  BITMAP *doublebuffer;
  BITMAP *logo;
  BITMAP *bgcarimg;
  BITMAP *optionmenu;
  BITMAP *tachometer;
  BITMAP *speedometer;
  BITMAP *numbers[10];
  BITMAP *numbers20[13];
  BITMAP *needle;
  BITMAP *whiteline;
  BITMAP *smallredline;
  BITMAP *bigredline;
  BITMAP *rpmx1000;
  BITMAP *kmperh;

};

/* Function declarations */
void alleg_init();
void cmdLineError();

/* Main Method */
int main(int argc, char ** argv) {
  
  bool dosimulate=false;

  //parse command line arguments
  for (int i=1;i<argc;i++) {
    if (strcasecmp(argv[i],"-S")==0)
      dosimulate=true;
    else if (strcasecmp(argv[i],"-R")==0) {
      if (argc<=i+2) cmdLineError();
      RESOLUTIONX=atoi(argv[i+1]);
      RESOLUTIONY=atoi(argv[i+2]);
      i+=2;
    }
    else if (strcasecmp(argv[i],"-T")==0) {
      if (argc<=i+1) cmdLineError();
      PULSETIMECONSIDERED=atof(argv[i+1]);
      i+=1;
    }
    else if (strcasecmp(argv[i],"-D")==0) {
      if (argc<=i+1) cmdLineError();
      POLLDELAYSEC=atof(argv[i+1]);
      i+=1;
    }
    else if (strcasecmp(argv[i],"-U")==0) {
      if (argc<=i+1) cmdLineError();
      UPDATESPERSEC=atof(argv[i+1]);
      i+=1;
    }
    else
      cmdLineError();
  }

  //dynamically set the number of input samples to the time for
  //important polls as well as the time between each poll
  NUMINPUTSAMPLES=(int)(PULSETIMECONSIDERED/POLLDELAYSEC);

  //set up environment
  alleg_init();

  //set up parallel port IO
  ParallelPortIO ppio;
  ppio.setSimulated(dosimulate);
  //set up simulator (used if needed)
  Simulator sim;
  //set up GUI
  GUI g;

  double lastpolltime=0.0;

  do {
    
    if (ppio.getSimulated())
      sim.update(&ppio);
    else {      
      ppio.addPoll(TACHBUSADDR,((double)clock())/CLOCKS_PER_SEC,ppio.pollSensor(TACHBUSADDR));
      ppio.addPoll(SPEEDBUSADDR,((double)clock())/CLOCKS_PER_SEC,ppio.pollSensor(SPEEDBUSADDR));
    }
 
    g.incrementPoll();
    g.update(&ppio);
    

    //delay
    while (((double)clock())/CLOCKS_PER_SEC<lastpolltime+POLLDELAYSEC) { }
    lastpolltime=(double)clock()/CLOCKS_PER_SEC;

  } while (true); 
}

/*
 * Sets up keyboard, timer and graphics mode.
 */
void alleg_init() {
  if (allegro_init()!=0) {
    exit(1);
  }  
  install_keyboard();

  set_color_depth(8);
  set_gfx_mode(GFX_AUTODETECT,RESOLUTIONX,RESOLUTIONY,0,0); 
  text_mode(-1);
}


void cmdLineError() {

  printf("Invalid usage of CF!\n");
  printf("Proper command line usage is of the form:\n");
  printf("  cf.exe [-S] [-R X Y] [-T t] [-D t] [-U n]\n");
  printf("-S\tSimulate sensor inputs (default is to not simulate)\n");
  printf("-R\tResolution of graphics, where X and Y are pixels (default 640 480)\n");
  printf("-T\tTime of polls taken t seconds ago considered (default is 1.0)\n");
  printf("-D\tDelay t seconds between polls (default is 0.05)\n");
  printf("-U\tUpdate GUI n times per second (default is 10)\n");

  exit(-1);

}

ParallelPortIO::ParallelPortIO() {

  for (int i=0;i<NUMINPUTSENSORS;i++) {
    inputsequence[i]=new Poll[NUMINPUTSAMPLES];
    for (int j=0;j<NUMINPUTSAMPLES;j++)
      inputsequence[i][j]=Poll(0.0,0);
  }
}

void ParallelPortIO::setSimulated(bool simulated) {

  this->simulated=simulated;

}

bool ParallelPortIO::getSimulated() {

  return simulated;

}

/*
 * Note: To be used by Simulator only
 */
void ParallelPortIO::addPoll(int whichsensor, double timestamp, unsigned char reading) {

  //set reading and timestamp for poll
  inputsequence[whichsensor][beginindex[whichsensor]].setReading(reading);
  inputsequence[whichsensor][beginindex[whichsensor]].setTimestamp(timestamp);

  //iterate to next poll element in array
  beginindex[whichsensor]++;
  beginindex[whichsensor]=beginindex[whichsensor]%NUMINPUTSAMPLES;

}

int ParallelPortIO::pollSensor(int sensoraddress) {
  
  outp(outport, (1<<(sensoraddress*2)));
  int busdata=inp(inport);
  outp(outport, (2<<(sensoraddress*2)));
  outp(outport, 0);

  return (busdata>>4)^8;

}

Poll *ParallelPortIO::obtainPolls(int busaddress) {

  return inputsequence[busaddress];

}

GUI::GUI() {
 
  lastpolls=0;
  numpolls=0;
  lastsecond=0.0;

  

  doublebuffer=create_bitmap(RESOLUTIONX,RESOLUTIONY);
  clear_bitmap(doublebuffer);

  loadGraphics();
  prepareTachometer();
  prepareSpeedometer();

  lastupdate=double(clock());
  currentscreen=0;
  
}

void GUI::loadGraphics() {

  char *eachfilename;

  logo=load_pcx("images/uprlogo.pcx",NULL); 
  optionmenu=load_pcx("images/optmenu.pcx",NULL);
  for (int i=0;i<10;i++) {
    sprintf(eachfilename,"images/%i.pcx",i);
    numbers[i]=load_pcx(eachfilename,NULL);
  }
  for (int i=0;i<13;i++) {
    sprintf(eachfilename,"images/s%i.pcx",i*20);
    numbers20[i]=load_pcx(eachfilename,NULL);
  }
  needle=load_pcx("images/needle.pcx",NULL);
  whiteline=load_pcx("images/whiteln.pcx",NULL);
  smallredline=load_pcx("images/smredln.pcx",NULL);
  bigredline=load_pcx("images/bigredln.pcx",NULL);
  rpmx1000=load_pcx("images/rpmx1000.pcx",NULL);
  kmperh=load_pcx("images/kmh.pcx",NULL);
  bgcarimg=load_pcx("images/bgcarimg.pcx",NULL);

}

void GUI::prepareTachometer() {

  float numx;
  float numy;
  int count=0;
  int eachnum=0;

  //prepare image of tachometer
  //create it at one resolution (scale it as necessary for others)
  BITMAP *tachometerbig=create_bitmap(GAUGERESOLUTIONX,GAUGERESOLUTIONY);
  tachometer=create_bitmap(RESOLUTIONX*3/4,RESOLUTIONY*7/8);
  clear_bitmap(tachometerbig);
  clear_bitmap(tachometer);
  for (float i=-45.0;i<=225.0;i+=270.0*(200.0/MAXTACHOMETERRANGE)) {
    count=count%5;
    if (count==0) {
      numx=cos((i*256.0/360.0)+128.0)*170.0;
      numy=sin((i*256.0/360.0)+128.0)*170.0;
      draw_sprite(tachometerbig,numbers[eachnum],GAUGERESOLUTIONX/2+(int)numx-12,GAUGERESOLUTIONY/2+(int)numy-12);
      eachnum++;
    }

    if ((i+45.0)/270.0>TACHOMETERREDLINE/MAXTACHOMETERRANGE)
      pivot_sprite(tachometerbig,bigredline,GAUGERESOLUTIONX/2,GAUGERESOLUTIONY/2,150,5,ftofix(i*256.0/360.0));
    else {      
      if (count==0) {
        pivot_sprite(tachometerbig,whiteline,GAUGERESOLUTIONX/2,GAUGERESOLUTIONY/2,150,5,ftofix(i*256.0/360.0));                
      }
      else
        pivot_sprite(tachometerbig,smallredline,GAUGERESOLUTIONX/2,GAUGERESOLUTIONY/2,150,5,ftofix(i*256.0/360.0));      
    }

    count++;
  }
  draw_sprite(tachometerbig,rpmx1000,GAUGERESOLUTIONX/2-35,GAUGERESOLUTIONY/2+100);

  //scale tach to proper resolution
  stretch_sprite(tachometer,tachometerbig,0,0,RESOLUTIONX*3/4,RESOLUTIONY);

}

void GUI::prepareSpeedometer() {

  float numx;
  float numy;
  int count=0;
  int eachnum=0;

  //prepare image of speedometer
  //create it at one resolution (scale it as necessary for others)
  BITMAP *speedometerbig=create_bitmap(GAUGERESOLUTIONX,GAUGERESOLUTIONY);
  speedometer=create_bitmap(RESOLUTIONX*3/4,RESOLUTIONY*7/8);
  clear_bitmap(speedometerbig);
  clear_bitmap(speedometer);
  for (float i=-45.0;i<=225.0;i+=270.0*(2.0/MAXSPEEDOMETERRANGE)) {
    count=count%10;
    if (count==0) {
      numx=cos((i*256.0/360.0)+128.0)*170.0;
      numy=sin((i*256.0/360.0)+128.0)*170.0;
      draw_sprite(speedometerbig,numbers20[eachnum],GAUGERESOLUTIONX/2+(int)numx-12,GAUGERESOLUTIONY/2+(int)numy-12);
      eachnum++;
    }
   
    if (count==0) {
      pivot_sprite(speedometerbig,whiteline,GAUGERESOLUTIONX/2,GAUGERESOLUTIONY/2,150,5,ftofix(i*256.0/360.0));                
    }
    else if (count==5) {
      pivot_sprite(speedometerbig,bigredline,GAUGERESOLUTIONX/2,GAUGERESOLUTIONY/2,150,5,ftofix(i*256.0/360.0));                
    }
    else {
      pivot_sprite(speedometerbig,smallredline,GAUGERESOLUTIONX/2,GAUGERESOLUTIONY/2,150,5,ftofix(i*256.0/360.0));      
    }

    count++;
  }
  draw_sprite(speedometerbig,kmperh,GAUGERESOLUTIONX/2-25,GAUGERESOLUTIONY/2+100);

  //scale speedo to proper resolution
  stretch_sprite(speedometer,speedometerbig,0,0,RESOLUTIONX*3/4,RESOLUTIONY);

}

void GUI::update(ParallelPortIO *ppio) {
   
  int keyread;

  //update only UPDATESPERSEC times per second
  if ((double(clock())-lastupdate)/CLOCKS_PER_SEC<=1.0/UPDATESPERSEC) return;
  lastupdate=double(clock());

  clear_to_color(doublebuffer,16);

  draw_sprite(doublebuffer,logo,0,0);
  draw_sprite(doublebuffer,bgcarimg,RESOLUTIONX-bgcarimg->w,RESOLUTIONY-bgcarimg->h);

  if (currentscreen==0)   //main menu
    draw_sprite(doublebuffer,optionmenu,50,100);
  else if (currentscreen==1) //tachometer
    drawTachometer(ppio);
  else if (currentscreen==2)
    drawSpeedometer(ppio);

  draw_sprite(screen,doublebuffer,0,0);

  //handle keypresses
  if (keypressed()) {
    keyread=readkey();

    if ((keyread&0xff)=='q')
      exit(0);    
    else if ((keyread&0xff)=='1')
      currentscreen=1;
    else if ((keyread&0xff)=='2')
      currentscreen=2;
    else
      currentscreen=0;
  }
  
  clear_keybuf();

}

void GUI::incrementPoll() {
  numpolls++;
}

void GUI::drawTachometer(ParallelPortIO *ppio) {

  //"fixed" is an allegro-specific unit of measure for an angle
  //a 32-bit data structure that represents a fixed point number
  //(a complete circle in "fixed" is represented by 256)

  Poll *currentpolls;

  int count;   
  
  float calculatedrpm;
  float needleangle;

  //draw tachometer sprite at correct scale
  draw_sprite(doublebuffer,tachometer,RESOLUTIONX/8,RESOLUTIONY/8);

  //process polls and calculate the rpm
  //calculated by considering the number of pulses to occur in a second
  //also note that 2 pulses occur for each revolution
  //preliminary formula:
  // E(pollreadingsinlastsecond)*60(sec/min)/2(pulse/revolution)
  calculatedrpm=0.0;
  count=0;
  currentpolls=ppio->obtainPolls(TACHBUSADDR);  
  for (int i=0;i<NUMINPUTSAMPLES;i++) {
    if ((((double)clock())/CLOCKS_PER_SEC-currentpolls[i].getTimestamp())<PULSETIMECONSIDERED&&currentpolls[i].getReading()<=15) {
      calculatedrpm+=(double)currentpolls[i].getReading();
      count++;
    }
  }
  calculatedrpm=calculatedrpm*60/2/PULSETIMECONSIDERED;
    
  //draw needle
  needleangle=-45.0+(270.0*calculatedrpm/MAXTACHOMETERRANGE);
  pivot_scaled_sprite(doublebuffer,needle,RESOLUTIONX/2,RESOLUTIONY*5/8,101,7,ftofix(needleangle*256.0/360.0),ftofix((float)RESOLUTIONY/GAUGERESOLUTIONY));
  
  textprintf(doublebuffer, font, 0,RESOLUTIONY/4,15, "RPM: %.1f", calculatedrpm);
  textprintf(doublebuffer, font, 0,RESOLUTIONY/4+10,15, "PPS: %i", lastpolls);

  if ((double)clock()/CLOCKS_PER_SEC>lastsecond+1.0) {
    lastpolls=numpolls;
    numpolls=0;
    lastsecond=(double)clock()/CLOCKS_PER_SEC;
  }

}

void GUI::drawSpeedometer(ParallelPortIO *ppio) {

 
  Poll *currentpolls;

  int count;   
  
  float calculatedspeed;
  float needleangle;

  //draw tachometer sprite at correct scale
  draw_sprite(doublebuffer,speedometer,RESOLUTIONX/8,RESOLUTIONY/8);

  //process polls and calculate speed
  //calculated by considering the number of pulses to occur in a second
  //also note that 2 pulses occur for each magnetic revolution for the speedometer
  //preliminary formula:
  // E(pollreadingsinlastsecond)*60(sec/min)/2(pulse/revolution)
  calculatedspeed=0.0;
  count=0;
  currentpolls=ppio->obtainPolls(SPEEDBUSADDR);  
 
  for (int i=0;i<NUMINPUTSAMPLES;i++) {
    if ((((double)clock())/CLOCKS_PER_SEC-currentpolls[i].getTimestamp())<PULSETIMECONSIDERED&&currentpolls[i].getReading()<=15) {
      calculatedspeed+=(double)currentpolls[i].getReading();      
      count++;
    }    
  }
   
  calculatedspeed=calculatedspeed*3600.0/PULSETIMECONSIDERED/SPEEDPULSESPERKM/2.0;
    
  //draw needle
  needleangle=-45.0+(270.0*calculatedspeed/MAXSPEEDOMETERRANGE);
  pivot_scaled_sprite(doublebuffer,needle,RESOLUTIONX/2,RESOLUTIONY*5/8,101,7,ftofix(needleangle*256.0/360.0),ftofix((float)RESOLUTIONY/GAUGERESOLUTIONY));
  
  textprintf(doublebuffer, font, 0,RESOLUTIONY/4,15, "SPD: %.1f", calculatedspeed);
  textprintf(doublebuffer, font, 0,RESOLUTIONY/4+10,15, "PPS: %i", lastpolls);

  if ((double)clock()/CLOCKS_PER_SEC>lastsecond+1.0) {
    lastpolls=numpolls;
    numpolls=0;
    lastsecond=(double)clock()/CLOCKS_PER_SEC;
  }

}

Poll::Poll() {

  timestamp=-5.0;
  reading=0;

}

Poll::Poll(double timestamp, unsigned char reading) {

  this->timestamp=timestamp;
  this->reading=reading;

}

void Poll::setReading(unsigned char reading) {
  this->reading=reading;
}

void Poll::setTimestamp(double timestamp) {
  this->timestamp=timestamp;
}

double Poll::getTimestamp() {
  return timestamp;
}

unsigned char Poll::getReading() {
  return reading;
}

Simulator::Simulator() {

  state=0;
  rpm=800;
  speed=0;
  gear=1;
  eventtimestamp=double(clock());
  tachpulsemissedlastpoll=0.0;
  speedpulsemissedlastpoll=0.0;
 
}

void Simulator::update(ParallelPortIO *ppio) {

  double deltatime=((double)clock())/CLOCKS_PER_SEC-eventtimestamp;
  float deltarpm;
  float deltaspeed;
  int newreading;

  if (state==0) {
    deltarpm=-1000;
    deltaspeed=-50;
  }
  else if (state==1) {
    deltarpm=1600;
    deltaspeed=20;
  }
  else if (state==2) {
    deltarpm=1200; 
    deltaspeed=16;
  }
  else if (state==3) {
    deltarpm=600;
    deltaspeed=12;
  }
  else if (state==4) {
    deltarpm=400;
    deltaspeed=8;
  }
  else if (state==5) {
    deltarpm=200;
    deltaspeed=4;
  }

  rpm+=deltarpm*deltatime;
  speed+=deltaspeed*deltatime;

  if (rpm<700) rpm=700;

  eventtimestamp=((double)clock())/CLOCKS_PER_SEC;

  //create new poll with eventtimestamp and approximate reading for tach  
  newreading=(int)((rpm*2/60.0)*deltatime+tachpulsemissedlastpoll);
  tachpulsemissedlastpoll=((rpm*2/60.0)*deltatime+tachpulsemissedlastpoll)-newreading;
  ppio->addPoll(TACHBUSADDR,eventtimestamp,newreading);

  //create new poll with eventtimestamp and approximate reading for speedo  
  newreading=(int)((speed*SPEEDPULSESPERKM/3600.0*2.0*deltatime)+speedpulsemissedlastpoll);
  speedpulsemissedlastpoll=((speed*SPEEDPULSESPERKM/3600.0*2.0*deltatime)+speedpulsemissedlastpoll)-newreading;
  ppio->addPoll(SPEEDBUSADDR,eventtimestamp,newreading);

  //state change
  if (state!=0) {
    if (rpm>3500) {
      rpm=1600;
      state++;
      gear=state;
    }
  
    if (state>5)
      state=0;
  }
  else if (state==0) {
    if (speed<=10)
      state=1;
  }

}