#include "Utils.h"
#include "Utils2.h"
#include "ctime"
#include "cstdlib"
#include "cstdio"

/////////////////////////////////////////////////////////////////////////////////////////
// Utility functions
/////////////////////////////////////////////////////////////////////////////////////////

long GetCurrentTimeTick()
{
    return (long)time(NULL);
}

long GetElapseTime(long lastTime)
{
    return (long)((long)time(NULL) - lastTime);
}

void GetCurrentCPUTime(std::clock_t &tmStart)
{
    //
    tmStart = std::clock();
}

double GetElapseCPUTime(const std::clock_t &tmStart)
{
    //
    return (std::clock() - tmStart) / (double)CLOCKS_PER_SEC;
}

bool IsBoolArrayAllTrue(bool *bArray, int size)
{
    for (int i = 0; i < size; ++i)
    {
        if (bArray[i] == false)
        {
            return false;
        }
    }
    return true;
}

void AppendIntVec(vector<int> &dest, const vector<int> &appending)
{
    for (unsigned int i = 0; i < appending.size(); ++i)
    {
        dest.push_back(appending[i]);
    }
}

bool IsSetContainer(const set<int> &container, const set<int> &contained)
{
    for (set<int>::iterator it = contained.begin(); it != contained.end(); ++it)
    {
        if (container.find(*it) == container.end())
        {
            return false;
        }
    }
    return true;
}

bool IsSetContainedInSets(const set<int> &s, const set<set<int>> &sets)
{
    // This function return true if ONE of the sets in sets contains s, false otherwise
    for (set<set<int>>::iterator it = sets.begin(); it != sets.end(); ++it)
    {
        if (IsSetContainer(*it, s) == true)
        {
            return true;
        }
    }
    return false;
}

bool IsSetContainingOneOfSets(const set<int> &s, const set<set<int>> &sets)
{
    // This function return true if ONE of the sets in sets contains s, false otherwise
    for (set<set<int>>::iterator it = sets.begin(); it != sets.end(); ++it)
    {
        if (IsSetContainer(s, *it) == true)
        {
            return true;
        }
    }
    return false;
}

void ConcatIntVec(vector<int> &vecAdded, const vector<int> &vecToAdd)
{
    // Append one vector to another
    for (unsigned int i = 0; i < vecToAdd.size(); i++)
    {
        vecAdded.push_back(vecToAdd[i]);
    }
}

int ConvIntSetToPosition(const set<int> &s)
{
    //cout << "In ConvIntSetToPosition: s = ";
    //DumpIntSet( s );
    // this function convert an integer set to a position index, for example,
    // if range = 8, s={2, 4}, then this converts to 00010100
    int res = 0;
    for (set<int>::iterator it = s.begin(); it != s.end(); ++it)
    {
        int a = *it;
        int mask = 0x1 << a;
        res = res | mask;
    }
    //cout << "conversion res = " << res << endl;
    return res;
}

void ConvPositionToIntSet(int val, set<int> &s)
{
    // inverse to ConvIntSetToPosition: convert an integer back to a set
    s.clear();
    int pos = 0;
    while (val != 0)
    {
        if ((val & 0x1) != 0)
        {
            s.insert(pos);
        }
        pos++;
        // left-shift val
        val = (val >> 1);
    }
}

void PopulateSetWithInterval(set<int> &s, int left, int right)
{
    s.clear();
    for (int i = left; i <= right; ++i)
    {
        s.insert(i);
    }
}

void GetSeqInterval(const SEQUENCE &row, SEQUENCE &rowIV, int left, int right)
{
    rowIV.clear();
    for (int i = left; i <= right; ++i)
    {
        rowIV.push_back(row[i]);
    }
}

bool IsIntervalContained(const set<SEQUENCE> &seqs, int left, int right, const SEQUENCE &seqIV)
{
    // This function check to see if the seqIV is contained in the seqs
    // when there is missing site, we use COMPABILITY instead of ==
    for (set<SEQUENCE>::iterator it = seqs.begin(); it != seqs.end(); ++it)
    {
        SEQUENCE substr;
        GetSeqInterval(*it, substr, left, right);
        if (AreTwoSeqsCompatible(substr, seqIV) == true)
        {
            return true;
        }
    }
    return false;
}

void SubtractSequenceSets(set<SEQUENCE> &s1, const set<SEQUENCE> &s2)
{
    if (s2.size() == 0)
    {
        return;
    }
    set<SEQUENCE> res;
    // this function performs set intersection, i.e. s1=s1 ^s2
    for (set<SEQUENCE>::iterator it = s1.begin(); it != s1.end(); ++it)
    {
        if (s2.find(*it) == s2.end())
        {
            res.insert(*it);
        }
    }
    s1.clear();
    s1 = res;
}

