#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>
#include <fstream>
#include <cstdio>
#include <cstdlib>
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>
#include "gluex_newCanlib_debug.h"
#include <cstring>
#include "gluex_pChCanlib.h"
#include <stdlib.h>
#include "includeAnaGate/AnaGateDllCan.h"
#include <time.h>


using namespace std;

int portNum;
int anaNum;
string anaIPs[8];
string anaPorts[4];
Device *dev[4];
int nReplies[4]={100,100,100,100};
int nPortsTot;
int ext_id = 0x00000000;
int linecount = 0;
string dummy;
int bases_on_strand=100;

//----------------------------------------------------------


//Things to edit between full FCAL and test setup:
//bases_on_strand
//IP & port
//big for loop


//This program does everything for a single FCAL ANAGATE port.
//It sets bootloader byte, uploads,
//and resets to firmware mode with proper power switching

//Also, this determines number of responses on a strand, so things
//can run even if a base is not responding at startup

//This code assumes you have a proper BASEFIRMWARE.HEX file in the
//current directory



int ResetBases(Device &dev, int nReplies);

int my_failure_exit(Device &dev, vector<int> initial, int nReplies);


int main( int argc, char **argv ){

    if(argc!=3) {
        cout << "ERROR: Unexpected number of arguments!!!" << endl;
        return 1;
    }

    int my_index = atoi(argv[1]);
    bool break_on_error = atoi(argv[2])==1 ? true : false;
    if(break_on_error) cout << "Exiting if errors are encountered (recommended)" << endl;
    else               cout << "Not exiting if errors are encountered (recommended only to find poorly communicating bases)" << endl;
    time_t start_time;
    time(&start_time);
      /*

//Note: must do each port separately due to power resets!!!

    anaIPs[0]="129.57.26.115";
    anaIPs[1]="129.57.26.107";
    anaIPs[2]="129.57.26.116";
    anaIPs[3]="129.57.26.109";
    anaIPs[4]="129.57.26.112";
    anaIPs[5]="129.57.26.108";
    anaIPs[6]="129.57.26.114";

    anaPorts[0]="A";
    anaPorts[1]="B";
    anaPorts[2]="C";
    anaPorts[3]="D";


    for(int i=0; i<7; ++i) {
      for(int j=0; j<4; ++j) {
        if(i*4+j==atoi(argv[1])) {
            dev[0] = OpenCANBus((char*)anaPorts[j].c_str(),(char*)anaIPs[i].c_str());
            cout << "Opening anagate at IP: " << anaIPs[i] << " port " << anaPorts[j] << endl;
        }
      }
    }
      */



      //Test setup in 351
    //anaIPs[0]="129.57.194.11";
    //nReplies[0]=1;


    //For test setup in EEL
    //anaIPs[0]="129.57.194.19";
    //nReplies[0]=5;
    //anaPorts[0]="B";


    //For testing single strand (#10) on FCAL
    anaIPs[0]="129.57.26.112";
    anaPorts[0]="B";




    //for(int i=0; i<4; ++i) {
    for(int i = 0; i<1; ++i) {
        dev[i] = OpenCANBus((char*)anaPorts[i].c_str(),(char*)anaIPs[my_index].c_str());
        cout << "Power cycling..." << endl;
        WriteDigital(*dev[0], 0x0f);
        usleep(100000);
        WriteDigital(*dev[0], 0x00);
        usleep(11000000);

        //Use this to get the true number of replies, then turn any LEDs off again
        cout << "Getting actual number of replies on port " << anaPorts[i] << endl;
        vector<int> retVals;
        LightBad(*dev[i], ext_id, nReplies[i], retVals); //Used to get number of replies
        LEDSwitch(*dev[i], ext_id, 0x02);
        nReplies[i] = retVals.size();
        cout << "Number of replies = " << nReplies[i] << endl;



        //Switch to bootloader mode, power cycle
        //Power cycle before too, just in case
        cout << endl<<"Switching to Bootloader Mode... " << endl;
        ResetBases(*dev[i],nReplies[i]);
        WriteStat(*dev[i], ext_id, nReplies[i], 1);
        usleep(1000000);
        int post_modeswitch_replies = ResetBases(*dev[i],nReplies[i]);
        if(post_modeswitch_replies!=nReplies[i]){
            cout << "Error: missing bases after switching to bootloader!!!" << endl;
            my_failure_exit(*dev[i],retVals,nReplies[i]);
            continue;
        }
        //cout << "Power cycle now" << endl;
        //cin >> dummy;


        //Clear firmware
          cout << "Clearing firmware..." << endl;
        BootloaderClear(*dev[i], ext_id, nReplies[i]);
        usleep(1000000);
        int post_clear_replies = ResetBases(*dev[i],nReplies[i]);
        if(post_clear_replies!=nReplies[i]){
            cout << "Error: missing bases after clearing!!!" << endl;
            my_failure_exit(*dev[i],retVals,nReplies[i]);
            continue;
        }
        //cout << "Power cycle now" << endl;
        //cin >> dummy;

        //Write new firmware
        cout << "Writing new firmware..." << endl;
        vector<int> retVals2;
        int upload_fail = BootloaderAct(*dev[i], ext_id, nReplies[i], linecount, retVals2, break_on_error);
        usleep(1000000);
        int post_upload_replies = ResetBases(*dev[i],nReplies[i]);
        if(post_upload_replies!=nReplies[i]){
            cout << "Error: missing bases after uploading!!!" << endl;
              my_failure_exit(*dev[i],retVals,nReplies[i]);
           continue;
        }
        //cout << "Power cycle now" << endl;
        //cin >> dummy;

//Bases can get (and have been) bricked if you're not careful
//So I require someone to manually put bases back into firmware
//mode right now.

          if(bases_on_strand==post_upload_replies&&upload_fail==0) {
            cout << endl <<"Got expected number of replies after, probably safe to switch to firmware mode"<< endl;
          }
          if(bases_on_strand!=post_upload) cout << "WARNING! Not all bases on strand responded at start." << endl;
          if(upload_fail!=0) cout << "WARNING! UPLOAD ENCOUNTERED ERRORS!!!! DO NOT SWITCH TO FW MODE!!!!" << endl;


        //}
      LEDSwitch(*dev[i], ext_id, 0x02); //Turns off any lit up bases

        //cout << "Size of error vector is: " << retVals2.size() << endl;
    }

      time_t end_time;
      time(&end_time);
      double time_diff = - difftime(start_time,end_time)/60.;

      cout << "Process took " << time_diff << " minutes to run" << endl;
      cout << "Done. Values read from DAC may not work properly until you reset everything by hand." << endl;


return 0;
}

