/* LAN temperature controller based on Peltier element, AD590 */
/* temperature sensor, PIC18F4550 digitiser and UART, PL155-P */
/* power supply and Spartan 3E board for routing commands and */
/* data over ethernet                                         */
/* Copyright I.Tsurin Liverpool University V1.0 (30.11.2011)  */
/*                                                            */                       
/* Acknowledgment: Paulo Pedreiras, Out.2005 RCAI 05/06       */
/* http://www.ieeta.pt/~pedreiras/resources/rcai/raw-sr.c     */
/* Acknowledgment: Finn Haugen  http://techteach.no           */

/* Includes */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <string.h>
#include <sys/time.h>
#include <ctype.h>

#include <signal.h>
#include <sys/ioctl.h>

#include <sys/socket.h>
#include <netinet/in.h>

#include <linux/if.h>
#include <netpacket/packet.h>
#include <net/ethernet.h>  // the L2 protocols
#include <asm/types.h>

#define SAFE_LEN 402 // for 40ms timeout
#define DEVICE "eth0"

int s = 0;		  // Socket descriptor
unsigned char *eth_head;  // byte pointer to ethenet header
unsigned char *eth_data;  // byte pointer to ethernet data field
struct ethhdr *eh;	  // structured pointer to ethernet header

struct sockaddr_ll socket_address;
void *buffer = NULL;

/* Local host NIC MAC address */
unsigned char   src_mac[6] = {0x00, 0x1D, 0x09, 0xB1, 0x46, 0x6F};

/* Front end address template */
unsigned char dest_mac[6] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x00};

void sigint(int signum);
int eth_raw_init();

unsigned char l=1;
unsigned char O=0;

unsigned ePackID;
unsigned eCommID;


/* Board control */

#define RPlength 652

#define MCA   0
#define LU    2

#define MC    l
#define MB    O

#define Reset 0
#define Write 1
#define Read  2


/* Temperature controller */

#define Ndet  13

#define Ts  1. /* Sampling interval in seconds */
#define Tf 30. /* Time constant of the lowpass filter in seconds */
#define A  Ts / (Ts + Tf)

/* Plant model */

#define Del  10.
#define Tau 150.
#define Tc  Del

#define min(a, b) ((a < b) ? a:b)
#define Ti min(Tau, 2.*(Tc + Del))

#define Kp(Iact) Tau / (19.75 - 2.17 * Iact) / (Tc + Del)

float Tact;
float Iact = 0.;
float Eold = 0.;

/* Power limits */

#define Imin 0.
#define Imax 5.


/***************************************/
/*       Ethernet packet sender        */
/***************************************/

void PING(int Device, int Address, int Command, int Dsize)
{
  int i;

  ePackID = (rand()&0x7fff) | ((rand()&0x7fff)<<15);

/* Re-build ethernet header */

  dest_mac[0] =  (   MCA<<4 | Device<<3 | LU)&0xFF;
  dest_mac[1] = (Address<<4 | Command)&0xFF;
  dest_mac[2] = (ePackID>>24)&0xFF;
  dest_mac[3] = (ePackID>>16)&0xFF;
  dest_mac[4] = (ePackID>>8)&0xFF;
  dest_mac[5] = (ePackID>>0)&0xFF;

  eCommID = dest_mac[0]<<8 | (dest_mac[1]&0xF3);

  memcpy((void *)eh->h_dest,  (void*)dest_mac, ETH_ALEN);
  memcpy((void *)eh->h_source,(void*)src_mac,  ETH_ALEN);

/* Update data size */

  eh->h_proto = htons(Dsize);

/* Output data string */

  i = sendto(s, buffer, (Dsize + ETHER_HDR_LEN), 0,
               ((struct sockaddr*)&socket_address), sizeof(socket_address));

  if (i == -1) { perror("sendto():"); exit(1); }
}


/***************************************/
/*       Ethernet packet receiver      */
/***************************************/

int PONG(int Dsize)
{
  int i;
  int TimeOut = 100;

  while(TimeOut)
    {
      unsigned CommID;
      unsigned PackID;

/* Read NC buffer */

      i = recvfrom(s, buffer, ETHER_MAX_LEN, 0, NULL, NULL); // Dsize (?)

      if (i == -1) { perror("recfrom():"); exit(1); }

      CommID = eh->h_source[0]<<8  | (eh->h_source[1]&0xF3);
      PackID = eh->h_source[2]<<24 |  eh->h_source[3]<<16 | eh->h_source[4]<<8 | eh->h_source[5];

      if ((CommID == eCommID)&&(PackID == ePackID)) break; 

      TimeOut--;

    } /* end while(TimeOut) */

  return TimeOut;
}


