#include "DataPacker.h"
#include <stdio.h>

unsigned char * DataPacker::msg;

unsigned char * DataPacker::packData(STATION *station) {
    int pos = 26;
    DataPacker packer;
    msg = packer.staticDataPack(station);
    pos = packer.dynamicDataPack(station, 5, pos);    
    pos = packer.dynamicDataPack(station, 6, pos);
    pos = packer.dynamicDataPack(station, 7, pos);
    pos = packer.dynamicDataPack(station, 8, pos);
    // Update data for BYTE 9, 16, 21, 22, 23, 24

    // what is the current metering rate?
    // check each LDS, if there is demand detector, the downstream node the
    // ldsMap[i].dataPack[22-1] = ldsMap[i].dataPack[9-1] = ;

    // will need to check if queue override is enabled (Currently, no queue
    // ldsMap[i].dataPack[16-1] = ;

    // byte 23: MAINLINE total volume
    msg[23 - 1] = station->MlTotVol;

    // byte 24: opposite total volume
    msg[24 - 1] = station->OppTotVol;

    // last BYTE: checksum 
    msg[station->length - 1] =
            packer.chksum(msg, station->length - 1 - 1);
    
    for (int j = 0; j < station->length; j++) {
        printf("%02X", msg[j]);
    }
    printf("\n");

    return msg;	
}

int DataPacker::dynamicDataPack(STATION *station, int packNo, int pos) {
    int i, j, vol, occ, haveData;
    //used for data byte 23 and 24
    station->MlTotVol = 0;
    station->OppTotVol = 0;

    for (j = 1; j <= 8; j++) {
        
        if (DataAvail(msg[packNo - 1], j)) {
            // loop at bit j has data
            for (int lane = 0; lane < station->loops.size(); lane++) {
                // what's the corresponding loop of bit j
                char * currLoopLoc = station->loops.at(lane)->loop_loc;
                if (packNo == 5)
                    haveData = strcmp(currLoopLoc, dp5[j - 1]);
                else if (packNo == 6)
                    haveData = strcmp(currLoopLoc, dp6[j - 1]);
                else if (packNo == 7)
                    haveData = strcmp(currLoopLoc, dp7[j - 1]);
                else if (packNo == 8)
                    haveData = strcmp(currLoopLoc, dp8[j - 1]);
                else
                    return -1;
                
                // if we have data (strcmp returns 0 if a string match)
                if (haveData == 0) {
                    LOOP *currLoop = station->loops.at(lane);
                    vol = currLoop->vol;
                    occ = (int) (currLoop->occ * 900 + 0.5);
                    cout << "Volume: " << vol << endl;
                    cout << "OCC : " << occ << endl;
                    VOLOCC packedVOLOCC = packVOLOCC(vol, occ);
                    pos++;
                    msg[pos - 1] = packedVOLOCC.high;
                    pos++;
                    msg[pos - 1] = packedVOLOCC.low;
                    
                    if (packNo == 5) {
                        station->MlTotVol += vol;
                    } else if (packNo == 6) {
                        station->OppTotVol += vol;
                    }
                }
            }
        }
    }
    return pos;
}