int ResetBases(Device &dev, int nReplies){


  //Does this twice, as sometimes bases go into error state on first reset
  //I've never seen bases go into error state on later resets

        cout << "Power cycling..." << endl;

        WriteDigital(dev, 0x0f);
        usleep(100000);
        WriteDigital(dev, 0x00);

        WriteDigital(dev, 0x0f);
        usleep(100000);
        WriteDigital(dev, 0x00);


        //Bases also seem to need ~11 seconds to leave error state on their own
        usleep(11000000);


        vector<int> retVals;
        LightBad(dev, 0x00000000, nReplies, retVals);

          int trynum=0;
          int n_responses=retVals.size();
          while(n_responses!=nReplies&&trynum!=20){
            cout << "Not all bases are responding. Resetting again... (try " << trynum << ")" << endl;
            vector<int> stuff;
            trynum++;
            WriteDigital(dev, 0x0f);
            usleep(100000);
            WriteDigital(dev, 0x00);
            usleep(11000000);
            LightBad(dev, 0x00000000, nReplies, stuff);
            n_responses=stuff.size();
          }

          if(n_responses!=nReplies) cout << "Error: could not get all bases to respond while resetting after 20 tries " << endl;

        return retVals.size();
}

int my_failure_exit(Device &dev, vector<int> initial, int nReplies) {

  cout << "Bases communicating at startup:" << endl;
  for(unsigned int ii = 0; ii<initial.size(); ++ii) {
    cout << initial[ii] << endl;
  }
  cout << initial.size() << " total bases at startup" << endl << endl;


  vector<int> final;
  LightBad(dev, ext_id, nReplies, final);
  cout << "Bases communicating at failure:" << endl;
  for(unsigned int ii = 0; ii<initial.size(); ++ii) {
    cout << initial[ii] << endl;
  }
  cout << initial.size() << " total bases at failure" << endl << endl;


  cout << "Bases that stopped responding during upload procedure: " << endl;
  for(unsigned int ii =0; ii < initial.size(); ++ii) {
    int initial_id = initial[ii];
    bool found = false;
    for(unsigned int j=0; j<final.size(); ++j) {
      if(initial_id==final[ii]) found = true;
    }
    if(!found) cout << initial[ii] << endl;
  }

  return 0;
}