#include "halio.h"
#include <vector>
#include <kinematics/scarakins.h>
#include <interface.h>


int unsigned comp_id=0;
int unsigned comp_id2=0;




joint_hal_t *joint[pr][3];
axis_hal_t  *axis[pr][3];
process_hal_t *process[pr];
scara_hal_t *scara_hal;
delta_hal_t *delta_hal;






//Scara scara_hal;
int halio::Init(){

     comp_id = hal_init("core");

    // *Joint[pr][3];
for(unsigned int i=0; i<pr; i++){
   process[i] = (process_hal_t*)hal_malloc(sizeof(process_hal_t));
    for(unsigned int k=0; k<5; k++)
    {
        char nameprocess[20]="process";
        char charprocess[3];
        sprintf(charprocess,".%d",i+1);
        strcat(nameprocess,charprocess);
        switch (k){
        case 0:
          strcat(nameprocess,".type-kinematic");
          std::cout << nameprocess << std::endl;
          hal_pin_u32_new(nameprocess,HAL_IN,&(process[i]->type_kinematic),comp_id);
        break;
        case 1:
          strcat(nameprocess,".enable");
          std::cout << nameprocess << std::endl;
          hal_pin_bit_new(nameprocess,HAL_IN,&(process[i]->enable),comp_id);
        break;
        case 2:
          strcat(nameprocess,".current-vel");
          std::cout << nameprocess << std::endl;
          hal_pin_float_new(nameprocess,HAL_OUT,&(process[i]->current_vel),comp_id);
        break;
        case 3:
          strcat(nameprocess,".current-acc");
          std::cout << nameprocess << std::endl;
          hal_pin_float_new(nameprocess,HAL_OUT,&(process[i]->current_acc),comp_id);
        break;
        default:
         break;
        }


       }
    }

     for(unsigned int i=0; i<pr; i++){
         for(unsigned int j=0; j<3; j++){
             joint[i][j] = (joint_hal_t*)hal_malloc(sizeof(joint_hal_t));
         for(unsigned int k=0; k<6; k++){
             char name[20]="joint";
             char charprocess[3];
             sprintf(charprocess,".%d",i+1);
             strcat(name,charprocess);
             char charnr[1];
             sprintf(charnr,".%d",j+1);
             strcat(name,charnr);



             switch (k){
             case 0:
               strcat(name,".pos-cmd");
               std::cout << name << std::endl;
               hal_pin_float_new(name,HAL_OUT,&(joint[i][j]->pos_cmd),comp_id);
             break;
             case 1:
               strcat(name,".pos-fb");
               std::cout << name << std::endl;
               hal_pin_float_new(name,HAL_OUT,&(joint[i][j]->pos_fb),comp_id);
             break;
             case 2:
               strcat(name,".motor-pos-fb");
               std::cout << name << std::endl;
               hal_pin_float_new(name,HAL_IN,&(joint[i][j]->motor_pos_fb),comp_id);
             break;
             case 3:
               strcat(name,".motor-pos-cmd");
               std::cout << name << std::endl;
               hal_pin_float_new(name,HAL_OUT,&(joint[i][j]->motor_pos_cmd),comp_id);
             break;
             case 4:
               strcat(name,".motor-offset");
               std::cout << name << std::endl;

               hal_pin_float_new(name,HAL_IN,&(joint[i][j]->motor_offset),comp_id);
             break;
             case 5:
             strcat(name,".enable");
             std::cout << name << std::endl;
             hal_pin_bit_new(name,HAL_OUT,&(joint[i][j]->enable),comp_id);
             break;
             default:
              break;
             }


            }
         }
      }
     for(unsigned int i=0; i<pr; i++){
         for(unsigned int j=0; j<3; j++){
         axis[i][j] = (axis_hal_t*)hal_malloc(sizeof(axis_hal_t));
         for(unsigned int k=0; k<2; k++){



     char nam[10] ="axis";

     char charprocess[3];
     sprintf(charprocess,".%d",i+1);
     strcat(nam,charprocess);

     //char c = "xyz"[i];
     switch (j){
     case 0:
         strcat(nam,".x");
         break;
     case 1:
         strcat(nam,".y");
         break;
     case 2:
         strcat(nam,".z");
         break;
     default:
      break;
     }

     switch (k){
     case 0:
         strcat(nam,".pos_cmd");
         std::cout << nam << std::endl;
         hal_pin_float_new(nam,HAL_OUT,&(axis[i][j]->pos_cmd),comp_id);
         break;
     case 1:
         strcat(nam,".eoffset");
         std::cout << nam << std::endl;
         hal_pin_s32_new(nam,HAL_OUT,&(axis[i][j]->eoffset),comp_id);
         break;
     default:
      break;
     }



}
         }
     }



    int error = hal_ready(comp_id);
    if(error==0){
        std::cout << "Hal component ok" << std::endl;
        // Load hal file
        system("halcmd \-f config/Cascada.hal");
        //system("halcmd start");
        return 1; //ok go on
    } else {
        std::cout << "Hal component error, performing [halrun -U] now." << std::endl;
        system("halrun -U");
        std::cout << "Restart application required." << std::endl;
        return 0; //not good, show error output in terminal.
    }

    // Normally we don't get here.
    return 1;

}