/***************************************/
/*       Ethernet packet composer      */
/***************************************/

void WritePort(unsigned char *ComPtr)
{
  unsigned char *Start;

  int i, ind;
  int LL = strlen(ComPtr);
  int Dsize;

  unsigned char CC;


/* allow 50 ms settling time */

  usleep(50000);

/* reset pointer to beginning of the ethernet data field */

  eth_data = buffer + ETH_HLEN;
  Start = eth_data;


/* Loop over characters in the command */

  for (ind=0; ind<LL; ind++)
     {
       CC = *ComPtr++;

       *(eth_data++) = 0x00;  // Start bit

/* Loop over data bits */

        for (i=0; i<8; i++) *(eth_data++) = ((CC>>i) & 0x01);

       *(eth_data++) = 0x01;  // Stop bit
       *(eth_data++) = 0x01;  // Safety delay

     } /* end for(LL) */

  Dsize = (int)(eth_data - Start);

  if (Dsize<SAFE_LEN) PING(MC, MCA, Write, Dsize);

} /* end WritePort */


/*********************************/
/*       Init power source       */
/*********************************/

void PL155init(void)
{
//  WritePort("*RST\n");

  WritePort("IFLOCK\n");
  WritePort("IRANGE1 2\n");

  WritePort("OCP1 5.000\n");
  WritePort("OVP1 5.000\n");

  WritePort("V1 5.000\n");
  WritePort("I1 0.0000\n");

  WritePort("OP1 1\n");
}


/*********************************/
/*      Release power source     */
/*********************************/

void PL155end(void)
{
  WritePort("IFUNLOCK\n");
  WritePort("LOCAL\n");
  printf("PL155 unlocked...\n");
}


/*********************************/
/*    Obtain temperature value   */
/*********************************/

float Tget(int Chan)
{
  int i;
  unsigned char *Wpt;

  int  Tmp[Ndet];
  int Temp[Ndet];

  float Value = -300.;

  for (i=0; i<Ndet; i++)  Tmp[i]=0;
  for (i=0; i<Ndet; i++) Temp[i]=0;

  sleep(1);

/* Request MC read */

  PING(MC, MCA, Read, 38);

/* pause between send and receive ethernet packets
  (hardware needs 10us to receive and to retransmit) */

  usleep(20);

  if (PONG(38));
    {
/* extract temperature values */

      eth_data = buffer + ETH_HLEN;

      Wpt = &eth_data[12];

      for (i=0; i<Ndet; i++)
         {
           int ADRESH;
           int ADRESL;
           int ADRESM;

           ADRESH = (int)*Wpt++;
           ADRESL = (int)*Wpt++;

           ADRESM = (ADRESH & 0x1C)<<3;
           ADRESH = (ADRESH & 0x03)<<8;

           Tmp[i] = ADRESH | ADRESM | ADRESL;

         } /* end for(Ndet) */

/* Mapping temperature channels and pedestal subtraction */

      Temp[0]  = Tmp[7]  - Tmp[2];
      Temp[1]  = Tmp[6]  - Tmp[2];
      Temp[2]  = Tmp[5]  - Tmp[2];
      Temp[3]  = Tmp[4]  - Tmp[2];
      Temp[4]  = Tmp[3]  - Tmp[2];
      Temp[5]  = Tmp[1]  - Tmp[2];
      Temp[6]  = Tmp[0]  - Tmp[2];
      Temp[7]  = Tmp[11] - Tmp[2];
      Temp[8]  = Tmp[9]  - Tmp[2];
      Temp[9]  = Tmp[8]  - Tmp[2];
      Temp[10] = Tmp[10] - Tmp[2];
      Temp[11] = Tmp[12] - Tmp[2];
      Temp[12] = Tmp[2];

      Value = ((float)Temp[Chan] - 127.94) / (-5.94);

    } /* end if(PONG) */

  return Value;

} /* end Tget */


/*********************************/
/*          PID algorithm        */
/*********************************/