void DumpSequence(const SEQUENCE &seq)
{
    for (unsigned int i = 0; i < seq.size(); ++i)
    {
        if (IsMissingValueBit(seq[i]) == false)
        {
            cout << seq[i];
        }
        else
        {
            cout << "*";
        }
    }
    cout << endl;
}

void DumpVecSequences(const vector<SEQUENCE> &vecSeqs)
{
    for (unsigned int i = 0; i < vecSeqs.size(); ++i)
    {
        DumpSequence(vecSeqs[i]);
    }
}

void DumpSetSequences(const set<SEQUENCE> &setSeqs)
{
    for (set<SEQUENCE>::iterator it = setSeqs.begin(); it != setSeqs.end(); ++it)
    {
        DumpSequence(*it);
    }
}

bool AreTwoInSameSet(int i1, int i2, const set<set<int>> &collections)
{
    // Check to see if i1 and i2 is in same set
    for (set<set<int>>::iterator it = collections.begin(); it != collections.end(); ++it)
    {
        bool found1 = false, found2 = false;
        if (it->find(i1) != it->end())
        {
            found1 = true;
        }
        if (it->find(i2) != it->end())
        {
            found2 = true;
        }
        if (found1 == true && found2 == true)
        {
            //cout << "i1 = " << i1 << ", i2 = " << i2 << " INDDED in same set.\n";
            return true;
        }
        if (found1 || found2)
        {
            //cout << "i1 = " << i1 << ", i2 = " << i2 << " not in same set.\n";
            return false;
        }
    }
    // should not need this, in case
    YW_ASSERT_INFO(false, "Bad i1 or i2.");
    return false;
}

int GetItemIndexInVec(const vector<int> &vec, int item)
{
    for (unsigned int i = 0; i < vec.size(); ++i)
    {
        if (vec[i] == item)
        {
            return i;
        }
    }
    return -1;
}

bool IsIntervalOverlap(const INTERVAL &iv1, const INTERVAL &iv2)
{
    if (iv1.second < iv2.first || iv2.second < iv1.first)
    {
        return false;
    }
    else
    {
        return true;
    }
}

bool GetIntervalOverlap(const INTERVAL &iv1, const INTERVAL &iv2, INTERVAL &ivBoth)
{
    int left = iv1.first;
    if (left < iv2.first)
    {
        left = iv2.first;
    }
    int right = iv1.second;
    if (right > iv2.second)
    {
        right = iv2.second;
    }
    if (left > right)
    {
        return false;
    }
    else
    {
        ivBoth.first = left;
        ivBoth.second = right;
        return true;
    }
}

void GenerateRandBinVector(int sz, vector<int> &randVec)
{
    //cout << "GenerateRandBinVector: sz = " << sz << endl;
    // Generate random vector
    randVec.clear();
    for (int i = 0; i < sz; ++i)
    {
        //cout << " i = " << i << endl;
        double r = GetRandFraction();
        if (r >= 0.5)
        {
            randVec.push_back(0);
        }
        else
        {
            randVec.push_back(1);
        }
    }
}
bool IsBinary(int val)
{
    if (val == 0 || val == 1)
    {
        return true;
    }
    else
    {
        return false;
    }
}
void ReOrderWithRemovedSites(const vector<int> &posAfterRem,
                             const vector<int> &removedPos, vector<int> &posBeforeRemove)
{
    // THis funciton is often used here
    // For example, we often removed sites from the matrix but then we need to know their original positions
    // this function consider that by adding the removed sites back into order (not directly into posBeforeRem)
    // but rather consider them when adding
    posBeforeRemove.clear();

    unsigned int pos = 0;
    for (unsigned int i = 0; i < posAfterRem.size(); ++i)
    {
        while (pos < removedPos.size() && posAfterRem[i] + (int)pos >= removedPos[pos])
        {
            pos++;
        }
        posBeforeRemove.push_back(posAfterRem[i] + pos);
    }
}

void GetSubsetVec(const vector<int> &vecOriginal, const set<int> &sitesToKeep, vector<int> &vecNew)
{
    vecNew.clear();
    for (unsigned int i = 0; i < vecOriginal.size(); ++i)
    {
        if (sitesToKeep.find(i) != sitesToKeep.end())
        {
            vecNew.push_back(vecOriginal[i]);
        }
    }
}