unsigned char * DataPacker::staticDataPack(STATION *station) {
    int j;
    // Allocate memory for dataPack
    unsigned char *dataPack = (unsigned char *) calloc(sizeof(unsigned char), station->length);

    // dataPack 5-8: lane config
    char d5 = 0, d6 = 0, d7 = 0, d8 = 0;
    for (j = 0; j < station->loops.size(); j++) {
        for (int k = 0; k < 8; k++) {
            char * currLoopLoc = station->loops.at(j)->loop_loc;
            if (strcmp(currLoopLoc, dp5[k]) == 0)
                d5 += pow(2, k);
            if (strcmp(currLoopLoc, dp6[k]) == 0)
                d6 += pow(2, k);
            if (strcmp(currLoopLoc, dp7[k]) == 0)
                d7 += pow(2, k);
            if (strcmp(currLoopLoc, dp8[k]) == 0)
                d8 += pow(2, k);
        }
    }
    dataPack[5 - 1] = d5;
    dataPack[6 - 1] = d6;
    dataPack[7 - 1] = d7;
    dataPack[8 - 1] = d8;

    // dataPack 1: Drop number, i.e. station address
    dataPack[1 - 1] = station->drop;
    // dataPack2 (2 bytes per loop)
    dataPack[2 - 1] = station->loops.size() * 2 + Fixed_Byte_To_Checksum;

    // dataPacket 3 (lowbyte: # of mainline loops, highbyte: # of opposite loops)
    int low = 0, high = 0;
    for (j = 1; j <= 6; j++) {
        low += DataAvail(dataPack[5 - 1], j);
        high += DataAvail(dataPack[6 - 1], j);
    }
    high = high << 4;
    dataPack[3 - 1] = high | low;

    // dataPack4 (Miscl. flags: samples are: 80, A0, E0, 00)
    dataPack[4 - 1] = 0xA0;

    // dataPack 9: initialized as 00 (meaning no metering); need to be updated every 30 sec
    dataPack[9 - 1] = 0;

    // datadataPack 10-13: lane malfunction? Assuming all functional
    dataPack[10 - 1] = 0;
    dataPack[11 - 1] = 0;
    dataPack[12 - 1] = 0;
    dataPack[13 - 1] = 0;

    // dataPack 14-22: ramp metering data
    // BYTE 16 and 22 need to be updated every 30 sec
    bool found = false;
    for (j = 0; j < station->loops.size(); j++) {
        if (strcmp(station->loops.at(j)->loop_loc, "DEMAND") == 0) {
            found = true;
            break;
        }
    }
    if (found) {
        // BYTE 14: mostly 07, some are 05, 03, 00
        dataPack[14 - 1] = 0x07;
        // mostly 06(TOD table 1); some are 0B (No metering) or 05(traffic responsive)
        dataPack[15 - 1] = 0x06;
        // most 00, some are 01 (queue override) or 80(Meter ON sign)
        dataPack[16 - 1] = 0x00;
        // Field Manual Rate
        dataPack[17 - 1] = 0xFF;
        // TOC Manual Rate
        dataPack[18 - 1] = 0xFF;
        // PSO Manual Rate
        dataPack[19 - 1] = 0xFF;
        // CORM Rate
        dataPack[20 - 1] = 0xFF;
        // Local Responsive Rate. DON'T UNDERSTAND YET
        dataPack[21 - 1] = 0x00;
        // TOD Rate: need to query RAMP plugin!
        dataPack[22 - 1] = 0x00;
    }// LDS: NO Metering
    else {
        dataPack[14 - 1] = 0x00;
        dataPack[15 - 1] = 0x0B;
        dataPack[16 - 1] = 0x00;
        dataPack[17 - 1] = 0xFF;
        dataPack[18 - 1] = 0xFF;
        dataPack[19 - 1] = 0xFF;
        dataPack[20 - 1] = 0xFF;
        dataPack[21 - 1] = 0x00;
        dataPack[22 - 1] = 0x00;
    }

    // dataPack 23-24: sum of mainline/Opp traffic data; need to be updated every 30 sec
    station->MlTotVol = 0;
    station->OppTotVol = 0;
    dataPack[23 - 1] = station->MlTotVol;
    dataPack[24 - 1] = station->OppTotVol;

    // dataPack 25-26: BYTE 25 is fixed, i.e. 03; BYTE 26 is either 0xA2 or 0x84
    dataPack[25 - 1] = 0x03;
    dataPack[26 - 1] = 0x84;

    return dataPack;
}

bool DataPacker::DataAvail(char flag, int num) {
    int mag, fel;

    // find mask value
    if (num == 1)
        mag = 0x01;
    else if (num == 2)
        mag = 0x02;
    else if (num == 3)
        mag = 0x04;
    else if (num == 4)
        mag = 0x08;
    else if (num == 5)
        mag = 0x10;
    else if (num == 6)
        mag = 0x20;
    else if (num == 7)
        mag = 0x40;
    else if (num == 8)
        mag = 0x80;

    fel = flag & mag;
    fel = fel >> (num - 1);

    if (fel == 1)
        return true;
    else
        return false;
}

// checksum based on data from byte 1 (after 0DOA) to the second last byte (the 
char DataPacker::chksum(unsigned char *dataptr, int len) {
    int i;
    unsigned char checksum = 0;

    for (i = 0; i < len; i++)
        checksum += dataptr[i];

    return checksum;
}

// convert vol and occ data to a two-byte data packet
VOLOCC DataPacker::packVOLOCC(int vol, int occ)
{
   int bit, i;
   int   high, low; 
   int high_high = 0, high_low = 0;
   VOLOCC com;
   
   // occ data: 10 bits
   low = 0;
   for (i=9; i>=0; i--) 
   {
      if (i >= 8)
      {
         bit = ((occ >> i) & 1);
         high_low += pow(2, (i - 8)) * bit; 
      }
      // lower byte
      else
      {
         bit = ((occ >> i) & 1);
         low += pow(2, i) * bit;
      }
   }

   // vol: 6 bits (1. vol, shift to the left for two times; 2. combine data)
   high_high = vol << 2;
   high = high_high | high_low;

   com.high = high;
   com.low = low;

   return com;
}