
#include <string>
#include <iostream>
#include <iomanip>
#include <cstdio>
#include <fstream>
#include <sstream>
#include <vector>

#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/PolyDriver.h>

#include "moveLib.h"
#include "touchLib.h"
#include "dataLib.h"

using namespace std;
using namespace yarp::os;
using namespace yarp::dev;

const size_t pos_size = 16, hand_touch_size = 192, arm_touch_size = 384;

Port *lh, *rh, *la, *ra;

void printArr(double arr[], size_t size)
{
    cout << "[";
    //for(int i = size-1; i != 0; i--){
    for (int i = 0; i < size; i++)
    {
        cout << arr[i] << ", ";
    }
    cout << arr[size - 1] << "]" << endl;
}

string arrToStr(double arr[], size_t size){
    ostringstream oos;
    for(int i = 0; i < size-1; i++){
        oos << arr[i] << " ";
    }
    oos<<arr[size-1];
    return oos.str();
}

void cpy(double to[], double from[], size_t size)
{
    for (int i = 0; i < size; i++)
    {
        to[i] = from[i];
    }
}

void add(double to[], double from[], size_t size)
{
    if (from == NULL || to == NULL)
    {
        return;
    }
    for (int i = 0; i < size; i++)
    {
        to[i] += from[i];
    }
}

WholeData makeData(PolyDriver *pd)
{
    WholeData res;

    // arm pos
    double *buff = new double[pos_size];
    int n;
    getArmPos(pd, buff);
    res.pos = buff;

    // touch
    // left hand
    buff = new double[hand_touch_size];
    getTouch(lh, buff, n);
    res.leftHandT = buff;

    // right hand
    buff = new double[hand_touch_size];
    getTouch(rh, buff, n);
    res.rightHandT = buff;

    // left arm
    buff = new double[arm_touch_size];
    getTouch(la, buff, n);
    res.leftArmT = buff;

    // right arm
    buff = new double[arm_touch_size];
    getTouch(ra, buff, n);
    res.rightArmT = buff;
    return res;
}

/**
 *  Function for search by @&disVector
 */
void discover(PolyDriver *pd, double *homePos, vector<double *> &disVectors, vector<WholeData> &res)
{
    for (double *pos : disVectors)
    {
        // go to position
        double now[pos_size];
        cpy(now, homePos, pos_size);
        add(now, pos, pos_size);
        myMoveArm(pd, lh, rh, la, ra, now, pos_size);
        res.push_back(makeData(pd));

        // go back
        myMoveArm(pd, lh, rh, la, ra, homePos, pos_size);
    }
}

/**
 * move arm via @*pd
 * @&moveVector - vectors for changing position
 * @&disVector vector with changes for search
 * @nuCycles number of iterations
 * &res place where configurations will be set 
 */
void moveCycleL(PolyDriver *pd, MoveVectros &moveVectors, vector<double *> &disVectors, size_t numCycles, vector<WholeData> &res)
{
    // copying data, because we don't want to change parameter
    double now[pos_size], tmpIn[pos_size], tmpOut[pos_size];
    cpy(now, moveVectors.start, pos_size);
    cpy(tmpIn, moveVectors.in, pos_size);
    cpy(tmpOut, moveVectors.out, pos_size);

    bool touch;
    for (size_t i = 0; i < numCycles; i++)
    {
        // step forward
        cout<<"robot add"<<endl;
        printArr(now, pos_size);
        add(now, tmpIn, pos_size);
        printArr(now, pos_size);
        cout<<"robot add end"<<endl;
        touch = myMoveArm(pd, lh, rh, la, ra, now, pos_size);
        if (touch)
        {
            res.push_back(makeData(pd));
        }
        else
        {
            //discover(pd, now, disVectors, res);
        }

        // step back
        add(now, tmpOut, pos_size);
        myMoveArm(pd, lh, rh, la, ra, now, pos_size);
        res.push_back(makeData(pd));

        // changing movment
        add(tmpIn, moveVectors.chIn, pos_size);
        add(tmpOut, moveVectors.chOut, pos_size);
    }
}