void AddMissingVecBits(vector<int> &rowOrig, const set<int> &sitesToAdd, vector<int> &partialRow)
{
    YW_ASSERT_INFO(sitesToAdd.size() == partialRow.size(), "Parameter size mismatch");

    // If there is othing to work, stop
    if (sitesToAdd.size() == 0)
    {
        return;
    }

    cout << "AddMissingVecBits: rowOrig = ";
    DumpSequence(rowOrig);
    cout << "Append sites ";
    DumpIntSet(sitesToAdd);
    cout << "Missing values = ";
    DumpIntVec(partialRow);
    // Here we try to add back some missing sites
    vector<int> missingSites;
    PopulateVecBySet(missingSites, sitesToAdd);

    vector<int> res;
    int posMiss = 0;
    int posOrig = 0;
    int curpos = 0;

    while (posMiss < (int)partialRow.size() || posOrig < (int)rowOrig.size())
    {
        // check to see which bit to use and move
        if (curpos != missingSites[posMiss])
        {
            // This bit is original
            YW_ASSERT_INFO(posOrig < (int)rowOrig.size(), "Serious error: not enough bits.");
            res.push_back(rowOrig[posOrig]);
            posOrig++;
        }
        else
        {
            // No this is a missing bit
            res.push_back(partialRow[posMiss]);
            posMiss++;
            ;
        }

        // now move on
        curpos++;
    }
    rowOrig = res;
    cout << "AddMissingVecBits: res = ";
    DumpSequence(rowOrig);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
bool IsSequenceHaplotype(const SEQUENCE &seq)
{
    // note need to consider missing value!
    for (unsigned int i = 0; i < seq.size(); ++i)
    {
        if (seq[i] != 0 && seq[i] != 1 && IsMissingValueBit(seq[i]) == false)
        {
            return false;
        }
    }
    return true;
}

bool IsSequenceGenotype(const SEQUENCE &seq)
{
    for (unsigned int i = 0; i < seq.size(); ++i)
    {
        if (seq[i] != 0 && seq[i] != 1 && seq[i] != 2 && IsMissingValueBit(seq[i]) == false)
        {
            return false;
        }
    }
    return true;
}

bool CanPhaseGenoRow(const SEQUENCE &hap1, const SEQUENCE &hap2, const SEQUENCE &geno)
{
    YW_ASSERT_INFO(IsSequenceHaplotype(hap1), "hap1 is not haplotype row.");
    YW_ASSERT_INFO(IsSequenceHaplotype(hap2), "hap1 is not haplotype row.");
    YW_ASSERT_INFO(IsSequenceGenotype(geno), "hap1 is not haplotype row.");
    YW_ASSERT_INFO(hap1.size() == hap2.size(), "Tow hap rows are not equal length");
    YW_ASSERT_INFO(geno.size() == geno.size(), "Geno row is not the same size as hap row.");
    // for now, do not allow hap1/hap2 contian missing value
    YW_ASSERT_INFO(IsSeqHasMV(hap1) == false && IsSeqHasMV(hap2) == false, "Hap1/Hap2 can not contain missing values");
    //cout << "hap1 = ";
    //DumpIntVec(hap1 );
    //cout << "hap2 = ";
    //DumpIntVec(hap2 );
    //cout << "geno = ";
    //DumpIntVec( geno );
    for (unsigned int i = 0; i < hap1.size(); i++)
    {
        // a missing vlaue can be phased either way
        if (IsMissingValueBit(geno[i]) == true)
        {
            continue;
        }

        if (geno[i] == 2)
        {
            if (hap1[i] + hap2[i] != 1)
            {
                return false;
            }
        }
        else
        {
            if (hap1[i] + hap2[i] != 2 * geno[i])
            {
                return false;
            }
        }
    }
    return true;
}

bool AreHapGenoRowCompatible(const SEQUENCE &hapRow, const SEQUENCE &genoRow, SEQUENCE *pComplement)
{
    if (pComplement != NULL)
    {
        pComplement->clear();
    }

    // Check if the haplotype row can be a phasing of the geno row
    YW_ASSERT_INFO(IsSequenceHaplotype(hapRow), "hap is not haplotype row.");
    YW_ASSERT_INFO(IsSequenceGenotype(genoRow), "genorow is not haplotype row.");
    for (unsigned int i = 0; i < hapRow.size(); i++)
    {
        // if either one is missing value, they match!
        if (IsMissingValueBit(genoRow[i]) == true || IsMissingValueBit(hapRow[i]) == true)
        {
            continue;
        }

        if (genoRow[i] != 2)
        {
            if (hapRow[i] != genoRow[i])
            {
                return false;
            }
            else
            {
                if (pComplement != NULL)
                {
                    pComplement->push_back(genoRow[i]);
                }
            }
        }
        else
        {
            if (pComplement != NULL)
            {
                if (hapRow[i] == 0)
                {
                    pComplement->push_back(1);
                }
                else
                {
                    pComplement->push_back(0);
                }
            }
        }
    }
    return true;
}

bool AreHapGenoRowsSame(const SEQUENCE &hapRow, const SEQUENCE &genoRow)
{
    YW_ASSERT_INFO(IsSequenceHaplotype(hapRow), "hap is not haplotype row.");
    YW_ASSERT_INFO(IsSequenceGenotype(genoRow), "genorow is not haplotype row.");
    return AreTwoSeqsCompatible(hapRow, genoRow);
}

bool IsTrivialRow(const SEQUENCE &row, SEQUENCE &resolved1, SEQUENCE &resolved2)
{
    resolved1.clear();
    resolved2.clear();
    // A row is trivial if it contains only a single 2
    YW_ASSERT_INFO(IsSequenceGenotype(row), "hap is not haplotype row.");
    int num2s = 0;
    for (unsigned int i = 0; i < row.size(); ++i)
    {
        if (row[i] == 2)
        {
            ++num2s;
            if (num2s > 1)
            {
                break;
            }
        }
        if (row[i] == 2)
        {
            resolved1.push_back(0);
            resolved2.push_back(1);
        }
        else
        {
            resolved1.push_back(row[i]);
            resolved2.push_back(row[i]);
        }
    }
    if (num2s == 1)
    {
        // For now, we do not consider a row with no twos as new
        return true;
    }
    else
    {
        return false;
    }
}

bool IsHapSeqSmaller(const SEQUENCE &hapRow1, const SEQUENCE &hapRow2)
{
    // Decide whether hapRow1 is smaller
    // used in situations when we need to compare two rows
    YW_ASSERT_INFO(IsSequenceHaplotype(hapRow1), "hap1 is not haplotype row.");
    YW_ASSERT_INFO(IsSequenceHaplotype(hapRow2), "hap2 is not haplotype row.");
    YW_ASSERT_INFO(hapRow1.size() == hapRow2.size(), "Tow hap rows are not equal length");
    // do not handle MV in this function
    YW_ASSERT_INFO(IsSeqHasMV(hapRow1) == false && IsSeqHasMV(hapRow2) == false, "Can not handle MV here");

    for (unsigned int i = 0; i < hapRow1.size(); ++i)
    {
        if (hapRow1[i] < hapRow2[i])
        {
            return true;
        }
    }
    return false;
}

void GetHyperCubeSeq(int hcSeq, SEQUENCE &seq, int hcWidth)
{
    ConvIntToVecMSB(hcSeq, seq, hcWidth);
}

int GetSeqIdFromSeq(const SEQUENCE &seq)
{
    // do not support MV
    YW_ASSERT_INFO(IsSeqHasMV(seq) == false, "Can not support MV");
    return ConvVecToIntMSB(seq);
}

int GetHyperCubSeqBit(int hcSeq, int bit, int hcWidth)
{
    // Retrive the bit in the hcSeq at the specified bit
    // But  note that we are assuming bit 0 is on the left (BIG ENDIAN)
    int shiftpos = hcWidth - bit - 1;
    int mask = 0x1 << shiftpos;
    int res = (hcSeq & mask) >> shiftpos;
    YW_ASSERT_INFO(res == 0 || res == 1, "Serious error here.");
    return res;
}

void FindNonSegSites(const set<HCSequence> &setSeqs, set<int> &sites, int dataWidth)
{
    // Now we find out whe mutation sites that are not used in the setSeqs
    // Find the set of sites that are not segragating in the set of sequences
    for (int i = 0; i < dataWidth; ++i)
    {
        bool fZero = false, fOne = false;
        // Check to see if site i is segragating or not
        for (set<int>::iterator it = setSeqs.begin(); it != setSeqs.end(); ++it)
        {
            int rn = *it;
            //cout <<"In FindNonSegragateSites: rn = " << rn << endl;
            if (GetHyperCubSeqBit(rn, i, dataWidth) == 0)
            {
                fZero = true;
            }
            else
            {
                fOne = true;
            }
            if (fZero && fOne)
            {
                break;
            }
        }
        if (fZero == false || fOne == false)
        {
            sites.insert(i);
        }
    }
}

void FindNonSegSites(const set<SEQUENCE> &setSeqs, set<int> &sites, int dataLen)
{
    sites.clear();
    if (setSeqs.size() == 0)
    {
        // Every one is non-segragating
        for (int i = 0; i < dataLen; ++i)
        {
            sites.insert(i);
        }
        return;
    }

    for (int i = 0; i < dataLen; ++i)
    {
        bool fZero = false, fOne = false;
        // Check to see if site i is segragating or not
        for (set<SEQUENCE>::iterator it = setSeqs.begin(); it != setSeqs.end(); ++it)
        {
            SEQUENCE row = *it;
            YW_ASSERT_INFO(IsSequenceHaplotype(row), "This function only works for haplotype");
            //cout <<"In FindNonSegragateSites: rn = " << rn << endl;
            if (row[i] == 0)
            {
                fZero = true;
            }
            else if (row[i] == 1)
            {
                fOne = true;
            }
            if (fZero && fOne)
            {
                break;
            }
        }
        if (fZero == false || fOne == false)
        {
            sites.insert(i);
        }
    }
}

void CreateGenoRowFromHapRows(const SEQUENCE &hapRow1, const SEQUENCE &hapRow2, SEQUENCE &genoRow)
{
    // Check if the haplotype row can be a phasing of the geno row
    YW_ASSERT_INFO(IsSequenceHaplotype(hapRow1), "hap1 is not haplotype row.");
    YW_ASSERT_INFO(IsSequenceHaplotype(hapRow2), "hap2 is not haplotype row.");
    // do not allow missing vlaues
    YW_ASSERT_INFO(IsSeqHasMV(hapRow1) == false && IsSeqHasMV(hapRow2) == false, "Can not handle MV");
    genoRow.clear();
    for (unsigned int i = 0; i < hapRow1.size(); i++)
    {
        if (hapRow1[i] == hapRow2[i])
        {
            genoRow.push_back(hapRow1[i]);
        }
        else
        {
            genoRow.push_back(2);
        }
    }
}

int IsHCSeqsMutPair(HCSequence seq1, HCSequence seq2, int dataWidth)
{
    // This function test if seq1/seq2 is mutation pair, and return the mut site if so
    for (int p = 0; p < dataWidth; p++)
    {
        int shiftpos = dataWidth - p - 1;
        int mask = 0x1 << shiftpos;

        if ((seq1 | mask) == (seq2 | mask))
        {
            return p;
        }
    }

    return -1; // indicate NOT-pair
}

bool IsHCSeqsMutPairAt(HCSequence seq1, HCSequence seq2, int dataWidth, int pos)
{
    // Different from the previous function, this check for a specific location
    // instead of trying all possible positions
    int shiftpos = dataWidth - pos - 1;
    int mask = 0x1 << shiftpos;

    if ((seq1 | mask) == (seq2 | mask))
    {
        return true;
    }
    return false;
}

void MutateHCSeqAt(const HCSequence seq, HCSequence &res, int dataWidth, int mutPos)
{
    int shiftpos = dataWidth - mutPos - 1;
    int mask = 0x1 << shiftpos;

    res = (seq ^ mask);
}

bool IsHCSeqRecombinnable(HCSequence hcSeq1, HCSequence hcSeq2, HCSequence st, int dataWidth)
{
    // ASSUME: s1 is LEFT part and s2 is RIGHT part
    // This function test if s1 and s2 can recombine into st
    // Now start recombining
    for (int bkpt = 0; bkpt < dataWidth - 1; ++bkpt)
    {
        unsigned int maskLower = (0x1 << (bkpt + 1)) - 1;
        unsigned int maskUpper = (0x1 << dataWidth) - 1 - maskLower;

        // Generate s sequence
        int seq1 = ((hcSeq1 & maskLower) | (hcSeq2 & maskUpper));
        if (seq1 == st)
        {
            return true;
        }
#if 0 // here we
        int seq2 = ((hcSeq1 & maskUpper)  | (hcSeq2 & maskLower) );
        if( seq2 == st)
        {
            return true;
        }
#endif
    }

    return false;
}

void RecombineHCSeqs(const HCSequence hcSeq1, const HCSequence hcSeq2, HCSequence &res, int dataWidth, int bkpt)
{
    // ASSUME: s1 is LEFT part and s2 is RIGHT part
    // This function test if s1 and s2 can recombine into st
    // Now start recombining
    unsigned int maskLower = (0x1 << (bkpt + 1)) - 1;
    unsigned int maskUpper = (0x1 << dataWidth) - 1 - maskLower;

    // Generate s sequence
    res = ((hcSeq1 & maskLower) | (hcSeq2 & maskUpper));
}
