// Read analog inputs from TS-9700 and TS-7650 and write to shared memory
// This is a periodic (1Hz) background task
// It may also perform 'brainstem' basic logic functions

// Note: shared memory max size appears to be 256 bytes on TS-7260

#include <stdio.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include "../io/io2.h"

// PC104 base address in TS=72xx address space
#define PC104_8BIT_IO_BASE 0x11C00000
#define PC104_8BIT_LEGACY_IO_BASE 0x11E00000

// Address in PC104 space of PC104 cards (determined by jumpers on board)
#define DIO64_BASE 0x100
#define AD1_BASE 0x160
#define AD2_BASE 0x168

// TS9700 #1
#define AD1COMMAND AD1_BASE
#define AD1DATA AD1_BASE + 2
#define AD1READYBIT 7

// TS9700 #2
#define AD2COMMAND AD2_BASE
#define AD2DATA AD2_BASE + 2
#define AD2READYBIT 7

// TS-DIO64
#define DIO64_ID DIO64_BASE
#define DIO64_OUT DIO64_BASE + 4
#define DIO64_IN DIO64_BASE + 8

// DIO 0 and 1 (lcd 0-7 and dio0 0-7)
#define DIO_BASE 0x80840000
#define DIO_0_DATA 0
#define DIO_0_DDR 0x10
#define DIO_1_DATA 4
#define DIO_1_DDR 0x14
// DIO 3 is lcd header en/rs/wr bits
#define DIO_3_DATA 40
#define DIO_3_DDR 0x44
#define DIO_3_MASK 0xE0

#define USEC_PER_SEC 1000000