inline bool fileExists(const string& fileName){
    
    if(FILE *file = fopen(fileName.c_str(), "r")){
        fclose(file);
        return true;
    }else{
        return false;
    }
}

/**
 *  Function that connect to iCub_SIM
 *  and order it to move arms, while
 *  colectiong info about touches
 */
int main()
{

    // init yarp
    Network yarp;
    if (!yarp.checkNetwork())
    {
        cout << "yarp server was not foud" << endl;
        return 1;
    }

    // init touch ports
    Port lh1, rh1, la1, ra1;
    leftHandPort(&lh1);
    lh = &lh1;
    rightHandPort(&rh1);
    rh = &rh1;
    leftForearmPort(&la1);
    la = &la1;
    rightForearmPort(&ra1);
    ra = &ra1;
    // return 0;

    // init arm drivers
    PolyDriver *leftMove = makeMoveDriver("/icubSim/left_arm", "/test/clientl");
    PolyDriver *rightMove = makeMoveDriver("/icubSim/right_arm", "/test/clientr");
    maxSpeed(leftMove);
    maxSpeed(rightMove);

    // home positioms
    double posL[] = {-28, 16, 66, 71, 0, 0, 0, 58, 20, 20, 20, 10, 10, 10, 10, 10};
    double posR[] = {-28, 23,  6, 50, 0, 0, 0, 58, 20, 20, 20, 10, 10, 10, 10, 10};
    // return 0;
    // move to start position
    myMoveArm(leftMove, lh, rh, la, ra, posL, pos_size);
    myMoveArm(rightMove, lh, rh, la, ra, posR, pos_size);

    double stay[] = {2,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0};
    double goIn[] = {0,-2, 0, 9, 0, 0,0,0,0,0,0,0,0,0,0,0};
    double gOut[] = {0, 7, 0, 0, 0, 0,0,0,0,0,0,0,0,0,0,0};

    int maxVec = 3;
    vector<double*> search;
    for(int i = 0; i < maxVec; i++){
        for(int j = i; i < maxVec; i++){
            for(int l = -4; l < 10; l+=10){
                double *arr = new double[16];
                for(int k = 0; k < 16;k++){
                    arr[k] = 0;
                }
                arr[i] = l;
                arr[j] = l;
                search.push_back(arr);
            }
        }    
    }

    MoveVectros movement;
    movement.start = posL;
    movement.in    = goIn;
    movement.out   = gOut;
    movement.chIn  = stay;
    movement.chOut = stay;
    
    vector<WholeData> res;
    moveCycleL(leftMove, movement, search, 3, res);
    string fileName = "collected_data/data.rawdata";
    bool exist = fileExists(fileName);
    ofstream out;
    out.open(fileName, std::ios::app);
    if(!exist){
        out << "[\n";
    }
    for(auto i = res.begin(); i != res.end(); ++i){
        out << "{"<< endl;
        out << "\"left-pos\": \"" << arrToStr((*i).leftPos, pos_size) << "\"," << endl;
        out << "\"right-pos\": \"" << arrToStr(posR, pos_size) << "\"," << endl;
        out << "\"left-hand\" : \"" << arrToStr((*i).leftHandT, hand_touch_size) << "\"," << endl;
        out << "\"right-hand\" : \"" << arrToStr((*i).rightHandT, hand_touch_size) << "\"," << endl;
        out << "\"left-forearm\" : \"" << arrToStr((*i).leftArmT, arm_touch_size) << "\"," << endl;
        out << "\"right-forearm\" : \"" << arrToStr((*i).leftArmT, arm_touch_size) << "\"" << endl;
        out << "}"<<endl;
        out << ","<<endl;
    }
    return 0;
}