int halio::Update(){

    *((hal_float_t *) (joint[1][0]->motor_pos_cmd)) = scara.J0 + *joint[1][0]->motor_offset;
    *((hal_float_t *) (joint[1][1]->motor_pos_cmd)) = scara.J1 + *joint[1][1]->motor_offset;
    *((hal_float_t *) (joint[1][2]->motor_pos_cmd)) = scara.J2 + *joint[1][2]->motor_offset;

    *((hal_float_t *) (joint[1][0]->pos_cmd)) = scara.J0;
    *((hal_float_t *) (joint[1][1]->pos_cmd)) = scara.J1;
    *((hal_float_t *) (joint[1][2]->pos_cmd)) = scara.J2;

    *((hal_float_t *) (axis[1][0]->pos_cmd)) = scara.tranX;
    *((hal_float_t *) (axis[1][1]->pos_cmd)) = scara.tranY;
    *((hal_float_t *) (axis[1][2]->pos_cmd)) = scara.tranZ;

    double oldScaraJ0=scara.J0;
    double oldScaraJ1=scara.J1;
    double oldScaraJ2=scara.J2;

    scara.J0=(*joint[1][0]->motor_pos_fb - *joint[1][0]->motor_offset)*(*joint[1][0]->enable);;
    scara.J1=(*joint[1][1]->motor_pos_fb - *joint[1][1]->motor_offset)*(*joint[1][1]->enable);;
    scara.J2=(*joint[1][2]->motor_pos_fb - *joint[1][2]->motor_offset)*(*joint[1][2]->enable);;

    scara=scaraKins().scaraKinematicsForward(scara);

    *((hal_float_t *) (joint[1][0]->pos_fb)) = scara.J0;
    *((hal_float_t *) (joint[1][0]->pos_fb)) = scara.J1;
    *((hal_float_t *) (joint[1][0]->pos_fb)) = scara.J2;



    scara.J0=oldScaraJ0;
    scara.J1=oldScaraJ1;
    scara.J2=oldScaraJ2;

    scara=scaraKins().scaraKinematicsForward(scara);
    return 1;
}

int halio::InitTwo(){
    std::cout << "Hal component ok" << std::endl;
    // Load hal file
    system("halcmd \-f config/CascadaTwo.hal");
    comp_id2 = hal_init("kin");
    for(unsigned int i=0; i<pr; i++){



    switch (*process[i]->type_kinematic) {
    case 1:
    {
        char nameprocess[20]="process";
        char charprocess[3];
        sprintf(charprocess,".%d",i+1);
        strcat(nameprocess,charprocess);
        delta_hal = (delta_hal_t*)hal_malloc(sizeof(delta_hal_t));
        strcat(nameprocess,".delta");
        for(unsigned int k=0; k<4; k++){
        std::cout << k << std::endl;
        switch (k) {
        case 0:
        //strcat(nameprocess,".footradius");
        //std::cout << nameprocess << std::endl;
        hal_pin_float_new(nameprocess,HAL_IN,&(delta_hal->footradius),comp_id2);
        break;
        case 1:
        strcat(nameprocess,".platformradius");
        std::cout << nameprocess << std::endl;
        hal_pin_float_new(nameprocess,HAL_IN,&(delta_hal->platformradius),comp_id2);
        break;
        case 2:
        strcat(nameprocess,".shinlength");
        std::cout << nameprocess << std::endl;
        hal_pin_float_new(nameprocess,HAL_IN,&(delta_hal->shinlength),comp_id2);
        break;
        case 3:
        strcat(nameprocess,".thighlength");
        std::cout << nameprocess << std::endl;
        hal_pin_float_new(nameprocess,HAL_IN,&(delta_hal->thighlength),comp_id2);
        break;
        }
         }
    break;
    }

    case 2:
        //strcat(nameprocess,".scara");
        //std::cout << nameprocess << std::endl;
        scara_hal = (scara_hal_t*)hal_malloc(sizeof(scara_hal_t));
        //hal_pin_float_new(nameprocess,HAL_IN,&(scara_hal->InArmLength),comp_id2);
        //hal_pin_float_new(nameprocess,HAL_IN,&(scara_hal->OutArmLength),comp_id2);
        break;

    default:
        break;

    }



}
     hal_ready(comp_id2);
    system("halcmd start");
}