main(){
  byte *pc104base;
  byte *ts9700base1;
  byte *ts9700base2;
  byte *diobase;
  byte *dio64base;
  volatile byte *ad1command;
  volatile byte *ad2command;
  volatile byte *cardid;
  volatile byte *lsb1;
  volatile byte *msb1;
  volatile byte *lsb2;
  volatile byte *msb2;
  // DIO 0 is lcd header bits 0-7
  volatile byte *dio0data;
  volatile byte *dio0ddr;
  // DIO 1 is dio1 header bits 0-7
  volatile byte *dio1data;
  volatile byte *dio1ddr;
  // DIO 3 is lcd header en/rs/wr bits
  volatile byte *dio3data;
  volatile byte *dio3ddr;
  byte dio3databuffer;
  byte dio3ddrbuffer;
  // DIO 64
  //volatile byte (*dio64_out)[4];
  //volatile byte (*dio64_in)[4];
  volatile byte *dio64_out;
  volatile byte *dio64_in;
   
  int fd;
  float reading;
  float volts;
  int i,j;
  FILE *fp;
  int temp[50];
  float lookup[16][36];
  float gain[20];
  float offset[20];
  char tname[80];
  char fname[20];
  short raw, oldraw;
  byte ts_dio64_1, ts_9700_1, ts_9700_2;
  
  struct timeval now;
  long start_usec;
  long now_usec;
  int shmid;
  key_t key;
  char *shmat();
  //short *shm, *s;
  shmstruct *shm;
   
  // Key for our shared memory is 9700
  
  key = 9700;
  
  printf("SHM size = %d\n", sizeof(shmstruct));
  
  // Create our shared memory
  if ((shmid = shmget(key, sizeof(shmstruct), IPC_CREAT | 0666)) < 0) {
    perror("shmget");
    exit(1);
  }
 
  // Link to our shared memory
  if ((shm=shmat(shmid, NULL, 0)) == (char *) -1) {
    perror("shmat");
    exit(1);
  }
  
  fd = open ("/dev/mem" , O_RDWR|O_SYNC);
  if (fd < 0){
      perror ("open");
      exit(1);
  }
  printf("Shared memory completed\n");
    
  // Read voltage lookup table data (Starts at 145 C, 5 degree increments)
  // Data from thermistor.csv, generated by thermistor.xls
  
  for (i=0;i<16;i++){
    sprintf(&fname,"lookup0%02d.csv",i);
    fp = fopen(fname,"r");
  
    while (fscanf(fp,"%d",&j)>0){
    fscanf(fp,"%s",&tname);
    printf("Channel %d: type %d thermistor\n",i,j);
    printf("  Range: %s\n",tname);
    fscanf(fp,"%s",&tname);
    printf("  Description: %s\n\n",tname);
    for(j=0;j<36;j++){
        fscanf(fp,"%f",&lookup[i][j]);
        //printf("  Lookup [%d][%d]: %f\n",i,j,lookup[i][j]);
        // Build corresponding entry in temp table. Starts at 145 deg C
        temp[j] = 145 - j*5;
      }
    }
  fclose(fp);
  }
  
  printf("Lookup tables read\n");
   
  // Read calibration data from file
  fp = fopen("thermistorcal.csv","r");
  i=0;    
  while (fscanf(fp,"%f,%f",&gain[i],&offset[i])>0){
    printf("Gain = %f, offset = %f\n",gain[i],offset[i]);
    i++;
  }
  fclose(fp);
  
  // **************************************************************************************
  // Map PC104 address space
  // 9700 and dio64 cards live there
  // **************************************************************************************
  
  pc104base = (byte *)mmap(0, getpagesize(),
          PROT_READ | PROT_WRITE, MAP_SHARED, fd, PC104_8BIT_IO_BASE);
  if (pc104base == MAP_FAILED){
      perror ("mmap");
      exit(1);
  }
  printf("ARM page size = %d\n",getpagesize());  
  
  // 9700 #1 pointers
     
  ad1command = pc104base + AD1COMMAND;
  cardid = pc104base + AD1COMMAND + 1;
  if (*cardid == 0x97){
    lsb1 = pc104base + AD1DATA;
    msb1 = pc104base + AD1DATA + 1;
  
    printf("TS-9700 Card #1  mapped: Card Id is %2x\n",*cardid);
    printf("  a/d cmd is %2x\n",*ad1command);
    printf("  lsb is %2x\n",*lsb1);
    printf("  msb is %2x\n",*msb1);
    ts_9700_1 = 1;
  }else{
    printf("TS-9700 Card #1 not found\n");
    ts_9700_1 = 0;
  }
  
  // 9700 #2 pointers
     
  ad2command = pc104base + AD2COMMAND;
  cardid = pc104base + AD2COMMAND + 1;
  if (*cardid == 0x97){
    lsb2 = pc104base + AD2DATA;
    msb2 = pc104base + AD2DATA + 1;
  
    printf("TS-9700 Card #2  mapped: Card Id is %2x\n",*cardid);
    printf("  a/d cmd is %2x\n",*ad2command);
    printf("  lsb is %2x\n",*lsb2);
    printf("  msb is %2x\n",*msb2);
    ts_9700_2 = 1;
  }else{
    printf("TS-9700 Card #2 not found\n");
    ts_9700_2 = 0;
  }
  
  // dio64 pointers
  // dio64 documentation says it's in 'legacy' PC104 space, (0x11E00100) but it appears 
  // at 0x11C00100
  
  cardid = pc104base + DIO64_ID;
  if (*cardid == 0xa4){
    dio64_out = pc104base + DIO64_OUT;
    dio64_in = pc104base + DIO64_IN;
    printf("TS-DIO64 Card mapped: Card Id is %2x\n",*cardid);
    ts_dio64_1 = 1;
  }else{
    printf("TS-DIO64 Card not found\n");
    ts_dio64_1 = 0;
  }

  // Map onboard (7260) dio address space
  diobase = (byte *)mmap(0, getpagesize(),
          PROT_READ | PROT_WRITE, MAP_SHARED, fd, DIO_BASE);
  if (diobase == MAP_FAILED){
      perror ("mmap");
      exit(1);
   }

  dio0data = diobase + DIO_0_DATA;
  dio0ddr = diobase + DIO_0_DDR;
  dio1data = diobase + DIO_1_DATA;
  dio1ddr = diobase + DIO_1_DDR;
  printf("Onboard DIO area mapped\n");
   
  gettimeofday(&now,NULL);
  start_usec = now.tv_sec * 1000000 + now.tv_usec;
  
  //******************************************************************************************
  //
  // Set initial conditions. For now, just dio direction. The control task may override.
  //
  //******************************************************************************************
  
  // dio 0 is input
  shm->diodir[0] = 0;
     
  // dio 1 is output
  shm->diodir[1] = 255;
     
  // dio 2 is input
  shm->diodir[2] = 0;
     
  // dio 3 is output
  shm->diodir[3] = 255;

  // dio 4 is input
  shm->diodir[4] = 0;
     
  // dio 5 is output
  shm->diodir[5] = 255;

  // dio 6 is input
  shm->diodir[6] = 0;
     
  // dio 7 is output
  shm->diodir[7] = 255;

  // dio 8 is input
  shm->diodir[8] = 0;
     
  // dio 9 is output
  shm->diodir[9] = 255;

  // Do forever
  while (1){
    
    // Cycle through channels, 9700 #1
    if(ts_9700_1){
      for (j=0; j<8; j++){
        // Double-cycle mode - should take 18 microseconds
        oldraw = shm->adraw[j];
        raw=5000;
        i=0;
        // Loop on noisy read - undiagnosed bug in 9700?
        while((abs(raw-oldraw) > 50) && (i++ <5)){
          *ad1command = j;
          usleep(20);
          // Make sure it's done
          while(!(*ad1command & (1 << AD1READYBIT)))
                ;
          raw= (short) (256 * *msb1 + *lsb1);
        }
        volts = (256 * *msb1 + *lsb1) * 2.5 / 4096;
        shm->adraw[j] = (short) (256 * *msb1 + *lsb1);
  
        // Out of range?
        if (volts < .01 || volts > 2.49 ){
          shm->advalue[j]=0;
          shm->adstatus[j]=0;
        }else{
          // Calibrate
          volts = volts * (1+gain[j]) +  offset[j];
          
          // Find proper cell in lookup table. 
          // value of temp (vt) is calculated from value of voltage (vv)
          // Lookup tables give upper and lower volts and temp
          // vt = lt + ((vv-lv) * (ut-lt) / (uv-lv)
          
          i=0;
          while (lookup[j][i] < volts){
            i++;
          }
          
          reading = temp[i-1] + ((volts-lookup[j][i-1]) * (temp[i]-temp[i-1]) / (lookup[j][i]-lookup[j][i-1]));
          // We store centigrade temp * 90 as short integer - divides evenly for both, maximizes resolution
          
          shm->advalue[j] = reading;
          shm->adstatus[j] = 1;
          //printf("Ch %d int %d raw = %d Volts = %f, reading = %f, lu1 %f lu2 %f \n",j,i,raw,volts,reading,lookup[j][i],lookup[j][i-1]);          
        }
      }
    } 
    // Cycle through channels, 9700 #2
    if(ts_9700_2){
      for (j=8; j<16; j++){
        // Double-cycle mode - should take 18 microseconds
        oldraw = shm->adraw[j];
        raw=5000;
        i=0;
        // Loop on noisy read - undiagnosed bug in 9700?
        while((abs(raw-oldraw) > 50) && (i++ <5)){
          *ad2command = j;
          usleep(20);
          // Make sure it's done
          while(!(*ad2command & (1 << AD2READYBIT)))
                ;
          raw= (short) (256 * *msb2 + *lsb2);
        }
        volts = (256 * *msb2 + *lsb2) * 2.5 / 4096;
        shm->adraw[j] = (short) (256 * *msb2 + *lsb2);
  
        reading = ((volts * 1000) - 32)/1.8;
        
        // Scale combustion 1.6 mv/deg (thermocouples)
        //if (j == 8){
        if (j == 8){
          reading = (volts * 713.7 - 4.72);
        }
  
        if (j == 9){
          reading = (volts * 726.47 - 3.34);
        }
  
        // Set inactive channels to 20 degrees.
        //if (j > 8){
        if (j > 9){
          reading = 20;
        }
        
        shm->advalue[j]=reading;
        shm->adstatus[j]=1;
        //printf("Ch %d int %d raw = %d Volts = %f, reading = %f, lu1 %f lu2 %f \n",j,i,raw,volts,reading,lookup[j][i],lookup[j][i-1]);          
      }
    }
    //printf("\n");
    
    // lcd 0-7
    *dio0ddr = shm->diodir[0];
    *dio0data = shm->diovalue[0];
    shm->diovalue[0] = *dio0data;
    
    // dio 0 0-7
    *dio1ddr = shm->diodir[1];
    *dio1data = shm->diovalue[1];
    shm->diovalue[1] = *dio1data;
   //printf ("Dir: %x, Cmd %x, Data: %x\n",shm->dio[0].dir,shm->dio[0].value,*dio0data);
    
    // lcd en/rs/wr
    //dio3ddrbuffer = (*dio3ddr & DIO_3_MASK) & (shm->dio[3].dir & !DIO_3_MASK);
    //*dio3ddr = dio3ddrbuffer;
    //dio3databuffer = (*dio3data & DIO_3_MASK) & (shm->dio[3].value & !DIO_3_MASK);
    //*dio3data = dio3databuffer;
    //shm->dio[3].value = *dio3data & DIO_3_MASK;
    
    //*dio3ddr = shm->dio[3].dir;
    //*dio3data = shm->dio[3].value;
    //shm->dio[3].value = *dio3data;
/*    
    // dio 2-5: dio64 out0-out3
    shm->dio[2].value = *dio64_in[0];
    
    *dio64_out[0] = shm->dio[3].value;
    shm->dio[3].value = *dio64_out[0];
*/    

    // Process dio64
    if(ts_dio64_1 == 1){    
      // dio 2: dio64 in0
      shm->diovalue[2] = dio64_in[0];
      
      // dio 3: dio64 out0
      dio64_out[0] = shm->diovalue[3];
      shm->diovalue[3] = dio64_out[0];
      
      // dio 4: dio64 in1
      shm->diovalue[4] = dio64_in[1];
  
      // dio 5: dio64 out1
      //printf("dio0 = %d, dio1 =%d\n",shm->diovalue[5],*dio64_out[1]);
      dio64_out[1] = shm->diovalue[5];
      shm->diovalue[5] = dio64_out[1];

      //printf("dio5 = %d\n",shm->diovalue[5]);

      // dio 6: dio64 in2
      shm->diovalue[6] = dio64_in[2];
  
      // dio 7: dio64 out2
      dio64_out[2] = shm->diovalue[7];
      shm->diovalue[7] = dio64_out[2];
      
      // dio 8: dio64 in3
      shm->diovalue[8] = dio64_in[3];
  
      // dio 9: dio64 out3
      dio64_out[3] = shm->diovalue[9];
      shm->diovalue[9] = dio64_out[3];
      
      /*
      for(i=0;i<4;i++){
        printf("Ch %d in:%02X %X out:%02X %X\n",i,dio64_in[i],&dio64_in[i],dio64_out[i],&dio64_out[i]);
      }
      */

    }else{
      printf("ts_dio64_1 = %d\n",ts_dio64_1);
    }

    // 1 second interval
    start_usec += 1000000;
    gettimeofday(&now,NULL);
    now_usec = now.tv_sec * 1000000 + now.tv_usec;
    usleep(start_usec - now_usec);
  }
}