void PL155drive(float Eact)
{
  int i;
  float Iinc;

  unsigned char Amps[6];
  unsigned char Comm[10];

  for (i=0; i<6;  i++) Amps[i] = 0x00;
  for (i=0; i<10; i++) Comm[i] = 0x00;

/* Calculate incremental control value */

  Iinc = Kp(Iact) * (Eact - Eold) + Kp(Iact) * Eact * Ts / Ti;

/* Integrator unti windup */

  if (((Iact + Iinc)>Imax) || ((Iact + Iinc)<Imin)) Iinc = 0.;

/* Actuate output */

  Iact += Iinc;
  Eold = Eact;

  sprintf(Amps, "%6.4f", Iact);
  strcat(Comm, "I1 ");
  strcat(Comm,  Amps);
  strcat(Comm, "\n");

  WritePort(Comm);
}


/***************************************/
/*             main module             */
/***************************************/

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

  float Tset = 20.;

  if (argc==2) Tset = atof(argv[1]);

/* Create socket */

  eth_raw_init();
  buffer = (void*)malloc(ETHER_MAX_LEN);

/* set structurised pointer to beginning of ethernet header */

  eth_head = buffer;  
  eh = (struct ethhdr *)eth_head;

/* Init devices */

  PL155init();


/* Let the data ripple through and get initial temperature */

  for (i=0; i<3; i++) while ((Tact = Tget(0)) == -300.);


/* PID loop */

  printf("Starting control loop...\n");
  while(1)
     {
       float Traw;

       while ((Traw = Tget(0)) == -300.);
       Tact = (1.- A) * Tact + A * Traw;

       printf("%8.3f\n", Tact);

       PL155drive(Tact - Tset);
     }

  return 0;

} /* end main */



/***************************************/
/*            eth_raw_init             */
/***************************************/

int eth_raw_init()
{
  int i;
  struct ifreq ifr;
  int ifindex = 0;  // Ethernet Interface index
  
/* Open socket */

  s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  if (s == -1) { perror("socket():"); exit(1); }

  printf("Successfully opened socket: %i\n", s);
  
/*Get ethernet interface index */

  strncpy(ifr.ifr_name, DEVICE, IFNAMSIZ);
  if (ioctl(s, SIOCGIFINDEX, &ifr) == -1) { perror("SIOCGIFINDEX"); exit(1); }

  ifindex = ifr.ifr_ifindex;
  printf("Successfully got interface index: %i\n", ifindex);
  
/* retrieve corresponding MAC */

  if (ioctl(s, SIOCGIFHWADDR, &ifr) == -1) { perror("SIOCGIFINDEX"); exit(1); }

  for (i=0; i<6; i++) src_mac[i] = ifr.ifr_hwaddr.sa_data[i];

  printf("Host MAC address: ");
  for (i=0; i<6; i++) printf("%02X:", src_mac[i]);
  printf("\n");
  
/* prepare sockaddr_ll */

  socket_address.sll_family   = PF_PACKET;
  socket_address.sll_protocol = 0x00;
  socket_address.sll_ifindex  = ifindex;
  socket_address.sll_pkttype  = PACKET_BROADCAST;
  socket_address.sll_halen    = ETH_ALEN;
  socket_address.sll_addr[0]  = dest_mac[0];
  socket_address.sll_addr[1]  = dest_mac[1];
  socket_address.sll_addr[2]  = dest_mac[2];
  socket_address.sll_addr[3]  = dest_mac[3];
  socket_address.sll_addr[4]  = dest_mac[4];
  socket_address.sll_addr[5]  = dest_mac[5];
  socket_address.sll_addr[6]  = 0x00; 
  socket_address.sll_addr[7]  = 0x00;
														
  
/* establish signal handler */

  signal(SIGINT, sigint);
  printf("Established signal handler for SIGINT\n");
  
  return 0;
}

/***************************************/
/*          Socket Terminator          */
/***************************************/

void sigint(int signum)
{
  struct ifreq ifr;
	
  if (s == -1) return; // No socket has been created
	
  PL155end();

  strncpy(ifr.ifr_name, DEVICE, IFNAMSIZ);
  ioctl(s, SIOCGIFFLAGS, &ifr);
  ifr.ifr_flags &= ~IFF_PROMISC;
  ioctl(s, SIOCSIFFLAGS, &ifr);
  close(s);

  free(buffer);
	
  printf("Client terminating...\n");
  exit(0);
}